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@2:c2ebab7c189f, 2013-06-24 (annotated)

Committer:
maetugr
Date:
Mon Jun 24 14:19:15 2013 +0000
Revision:
2:c2ebab7c189f
Parent:
1:b4f1227a0ff6
Child:
3:9cde4e9740fc
latest changes online for Roland Elmiger

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