Different IMU sketch experiment

[FocusWii]

I have a Flip board from Whitespy. This is a 328 based board and has a 6050 MPU. Fabio's FreeIMU library will run on it, though the orientation of the sensor has given me a small amount of trouble with roll and pitch reversed. What I can't seem to figure out is how to use the FreeIMU orientation solution in the MultiWii code. I can see that the MultiWii IMU sketch is working with acc and gyro raw data while I have written some code that takes the quaternion output from the MPU and converts to a 3x3 matrix. This 3x3 matrix is then used to calculate roll,pitch,yaw. It works great by itself. It's about 100 lines of code and uses about 15,000 bytes.

How would I take already computed roll, pitch, yaw and use it in the MultiWii code? At what point does MultiWii call it roll, pitch, yaw?

Could I substitute elements of my matrix, with some maths, for IMU sketch fused data?

[FocusWii]

Here's what I'm thinking, v2.2 IMU sketch has something that looks similar to my code in that it looks to me like it is using certain elements of a matrix.

Code: Select all

  int32_t sqGZ = sq(EstG32.V.Z);
  int32_t sqGX = sq(EstG32.V.X);
  int32_t sqGY = sq(EstG32.V.Y);
  int32_t sqGX_sqGZ = sqGX + sqGZ;
  float invmagXZ  = InvSqrt(sqGX_sqGZ);
  invG = InvSqrt(sqGX_sqGZ + sqGY);
  angle[ROLL]  = _atan2(EstG32.V.X , EstG32.V.Z);
  angle[PITCH] = _atan2(EstG32.V.Y , invmagXZ*sqGX_sqGZ);
  
  
  
    #if MAG
    heading = _atan2(
      EstM32.V.Z * EstG32.V.X - EstM32.V.X * EstG32.V.Z,
      EstM32.V.Y * invG * sqGX_sqGZ  - (EstM32.V.X * EstG32.V.X + EstM32.V.Z * EstG32.V.Z) * invG * EstG32.V.Y ); 
    heading += MAG_DECLINIATION * 10; //add declination
    heading = heading /10;
  #endif
}

Here's what my code has:

Code: Select all

    euler_y = atan2(-DCM_Matrix[2][0], DCM_Matrix[0][0]); //roll
    euler_z = asin(DCM_Matrix[1][0]); //,DCM_Matrix[2][2]); //pitch
    euler_x = atan2(-DCM_Matrix[1][2],DCM_Matrix[1][1]);  //yaw

[crazyal]

it's actually pretty easy, just do angle[ROLL] = euler_y; angle[PITCH] = euler_z; and heading = euler_x;
for the heading that is if you are also using the magnetometer with the mpu6050 otherwise don't change it.
just note that heading is in degrees and roll/pitch is in degrees*10 so 100 means 10.0 degrees. So if the angles differ in that matter you'd have to adapt the scale.

[FocusWii]

it's actually pretty easy, just do angle[ROLL] = euler_y; angle[PITCH] = euler_z; and heading = euler_x;
for the heading that is if you are also using the magnetometer with the mpu6050 otherwise don't change it.
just note that heading is in degrees and roll/pitch is in degrees*10 so 100 means 10.0 degrees. So if the angles differ in that matter you'd have to adapt the scale.

That's a big help, thanks. I'll fiddle with that and see what I get.

[FocusWii]

I tried to just add the FreeIMU libraries and I get an error about __vector_18 and __vector_19 multiple definition. It mentions HardwareSerial.cpp and Serial.ino. I can't even find a file named Serial.ino to check the line number. I don't know what happened. At one point I was able to put my code in. I got lots of errors but it accepted the libraries. I looked at HardwareSerial.cpp and line 116 refers to this:

Code: Select all

#if defined(USART_RX_vect)
  SIGNAL(USART_RX_vect)

For what it's worth here's my code:

Code: Select all

float test = 0.000;
float euler_x;
float euler_y;
float euler_z;


float q[4];


long convert_to_dec(float x)
{
  return x*10000000;
}



// Set the FreeIMU object
FreeIMU my3IMU = FreeIMU();

void setup() {
  //Serial.begin(115200);
  Serial.begin(38400);
  Wire.begin();
  

  
  delay(5);
  my3IMU.init();
  delay(5);
}


