The RC-Rider Project (Let's 'Ride' Radio Controlled Models!)

I was working on combining 6DOF inverse kinematics in the Sparkfun IMU M0 code, and forward the position information for each actuator via the serial to wireless, but the response is a little slow. I'm attaching the code for anyone interested into improving it... Its not perfect, and not fully tested!

Code:

/******************************************************************************
  SparkFun 9DoF Razor M0 Example Firmware
  Jim Lindblom @ SparkFun Electronics
  Original creation date: November 22, 2016
  https://github.com/sparkfun/9DOF_Razor_IMU/Firmware

  This example firmware for the SparkFun 9DoF Razor IMU M0
  demonstrates how to grab accelerometer, gyroscope, magnetometer,
  and quaternion values from the MPU-9250's digital motion processor
  (DMP). It prints those values to a serial port.

  Resources:
  SparkFun MPU9250-DMP Arduino Library:
  https://github.com/sparkfun/SparkFun_MPU-9250-DMP_Arduino_Library
  FlashStorage Arduino Library
  https://github.com/cmaglie/FlashStorage

  Development environment specifics:
  Firmware developed using Arduino IDE 1.6.12

  Hardware:
  SparkFun 9DoF Razor IMU M0 (SEN-14001)
  https://www.sparkfun.com/products/14001
******************************************************************************/
#include <SparkFunMPU9250-DMP.h>

#define SerialPort SerialUSB

MPU9250_DMP imu;

/////////////////////
// Math constants //
/////////////////////

const float pi = 3.14159,
            theta_r = radians(48.0),
            theta_p = radians(23.2),
            theta_s[] = { -pi / 3, 2 * pi / 3, pi, 0, pi / 3, -2 * pi / 3},
                        RD = 2.395,
                        PD = 3.3,
                        L1 = 1.0,
                        L2 = 4.72,
                        z_home = 3.25,
                        servo_min = -100,
                        servo_max = 100,
                        servo_mult = 10,
p[2][6] = {{
    PD * cos(pi / 6 + theta_p),
    PD * cos(pi / 6 - theta_p), PD * cos(-(pi / 2 - theta_p)), -PD * cos(-(pi / 2 - theta_p)), -PD * cos(pi / 6 - theta_p), -PD * cos(pi / 6 + theta_p)
  },

  {PD * sin(pi / 6 + theta_p), PD * sin(pi / 6 - theta_p), PD * sin(-(pi / 2 - theta_p)), PD * sin(-(pi / 2 - theta_p)), PD * sin(pi / 6 - theta_p), PD * sin(pi / 6 + theta_p)}
},

re[2][6] = {{RD * cos(pi / 6 + theta_r), RD * cos(pi / 6 - theta_r), RD * cos(-(pi / 2 - theta_r)), -RD * cos(-(pi / 2 - theta_r)), -RD * cos(pi / 6 - theta_r), -RD * cos(pi / 6 + theta_r)},

  {RD * sin(pi / 6 + theta_r), RD * sin(pi / 6 - theta_r), RD * sin(-(pi / 2 - theta_r)), RD * sin(-(pi / 2 - theta_r)), RD * sin(pi / 6 - theta_r), RD * sin(pi / 6 + theta_r)}
};

float pos[6];
float servo_pos_out[6];


/*
  theta_r = angle between attachment points
  theta_p = angle between rotation points
  theta_s = orientation of the servos
  RD = distance to end effector attachment points
  PD = distance to servo rotation points
  L1 = servo arm length
  L2 = connecting arm length
  z_home = default z height with servo arms horizontal
  servo_min = lower limit for servo arm angle
  servo_max = upper limit for servo arm angle
  servo_mult = multiplier to convert to milliseconds
  p = location of servo rotation points in base frame [x/y][1-6]
  re = location of attachment points in end effector frame [x/y][1-6]
*/

const int servo_pin[] = {1, 2, 3, 4, 5, 6}, servo_zero[6] = {1500, 1500, 1500, 1500, 1500, 1500};
/*
  servo_pin = servo pin assignments,
  servo_zero = zero angles for each servo (horizontal)
*/




void setup()
{
  SerialPort.begin(115200);

  // Call imu.begin() to verify communication with and
  // initialize the MPU-9250 to it's default values.
  // Most functions return an error code - INV_SUCCESS (0)
  // indicates the IMU was present and successfully set up
  if (imu.begin() != INV_SUCCESS)
  {
    while (1)
    {
      SerialPort.println("Unable to communicate with MPU-9250");
      SerialPort.println("Check connections, and try again.");
      SerialPort.println();
      delay(5000);
    }
  }

  // Use setSensors to turn on or off MPU-9250 sensors.
  // Any of the following defines can be combined:
  // INV_XYZ_GYRO, INV_XYZ_ACCEL, INV_XYZ_COMPASS,
  // INV_X_GYRO, INV_Y_GYRO, or INV_Z_GYRO
  // Enable all sensors:
  imu.setSensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);

  // Use setGyroFSR() and setAccelFSR() to configure the
  // gyroscope and accelerometer full scale ranges.
  // Gyro options are +/- 250, 500, 1000, or 2000 dps
  imu.setGyroFSR(2000); // Set gyro to 2000 dps
  // Accel options are +/- 2, 4, 8, or 16 g
  imu.setAccelFSR(2); // Set accel to +/-2g
  // Note: the MPU-9250's magnetometer FSR is set at
  // +/- 4912 uT (micro-tesla's)

  // setLPF() can be used to set the digital low-pass filter
  // of the accelerometer and gyroscope.
  // Can be any of the following: 188, 98, 42, 20, 10, 5
  // (values are in Hz).
  imu.setLPF(188); // Set LPF corner frequency to 5Hz

  // The sample rate of the accel/gyro can be set using
  // setSampleRate. Acceptable values range from 4Hz to 1kHz
  imu.setSampleRate(1000); // Set sample rate to 10Hz

  // Likewise, the compass (magnetometer) sample rate can be
  // set using the setCompassSampleRate() function.
  // This value can range between: 1-100Hz
  imu.setCompassSampleRate(10); // Set mag rate to 10Hz
}

