Page 1 of 1

IMU Sensor Upside down (specific lines to modify)

Posted: Tue Jan 22, 2019 6:14 am
by cafeciaojoe
Hi Everyone,

Looking to find out exactly what was modified in the CF firmware to enable the crazyflie to fly with the deck upside down.

Here is the topic. I cannot see the part of the code that arnaud points to as it is a broken link.
viewtopic.php?t=1784

Furthermore, the user YARO states that "I'm here again! I've solved this problem, I was not correctly changing sings and not in the right place. " However he does not go into detail as to what he was changing...

I am a complete newbie when it comes to using eclipse and modifying firmware. pretty familiar with arduino IDE so I that is a start.

I have PMed YARO with this thread, but it was almost 4 years ago now, so alot has probably changed in the firmware I imagine.

Thanks for the help.

Joe

Re: IMU Sensor Upside down (specific lines to modify)

Posted: Tue Jan 29, 2019 10:32 pm
by cafeciaojoe
bump,

cheers
joe

Re: IMU Sensor Upside down (specific lines to modify)

Posted: Thu Jan 31, 2019 2:53 am
by cafeciaojoe
Hey everyone,

have done a little more investigation. And have tried to change the power distribution function. I think we are close. we have tried many different variations with varying results, sometimes it flips over sometimes it takes of and crashes seconds after it tried to correct pitch or roll.

Here is the latest attempt. from power_distribution_stock.c

Code: Select all

void powerDistribution(const control_t *control)
{
  #ifdef QUAD_FORMATION_X
    int16_t r = control->roll / 2.0f;
    int16_t p = control->pitch / 2.0f;
    motorPower.m1 = limitThrust(control->thrust + r + p + control->yaw);
    motorPower.m2 = limitThrust(control->thrust + r - p + control->yaw);
    motorPower.m3 =  limitThrust(control->thrust - r - p - control->yaw);
    motorPower.m4 =  limitThrust(control->thrust - r + p - control->yaw);
The questions I have are...

[1] What would be the correct hardware configuration for this inversion?
- do the polarity of the motors neet to be inverted?
-does the A and B props have to stay on their respective arms after the deck is flipped (eg A stays with M4 and M2, Bstays with M3 and M1) This is what we are finding gets the thing off the ground.

[2] we have tried to work out the power distribution function as above? are we doing this right? See attched picture

[3] does anything else need to change in the code?

[4]
i also noticed this on line 276 in sensefusion6.c

Code: Select all

*pitch = asinf(gx) * 180 / M_PI_F; //Pitch seems to be inverted
does this have anything to do with the problem I am having?

[5] i eventuall want this to work with the qualisys system, I was wonderig if i had to invert the axis there as well. But I am pretty sure nothing has to happen in the qualisys tab of the client. this is just a firmware thing.

Re: IMU Sensor Upside down (specific lines to modify)

Posted: Fri Feb 01, 2019 11:07 am
by tobias
I gave it a quick shot and it this worked.

Flip the IMU. I did this in the sensor reading acc,gyro function and to keep right hand rule I flipped not only Z but Y as well.

Code: Select all

void processAccGyroMeasurements(const uint8_t *buffer)
...
  sensorData.gyro.x = -(gyroRaw.x - gyroBias.x) * SENSORS_DEG_PER_LSB_CFG;
  sensorData.gyro.y = -(gyroRaw.y - gyroBias.y) * SENSORS_DEG_PER_LSB_CFG;
  sensorData.gyro.z = -(gyroRaw.z - gyroBias.z) * SENSORS_DEG_PER_LSB_CFG;
  applyAxis3fLpf((lpf2pData*)(&gyroLpf), &sensorData.gyro);

  accScaled.x = -(accelRaw.x) * SENSORS_G_PER_LSB_CFG / accScale;
  accScaled.y = -(accelRaw.y) * SENSORS_G_PER_LSB_CFG / accScale;
  accScaled.z = -(accelRaw.z) * SENSORS_G_PER_LSB_CFG / accScale;
 ...
 }
Then since it is now up-side-down I flipped M1.M4 and M2,M3 in power distribution

Code: Select all

void powerDistribution(const control_t *control)
{
...    
    motorPower.m4 = limitThrust(control->thrust - r + p + control->yaw);
    motorPower.m3 = limitThrust(control->thrust - r - p - control->yaw);
    motorPower.m2 =  limitThrust(control->thrust + r - p + control->yaw);
    motorPower.m1 =  limitThrust(control->thrust + r + p - control->yaw);
...

Re: IMU Sensor Upside down (specific lines to modify)

Posted: Tue Mar 19, 2019 12:18 am
by cafeciaojoe
This worked brilliantly thank you!

Re: IMU Sensor Upside down (specific lines to modify)

Posted: Mon Apr 29, 2019 10:16 am
by cafeciaojoe
This works brilliantly on CF 2.0s but not CF 2.1 is there something I missed?

Re: IMU Sensor Upside down (specific lines to modify)

Posted: Mon Apr 29, 2019 12:53 pm
by tobias
The 2.1 uses another IMU (BMI088) and flipping the IMU axis was done in the driver (could have been made higher up) so this needs to be done in the sensors_bmi088_bmp388.c for the CF2.1

Code: Select all

static void sensorsTask(void *param)
{
...
      /* Gyro */
      sensorData.gyro.x =  (gyroRaw.x - gyroBias.x) * SENSORS_BMI088_DEG_PER_LSB_CFG;
      sensorData.gyro.y =  -(gyroRaw.y - gyroBias.y) * SENSORS_BMI088_DEG_PER_LSB_CFG;
      sensorData.gyro.z =  -(gyroRaw.z - gyroBias.z) * SENSORS_BMI088_DEG_PER_LSB_CFG;
      applyAxis3fLpf((lpf2pData*)(&gyroLpf), &sensorData.gyro);

      /* Acelerometer */
      accScaled.x = accelRaw.x * SENSORS_BMI088_G_PER_LSB_CFG / accScale;
      accScaled.y = -accelRaw.y * SENSORS_BMI088_G_PER_LSB_CFG / accScale;
      accScaled.z = -accelRaw.z * SENSORS_BMI088_G_PER_LSB_CFG / accScale;
      sensorsAccAlignToGravity(&accScaled, &sensorData.acc);
      applyAxis3fLpf((lpf2pData*)(&accLpf), &sensorData.acc);
 ...