Espritmodel.com Telemetry Radio
Reply
Thread Tools
Old Jun 11, 2012, 05:11 AM
Registered User
unisev's Avatar
Joined Sep 2011
54 Posts
Step 1

Here is the old version schema :



And the associated timeline :



This version is fully working, here is the non-cleaned & non-optimised code :

Code:
// Sketch to read a Rx receiver and to apply first channels to some servos
// V003
// Frome Sev

#define NO_PORTB_PINCHANGES //to go faster
#define NO_PORTC_PINCHANGES //to go faster

#include "PinChangeInt.h"

#include <Servo.h>

// LED pins
#define LEDPINO 13 //local Arduino PIN
#define LEDPINR 12 //RED LED for visible failsafe

// Parameters (total NBOFCH+NBOFSERVO can only be 10 MAX)
#define NBOFCH 6 // Put here the total Rx channel number and plug the FIRST channel on the PIN 2
#define NBOFSERVO 4 // Put here the total servo number and plug the FIRST one directely after the last Rx channel
#define FSCH 1 // This is the channel that will be survey for failsafe

// ServoParam
Servo outServ[NBOFSERVO];
int servPos[NBOFSERVO];


// Loop TimeStamp variables
unsigned long startLpTS, endLpTS, loopTime;
int i;

// Blink LED variables
int ledState[15] = {
  LOW,LOW,LOW,LOW,LOW,LOW,LOW,LOW,LOW,LOW,LOW,LOW,LOW,LOW,LOW};  //TODO : Clean this dirty memory usage
unsigned long previousTS[15] = {
  0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};                //TODO : Clean this dirty memory usage

// Blink LED function
void BLINK_LED(const int bLed,int bTimeUp,int bTimeDo)
{
  unsigned long currentTS = millis();
  int nowDelay;
  if (ledState[bLed]==HIGH) nowDelay=bTimeUp;
  else nowDelay=bTimeDo;
  if(currentTS - previousTS[bLed] > nowDelay) {
    previousTS[bLed] = currentTS;   
    if (ledState[bLed] == LOW) ledState[bLed] = HIGH;
    else       ledState[bLed] = LOW;
    digitalWrite(bLed, ledState[bLed]);
  }
}

// ReadReciever variables
boolean failsafe;
boolean endTrame;
unsigned long vchUp[NBOFCH+1];
int preCh; //TODO : Make this varaible cleared when entering in main loop (just use in setup)

unsigned long vchOld;

unsigned long vchLenght;

struct channel {
  int pos; // Why don't we just use an int ?
  int nxtCh;
  boolean autoP;
  int fPos;
};

struct channel ch[NBOFCH];

void DO_CHA()  // Interrupt use only to detect channel order
{
  if (PCintPort::arduinoPin==2){
    preCh=0;
    i=0;
  }
  else {
    ch[preCh].nxtCh = PCintPort::arduinoPin-2;
    preCh=PCintPort::arduinoPin-2;
    i++;
    if (i==NBOFCH-1) {
      ch[PCintPort::arduinoPin-2].nxtCh=NBOFCH;
      endTrame=1;
      preCh=PCintPort::arduinoPin-2;
    }
  }
}  