void loop()
{
  if ( imu.dataReady() )
  {
    // Call update() to update the imu objects sensor data.
    // You can specify which sensors to update by combining
    // UPDATE_ACCEL, UPDATE_GYRO, UPDATE_COMPASS, and/or
    // UPDATE_TEMPERATURE.
    // (The update function defaults to accel, gyro, compass,
    //  so you don't have to specify these values.)
    //imu.update(UPDATE_ACCEL | UPDATE_GYRO | UPDATE_COMPASS);
    imu.update(UPDATE_ACCEL | UPDATE_GYRO);

  }
  printIMUData();

}

void printIMUData(void)
{
  // After calling update() the ax, ay, az, gx, gy, gz, mx,
  // my, mz, time, and/or temerature class variables are all
  // updated. Access them by placing the object. in front:

  // Use the calcAccel, calcGyro, and calcMag functions to
  // convert the raw sensor readings (signed 16-bit values)
  // to their respective units.
  float accelX = imu.calcAccel(imu.ax);
  float accelY = imu.calcAccel(imu.ay);
  float accelZ = imu.calcAccel(imu.az);
  float gyroX = imu.calcGyro(imu.gx);
  float gyroY = imu.calcGyro(imu.gy);
  float gyroZ = imu.calcGyro(imu.gz);
  //float magX = imu.calcMag(imu.mx);
  //float magY = imu.calcMag(imu.my);
  //float magZ = imu.calcMag(imu.mz);

  //  SerialPort.print("Accel: " + String(accelX) + ", " +
  //                     String(accelY) + ", " + String(accelZ) + " g");
  //  SerialPort.println("Gyro: " + String(gyroX) + ", " +
  //                     String(gyroY) + ", " + String(gyroZ) + " dps");
  // SerialPort.println("Mag: " + String(magX) + ", " +
  //                    String(magY) + ", " + String(magZ) + " uT");
  //SerialPort.println("Time: " + String(imu.time) + " ms");
  //SerialPort.println();

  //pos[0] = abs(imu.calcAccel(imu.ax));
  pos[0] = accelX * 4;
  //pos[1] = abs(imu.calcAccel(imu.ay));
  pos[1] = accelY * 4;
  //pos[2] = abs(imu.calcAccel(imu.az);
  pos[2] = accelZ * 4;
  //pos[3] = abs(imu.calcGyro(imu.gx) * 180);
  pos[3] = gyroX * 360;
  //pos[4] = abs(imu.calcGyro(imu.gy) * 180);
  pos[4] = gyroY * 360;
  //pos[5] = abs(imu.calcGyro(imu.gz) * 180);
  pos[5] = gyroZ * 360;
  calc6dof();

  SerialPort.print(" 1: ");
  SerialPort.print(servo_pos_out[0]);
  SerialPort.print(" 2: ");
  SerialPort.print(servo_pos_out[1]);
  SerialPort.print(" 3: ");
  SerialPort.print(servo_pos_out[2]);
  SerialPort.print(" 4: ");
  SerialPort.print(servo_pos_out[3]);
  SerialPort.print(" 5: ");
  SerialPort.print(servo_pos_out[4]);
  SerialPort.print(" 6: ");
  SerialPort.println(servo_pos_out[5]);
  //SerialPort.println();


}