void loop() { 
  my3IMU.getQ(q);
 
float sqw = q[0]*q[0];
float sqx = q[3]*q[3];
float sqy = q[2]*q[2];
float sqz = q[1]*q[1];

//q[3] = x, q[2] = y, q[1] = z, q[0] = w
float DCM_Matrix[3][3]=
{
  {(1-2*sqy -2*sqz), (2*q[3]*q[2]-2*q[1]*q[0]), (2*q[3]*q[1]+2*q[2]*q[0])},
  {(2*q[3]*q[2]+2*q[1]*q[0]), (1-2*sqx-2*sqz), (2*q[2]*q[1]-2*q[3]*q[0])},
  {(2*q[3]*q[1]-2*q[2]*q[0]), (2*q[2]*q[1]+2*q[3]*q[0]), (1-2*sqx-2*sqy)}
  
};

if (DCM_Matrix[1][0] > 0.998){
  euler_y = atan2(DCM_Matrix[0][2], DCM_Matrix[2][2]);
  euler_z = M_PI/2;
  euler_x = 0;
}
if (DCM_Matrix[1][0] < -0.998){
  euler_y = atan2(DCM_Matrix[0][2], DCM_Matrix[2][2]);
  euler_z = -M_PI/2;
  euler_x = 0;
}


    euler_y = atan2(-DCM_Matrix[2][0], DCM_Matrix[0][0]); //roll
    euler_z = asin(DCM_Matrix[1][0]); //,DCM_Matrix[2][2]); //pitch
    euler_x = atan2(-DCM_Matrix[1][2],DCM_Matrix[1][1]);  //yaw
 

[FocusWii]

Of course I broke something by trying this. It has to do with Serial, duh. Once I disabled that with a /* */, I put my code in and it runs, but I have no idea what's going on with it. Something gets redefined by the FreeIMU library, but I'm clueless how to fix it or how to get some kind of output so I can see how crummy it's working. By taking all the sensor initialization out though, the code is now 17,000 bytes.

Code: Select all

#if defined(PROMINI) || defined(MEGA)
  #if defined(PROMINI)
  ISR(USART_UDRE_vect) {  // Serial 0 on a PROMINI
  #endif
  #if defined(MEGA)
  ISR(USART0_UDRE_vect) { // Serial 0 on a MEGA
  #endif
    uint8_t t = serialTailTX[0];
    if (serialHeadTX[0] != t) {
      if (++t >= TX_BUFFER_SIZE) t = 0;
      UDR0 = serialBufferTX[t][0];  // Transmit next byte in the ring
      serialTailTX[0] = t;
    }
    if (t == serialHeadTX[0]) UCSR0B &= ~(1<<UDRIE0); // Check if all data is transmitted . if yes disable transmitter UDRE interrupt
  }
#endif 

[FocusWii]

I am getting something now. I stuck some Serial.print in the main loop to try and print out the angle[ROLL] angle[PITCH] and for a split second I get numbers and then it's just zeros.

[timecop]

Don't you wish you were doing this on hardware where you can have proper debugging, instead of printing shit to serial port?

[FocusWii]

How would that work? I've got a lot to learn.

I got it working somewhat, but I broke all kinds of stuff in the process. I can print angle[ROLL] angle[PITCH] angle[YAW] after adding Wire.begin to setup(), but when I run the MW Gui, nothng but zeros.

[FocusWii]

Don't you wish you were doing this on hardware where you can have proper debugging, instead of printing shit to serial port?

I probably do, but I'm not sure what solution to debugging you have. Whatcha talking about now you got my attention?

[FocusWii]

it's actually pretty easy, just do angle[ROLL] = euler_y; angle[PITCH] = euler_z; and heading = euler_x;
for the heading that is if you are also using the magnetometer with the mpu6050 otherwise don't change it.
just note that heading is in degrees and roll/pitch is in degrees*10 so 100 means 10.0 degrees. So if the angles differ in that matter you'd have to adapt the scale.

You were right on, thanks. It's working as far as getting angle[xxx] but angle[YAW] has issues. Something in the MW code is blowing up the angle[YAW]. I can see the underlying FreeIMU solution and then about every second or so, angle[YAW] is some huge number and then back to normal. My standalone version of FreeIMU doesnt do that, so I'm thinking it's something in the MW but what?

[FocusWii]

Here's what I mean by angle[YAW] going along fine and then Bam, blows up, and then back to normal.

[FocusWii]

That bug was too much for me. What I'm thinking now is to just borrow the MultiWii PPM stuff that works on this board since I broke the Serial.

[timecop]

Don't you wish you were doing this on hardware where you can have proper debugging, instead of printing shit to serial port?

I probably do, but I'm not sure what solution to debugging you have. Whatcha talking about now you got my attention?

ARM Cortex-M with debugging by SWD, where you can watch variables in memory in realtime, set breakpoints, tracepoints, conditional breakpoints, oh and the part where you have tons of peripherals for trivial things like PWM output or capture, so you don't have to waste time optimizing software PWM generation or worrying about missing RC pulse ticks...

[FocusWii]

Don't you wish you were doing this on hardware where you can have proper debugging, instead of printing shit to serial port?

I probably do, but I'm not sure what solution to debugging you have. Whatcha talking about now you got my attention?

ARM Cortex-M with debugging by SWD, where you can watch variables in memory in realtime, set breakpoints, tracepoints, conditional breakpoints, oh and the part where you have tons of peripherals for trivial things like PWM output or capture, so you don't have to waste time optimizing software PWM generation or worrying about missing RC pulse ticks...

That sounds very powerful in the right hands. Right now I'm wasting time trying to understand why I can't use pin D2 to read PPM. I know this sketch works on a board which has pin D8, so I just changed the #define icpPin number to 2 and nothing happens. I don't really know assembly so...

Here's what I'm trying to use now to read PPM:

Code: Select all

// R/C RADIO PPM SIGNAL READ (USING TIMER1 INPUT CAPTURE AND OVERFLOW INTERRUPTS)
// AND SERVO OUTPUT ON OC1A, OC1B pins [Disabled now]
// SERVO OUTPUT FUNCTION USING TIMER2 OVERFLOW INTERRUPT 

// Timer1 Overflow 
// Detects radio signal lost and generate servo outputs (overflow at 22ms (45Hz))
ISR(TIMER1_OVF_vect){

  //TCNT1 = 20000;        // at 16Mhz (0.5us) (65535-20000) = 22 ms (45Hz)
  //OCR1A = 20000 + (Servo1<<1);  // Output for servos...
  //OCR1B = 20000 + (Servo2<<1);
  
  //TCCR1A = 0xF0;        // Set OC1A/OC1B on Compare Match
  //TCCR1C = 0xC0;        // Force Output Compare A/B (Start Servo pulse)
  //TCCR1C = 0x00;        
  //TCCR1A = 0xA0;     // Clear OC1A/OC1B on Compare Match
  
  TCNT1 = 0;
  Timer1_last_value=0xFFFF;  // Last value before overflow...

  // Radio signal lost...
  radio_status = 0;
}


// Capture RX pulse train using TIMER 1 CAPTURE INTERRUPT
// And also send Servo pulses using OCR1A and OCR1B [disabled now]
// Servo output is synchronized with input pulse train
ISR(TIMER1_CAPT_vect)
{
   if(!bit_is_set(TCCR1B ,ICES1)){       // falling edge?
    if(Rx_ch == MAX_CHANNELS) {        // This should be the last pulse...
        Pulses[Rx_ch++] = ICR1;  
             radio_status = 1;              // Rx channels ready...
     }
         TCCR1B = 0x42;              // Next time : rising edge
         Timer1_last_value = TCNT1;  // Take last value before reset         
         TCNT1 = 0;                  // Clear counter
         
         // Servo Output on OC1A/OC1B... (syncronised with radio pulse train)
         //TCCR1A = 0xF0;        // Set OC1A/OC1B on Compare Match
         //TCCR1C = 0xC0;        // Force Output Compare A/B (Start Servo pulse)
         //TCCR1C = 0x00;      
         //TCCR1A = 0xA0;     // Clear OC1A/OC1B on Compare Match         
   }
   else {              // Rise edge
     if ((ICR1-ICR1_old) >= SYNC_GAP_LEN){   // SYNC pulse?
      Rx_ch = 1;             // Channel = 1
                Pulses[0] = ICR1;
       }
          else {
            if(Rx_ch <= MAX_CHANNELS)
          Pulses[Rx_ch++] = ICR1;    // Store pulse length
            if(Rx_ch == MAX_CHANNELS)
               TCCR1B = 0x02;             // Next time : falling edge
            }
   }
   ICR1_old = ICR1;
}


// Servo Input PPM Initialization routine
void RxServoInput_ini()
{
  pinMode(icpPin,INPUT);
  Rx_ch = 1;
  //TCCR1A = 0xA0;           // Normal operation mode, PWM Operation disabled, clear OC1A/OC1B on Compare Match
  TCCR1A = 0x00;        // COM1A1=0, COM1A0=0 => Disconnect Pin OC1 from Timer/Counter 1 -- PWM11=0,PWM10=0 => PWM 
  TCCR1B = 0x42;        // TCNT1 preescaler/8 (16Mhz => 0.5useg, 8 Mhz => 1useg), Rising edge
  TIMSK1 = _BV(ICIE1)|_BV (TOIE1);   // Enable interrupts : Timer1 Capture and Timer1 Overflow
 
  Neutral[1] = MIN_THROTTLE;
  Neutral[2] = MIN_THROTTLE;
  Neutral[3] = MIN_THROTTLE;
  Neutral[4] = CHANN_CENTER;
  Neutral[5] = CHANN_CENTER;
  Neutral[6] = CHANN_CENTER;
  Servo1 = MIN_THROTTLE;
  Servo2 = MIN_THROTTLE;
}

int RxGetChannelPulseWidth( uint8_t channel)
{
  unsigned int result;
  unsigned int result2;
  unsigned int pulso_ant;
  unsigned int pulso_act;
  
  // Because servo pulse variables are 16 bits and the interrupts are running values could be corrupted.
  // We dont want to stop interrupts to read radio channels so we have to do two readings to be sure that the value is correct...
  result =  Pulses[channel];
  result2 =  Pulses[channel];
  if (result != result2)
    result =  Pulses[channel];   // if the results are different we make a third reading (this should be fine)
  pulso_act = result;
  
  pulso_ant = Pulses[channel-1];
  result2 =  Pulses[channel-1];
  if (pulso_ant != result2)
     pulso_ant = Pulses[channel-1];   // if the results are different we make a third reading (this should be fine)

    result = (result - pulso_ant)>>1;        // Restamos con el valor del pulso anterior y pasamos a microsegundos (reloj a 0.5us)
      
  if ((result > MIN_IN_PULSE_WIDTH)&&(result < MAX_IN_PULSE_WIDTH))  // Out of range?
    return result;
  else
    {
    return Neutral[channel];
    }
}

// SERVO SIGNAL OUTPUT (USING TIMER2 OVERFLOW INTERRUPT AND TIMER1 READINGS)
ISR (TIMER2_OVF_vect)
{ 
  int us;
  int aux;
  
  if (Servo_Channel < num_servos){
    Servo_Timer2_timer1_stop = TCNT1;       // take the timer1 value at this moment
    
    // Now we are going to calculate the time we need to wait until pulse end
    if (Servo_Timer2_timer1_stop>Servo_Timer2_timer1_start)   // Timer1 reset during the pulse?
      Servo_Timer2_pulse_length = Servo_Timer2_timer1_stop-Servo_Timer2_timer1_start;
    else
      Servo_Timer2_pulse_length = ((long)Servo_Timer2_timer1_stop + Timer1_last_value) - (long)Servo_Timer2_timer1_start;
    us = (Servos[Servo_Channel].value) - (Servo_Timer2_pulse_length>>1);
    us -= 2;  // Adjust for the time of this code
    if (us>1)
      {
      us <<= 2; // Translate us to the 4 cyles loop (1/4 us)
      __asm__ __volatile__ (  // 4 cycles loop = 1/4 us  (taken from delayMicroSeconds function)
   "1: sbiw %0,1" "\n\t"            // 2 cycles
   "brne 1b" : "=w" (us) : "0" (us) // 2 cycles
   );
      }
    digitalWrite( Servos[Servo_Channel].pin,LOW);    // pulse this channel low
    Servo_Channel++;                                 // increment to the next channel
    }
  else
    Servo_Channel = 0;                // SYNC pulse end => Start again on first channel
   
  if (Servo_Channel == num_servos){           // This is the SYNC PULSE
    TCCR2B = _BV(CS20)|_BV(CS21)|_BV(CS22);   // set prescaler of 1024 => 64us resolution (overflow = 16384uS) 
    TCNT2 = 0x04;   //64usx4 = 256us
    }
  else{
    TCCR2B = _BV(CS20)|_BV(CS22);                  // Set prescaler of 128  (8uS resolution at 16Mhz) 
    TCNT2 = Servos[Servo_Channel].counter;         // Set the clock counter register for the overflow interrupt
    Servo_Timer2_timer1_start = TCNT1;             // we take the value of Timer1 at the start of the pulse
    digitalWrite(Servos[Servo_Channel].pin,HIGH);  // Pulse start. Let´s go...
    }
}

void Servo_Timer2_ini()
{ 
  // Servos must have correct values at this moment !! Call First Servo_Timer2_set() function...
  // Variable initialization  
  Servo_Channel = 0;
  TIMSK2 = 0;  // Disable interrupts 
  TCCR2A = 0;  // normal counting mode 
  TCCR2B = _BV(CS20)|_BV(CS22);            // Set prescaler of 128  (8uS resolution at 16Mhz)
  TCNT2 = Servos[Servo_Channel].counter;   // Set the clock counter register for the overflow interrupt
  TIFR2 = _BV(TOV2);  // clear pending interrupts; 
  TIMSK2 =  _BV(TOIE2) ; // enable the overflow interrupt     
}

void Servo_Timer2_set(uint8_t servo_index, int value)
{
  int aux;
    
  if (value > SERVO_MAX_PULSE_WIDTH)
    value = SERVO_MAX_PULSE_WIDTH;
  else if (value < SERVO_MIN_PULSE_WIDTH)
    value = SERVO_MIN_PULSE_WIDTH;
  
  Servos[servo_index].value = value; // Store the desired value on Servo structure
  
  value = value - 20;  // We reserve 20us for compensation...
  
  // Calculate the overflow interrupt counter (8uS step)
  aux = value>>3;  // value/8
  Servos[servo_index].counter = 256 - aux;
}

[FocusWii]

I found out why that Rx sketch won't work on pin D2 and that's because TIMER1_CAPT_vect is only available on pin D8. Unless there's a way to redirect timer1_capture to pin D2, I'll have to try to use the MultiWii way of reading D2.

[FocusWii]

I got back to this little experiment and got it reading PPM on D2 and using FreeIMU to determine attitude. I'm using a different stability and mixing program, but I'm using the MultiWii way for reading the Rx and for outputting to the motors on a tricopter. That works, but the servo doesn't.

What I still can't figure out is how to properly work with interrupts and registers. I tried outputting to the servo on D3 like it's a motor[3] because I can see the writeMotors() activates that pin if I tell it I have 4 MOTORS. The servo doesn't like it, but at least there's something getting through.

If I could weed through how to get writeServos() to write to the servo, It would work as a tricopter and I could clean up the code and work on the PID's and control.
I can read the Rx, I can calculate attitude and then apply that and get a value for what the servo[5] should be. I just can't figure out how to write that value.

Any clues on how to setup the servo interrupt, timing, output??

[timecop]

Unless you're using mega or tarduino32u4, servo writes are basically bitbanged.

[FocusWii]

This is the only thing I can find in MultiWii that seems related but I'm not sure how to use that. The original servo output program I had used timer 1 overflow, I can't use that because it conflicts with the motors, so basically I can run the motors or I can control a servo but not both.

#if defined(SERVO_1_HIGH)
#if defined(PROMINI) || (defined(PROMICRO) && defined(HWPWM6)) // uses timer 0 Comperator A (8 bit)
TCCR0A = 0; // normal counting mode
TIMSK0 |= (1<<OCIE0A); // Enable CTC interrupt
#define SERVO_ISR TIMER0_COMPA_vect
#define SERVO_CHANNEL OCR0A
#define SERVO_1K_US 250
#endif

[timecop]

time to upgrade to real hardware and software.

[shikra]

@ focus - interesting post this because last night I was looking into some odd yaw behaviour reported with 2.3 on tricopter servo and I noticed that in moving yaw I am seeing fast repetitive glitches like this when rotating. Because so short wouldn't notice unless looked at it
Just from the glitch it looks like something like you highlight in the data. I haven't looked at the data yet, but I was going to try capturing as you have as next step...

TC - yes. I have a Naza on order

[FocusWii]

shikra, I have no idea what that glitch is. It only shows up once in a while for one iteration and then right back to normal.

I'm trying to hack up my own version and can't figure out how MultiWii controls the servo. I know there's a function called writeServos() that I haven't been able to use. I add it to my sketch and it just sits there while the writeMotors() works fine.

[shikra]

I'll do some digging on the glitch in a couple of weeks when freed up

I don't follow the servo / motor code at all, but as a start... have you defined these to enable .. only pointer can give..
SERVO
PRI_SERVO_FROM
PRI_SERVO_TO

[timecop]

TC - yes. I have a Naza on order

naza tricopter support is great

[FocusWii]

I'll do some digging on the glitch in a couple of weeks when freed up

I don't follow the servo / motor code at all, but as a start... have you defined these to enable .. only pointer can give..
SERVO
PRI_SERVO_FROM
PRI_SERVO_TO

Thanks, I was looking at all that and you triggered something. I'm reducing all the servo and motor stuff and removing conditional defines to reveal what makes it go. I'm taking the Multi out of it.

[shikra]

I just don't understand why they don't support Tri..... Its why I have never bought one

I was jesting, although one day I am going to try one just to see...
I do have one of the earliest naze ....

TC - yes. I have a Naza on order

naza tricopter support is great

[FocusWii]

This is frustrating. I was able to reduce the writeMotors() function to just a few lines that apply to my tricopter and it works. When I try to do the same with the writeServos() I get nothing. Here's how simple the motors looks when it doesn't have to eliminate all the different board types.

/**************************************************************************************/
/************  Writes the Motors values to the PWM compare register  ******************/
/**************************************************************************************/
void writeMotors() { // [1000;2000] => [125;250]

OCR1A = motor[0]>>3; // pin 9
OCR1B = motor[1]>>3; // pin 10
OCR2A = motor[2]>>3; // pin 11

    #if (NUMBER_MOTOR > 0)
      TCCR1A |= _BV(COM1A1); // connect pin 9 to timer 1 channel A
    #endif
    #if (NUMBER_MOTOR > 1)
      TCCR1A |= _BV(COM1B1); // connect pin 10 to timer 1 channel B
    #endif
    #if (NUMBER_MOTOR > 2)
      TCCR2A |= _BV(COM2A1); // connect pin 11 to timer 2 channel A
    #endif

}

[Crashpilot1000]

Sorry for being OT concerning alternative IMU and mwii2.3. My last Tri (years ago) worked perfectly with mwii 1.9 (until i sank it into a river...).
Maybe take a look into the older 1.9 mwii code?
P.s.: Yes, it is a shame that DJI has no official Tricopter mixer.

[FocusWii]

I got it working. I'm not real happy about the initialization of the servo since it gets lumped in with the motor[] and I can hear a buzz before the main loop runs. I changed the >>3 to a >>2 and just treat the servo[] as a motor[].

OCR2B |= ((motor[3]>>2) - 250); // pin 3

I tried - 250 and see somewhere that the kHz is 420 or something and should be 1 kHz, so I changed it to OCR2B |= ((motor[3]>>2) - 1000); // pin 3

That seems to work better, but I'm not understanding how servos use High and Low or how to set it up to use timer0 for the servos. I'm only using one servo which should be simple, but I'm no programmer and barely understand what I'm doing. Is the High and Low for the bit banging that timecop mentioned?

[FocusWii]

it's actually pretty easy, just do angle[ROLL] = euler_y; angle[PITCH] = euler_z; and heading = euler_x;
for the heading that is if you are also using the magnetometer with the mpu6050 otherwise don't change it.
just note that heading is in degrees and roll/pitch is in degrees*10 so 100 means 10.0 degrees. So if the angles differ in that matter you'd have to adapt the scale.

I tried my control code and it was horrible, so I went back and tried to insert my IMU into the Multiwii. It accepts my attitude data, but It breaks serial and I don't know why. Here it is forcing serial.print at the end of the main loop:

https://www.youtube.com/watch?v=0qG0WbEu5qI&feature=youtu.be

I can't arm it to try and fly it. When I flip the gear switch the blue light just blinks. Looks like I have to redo all the PID stuff since I'm not using any of the sensor routines.