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@1:b4f1227a0ff6, 2013-06-15 (annotated)

Committer:
maetugr
Date:
Sat Jun 15 14:29:26 2013 +0000
Revision:
1:b4f1227a0ff6
Parent:
0:d51bf879e9df
Child:
2:c2ebab7c189f
IT WORKED!!

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