Lightweight FlyBed1, structure based on KK-Board Firmware First this one should work, then I can get more complex FlyBed1 working :)
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?
User | Revision | Line number | New 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, ®, 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 | } |