void calc6dof(void)
{

  static float pe[6] = {0, 0, 0, radians(0), radians(0), radians(0)}, theta_a[6], servo_pos[6],
                       q[3][6], r[3][6], dl[3][6], dl2[6];


  for (int i = 0; i < 6; i++) {
    pe[i] = pos[i];
  }

  //static float pe[6] = {pos[0], pos[1], pos[2], radians(pos[3]), radians(pos[4]), radians(pos[5])}, theta_a[6], servo_pos[6],
  //                     q[3][6], r[3][6], dl[3][6], dl2[6];
  /*
    pe = location and orientation of end eector frame relative to the base frame [sway, surge,
    heave, pitch, roll, yaw)
    theta_a = angle of the servo arm
    servo_pos = value written to each servo
    q = position of lower mounting point of connecting link [x,y,x][1-6]
    r = position of upper mounting point of connecting link
    dl = dierence between x,y,z coordinates of q and r
    dl2 = distance between q and r
  */

  for (int i = 0; i < 6; i++) {
    q[0][i] = L1 * cos(-theta_a[i]) * cos(theta_s[i]) + p[0][i];
    q[1][i] = L1 * cos(-theta_a[i]) * sin(theta_s[i]) + p[1][i];
    q[2][i] = -L1 * sin(-theta_a[i]);
    r[0][i] = re[0][i] * cos(pe[4]) * cos(pe[5]) + re[1][i] * (sin(pe[3]) * sin(pe[4]) * cos(pe[5]) -
              cos(pe[3]) * sin(pe[5])) + pe[0];
    r[1][i] = re[0][i] * cos(pe[4]) * sin(pe[5]) + re[1][i] * (cos(pe[3]) * cos(pe[5]) +
              sin(pe[3]) * sin(pe[4]) * sin(pe[5])) + pe[1];
    r[2][i] = -re[0][i] * sin(pe[4]) + re[1][i] * sin(pe[3]) * cos(pe[4]) + z_home + pe[2];
    dl[0][i] = q[0][i] - r[0][i];
    dl[1][i] = q[1][i] - r[1][i];
    dl[2][i] = q[2][i] - r[2][i];
    dl2[i] = sqrt(dl[0][i] * dl[0][i] + dl[1][i] * dl[1][i] + dl[2][i] * dl[2][i]) - L2;
    theta_a[i] += dl2[i];
    theta_a[i] = constrain(theta_a[i], servo_min, servo_max);
    servo_pos[i] = servo_zero[i] + theta_a[i] * servo_mult;
    //if (i % 2 == 1) servo_pos[i] = servo_zero[i] + theta_a[i] * servo_mult;
    // else servo_pos[i] = servo_zero[i] - theta_a[i] * servo_mult;
  }
  for (int i = 0; i < 6; i++) {
    servo_pos_out[i] = servo_pos[i];
  }

}

