Matthias Grob / Mbed 2 deprecated FlyBedLight

Lightweight FlyBed1, structure based on KK-Board Firmware First this one should work, then I can get more complex FlyBed1 working :)

Dependencies:   mbed MODI2C

main.cpp@0:d51bf879e9df, 2013-04-29 (annotated)

Committer:
maetugr
Date:
Mon Apr 29 19:41:06 2013 +0000
Revision:
0:d51bf879e9df
Child:
1:b4f1227a0ff6
First version of a lightweight FlyBed, that's a bit based on the KK-Board Firmware. First, this one should work, then I can get to the more complex FlyBed1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maetugr 0:d51bf879e9df 1 #include "mbed.h"
maetugr 0:d51bf879e9df 2 #include "PC.h" // Serial Port via USB by Roland Elmiger for debugging with Terminal (driver needed: https://mbed.org/media/downloads/drivers/mbedWinSerial_16466.exe)
maetugr 0:d51bf879e9df 3 #include "RC_Channel.h" // RemoteControl Channels with PPM
maetugr 0:d51bf879e9df 4 #include "Servo_PWM.h" // Motor PPM using PwmOut
maetugr 0:d51bf879e9df 5
maetugr 0:d51bf879e9df 6 // Defines
maetugr 0:d51bf879e9df 7 #define PPM_FREQU 495 // Hz Frequency of PPM Signal for ESCs (maximum <500Hz)
maetugr 0:d51bf879e9df 8 #define RC_SENSITIVITY 0.2 // Hz Frequency of PPM Signal for ESCs (maximum <500Hz)
maetugr 0:d51bf879e9df 9 // RC
maetugr 0:d51bf879e9df 10 #define AILERON 0
maetugr 0:d51bf879e9df 11 #define ELEVATOR 1
maetugr 0:d51bf879e9df 12 #define RUDDER 2
maetugr 0:d51bf879e9df 13 #define THROTTLE 3
maetugr 0:d51bf879e9df 14 // Axes
maetugr 0:d51bf879e9df 15 #define ROLL 0
maetugr 0:d51bf879e9df 16 #define PITCH 1
maetugr 0:d51bf879e9df 17 #define YAW 2
maetugr 0:d51bf879e9df 18 // Motors
maetugr 0:d51bf879e9df 19 #define FRONT 0
maetugr 0:d51bf879e9df 20 #define RIGHT 1
maetugr 0:d51bf879e9df 21 #define BACK 2
maetugr 0:d51bf879e9df 22 #define LEFT 3
maetugr 0:d51bf879e9df 23
maetugr 0:d51bf879e9df 24 // Gyro
maetugr 0:d51bf879e9df 25 #define L3G4200D_I2C_ADDRESS 0xD0
maetugr 0:d51bf879e9df 26 #define L3G4200D_CTRL_REG1 0x20
maetugr 0:d51bf879e9df 27 #define L3G4200D_CTRL_REG2 0x21
maetugr 0:d51bf879e9df 28 #define L3G4200D_CTRL_REG3 0x22
maetugr 0:d51bf879e9df 29 #define L3G4200D_CTRL_REG4 0x23
maetugr 0:d51bf879e9df 30 #define L3G4200D_CTRL_REG5 0x24
maetugr 0:d51bf879e9df 31 #define L3G4200D_REFERENCE 0x25
maetugr 0:d51bf879e9df 32 #define L3G4200D_OUT_X_L 0x28
maetugr 0:d51bf879e9df 33
maetugr 0:d51bf879e9df 34 // Hardware connections
maetugr 0:d51bf879e9df 35 DigitalOut loopLED(LED1);
maetugr 0:d51bf879e9df 36 DigitalOut armedLED(LED2);
maetugr 0:d51bf879e9df 37 PC pc(USBTX, USBRX, 38400); // USB
maetugr 0:d51bf879e9df 38 I2C i2c(p28, p27); // I2C-Bus for sensors
maetugr 0:d51bf879e9df 39 RC_Channel RC[] = {RC_Channel(p11,1), RC_Channel(p12,2), RC_Channel(p13,4), RC_Channel(p14,3)}; // no p19/p20 !
maetugr 0:d51bf879e9df 40 Servo_PWM ESC[] = {Servo_PWM(p21,PPM_FREQU), Servo_PWM(p24,PPM_FREQU), Servo_PWM(p23,PPM_FREQU), Servo_PWM(p22,PPM_FREQU)}; // p21 - p26 only because PWM needed!
maetugr 0:d51bf879e9df 41
maetugr 0:d51bf879e9df 42 // Global variables
maetugr 0:d51bf879e9df 43 bool armed = false;
maetugr 0:d51bf879e9df 44 float ESC_value[4] = {0,0,0,0};
maetugr 0:d51bf879e9df 45 float Error[3] = {0,0,0};
maetugr 0:d51bf879e9df 46 float P[3] = {3,3,3};
maetugr 0:d51bf879e9df 47 // Timing
maetugr 0:d51bf879e9df 48 float dt = 0;
maetugr 0:d51bf879e9df 49 float time_for_dt = 0;
maetugr 0:d51bf879e9df 50 Timer GlobalTimer;
maetugr 0:d51bf879e9df 51 // IMU
maetugr 0:d51bf879e9df 52 float Gyro[3] = {0,0,0};
maetugr 0:d51bf879e9df 53 float Gyro_sum[3] = {0,0,0};
maetugr 0:d51bf879e9df 54 float Gyro_history[3][5] = {{0,0,0,0,0},{0,0,0,0,0},{0,0,0,0,0}};
maetugr 0:d51bf879e9df 55 int Gyro_history_index = 0;
maetugr 0:d51bf879e9df 56 float offset[3] = {0,0,0};
maetugr 0:d51bf879e9df 57
maetugr 0:d51bf879e9df 58 void writeRegister(char reg, char data)
maetugr 0:d51bf879e9df 59 {
maetugr 0:d51bf879e9df 60 char buffer[2] = {reg, data};
maetugr 0:d51bf879e9df 61 i2c.write(L3G4200D_I2C_ADDRESS, buffer, 2, true);
maetugr 0:d51bf879e9df 62 }
maetugr 0:d51bf879e9df 63
maetugr 0:d51bf879e9df 64 void readMultiRegister(char reg, char* output, int size)
maetugr 0:d51bf879e9df 65 {
maetugr 0:d51bf879e9df 66 i2c.write (L3G4200D_I2C_ADDRESS, &reg, 1, true); // tell register address of the MSB get the sensor to do slave-transmit subaddress updating.
maetugr 0:d51bf879e9df 67 i2c.read (L3G4200D_I2C_ADDRESS, output, size, true); // tell it where to store the data read
maetugr 0:d51bf879e9df 68 }
maetugr 0:d51bf879e9df 69
maetugr 0:d51bf879e9df 70 void readGyro()
maetugr 0:d51bf879e9df 71 {
maetugr 0:d51bf879e9df 72 char buffer[6]; // 8-Bit pieces of axis data
maetugr 0:d51bf879e9df 73 int raw[3];
maetugr 0:d51bf879e9df 74 readMultiRegister(L3G4200D_OUT_X_L | (1 << 7), buffer, 6); // read axis registers using I2C // TODO: why?! | (1 << 7)
maetugr 0:d51bf879e9df 75 raw[0] = (short) (buffer[1] << 8 | buffer[0]); // join 8-Bit pieces to 16-bit short integers
maetugr 0:d51bf879e9df 76 raw[1] = (short) (buffer[3] << 8 | buffer[2]);
maetugr 0:d51bf879e9df 77 raw[2] = (short) (buffer[5] << 8 | buffer[4]);
maetugr 0:d51bf879e9df 78 for(int i=0;i<3;i++)
maetugr 0:d51bf879e9df 79 Gyro[i] = raw[i] - offset[i];// * 0.07;
maetugr 0:d51bf879e9df 80 }
maetugr 0:d51bf879e9df 81
maetugr 0:d51bf879e9df 82 void setup() {
maetugr 0:d51bf879e9df 83 // init screen -------------------------------------------------------------------------------------------------
maetugr 0:d51bf879e9df 84 pc.locate(10,5);
maetugr 0:d51bf879e9df 85 pc.printf("Flybed Light");
maetugr 0:d51bf879e9df 86 pc.locate(10,7);
maetugr 0:d51bf879e9df 87 pc.printf("Init...");
maetugr 0:d51bf879e9df 88
maetugr 0:d51bf879e9df 89 // init Gyro ---------------------------------------------------------------------------------------------------
maetugr 0:d51bf879e9df 90 writeRegister(L3G4200D_CTRL_REG1, 0x8F); // starts Gyro measurement
maetugr 0:d51bf879e9df 91 //writeRegister(L3G4200D_CTRL_REG2, 0x00); // highpass filter disabled
maetugr 0:d51bf879e9df 92 //writeRegister(L3G4200D_CTRL_REG3, 0x00);
maetugr 0:d51bf879e9df 93 writeRegister(L3G4200D_CTRL_REG4, 0x20); // sets acuracy to 2000 dps (degree per second)
maetugr 0:d51bf879e9df 94 writeRegister(L3G4200D_CTRL_REG5, 0x02);
maetugr 0:d51bf879e9df 95 //writeRegister(L3G4200D_REFERENCE, 0x00);
maetugr 0:d51bf879e9df 96
maetugr 0:d51bf879e9df 97 // calibrate Gyro ----------------------------------------------------------------------------------------------
maetugr 0:d51bf879e9df 98 int calib[3] = {0,0,0}; // temporary array for the sum of calibration measurement
maetugr 0:d51bf879e9df 99 const int times = 50;
maetugr 0:d51bf879e9df 100 for (int i = 0; i < times; i++) { // read 'times' times the data in a very short time
maetugr 0:d51bf879e9df 101 readGyro();
maetugr 0:d51bf879e9df 102 for (int j = 0; j < 3; j++)
maetugr 0:d51bf879e9df 103 calib[j] += Gyro[j];
maetugr 0:d51bf879e9df 104 wait(0.05);
maetugr 0:d51bf879e9df 105 }
maetugr 0:d51bf879e9df 106 for (int i = 0; i < 3; i++)
maetugr 0:d51bf879e9df 107 offset[i] = (float)calib[i]/(float)times; // take the average of the calibration measurements
maetugr 0:d51bf879e9df 108
maetugr 0:d51bf879e9df 109 GlobalTimer.start(); // Start Timemeasurement for first loop
maetugr 0:d51bf879e9df 110
maetugr 0:d51bf879e9df 111 pc.locate(10,7);
maetugr 0:d51bf879e9df 112 pc.printf("READY!!");
maetugr 0:d51bf879e9df 113 }
maetugr 0:d51bf879e9df 114
maetugr 0:d51bf879e9df 115 void loop() {
maetugr 0:d51bf879e9df 116 // meassure dt
maetugr 0:d51bf879e9df 117 dt = GlobalTimer.read() - time_for_dt; // time in us since last loop
maetugr 0:d51bf879e9df 118 time_for_dt = GlobalTimer.read(); // set new time for next measurement
maetugr 0:d51bf879e9df 119
maetugr 0:d51bf879e9df 120 // Arming / disarming
maetugr 0:d51bf879e9df 121 if(RC[THROTTLE].read() < 20 && RC[RUDDER].read() > 920) {
maetugr 0:d51bf879e9df 122 armed = true;
maetugr 0:d51bf879e9df 123 }
maetugr 0:d51bf879e9df 124 if((RC[THROTTLE].read() < 30 && RC[RUDDER].read() < 30) || RC[2].read() < -10 || RC[3].read() < -10 || RC[1].read() < -10 || RC[0].read() < -10) {
maetugr 0:d51bf879e9df 125 armed = false;
maetugr 0:d51bf879e9df 126 }
maetugr 0:d51bf879e9df 127
maetugr 0:d51bf879e9df 128 // get sensor data ----------------------------------------------------------------------------------------------------------
maetugr 0:d51bf879e9df 129 readGyro();
maetugr 0:d51bf879e9df 130 for (int i = 0; i < 3; i++) {
maetugr 0:d51bf879e9df 131 Gyro[i] *= 0.07; // make result degree per second
maetugr 0:d51bf879e9df 132
maetugr 0:d51bf879e9df 133 // fill ringbuffer
maetugr 0:d51bf879e9df 134 Gyro_history[i][Gyro_history_index] = Gyro[i]; // save newest value to ringbuffer
maetugr 0:d51bf879e9df 135 if (Gyro_history_index < 5-1)
maetugr 0:d51bf879e9df 136 Gyro_history_index++;
maetugr 0:d51bf879e9df 137 else
maetugr 0:d51bf879e9df 138 Gyro_history_index = 0;
maetugr 0:d51bf879e9df 139
maetugr 0:d51bf879e9df 140 // calculate average of ringbuffer
maetugr 0:d51bf879e9df 141 float sum = 0;
maetugr 0:d51bf879e9df 142 for (int j = 0; j < 5; j++) {
maetugr 0:d51bf879e9df 143 sum += Gyro_history[i][j];
maetugr 0:d51bf879e9df 144 }
maetugr 0:d51bf879e9df 145 Gyro[i] = sum/5;
maetugr 0:d51bf879e9df 146
maetugr 0:d51bf879e9df 147 Gyro_sum[i] += Gyro[i]*dt; // integrate speed to get angle
maetugr 0:d51bf879e9df 148 }
maetugr 0:d51bf879e9df 149
maetugr 0:d51bf879e9df 150 // calculate ESC -------------------------------------------------------------------------------------------------------------
maetugr 0:d51bf879e9df 151 for (int i = 0; i < 3; i++)
maetugr 0:d51bf879e9df 152 if (RC[i].read() != -100) // only count RC when it's available
maetugr 0:d51bf879e9df 153 Error[i] = ((RC[i].read() - 500)*RC_SENSITIVITY - Gyro[i]) * P[i];
maetugr 0:d51bf879e9df 154 else
maetugr 0:d51bf879e9df 155 Error[i] = (- Gyro[i]) * P[i];
maetugr 0:d51bf879e9df 156
maetugr 0:d51bf879e9df 157 for (int i = 0; i < 4; i++)
maetugr 0:d51bf879e9df 158 ESC_value[i] = RC[THROTTLE].read();
maetugr 0:d51bf879e9df 159
maetugr 0:d51bf879e9df 160 ESC_value[RIGHT] -= Error[ROLL];
maetugr 0:d51bf879e9df 161 ESC_value[LEFT] += Error[ROLL];
maetugr 0:d51bf879e9df 162
maetugr 0:d51bf879e9df 163 ESC_value[FRONT] -= Error[PITCH];
maetugr 0:d51bf879e9df 164 ESC_value[BACK] += Error[PITCH];
maetugr 0:d51bf879e9df 165
maetugr 0:d51bf879e9df 166 ESC_value[FRONT] -= Error[YAW];
maetugr 0:d51bf879e9df 167 ESC_value[BACK] -= Error[YAW];
maetugr 0:d51bf879e9df 168 ESC_value[RIGHT] += Error[YAW];
maetugr 0:d51bf879e9df 169 ESC_value[LEFT] += Error[YAW];
maetugr 0:d51bf879e9df 170
maetugr 0:d51bf879e9df 171
maetugr 0:d51bf879e9df 172 const int minimum_throttle = 140;
maetugr 0:d51bf879e9df 173 for (int i = 0; i < 4; i++) { // make shure there are no negative or too high ESC_values
maetugr 0:d51bf879e9df 174 if (ESC_value[i] < minimum_throttle) {
maetugr 0:d51bf879e9df 175 /*switch (i){
maetugr 0:d51bf879e9df 176 case FRONT: ESC_value[BACK] -= ESC_value[i]+minimum_throttle; break;
maetugr 0:d51bf879e9df 177 case BACK: ESC_value[FRONT] -= ESC_value[i]+minimum_throttle; break;
maetugr 0:d51bf879e9df 178 case LEFT: ESC_value[RIGHT] -= ESC_value[i]+minimum_throttle; break;
maetugr 0:d51bf879e9df 179 case RIGHT: ESC_value[LEFT] -= ESC_value[i]+minimum_throttle; break; //default:
maetugr 0:d51bf879e9df 180 }*/
maetugr 0:d51bf879e9df 181 ESC_value[i] = minimum_throttle;
maetugr 0:d51bf879e9df 182 }
maetugr 0:d51bf879e9df 183 if (ESC_value[i] > 1000) {
maetugr 0:d51bf879e9df 184 /*switch (i){
maetugr 0:d51bf879e9df 185 case FRONT: ESC_value[BACK] -= ESC_value[i] - 1000; break;
maetugr 0:d51bf879e9df 186 case BACK: ESC_value[FRONT] -= ESC_value[i] - 1000; break;
maetugr 0:d51bf879e9df 187 case LEFT: ESC_value[RIGHT] -= ESC_value[i] - 1000; break;
maetugr 0:d51bf879e9df 188 case RIGHT: ESC_value[LEFT] -= ESC_value[i] - 1000; break; //default:
maetugr 0:d51bf879e9df 189 }*/
maetugr 0:d51bf879e9df 190 ESC_value[i] = 1000;
maetugr 0:d51bf879e9df 191 }
maetugr 0:d51bf879e9df 192 }
maetugr 0:d51bf879e9df 193
maetugr 0:d51bf879e9df 194 // set ESC -------------------------------------------------------------------------------------------------------------------
maetugr 0:d51bf879e9df 195 if (armed) {
maetugr 0:d51bf879e9df 196 ESC[FRONT] = (int)ESC_value[FRONT];
maetugr 0:d51bf879e9df 197 ESC[BACK] = (int)ESC_value[BACK];
maetugr 0:d51bf879e9df 198 ESC[LEFT] = (int)ESC_value[LEFT];
maetugr 0:d51bf879e9df 199 ESC[RIGHT] = (int)ESC_value[RIGHT];
maetugr 0:d51bf879e9df 200 /*for (int i = 0; i < 4; i++)
maetugr 0:d51bf879e9df 201 ESC[i] = (int)ESC_value[i];*/
maetugr 0:d51bf879e9df 202 } else
maetugr 0:d51bf879e9df 203 for (int i = 0; i < 4; i++)
maetugr 0:d51bf879e9df 204 ESC[i] = 0;
maetugr 0:d51bf879e9df 205
maetugr 0:d51bf879e9df 206 // display --------------------------------------------------------------------------------------------------------------------
maetugr 0:d51bf879e9df 207 #if 0
maetugr 0:d51bf879e9df 208 pc.locate(10,7);
maetugr 0:d51bf879e9df 209 pc.printf("Gyro: X:%6.3f Y:%6.3f Z:%6.3f ", Gyro[0], Gyro[1], Gyro[2]);
maetugr 0:d51bf879e9df 210 pc.locate(10,8);
maetugr 0:d51bf879e9df 211 pc.printf("Gyro_sum: X:%6.1f Y:%6.1f Z:%6.1f dt: %1.4f ", Gyro_sum[ROLL], Gyro_sum[PITCH], Gyro_sum[YAW], dt);
maetugr 0:d51bf879e9df 212 pc.locate(10,10);
maetugr 0:d51bf879e9df 213 pc.printf("RC Aileron: %4d Elevator: %4d Rudder: %4d Throttle: %4d ", RC[AILERON].read(), RC[ELEVATOR].read(), RC[RUDDER].read(), RC[THROTTLE].read());
maetugr 0:d51bf879e9df 214 pc.locate(10,17);
maetugr 0:d51bf879e9df 215 pc.printf("Error: Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", Error[ROLL], Error[PITCH], Error[YAW]);
maetugr 0:d51bf879e9df 216 pc.locate(10,19);
maetugr 0:d51bf879e9df 217 pc.printf("ESC: Front:%6.1f Right:%6.1f Back:%6.1f Left:%6.1f ", ESC_value[FRONT], ESC_value[RIGHT], ESC_value[BACK], ESC_value[LEFT]);
maetugr 0:d51bf879e9df 218 #endif
maetugr 0:d51bf879e9df 219
maetugr 0:d51bf879e9df 220 // LED
maetugr 0:d51bf879e9df 221 armedLED = armed;
maetugr 0:d51bf879e9df 222 loopLED = 1;
maetugr 0:d51bf879e9df 223 wait(0.0005);
maetugr 0:d51bf879e9df 224 loopLED = 0;
maetugr 0:d51bf879e9df 225 wait(0.001);
maetugr 0:d51bf879e9df 226 }
maetugr 0:d51bf879e9df 227
maetugr 0:d51bf879e9df 228 int main() {
maetugr 0:d51bf879e9df 229 setup();
maetugr 0:d51bf879e9df 230 while(1) {
maetugr 0:d51bf879e9df 231 loop();
maetugr 0:d51bf879e9df 232 }
maetugr 0:d51bf879e9df 233 }