void setup()
{
  // DEBUG serial OUTPUT
  Serial.begin(57600);
  Serial.println(";");
  Serial.print("Receiver with ");
  Serial.print(NBOFCH);
  Serial.print(" channels and ");
  Serial.print(NBOFSERVO);
  Serial.println(" servos");

  Serial.print("Channel order : ");

  pinMode(LEDPINO, OUTPUT);
  pinMode(LEDPINR, OUTPUT);

  for (i=0; i < NBOFCH; i++){
    PCintPort::attachInterrupt(i+2, DO_CHA,RISING);
  }

  while(!endTrame){
    BLINK_LED(LEDPINO,50,50);
    BLINK_LED(LEDPINR,200,50);
  }

  for (i=0; i < NBOFCH; i++){
    PCintPort::detachInterrupt(i+2);
  }

  endTrame=0;

  for (int i=0; i < NBOFCH;i++) {
    Serial.print(ch[i].nxtCh);
    Serial.print(";");
  }
  Serial.println(";");

  // FailSafe Servos position
  ch[0].fPos = 1000;
  ch[1].fPos = 1500;
  ch[2].fPos = 1500;
  ch[3].fPos = 1500;

  FAILSAFE(); //Enabling FailSafe for a safe start



  // Enable final Interruption on channel's PIN
  for (int i=0; i < NBOFCH; i++){
    PCintPort::attachInterrupt(i+2, TS_REDG,RISING);
  }
  PCintPort::detachInterrupt(preCh+2);    // Detach last channel normal interrupt
  PCintPort::attachInterrupt(preCh+2, TS_CEDG,CHANGE); // Attach last channel special interrupt

  // Servo attachement
  for (int i=0; i < NBOFSERVO; i++){
    outServ[i].attach(i+NBOFCH+2);
  }

  Serial.print("SREG :");
  Serial.print(SREG,BIN);
  Serial.println(";");
  Serial.print("PCICR :");
  Serial.print(PCICR,BIN);
  Serial.println(";");
  Serial.print("PCMSK2 :");
  Serial.print(PCMSK2,BIN);
  Serial.println(";");
  Serial.print("PCMSK1 :");
  Serial.print(PCMSK1,BIN);
  Serial.println(";");
  Serial.print("PCMSK0 :");
  Serial.print(PCMSK0,BIN);
  Serial.println(";");

}




void loop()
{
  //TimeStamp Loop begining
  startLpTS = millis();

  if ((startLpTS*1000)>(vchUp[FSCH-1]+100000)){
    FAILSAFE();
  } //Detect failsafe on the (FSCH) Channel
  else{
    if (failsafe) {     //failsafe Exit  
      failsafe=0;
      digitalWrite(LEDPINR, LOW);
      endTrame=0;
    }
    else{
      if  (endTrame) { //End of trame detection
        noInterrupts(); //Stop IRQ & copy channels values
        for (int i=0; i < NBOFCH; i++){
          ch[i].pos=vchUp[ch[i].nxtCh]-vchUp[i];
        }
        vchLenght=vchUp[0]-vchOld;
        vchOld=vchUp[0];
        interrupts();  //Restart IRQ & Reset endTrameflag
        endTrame=0;
        // Set servos as Channel
        Serial.print("UPD");
        for (int i=0; i < NBOFSERVO; i++){
          outServ[i].writeMicroseconds(ch[i].pos);
        }
      }
    }
  }

  // DEBUG : Serial output the channels
  for (int i=0; i < NBOFCH; i++){
    Serial.print(ch[i].pos);
    Serial.print(";");
  }

  //Blink "normaly" the Orange LED
  BLINK_LED(LEDPINO,500,500);



  // DEBUG : Serial output the Loop Duration (ms)
  Serial.print(vchLenght); 
  Serial.print(";");
  Serial.print(micros()-vchUp[0]); 
  Serial.print(";");

  //EndLoop TimeStamp
  endLpTS = millis();
  loopTime = endLpTS - startLpTS;

  Serial.print(loopTime); 
  Serial.println(";");
}

void FAILSAFE()  // In case of lost signal
{
  Serial.print("FailSafe;");
  if (!failsafe) {
    digitalWrite(LEDPINR, HIGH);
    // Set channels on failsafe positions
    for (int i=0; i < NBOFCH; i++){
      ch[i].pos=0;
    }
    // Set servos on failsafe positions
    for (int i=1; i < NBOFSERVO; i++){
      outServ[i].writeMicroseconds(ch[i].fPos);

    }
    failsafe=1;
  }
}  


void TS_REDG()  // Same Interrupt code on all channels except the last one
{
  vchUp[PCintPort::arduinoPin-2] = micros();
}  