Thank you @yobuddy !

I need to follow you data order : roll, pitch, yaw, surge, sway and surge on 4 bytes
I start from the example sketch from Sparkfun to be used with my Razor 9dof IMU : https://learn.sparkfun.com/tutorial...ide#getting-started-with-the-example-firmware

I will remove some parts and adapt of what I need exactly. What is the range expected by your software for roll pitch and yaw (0 /360 or -180/180) ?

I cleaned up the sketch a lot.
output are only byte and rotation are from -180 to 180

Code:

#include <SparkFunMPU9250-DMP.h>

MPU9250_DMP imu; // Create an instance of the MPU9250_DMP class

unsigned short accelFSR = 8;
unsigned short gyroFSR = 2000;
unsigned short fifoRate = 100;
unsigned short agLPF = 5; //LPF corner frequency
unsigned short agRate = 100; //Sample rate (between 4Hz and 1kHz)

///////////////////////
// LED Blink Control //
///////////////////////
//bool ledState = false;
uint32_t lastBlink = 0;
void blinkLED()
{
  static bool ledState = false;
  digitalWrite(13, ledState);
  ledState = !ledState;
}

void setup() {
  // Initialize LED, interrupt input, and serial port.
  // LED defaults to off:
  initHardware();

  // Initialize the MPU-9250. Should return true on success:
  if ( !initIMU() )
  {
    Serial1.println("Error connecting to MPU-9250");
    while (1) ; // Loop forever if we fail to connect
    // LED will remain off in this state.
  }
}

void initHardware(void)
{
  // Set up LED pin (active-high, default to off)
  pinMode(13, OUTPUT);
  digitalWrite(13, LOW);

  // Set up MPU-9250 interrupt input (active-low)
  pinMode(4, INPUT_PULLUP);

  // Set up serial log port
  Serial1.begin(115200);
}

void logIMUData(void)
{
  imu.computeEulerAngles();
  float Roll  = imu.roll;
  if (Roll > 180) Roll -= 360;
  float Pitch = imu.pitch;
  if (Pitch > 180) Pitch -= 360;
  float Yaw   = imu.yaw;
  if (Yaw > 180) Yaw -= 360;
  float Surge = imu.calcAccel(imu.ax);
  float Sway  = imu.calcAccel(imu.ay);
  float Heave = imu.calcAccel(imu.az);

  byte* RollPtr  = (byte*) &Roll;
  Serial1.write( RollPtr[0] );
  Serial1.write( RollPtr[1] );
  Serial1.write( RollPtr[2] );
  Serial1.write( RollPtr[3] );

  byte* PitchPtr = (byte*) &Pitch;
  Serial1.write( PitchPtr[0] );
  Serial1.write( PitchPtr[1] );
  Serial1.write( PitchPtr[2] );
  Serial1.write( PitchPtr[3] );

  byte* HeavePtr = (byte*) &Heave;
  Serial1.write( HeavePtr[0] );
  Serial1.write( HeavePtr[1] );
  Serial1.write( HeavePtr[2] );
  Serial1.write( HeavePtr[3] );

  byte* YawPtr   = (byte*) &Yaw;
  Serial1.write( YawPtr[0] );
  Serial1.write( YawPtr[1] );
  Serial1.write( YawPtr[2] );
  Serial1.write( YawPtr[3] );

  byte* SwayPtr  = (byte*) &Sway;
  Serial1.write( SwayPtr[0] );
  Serial1.write( SwayPtr[1] );
  Serial1.write( SwayPtr[2] );
  Serial1.write( SwayPtr[3] );

  byte* SurgePtr = (byte*) &Surge;
  Serial1.write( SurgePtr[0] );
  Serial1.write( SurgePtr[1] );
  Serial1.write( SurgePtr[2] );
  Serial1.write( SurgePtr[3] );

  delay(25);

  // Blink LED once every second
  if ( millis() > lastBlink + 1000 )
  {
    blinkLED();
    lastBlink = millis();
  }
}

