I am working to build a stabilizer for a remote controlled airplane. The project utilizes a Mega reading serial from a Spektrum satellite receiver, Adafruit's PWM Servo I2C Interface shield, along with Adafruit's BNO055 IMU.
The project works just fine when I am only reading Spektrum serial and driving servos on the PWM/I2C shield. When I add in the BNO055 IMU the serial data becomes erratic, yet the orientation data appears to be smooth and correct. Also, errors appear when power is applied to the IMU even when the code is commented out.
Does this appear to be a hardware issue or is there a problem with the way I have written the code?
The code below is currently commented out i a way that works as described above. To "break it" and start reading the IMU remove the /* */ tags in void setup() and void().
SpektrumSattelite library: GitHub - Quarduino/SpektrumSatellite: Rx receiver library for Arduino DUE and the Spektrum DSMX Satellite receiver. Probably works with other Arduinos with some modifications.
Adafruit PWM/I2C Shield: Overview | Adafruit 16-channel PWM/Servo Shield | Adafruit Learning System
Adafruit BNO055 IMU: Overview | Adafruit BNO055 Absolute Orientation Sensor | Adafruit Learning System
/*
-- Radio Configuration --
DX8 G2
11ms DSMX
+-150% Travel
-- RX Connections (MEGA 2650) --
Connect Orange to 3.3V
Connect Black to GND
Connect Grey to Digital 15 - RX3
Min Command 0 = -150% Both Gimbals Bottom Right
Mid Command 1024 = 0% Both Gimbals Centered
Max Command 2048 = +150% Both Gimbals Upper Left
-- BNO055 Connections (MEGA 2650) --
SCL to SCL MEGA
SDA to SDA MEGA
Vin to 5.0V DC
GROUND to common ground
*/
//Reading Spektrum Sattelite
#include <SpektrumSattelite.h>
SpektrumSattelite rx;
//Driving Servos Via I2C Shield
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
//Reading IMU
//#include <Wire.h> Already required for I2C Shield
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#define BNO055_SAMPLERATE_DELAY_MS (100)
Adafruit_BNO055 bno = Adafruit_BNO055();
//Initialize Input/Output Varibles
int CH1 = 1000;
int Thro = 0;
int CH2 = 1000;
int Aile = 0;
int CH3 = 1000;
int Elev = 0;
int CH4 = 1000;
int Rudd = 0;
int Gear = 0;
float YAWRATE = 0;
float ROLL = 0;
float PITCH = 0;
void setup()
{
Serial.begin(115200);
Serial3.begin(115200); //SpektrumSattelite uses Serial3 for input
//Initialise BNO055 Sensor
/*if(!bno.begin())
{
//There was a problem detecting the BNO055 ... check your connections
Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
while(1);
}
bno.setExtCrystalUse(true);
*/
//Initialise PWM/I2C Shield
pwm.begin();
pwm.setOscillatorFrequency(27000000); // The int.osc. is closer to 27MHz
pwm.setPWMFreq(50); //
Wire.setClock(400000);
}
void loop(){
//Read BNO055 Data
/*imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
imu::Vector<3> gyro = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
YAWRATE = gyro.z()*180/3.14159;
ROLL = euler.z();
PITCH = euler.y();
*/
rx.getFrame(); //READ CH1-CH9 Spektrum
ErrorChk(); //Report Current Varibles
Thro = rx.getThro();
Aile = rx.getAile();
Elev = rx.getElev();
Rudd = rx.getRudd();
Gear = rx.getGear();
CH1 = map(Thro,0,2048,900,2100); // THROTTLE - map input to pwm freq
pwm.writeMicroseconds(0,CH1); // Write PWM
CH2 = map(Aile,0,2048,900,2100); // AILERON
pwm.writeMicroseconds(1,CH2);
CH3 = map(Elev,0,2048,900,2100); // ELEVATOR
pwm.writeMicroseconds(2,CH3);
CH4 = map(Rudd,0,2048,900,2100); // RUDDER
pwm.writeMicroseconds(3,CH4);
}
void ErrorChk(){
Serial.print(rx.getThro());
Serial.print("\t");
Serial.print(rx.getAile());
Serial.print("\t");
Serial.print(rx.getElev());
Serial.print("\t");
Serial.print(rx.getRudd());
Serial.print("\t");
Serial.print(rx.getGear());
Serial.print("\t");
Serial.print(rx.getPitc());
Serial.print("\t");
Serial.print(rx.getAux2());
Serial.print("\t");
Serial.print(rx.getAux3());
Serial.print("\t");
Serial.print(rx.getAux4());
Serial.print("\t");
Serial.print(CH1);
Serial.print("\t");
Serial.print(CH2);
Serial.print("\t");
Serial.print(CH3);
Serial.print("\t");
Serial.print(CH4);
Serial.print("\t");
Serial.print(YAWRATE);
Serial.print("\t");
Serial.print(ROLL);
Serial.print("\t");
Serial.print(PITCH);
Serial.print('\n');
}