void TS_CEDG()  // Special interrupt on last channel
{

  // If this is a rising edge, record it
  if(PCintPort::pinState == HIGH)
  { 
    vchUp[PCintPort::arduinoPin-2] = micros();
  }
  else
  {
    //If this is a falling edge, record it as last value
    vchUp[NBOFCH] = micros();
    endTrame=1; // End of trame is set when the last falling edge of the last channel has gone !
  }
}
unisev is offline Find More Posts by unisev
Last edited by unisev; Jul 15, 2012 at 04:37 PM.
Reply With Quote
Sign up now
to remove ads between posts
Old Jun 12, 2012, 05:44 AM
Registered User
unisev's Avatar
Joined Sep 2011
54 Posts
Step 2

The above project is working but some glitches are constated on input channel measures (around 4µs) and one some others on the servos output (4µs).

As everything is based on 4µs timers this seems normal (kind of "leap year" problem) and maybe acceptable.

But I'd like to go ahead, test new things... and without library if possible.

Sev
unisev is offline Find More Posts by unisev
Last edited by unisev; Jul 05, 2012 at 11:20 AM.
Reply With Quote
Old Jun 12, 2012, 12:24 PM
Registered User
unisev's Avatar
Joined Sep 2011
54 Posts
Here is the actual version schema :

---------------------------------------------------------------------------------------------------------------------------------------

---------------------------------------------------------------------------------------------------------------------------------------

And the actual time line :
---------------------------------------------------------------------------------------------------------------------------------------

---------------------------------------------------------------------------------------------------------------------------------------

The working up-to-date code :
Code:
// PPM read & PWM write V001
// Using timer1 to capture PPM signal & generate 2 servo signal

#define LEDPINO 13 //local Arduino PIN

boolean endTrame=0,failsafe=0,firstEdge=1;

// Rx capture data
volatile unsigned int vchUp[8];
volatile unsigned int vchPos[6];
unsigned int chPos[6];
byte curEdge;

// PWM Generator data
volatile unsigned int ocr1a,ocr1b;

void setup()
{
  // Pin setup
  pinMode(13,OUTPUT); //Arduino LED
  pinMode(9,OUTPUT); //Timer1 PWM1 (OCR1A)
  pinMode(10,OUTPUT); //Timer1 PWM2 (OCR1B)
  digitalWrite(13,HIGH); //Switch on the arduino LED

  // Timer1 setup
  TCCR1A=B10100000; // OCR2A/OCR2B : falling mode, timer: normal mode
  TCCR1B=B00000010; // 2MHz (arround 0.5µs precision), with falling edge CAPTUREPIN detection
  TIMSK1=B00100001; // Activate CAPTUREPIN interrupt & OVF interrupt
}

ISR(TIMER1_CAPT_vect)
{
  vchUp[curEdge]=TCNT1; // Take TimeStamp from 1 to 6
  curEdge++;
  if (curEdge>7) {     // From the 7th timestamp
    OCR1A=ocr1a;     //Update OCR1A & OCR1B when they are not read
    OCR1B=ocr1b;
    TCNT1=0;           // RESET COUNTER at end of trame, to avoid FailSafe detection
    TCCR1A=B11110000; // OCR2A/OCR2B : Rise mode
    TCCR1C=B11000000; // Trigger the rise
    TCCR1A=B10100000; // OCR2A/OCR2B : Falling mode
    if ((vchUp[7]-vchUp[1]) > 30000) {        //If total trame is higher than 15ms - false trame, bad synchro
      curEdge=0;            //Begining from 0 !!                      
    }
    else                                          //Else, a good trame is kept, reset edge counter and calculate channel values
    curEdge=1;
    for (int i=0;i<6;i++) {
      vchPos[i]=(vchUp[i+2]-vchUp[i+1])/2; //Measure channel values, divided by 2 to obtain µs
    }
    endTrame=1;           // To tell the main loop that news values are avaliable
  }
}