bool initIMU(void)
{
  // imu.begin() should return 0 on success. Will initialize
  // I2C bus, and reset MPU-9250 to defaults.
  if (imu.begin() != INV_SUCCESS)
    return false;

  // Set up MPU-9250 interrupt:
  imu.enableInterrupt(); // Enable interrupt output
  imu.setIntLevel(1);    // Set interrupt to active-low
  imu.setIntLatched(1);  // Latch interrupt output

  // Configure sensors:
  // Set gyro full-scale range: options are 250, 500, 1000, or 2000:
  imu.setGyroFSR(gyroFSR);
  // Set accel full-scale range: options are 2, 4, 8, or 16 g
  imu.setAccelFSR(accelFSR);
  // Set gyro/accel LPF: options are 5, 10, 20, 42, 98, 188 Hz
  imu.setLPF(agLPF);
  // Set gyro/accel sample rate: must be between 4-1000Hz
  // (note: this value will be overridden by the DMP sample rate)
  imu.setSampleRate(agRate);


  // Configure digital motion processor. Use the FIFO to get
  // data from the DMP.
  unsigned short dmpFeatureMask = 0;
  // Gyro calibration re-calibrates the gyro after a set amount
  // of no motion detected
  dmpFeatureMask |= DMP_FEATURE_SEND_CAL_GYRO;

  // Add accel and quaternion's to the DMP
  dmpFeatureMask |= DMP_FEATURE_SEND_RAW_ACCEL;
  dmpFeatureMask |= DMP_FEATURE_6X_LP_QUAT;

  // Initialize the DMP, and set the FIFO's update rate:
  imu.dmpBegin(dmpFeatureMask, fifoRate);

  return true; // Return success
}


void loop() {
  // Check IMU for new data, and log it
  if ( !imu.fifoAvailable() ) // If no new data is available
    return;                   // return to the top of the loop

  // Read from the digital motion processor's FIFO
  if ( imu.dmpUpdateFifo() != INV_SUCCESS )
    return; // If that fails (uh, oh), return to top

  logIMUData(); // Log new data

}

You do not need a "start or end" character to sychronize data ?

I wonder if an arduino usb plugged can power the transmitter, the receiver and IMU

I get some water in my cellar... So I could not do anything

The city sewers that went up. I let you imagine the dirt and smell

Greetings.
Is this project complete? Being an avid FPV flyer it looks very interesting.
The entry cost is low and I would like to try it out..
Best regards

I haven’t worked it for a while now because life, but this is a good reminder to pick it back up again. You already have a sim build? What FPV are you doing, planes or quads?

Yes I have a 2dof dc motor sim I could take to the clubs airfield.
I am a active FPV flyer.
I have 3 - 4 airplanes equipped with standard definition 5.8ghz video and 2 with connex HD and a third with DJI occusync HD.
I fly a Connex HD Falcore quad and many SD racing quads. ( I was given several quads by friends who gave up the hobby)

I fly Taranis and have implemented telemetry on most planes.
I have some 900mhz telemetry modules that used the Mavlink protocol with the Pixhawk FC

I just ordered the GY-85 sensor and a couple of nanos and with flying season a month or two away the sim project looks like a lot of fun..

@T R Para sorry for the slow response, crazy week but I did get some time to dig back into the project a little. That's an awesome arsenal you've got! Sounds like the perfect test bed and it should be helpful that you're already familiar with the telemetry radios. At any rate, despite wasting several hours trying to get my telemetry radios to behave (it ended up being a baud rate issue), I did finally get everything up and running again and I think we are actually very close!

1. Using the GY-85 sensor, we do have a working setup as shown in my videos earlier in the thread, the issue with this is that it accounts for linear acceleration only, with no compensation for gravity so we were just outputting surge and sway, however if you turn the sensor sideways or forward, you get 1G of sway or surge respectively. This actually worked alright for the car as I was using it, however for flight you would certianly want all 6 DOF to play with. Though the GY-85 in fact has the ability to read all 6 DOF, it requires complex "sensor fusion" as it is called to combine the accelerometers, gyro and compass to actually give you meaningful data. Accelerometer data is usable (which is why it is all we used) however gyro gives you angular velocity and the compass measures magnetic field. With sensor fusion algorithms you get remove gravity from accelerations and extract Orientation (roll, pitch, yaw) which is what you really want. basically I gave up on trying to find/write such an algorithm so I turned to an easier, albeit slightly more expensive option described below
2. BNO055 sensor from adafruit (https://learn.adafruit.com/adafruit-bno055-absolute-orientation-sensor/overview) this sensor does the math for you and just spits out the 6 degrees of freedom that we really want. I have this one pretty well figured out now with all 6 DOF coming into simtools wirelessly, however I need some help from @yobuddy making some slight adjustments there(all 6 DOF look good in the RC Rider app, but only surge and sway seem to be correctly mapping to simtools) now that I have things working on my end.
3. Of course if you are using a vehicle that already has a flight controller putting out the six DOF you could theoretically achieve the same thing, but for now we were hoping to do a stand-alone setup to make it easier to put onto anything

I know you already ordered the GY-85 (sorry) but if you want to order the BNO055 sensor (37 bucks on Amazon) I can send you my current code/setup to start playing with. Should help to have someone else working it. Let me know if you have any questions

I'm hoping to work it a little more this week and post some pictures/videos on my progress.

OK Matthew. I will just send the GY-85 back to Amazon. So no problem.
The BNO055 is on order and will be here this week.
I am not a programmer. More of a solder jockey.
So whatever works best makes best sense to me.
Adafruit is beyond a doubt the most expensive option out there but I know they are a high quality company.
I am looking forward to your progress.

@T R Para sorry, didnt get a chance to work it this week, did your sensor show up? I'll keep working to get some code worth posting, but in the mean time, you can see the wiring diagrams and try out the test code just to play with it https://learn.adafruit.com/adafruit-bno055-absolute-orientation-sensor/arduino-code

OK. I loaded up a nano and piggybacked the sensor on to it.
It is displaying X Y Z on the monitor. The axis all display properly..

Perfect! I'm just about there, stay tuned!

@yobuddy & @Matthew Bangerter I gave RCRider a try today and had some success getting the IMU to drive a 3D platform with @hexpod PlugIn and the 6 available axis forces from the Adafruit 9DoF IMU. The acceleration forces seem to move very fast so it would be good to apply some sort of filter to allow the return to be slower.
Washout seemed to work ok with the yaw, but needs to be stronger on it's return to zero. Currently washout is set to 75 in the attached video. Is there a way to make the yaw not skip like you can see in the video?
Also in the video I have applied a bit of smoothing so it's a little slow to react, but the concept seems to be working. I could add washout to each and up the gain a little and it might behave more predictably.

For the yaw not to skip, it needs to be converted to speed (angle per second or something), instead of absolute angle value. I believe this calculation can be done within the arduino code...

the actual data from the IMU jumps because I think it moves across the 360 mark strait to zero.
I have no idea how to program Arduino so any help here would be appreciated. Thanks.