ISR(TIMER1_OVF_vect)
{
  TCCR1A=B11110000; // OCR2A/OCR2B : Rise mode
  TCCR1C=B11000000; // Trigger the rise
  TCCR1A=B10100000; // OCR2A/OCR2B : Falling mode
  OCR1A=ocr1a;     //Update OCR1A & OCR1B (while they are not read)
  OCR1B=ocr1b;
  failsafe=1; //The counter overflow around 32ms, used to activate failsafe
}

void loop()
{
  if (endTrame==1) {
    cli();
    for (int i=0;i<6;i++) {
      chPos[i]=vchPos[i]; //Take channel values
    }
    sei();
    endTrame=0;
    if (failsafe){ 
      failsafe=0;
    }

    // Here intelligent code that run only when Rx values are updated

    ocr1a=(chPos[4]*2); // Set servo 1 as channel 5
    ocr1b=(chPos[5]*2); // Set servo 2 as channel 6
  }

  // Here is the intelligent code that ALWAYS run

}
unisev is offline Find More Posts by unisev
Last edited by unisev; Jul 15, 2012 at 04:41 PM.
Reply With Quote
Old Jul 26, 2012, 07:41 AM
Registered User
Joined Jul 2012
1 Posts
Hi UniSev,

Are you the same that use this name (UniSev) on the french forum of the Arduino community ?

This guy seems to be french... I think he uses the same name that you.

JLB
jihelbi is offline Find More Posts by jihelbi
Reply With Quote
Old Jul 27, 2012, 03:12 AM
Registered User
unisev's Avatar
Joined Sep 2011
54 Posts
I'm french and I'm only one person, with same name & avatar on every forum.
unisev is offline Find More Posts by unisev
Reply With Quote
Old Aug 31, 2012, 02:14 PM
Registered User
Joined Sep 2010
2,408 Posts
Hi unisev! So could this add failsafe to cheap turnigy 9x receivers? I would need failsafe so that I could activate RTH on Arduplane_NG flight controller.
msev is online now Find More Posts by msev
Reply With Quote
Old Sep 10, 2012, 03:21 AM
Registered User
unisev's Avatar
Joined Sep 2011
54 Posts
Hello,

Maybe the Arduplane could directely detect lost signal on this cheap Rx, because when a loss signal arrive, this Rx is doing that :

Ch1 : No Pulse
Ch2 : No Pulse
Ch3 : No Pulse
Ch4 : Maintain last received signal
Ch5 : Maintain last received signal
Ch6 : No Pulse
PPM-out (on the batt pin) : No pulse

Sev
unisev is offline Find More Posts by unisev
Reply With Quote
Old Sep 10, 2012, 03:25 AM
Registered User
Joined Sep 2010
2,408 Posts
Nope the programmer Jean Louis Nadine doesn't want to add support...so an arduino between the receiver and FC would have to do that.
msev is online now Find More Posts by msev
Reply With Quote
Old Sep 10, 2012, 05:43 AM
Registered User
unisev's Avatar
Joined Sep 2011
54 Posts
I've take a rapid look at the ArduPlaneNG, the failsafe detection seems to be on the Throttle channel pulse.

I think the failsafe function should work as long as your throttle channel is not channel 4 or channel 5.

I don't think that add another arduino is a good solution and I don't think this topic is a ArduPilotNG support

UniseV
unisev is offline Find More Posts by unisev
Last edited by unisev; Sep 10, 2012 at 07:46 AM.
Reply With Quote
Reply


Thread Tools

Similar Threads
Category Thread Thread Starter Forum Replies Last Post
Question Program/configure ESC with Arduino bogophan Multirotor Talk 12 Jun 28, 2012 06:28 PM
Help! 1st Arduino nano quad copter build need help programming thorek Multirotor Talk 6 Mar 26, 2012 03:54 PM
Discussion Multiwii Arduino Programming dyarrow Multirotor Talk 3 Mar 07, 2012 10:50 AM
Discussion Engine Run In program using Arduino board cadzilla DIY Electronics 4 Aug 16, 2010 08:21 PM