├── README.md ├── tinygps.h ├── EEPROM.ino ├── CREDITS.txt ├── IMU.ino ├── Alarms.ino ├── RX.ino ├── Serial.ino └── HexNanoMWC_QUAD.ino /README.md: -------------------------------------------------------------------------------- 1 | HexNanoMWC_QUAD 2 | =============== 3 | 4 | A modified MWC2.2 flight control for FlexBot quad board. 5 | 6 | And the project is one part of the [FlexBot Project](http://http://flexbot.cc/wiki/). 7 | 8 | ##Source Code 9 | 10 | You can either download the source using the "Download ZIP" button at the bottom left of the github page, or you can make a clone using git: 11 | 12 | ``` 13 | git clone https://github.com/HexAirbot/HexNanoMWC_QUAD.git 14 | ``` 15 | 16 | ####Latest release 17 | 18 | [version 1.0.3 source code](https://github.com/HexAirbot/HexNanoMWC_QUAD/archive/v1.0.3.zip) 19 | 20 | 21 | ####How to compile 22 | 23 | 1.Download the Arduino IDE (you should use the version 1.0.5+) 24 | 25 | 2.Download the source code here, and put all the source files (.ino file) under folder HexNanoMWC_QUAD. 26 | 27 | 3.Open the main file HexNanoMWC_QUAD.ino with the Arduino IDE, and the IDE will also automatically open other source files. 28 | 29 | 4.Connect your Flexbot via USB 30 | 31 | 5.Set the Board in your IDE to Arduino Leonardo 32 | Select the correct COM Port 33 | 34 | 6.Click the verify and then the upload button 35 | 36 | ##Development 37 | The FlexBot Project is open source. 38 | 39 | To contribute, you can send a pull request on Github, or share your idea to the [forum](http://makedesignshare.com). 40 | 41 | 42 | ##Support 43 | [Website](http://flexbot.cc) 44 | 45 | [Wiki](http://flexbot.cc/wiki) 46 | 47 | [Forum](http://makedesignshare.com) 48 | -------------------------------------------------------------------------------- /tinygps.h: -------------------------------------------------------------------------------- 1 | #ifndef NMEA_STRUCTS_H 2 | #define NMEA_STRUCTS_H 3 | 4 | #define NMEA_MINUTE_FRACTS 4 5 | #define NMEA_ALTITUDE_FRACTS 2 6 | 7 | #define NMEA_RMC_FLAGS_STATUS_OK 0 8 | #define NMEA_RMC_FLAGS_LAT_NORTH 1 9 | #define NMEA_RMC_FLAGS_LON_EAST 2 10 | 11 | struct coord { 12 | /* degrees, 0-180 or 0-90 */ 13 | uint8_t deg; 14 | /* minutes, 0-60 */ 15 | uint8_t min; 16 | /* fractions of minutes saved as BCD */ 17 | uint8_t frac[(NMEA_MINUTE_FRACTS+1)/2]; 18 | }; 19 | 20 | struct altitude_t { 21 | int16_t m; 22 | uint8_t frac[(NMEA_ALTITUDE_FRACTS+1)/2]; 23 | }; 24 | 25 | struct clock_t { 26 | uint8_t hour; 27 | uint8_t minute; 28 | uint8_t second; 29 | }; 30 | 31 | struct date_t { 32 | uint8_t day; 33 | uint8_t month; 34 | uint8_t year; 35 | }; 36 | 37 | struct nmea_data_t { 38 | uint8_t flags; 39 | struct date_t date; 40 | struct clock_t clock; 41 | struct coord lat; 42 | struct coord lon; 43 | struct altitude_t alt; 44 | uint8_t quality; 45 | uint8_t sats; 46 | }; 47 | 48 | #endif 49 | 50 | #ifndef OPTICAL_STRUCTS_H 51 | #define OPTICAL_STRUCTS_H 52 | 53 | struct optical_data_t { 54 | int16_t dx; 55 | int16_t dy; 56 | }; 57 | 58 | #endif 59 | 60 | #ifndef SONAR_STRUCTS_H 61 | #define SONAR_STRUCTS_H 62 | 63 | struct sonar_data_t { 64 | int16_t distance; 65 | }; 66 | 67 | #endif 68 | 69 | #ifndef NAV_STRUCTS_H 70 | #define NAV_STRUCTS_H 71 | struct nav_data_t { 72 | struct nmea_data_t gps; 73 | struct sonar_data_t sonar; 74 | struct optical_data_t optical; 75 | }; 76 | #endif 77 | -------------------------------------------------------------------------------- /EEPROM.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | 4 | uint8_t calculate_sum(uint8_t *cb , uint8_t siz) { 5 | uint8_t sum=0x55; // checksum init 6 | while(--siz) sum += *cb++; // calculate checksum (without checksum byte) 7 | return sum; 8 | } 9 | 10 | void readGlobalSet() { 11 | eeprom_read_block((void*)&global_conf, (void*)0, sizeof(global_conf)); 12 | if(calculate_sum((uint8_t*)&global_conf, sizeof(global_conf)) != global_conf.checksum) { 13 | global_conf.currentSet = 0; 14 | global_conf.accZero[ROLL] = 5000; // for config error signalization 15 | } 16 | } 17 | 18 | void readEEPROM() { 19 | uint8_t i; 20 | #ifdef MULTIPLE_CONFIGURATION_PROFILES 21 | if(global_conf.currentSet>2) global_conf.currentSet=0; 22 | #else 23 | global_conf.currentSet=0; 24 | #endif 25 | eeprom_read_block((void*)&conf, (void*)(global_conf.currentSet * sizeof(conf) + sizeof(global_conf)), sizeof(conf)); 26 | if(calculate_sum((uint8_t*)&conf, sizeof(conf)) != conf.checksum) { 27 | blinkLED(6,100,3); 28 | #if defined(BUZZER) 29 | alarmArray[7] = 3; 30 | #endif 31 | LoadDefaults(); // force load defaults 32 | } 33 | for(i=0;i<6;i++) { 34 | lookupPitchRollRC[i] = (2500+conf.rcExpo8*(i*i-25))*i*(int32_t)conf.rcRate8/2500; 35 | } 36 | for(i=0;i<11;i++) { 37 | int16_t tmp = 10*i-conf.thrMid8; 38 | uint8_t y = 1; 39 | if (tmp>0) y = 100-conf.thrMid8; 40 | if (tmp<0) y = conf.thrMid8; 41 | lookupThrottleRC[i] = 10*conf.thrMid8 + tmp*( 100-conf.thrExpo8+(int32_t)conf.thrExpo8*(tmp*tmp)/(y*y) )/10; // [0;1000] 42 | lookupThrottleRC[i] = conf.minthrottle + (int32_t)(MAXTHROTTLE-conf.minthrottle)* lookupThrottleRC[i]/1000; // [0;1000] -> [conf.minthrottle;MAXTHROTTLE] 43 | } 44 | 45 | #if defined(POWERMETER) 46 | pAlarm = (uint32_t) conf.powerTrigger1 * (uint32_t) PLEVELSCALE * (uint32_t) conf.pleveldiv; // need to cast before multiplying 47 | #endif 48 | #ifdef FLYING_WING 49 | #ifdef LCD_CONF 50 | conf.wing_left_mid = constrain(conf.wing_left_mid, WING_LEFT_MIN, WING_LEFT_MAX); //LEFT 51 | conf.wing_right_mid = constrain(conf.wing_right_mid, WING_RIGHT_MIN, WING_RIGHT_MAX); //RIGHT 52 | #else // w.o LCD support user may not find this value stored in eeprom, so always use the define value 53 | conf.wing_left_mid = WING_LEFT_MID; 54 | conf.wing_right_mid = WING_RIGHT_MID; 55 | #endif 56 | #endif 57 | #ifdef TRI 58 | #ifdef LCD_CONF 59 | conf.tri_yaw_middle = constrain(conf.tri_yaw_middle, TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX); //REAR 60 | #else // w.o LCD support user may not find this value stored in eeprom, so always use the define value 61 | conf.tri_yaw_middle = TRI_YAW_MIDDLE; 62 | #endif 63 | #endif 64 | #if GPS 65 | if (f.I2C_INIT_DONE) GPS_set_pids(); 66 | #endif 67 | #ifdef POWERMETER_HARD 68 | conf.pleveldivsoft = PLEVELDIVSOFT; 69 | #endif 70 | #ifdef POWERMETER_SOFT 71 | conf.pleveldivsoft = conf.pleveldiv; 72 | #endif 73 | #if defined(ARMEDTIMEWARNING) 74 | ArmedTimeWarningMicroSeconds = (conf.armedtimewarning *1000000); 75 | #endif 76 | } 77 | 78 | void writeGlobalSet(uint8_t b) { 79 | global_conf.checksum = calculate_sum((uint8_t*)&global_conf, sizeof(global_conf)); 80 | eeprom_write_block((const void*)&global_conf, (void*)0, sizeof(global_conf)); 81 | if (b == 1) blinkLED(15,20,1); 82 | #if defined(BUZZER) 83 | alarmArray[7] = 1; 84 | #endif 85 | 86 | } 87 | 88 | void writeParams(uint8_t b) { 89 | #ifdef MULTIPLE_CONFIGURATION_PROFILES 90 | if(global_conf.currentSet>2) global_conf.currentSet=0; 91 | #else 92 | global_conf.currentSet=0; 93 | #endif 94 | conf.checksum = calculate_sum((uint8_t*)&conf, sizeof(conf)); 95 | eeprom_write_block((const void*)&conf, (void*)(global_conf.currentSet * sizeof(conf) + sizeof(global_conf)), sizeof(conf)); 96 | readEEPROM(); 97 | if (b == 1) blinkLED(15,20,1); 98 | #if defined(BUZZER) 99 | alarmArray[7] = 1; //beep if loaded from gui or android 100 | #endif 101 | } 102 | 103 | void LoadDefaults() { 104 | conf.P8[ROLL] = 33; conf.I8[ROLL] = 30; conf.D8[ROLL] = 23; 105 | conf.P8[PITCH] = 33; conf.I8[PITCH] = 30; conf.D8[PITCH] = 23; 106 | conf.P8[YAW] = 68; conf.I8[YAW] = 45; conf.D8[YAW] = 0; 107 | conf.P8[PIDALT] = 100; conf.I8[PIDALT] = 25; conf.D8[PIDALT] = 24; 108 | 109 | conf.P8[PIDPOS] = POSHOLD_P * 100; conf.I8[PIDPOS] = POSHOLD_I * 100; conf.D8[PIDPOS] = 0; 110 | conf.P8[PIDPOSR] = POSHOLD_RATE_P * 10; conf.I8[PIDPOSR] = POSHOLD_RATE_I * 100; conf.D8[PIDPOSR] = POSHOLD_RATE_D * 1000; 111 | conf.P8[PIDNAVR] = NAV_P * 10; conf.I8[PIDNAVR] = NAV_I * 100; conf.D8[PIDNAVR] = NAV_D * 1000; 112 | 113 | conf.P8[PIDLEVEL] = 90; conf.I8[PIDLEVEL] = 10; conf.D8[PIDLEVEL] = 100; 114 | conf.P8[PIDMAG] = 40; 115 | 116 | conf.P8[PIDVEL] = 0; conf.I8[PIDVEL] = 0; conf.D8[PIDVEL] = 0; 117 | 118 | conf.rcRate8 = 90; conf.rcExpo8 = 0; 119 | conf.rollPitchRate = 0; 120 | conf.yawRate = 0; 121 | conf.dynThrPID = 0; 122 | conf.thrMid8 = 50; conf.thrExpo8 = 50; 123 | for(uint8_t i=0;i>2; 27 | gyroADCprevious[axis] = gyroADC[axis]; 28 | } 29 | #else 30 | #if ACC 31 | ACC_getADC(); 32 | getEstimatedAttitude(); 33 | #endif 34 | #if GYRO 35 | Gyro_getADC(); 36 | #endif 37 | for (axis = 0; axis < 3; axis++) 38 | gyroADCp[axis] = gyroADC[axis]; 39 | timeInterleave=micros(); 40 | annexCode(); 41 | if ((micros()-timeInterleave)>650) { 42 | annex650_overrun_count++; 43 | } else { 44 | while((micros()-timeInterleave)<650) ; //empirical, interleaving delay between 2 consecutive reads 45 | } 46 | #if GYRO 47 | Gyro_getADC(); 48 | #endif 49 | for (axis = 0; axis < 3; axis++) { 50 | gyroADCinter[axis] = gyroADC[axis]+gyroADCp[axis]; 51 | // empirical, we take a weighted value of the current and the previous values 52 | gyroData[axis] = (gyroADCinter[axis]+gyroADCprevious[axis])/3; 53 | gyroADCprevious[axis] = gyroADCinter[axis]>>1; 54 | if (!ACC) accADC[axis]=0; 55 | } 56 | #endif 57 | #if defined(GYRO_SMOOTHING) 58 | static int16_t gyroSmooth[3] = {0,0,0}; 59 | for (axis = 0; axis < 3; axis++) { 60 | gyroData[axis] = (int16_t) ( ( (int32_t)((int32_t)gyroSmooth[axis] * (conf.Smoothing[axis]-1) )+gyroData[axis]+1 ) / conf.Smoothing[axis]); 61 | gyroSmooth[axis] = gyroData[axis]; 62 | } 63 | #elif defined(TRI) 64 | static int16_t gyroYawSmooth = 0; 65 | gyroData[YAW] = (gyroYawSmooth*2+gyroData[YAW])/3; 66 | gyroYawSmooth = gyroData[YAW]; 67 | #endif 68 | } 69 | 70 | // ************************************************** 71 | // Simplified IMU based on "Complementary Filter" 72 | // Inspired by http://starlino.com/imu_guide.html 73 | // 74 | // adapted by ziss_dm : http://www.multiwii.com/forum/viewtopic.php?f=8&t=198 75 | // 76 | // The following ideas was used in this project: 77 | // 1) Rotation matrix: http://en.wikipedia.org/wiki/Rotation_matrix 78 | // 2) Small-angle approximation: http://en.wikipedia.org/wiki/Small-angle_approximation 79 | // 3) C. Hastings approximation for atan2() 80 | // 4) Optimization tricks: http://www.hackersdelight.org/ 81 | // 82 | // Currently Magnetometer uses separate CF which is used only 83 | // for heading approximation. 84 | // 85 | // ************************************************** 86 | 87 | //****** advanced users settings ******************* 88 | /* Set the Low Pass Filter factor for ACC 89 | Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time 90 | Comment this if you do not want filter at all. 91 | unit = n power of 2 */ 92 | // this one is also used for ALT HOLD calculation, should not be changed 93 | #ifndef ACC_LPF_FACTOR 94 | #define ACC_LPF_FACTOR 4 // that means a LPF of 16 95 | #endif 96 | 97 | /* Set the Gyro Weight for Gyro/Acc complementary filter 98 | Increasing this value would reduce and delay Acc influence on the output of the filter*/ 99 | #ifndef GYR_CMPF_FACTOR 100 | #define GYR_CMPF_FACTOR 600 101 | #endif 102 | 103 | /* Set the Gyro Weight for Gyro/Magnetometer complementary filter 104 | Increasing this value would reduce and delay Magnetometer influence on the output of the filter*/ 105 | #define GYR_CMPFM_FACTOR 250 106 | 107 | //****** end of advanced users settings ************* 108 | #define INV_GYR_CMPF_FACTOR (1.0f / (GYR_CMPF_FACTOR + 1.0f)) 109 | #define INV_GYR_CMPFM_FACTOR (1.0f / (GYR_CMPFM_FACTOR + 1.0f)) 110 | 111 | #define GYRO_SCALE ((2279 * PI)/((32767.0f / 4.0f ) * 180.0f * 1000000.0f)) //(ITG3200 and MPU6050) 112 | // +-2000/sec deg scale 113 | // for WMP, empirical value should be #define GYRO_SCALE (1.0f/200e6f) 114 | // !!!!should be adjusted to the rad/sec and be part defined in each gyro sensor 115 | 116 | typedef struct fp_vector { 117 | float X,Y,Z; 118 | } t_fp_vector_def; 119 | 120 | typedef union { 121 | float A[3]; 122 | t_fp_vector_def V; 123 | } t_fp_vector; 124 | 125 | typedef struct int32_t_vector { 126 | int32_t X,Y,Z; 127 | } t_int32_t_vector_def; 128 | 129 | typedef union { 130 | int32_t A[3]; 131 | t_int32_t_vector_def V; 132 | } t_int32_t_vector; 133 | 134 | int16_t _atan2(int32_t y, int32_t x){ 135 | float z = (float)y / x; 136 | int16_t a; 137 | if ( abs(y) < abs(x) ){ 138 | a = 573 * z / (1.0f + 0.28f * z * z); 139 | if (x<0) { 140 | if (y<0) a -= 1800; 141 | else a += 1800; 142 | } 143 | } else { 144 | a = 900 - 573 * z / (z * z + 0.28f); 145 | if (y<0) a -= 1800; 146 | } 147 | return a; 148 | } 149 | 150 | float InvSqrt (float x){ 151 | union{ 152 | int32_t i; 153 | float f; 154 | } conv; 155 | conv.f = x; 156 | conv.i = 0x5f3759df - (conv.i >> 1); 157 | return 0.5f * conv.f * (3.0f - x * conv.f * conv.f); 158 | } 159 | 160 | // Rotate Estimated vector(s) with small angle approximation, according to the gyro data 161 | void rotateV(struct fp_vector *v,float* delta) { 162 | fp_vector v_tmp = *v; 163 | v->Z -= delta[ROLL] * v_tmp.X + delta[PITCH] * v_tmp.Y; 164 | v->X += delta[ROLL] * v_tmp.Z - delta[YAW] * v_tmp.Y; 165 | v->Y += delta[PITCH] * v_tmp.Z + delta[YAW] * v_tmp.X; 166 | } 167 | 168 | 169 | static int32_t accLPF32[3] = {0, 0, 1}; 170 | static float invG; // 1/|G| 171 | 172 | static t_fp_vector EstG; 173 | static t_int32_t_vector EstG32; 174 | #if MAG 175 | static t_int32_t_vector EstM32; 176 | static t_fp_vector EstM; 177 | #endif 178 | 179 | void getEstimatedAttitude(){ 180 | uint8_t axis; 181 | int32_t accMag = 0; 182 | float scale, deltaGyroAngle[3]; 183 | static uint16_t previousT; 184 | uint16_t currentT = micros(); 185 | 186 | scale = (currentT - previousT) * GYRO_SCALE; 187 | previousT = currentT; 188 | 189 | // Initialization 190 | for (axis = 0; axis < 3; axis++) { 191 | deltaGyroAngle[axis] = gyroADC[axis] * scale; 192 | 193 | accLPF32[axis] -= accLPF32[axis]>>ACC_LPF_FACTOR; 194 | accLPF32[axis] += accADC[axis]; 195 | accSmooth[axis] = accLPF32[axis]>>ACC_LPF_FACTOR; 196 | 197 | accMag += (int32_t)accSmooth[axis]*accSmooth[axis] ; 198 | } 199 | accMag = accMag*100/((int32_t)acc_1G*acc_1G); 200 | 201 | rotateV(&EstG.V,deltaGyroAngle); 202 | #if MAG 203 | rotateV(&EstM.V,deltaGyroAngle); 204 | #endif 205 | 206 | if ( abs(accSmooth[ROLL])0) { 207 | f.SMALL_ANGLES_25 = 1; 208 | } else { 209 | f.SMALL_ANGLES_25 = 0; 210 | } 211 | 212 | // Apply complimentary filter (Gyro drift correction) 213 | // If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation. 214 | // To do that, we just skip filter, as EstV already rotated by Gyro 215 | if ( 72 < accMag && accMag < 133 ) 216 | for (axis = 0; axis < 3; axis++) { 217 | EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + accSmooth[axis]) * INV_GYR_CMPF_FACTOR; 218 | } 219 | #if MAG 220 | for (axis = 0; axis < 3; axis++) { 221 | EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR + magADC[axis]) * INV_GYR_CMPFM_FACTOR; 222 | EstM32.A[axis] = EstM.A[axis]; 223 | } 224 | #endif 225 | 226 | for (axis = 0; axis < 3; axis++) 227 | EstG32.A[axis] = EstG.A[axis]; //int32_t cross calculation is a little bit faster than float 228 | 229 | // Attitude of the estimated vector 230 | int32_t sqGZ = sq(EstG32.V.Z); 231 | int32_t sqGX = sq(EstG32.V.X); 232 | int32_t sqGY = sq(EstG32.V.Y); 233 | int32_t sqGX_sqGZ = sqGX + sqGZ; 234 | float invmagXZ = InvSqrt(sqGX_sqGZ); 235 | invG = InvSqrt(sqGX_sqGZ + sqGY); 236 | angle[ROLL] = _atan2(EstG32.V.X , EstG32.V.Z); 237 | angle[PITCH] = _atan2(EstG32.V.Y , invmagXZ*sqGX_sqGZ); 238 | 239 | #if MAG 240 | heading = _atan2( 241 | EstM32.V.Z * EstG32.V.X - EstM32.V.X * EstG32.V.Z, 242 | EstM32.V.Y * invG * sqGX_sqGZ - (EstM32.V.X * EstG32.V.X + EstM32.V.Z * EstG32.V.Z) * invG * EstG32.V.Y ); 243 | heading += MAG_DECLINIATION * 10; //add declination 244 | heading = heading /10; 245 | #endif 246 | } 247 | 248 | #define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc) 249 | #define BARO_TAB_SIZE 21 250 | 251 | #define ACC_Z_DEADBAND (acc_1G>>5) // was 40 instead of 32 now 252 | 253 | 254 | #define applyDeadband(value, deadband) \ 255 | if(abs(value) < deadband) { \ 256 | value = 0; \ 257 | } else if(value > 0){ \ 258 | value -= deadband; \ 259 | } else if(value < 0){ \ 260 | value += deadband; \ 261 | } 262 | 263 | #if BARO 264 | uint8_t getEstimatedAltitude(){ 265 | static float baroGroundTemperatureScale,logBaroGroundPressureSum; 266 | static uint32_t deadLine; 267 | static int32_t baroGroundPressure; 268 | static uint16_t previousT; 269 | uint16_t currentT = micros(); 270 | uint16_t dTime; 271 | 272 | dTime = currentT - previousT; 273 | if (dTime < UPDATE_INTERVAL) return 0; 274 | previousT = currentT; 275 | 276 | if(calibratingB > 0) { 277 | logBaroGroundPressureSum = log(baroPressureSum); 278 | baroGroundTemperatureScale = (baroTemperature + 27315) * 29.271267f; 279 | //baroGroundPressure = baroPressureSum/(BARO_TAB_SIZE - 1); 280 | calibratingB--; 281 | } 282 | 283 | // pressure relative to ground pressure with temperature compensation (fast!) 284 | // baroGroundPressure is not supposed to be 0 here 285 | // see: https://code.google.com/p/ardupilot-mega/source/browse/libraries/AP_Baro/AP_Baro.cpp 286 | BaroAlt = (logBaroGroundPressureSum - log(baroPressureSum)) * baroGroundTemperatureScale; 287 | 288 | EstAlt = (EstAlt * 6 + BaroAlt * 2) >> 3; // additional LPF to reduce baro noise (faster by 30 µs) 289 | 290 | #if (defined(VARIOMETER) && (VARIOMETER != 2)) || !defined(SUPPRESS_BARO_ALTHOLD) 291 | int16_t targetVel = constrain(AltHold - EstAlt, -100, 100); 292 | 293 | // projection of ACC vector to global Z, with 1G subtructed 294 | // Math: accZ = A * G / |G| - 1G 295 | int16_t accZ = (accSmooth[ROLL] * EstG32.V.X + accSmooth[PITCH] * EstG32.V.Y + accSmooth[YAW] * EstG32.V.Z) * invG; 296 | 297 | static int16_t accZoffset = 0; // = acc_1G*6; //58 bytes saved and convergence is fast enough to omit init 298 | if (!f.ARMED) { 299 | accZoffset -= accZoffset>>3; 300 | accZoffset += accZ; 301 | } 302 | accZ -= accZoffset>>3; 303 | 304 | //applyDeadband(accZ, ACC_Z_DEADBAND); 305 | 306 | static float vel = 0.0f; 307 | static float accVelScale = 9.80665f / 10000.0f / acc_1G ; 308 | 309 | //I 310 | // Integrator - velocity, cm/sec 311 | vel += accZ * accVelScale * dTime; 312 | 313 | static int32_t lastBaroAlt; 314 | int16_t baroVel = (EstAlt - lastBaroAlt) * 1000000.0f / dTime; 315 | lastBaroAlt = EstAlt; 316 | 317 | baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s 318 | applyDeadband(baroVel, 10); // to reduce noise near zero 319 | 320 | // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity). 321 | // By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay 322 | vel = vel * 0.985f+ baroVel * 0.015f; 323 | 324 | int16_t error16 = targetVel - vel; 325 | BaroPID = constrain((conf.P8[PIDALT] * error16 >>7), -200, +200); 326 | 327 | errorAltitudeI += conf.I8[PIDALT] * error16 >>6; 328 | errorAltitudeI = constrain(errorAltitudeI,-30000,30000); 329 | BaroPID += errorAltitudeI>>8; //I in range +/-60 330 | 331 | BaroPID = constrain(BaroPID, -200, 200); 332 | 333 | debug[0] = vel; 334 | debug[1] = error16; 335 | debug[2] = BaroPID; 336 | debug[3] = AltHold; 337 | 338 | /* 339 | //D 340 | int16_t vel_tmp = vel; 341 | applyDeadband(vel_tmp, 5); 342 | vario = vel_tmp; 343 | 344 | BaroPID -= constrain(conf.D8[PIDALT] * vel_tmp >>4, -150, 150); 345 | */ 346 | #endif 347 | return 1; 348 | } 349 | #endif //BARO 350 | -------------------------------------------------------------------------------- /Alarms.ino: -------------------------------------------------------------------------------- 1 | static uint8_t cycleDone[5]={0,0,0,0,0}, 2 | resourceIsOn[5] = {0,0,0,0,0}; 3 | static uint32_t LastToggleTime[5] ={0,0,0,0,0}; 4 | static int16_t i2c_errors_count_old = 0; 5 | 6 | static uint8_t SequenceActive[5]={0,0,0,0,0}; 7 | 8 | #if defined(BUZZER) 9 | uint8_t isBuzzerON() { return resourceIsOn[1]; } // returns true while buzzer is buzzing; returns 0 for silent periods 10 | #else 11 | uint8_t isBuzzerON() { return 0; } 12 | #endif //end of buzzer define 13 | /********************************************************************/ 14 | /**** Alarm Handling ****/ 15 | /********************************************************************/ 16 | /* 17 | AlarmArray 18 | 0: toggle 19 | 1: failsafe 20 | 2: noGPS 21 | 3: beeperOn 22 | 4: pMeter 23 | 5: runtime 24 | 6: vBat 25 | 7: confirmation 26 | 8: Acc 27 | 9: I2C Error 28 | */ 29 | /* 30 | Resources: 31 | 0: onboard LED 32 | 1: Buzzer 33 | 2: PL GREEN 34 | 3: PL BLUE 35 | 4: PL RED 36 | */ 37 | void alarmHandler(){ 38 | 39 | #if defined(RCOPTIONSBEEP) 40 | static uint8_t i = 0,firstrun = 1, last_rcOptions[CHECKBOXITEMS]; 41 | 42 | if (last_rcOptions[i] != rcOptions[i]) alarmArray[0] = 1; 43 | last_rcOptions[i] = rcOptions[i]; 44 | i++; 45 | if(i >= CHECKBOXITEMS)i=0; 46 | 47 | if(firstrun == 1 && alarmArray[7] == 0) { 48 | alarmArray[0] = 0; //only enable options beep AFTER gyro init 49 | alarmArray[3] = 0; 50 | } 51 | else firstrun = 0; 52 | #endif 53 | 54 | #if defined(FAILSAFE) 55 | if ( failsafeCnt > (5*FAILSAFE_DELAY) && f.ARMED) { 56 | alarmArray[1] = 1; //set failsafe warning level to 1 while landing 57 | if (failsafeCnt > 5*(FAILSAFE_DELAY+FAILSAFE_OFF_DELAY)) alarmArray[1] = 2; //start "find me" signal after landing 58 | } 59 | if ( failsafeCnt > (5*FAILSAFE_DELAY) && !f.ARMED) alarmArray[1] = 2; // tx turned off while motors are off: start "find me" signal 60 | if ( failsafeCnt == 0) alarmArray[1] = 0; // turn off alarm if TX is okay 61 | #endif 62 | 63 | #if GPS 64 | if ((f.GPS_HOME_MODE || f.GPS_HOLD_MODE) && !f.GPS_FIX) alarmArray[2] = 2; 65 | else if (!f.GPS_FIX)alarmArray[2] = 1; 66 | else alarmArray[2] = 0; 67 | #endif 68 | 69 | #if defined(BUZZER) 70 | if ( rcOptions[BOXBEEPERON] )alarmArray[3] = 1; 71 | else alarmArray[3] = 0; 72 | #endif 73 | 74 | #if defined(POWERMETER) 75 | if ( (pMeter[PMOTOR_SUM] < pAlarm) || (pAlarm == 0) || !f.ARMED) alarmArray[4] = 0; 76 | else if (pMeter[PMOTOR_SUM] > pAlarm) alarmArray[4] = 1; 77 | #endif 78 | 79 | #if defined(ARMEDTIMEWARNING) 80 | if (ArmedTimeWarningMicroSeconds > 0 && armedTime >= ArmedTimeWarningMicroSeconds && f.ARMED) alarmArray[5] = 1; 81 | else alarmArray[5] = 0; 82 | #endif 83 | 84 | #if defined(VBAT) 85 | if (vbatMin < conf.vbatlevel_crit) alarmArray[6] = 4; 86 | else if ( (vbat>conf.vbatlevel_warn1) || (conf.no_vbat > vbat)) alarmArray[6] = 0; 87 | else if (vbat > conf.vbatlevel_warn2) alarmArray[6] = 2; 88 | else alarmArray[6] = 4; 89 | #endif 90 | 91 | if (i2c_errors_count > i2c_errors_count_old+100 || i2c_errors_count < -1) alarmArray[9] = 1; 92 | else alarmArray[9] = 0; 93 | 94 | alarmPatternComposer(); 95 | } 96 | 97 | void alarmPatternComposer(){ 98 | static char resource = 0; 99 | // patternDecode(length1,length2,length3,beeppause,endpause,loop) 100 | #if defined(BUZZER) 101 | resource = 1; //buzzer selected 102 | if (alarmArray[1] == 2) patternDecode(resource,200,0,0,50,2000); //failsafe "find me" signal 103 | else if (alarmArray[1] == 1 || alarmArray[8] == 1) patternDecode(resource,50,200,200,50,50); //failsafe "panic" or Acc not calibrated 104 | else if (alarmArray[0] == 1) patternDecode(resource,50,0,0,50,0); //toggle 1 105 | else if (alarmArray[0] == 2) patternDecode(resource,50,50,0,50,0); //toggle 2 106 | else if (alarmArray[0] > 2) patternDecode(resource,50,50,50,50,0); //toggle else 107 | else if (alarmArray[2] == 2) patternDecode(resource,50,50,0,50,50); //gps installed but no fix 108 | else if (alarmArray[3] == 1) patternDecode(resource,50,50,50,50,50); //BeeperOn 109 | else if (alarmArray[4] == 1) patternDecode(resource,50,50,0,50,120); //pMeter Warning 110 | else if (alarmArray[5] == 1) patternDecode(resource,50,50,50,50,0); //Runtime warning 111 | else if (alarmArray[6] == 4) patternDecode(resource,50,50,200,50,2000); //vbat critical 112 | else if (alarmArray[6] == 2) patternDecode(resource,50,200,0,50,2000); //vbat warning 113 | else if (alarmArray[6] == 1) patternDecode(resource,200,0,0,50,2000); //vbat info 114 | else if (alarmArray[7] == 1) patternDecode(resource,200,0,0,50,200); //confirmation indicator 1x 115 | else if (alarmArray[7] == 2) patternDecode(resource,200,200,0,50,200); //confirmation indicator 2x 116 | else if (alarmArray[7] > 2) patternDecode(resource,200,200,200,50,200); //confirmation indicator 3x 117 | else if (SequenceActive[(uint8_t)resource] == 1) patternDecode(resource,0,0,0,0,0); // finish last sequence if not finished yet 118 | else turnOff(resource); // turn off the resource 119 | alarmArray[8] = 0; //reset acc not calibrated 120 | 121 | #endif 122 | #if defined(PILOTLAMP) 123 | if (alarmArray[9] == 1) PilotLampSequence(100,B000111,2); //I2C Error 124 | else if (alarmArray[3] == 1) PilotLampSequence(100,B0101<<8|B00010001,4); //BeeperOn 125 | else{ 126 | resource = 2; 127 | if (f.ARMED && f.ANGLE_MODE) patternDecode(resource,100,100,100,100,1000); //Green Slow Blink-->angle 128 | else if (f.ARMED && f.HORIZON_MODE) patternDecode(resource,200,200,200,100,1000); //Green mid Blink-->horizon 129 | else if (f.ARMED) patternDecode(resource,100,100,0,100,1000); //Green fast Blink-->acro 130 | else turnOff(resource); //switch off 131 | resource = 3; 132 | #if GPS 133 | if (alarmArray[2]==1) patternDecode(resource,100,100,100,100,100); // blue fast blink -->no gps fix 134 | else if (f.GPS_HOME_MODE || f.GPS_HOLD_MODE) patternDecode(resource,100,100,100,100,1000); //blue slow blink --> gps active 135 | else setTiming(resource,100,1000); //blue short blink -->gps fix ok 136 | #else 137 | turnOff(resource); 138 | #endif 139 | resource = 4; 140 | if (alarmArray[1] == 1) setTiming(resource,100,100); //Red fast blink--> failsafe panic 141 | else if (alarmArray[1] == 2) patternDecode(resource,1000,0,0,0,2000); //red slow blink--> failsafe find me 142 | else turnOff(resource); 143 | } 144 | #endif 145 | } 146 | 147 | void patternDecode(uint8_t resource,uint16_t first,uint16_t second,uint16_t third,uint16_t cyclepause, uint16_t endpause){ 148 | static uint16_t pattern[5][5]; 149 | static uint8_t icnt[5] = {0,0,0,0,0}; 150 | 151 | if(SequenceActive[resource] == 0){ 152 | SequenceActive[resource] = 1; 153 | pattern[resource][0] = first; 154 | pattern[resource][1] = second; 155 | pattern[resource][2] = third; 156 | pattern[resource][3] = endpause; 157 | pattern[resource][4] = cyclepause; 158 | } 159 | if(icnt[resource] <3 ){ 160 | if (pattern[resource][icnt[resource]] != 0){ 161 | setTiming(resource,pattern[resource][icnt[resource]],pattern[resource][4]); 162 | } 163 | } 164 | else if (LastToggleTime[resource] < (millis()-pattern[resource][3])) { //sequence is over: reset everything 165 | icnt[resource]=0; 166 | SequenceActive[resource] = 0; //sequence is now done, cycleDone sequence may begin 167 | alarmArray[0] = 0; //reset toggle bit 168 | alarmArray[7] = 0; //reset confirmation bit 169 | turnOff(resource); 170 | return; 171 | } 172 | if (cycleDone[resource] == 1 || pattern[resource][icnt[resource]] == 0) { //single on off cycle is done 173 | if (icnt[resource] < 3) { 174 | icnt[resource]++; 175 | } 176 | cycleDone[resource] = 0; 177 | turnOff(resource); 178 | } 179 | } 180 | 181 | void turnOff(uint8_t resource){ 182 | if (resource == 1) { 183 | if (resourceIsOn[1]) { 184 | BUZZERPIN_OFF; 185 | resourceIsOn[1] = 0; 186 | } 187 | }else if (resource == 0) { 188 | if (resourceIsOn[0]) { 189 | resourceIsOn[0] = 0; 190 | LEDPIN_OFF; 191 | } 192 | }else if (resource == 2) { 193 | if (resourceIsOn[2]) { 194 | resourceIsOn[2] = 0; 195 | #if defined (PILOTLAMP) 196 | PilotLamp(PL_GRN_OFF); 197 | #endif 198 | } 199 | }else if (resource == 3) { 200 | if (resourceIsOn[3]) { 201 | resourceIsOn[3] = 0; 202 | #if defined (PILOTLAMP) 203 | PilotLamp(PL_BLU_OFF); 204 | #endif 205 | } 206 | }else if (resource == 4) { 207 | if (resourceIsOn[4]) { 208 | resourceIsOn[4] = 0; 209 | #if defined (PILOTLAMP) 210 | PilotLamp(PL_RED_OFF); 211 | #endif 212 | } 213 | } 214 | } 215 | 216 | #if defined (PILOTLAMP) 217 | //original code based on mr.rc-cam and jevermeister work 218 | //modified by doughboy to use timer interrupt 219 | 220 | #define PL_BUF_SIZE 8 221 | volatile uint8_t queue[PL_BUF_SIZE]; //circular queue 222 | volatile uint8_t head = 0; 223 | volatile uint8_t tail = 0; 224 | 225 | /********************************************************************/ 226 | /**** Pilot Lamp Handling ****/ 227 | /********************************************************************/ 228 | 229 | 230 | //define your light pattern by bits, 0=off 1=on 231 | //define up to 5 patterns that cycle using 15 bits, pattern starts at bit 0 in groups of 3 232 | // 14 13 12 11 10 9 8 7 6 5 4 3 2 1 0 233 | // R B G R B G R B G R B G R B G 234 | //parameters speed is the ms between patterns 235 | // pattern is the 16bit (uses only 15 bits) specifying up to 5 patterns 236 | // num_patterns is the number of patterns defined 237 | //example to define sequential light where G->B->R then back to G, you need 4 patterns 238 | // B00000101<<8 | B00010001 239 | //example: to define alternate all on and al off, you only need two patterns 240 | // B00000111 241 | void PilotLampSequence(uint16_t speed, uint16_t pattern, uint8_t num_patterns){ 242 | static uint32_t lastswitch = 0; 243 | static uint8_t seqno = 0; 244 | static uint16_t lastpattern = 0; //since variables are static, when switching patterns, the correct pattern will start on the second sequence 245 | 246 | if(millis() < (lastswitch + speed)) 247 | return; //not time to change pattern yet 248 | lastswitch = millis(); 249 | 250 | for (uint8_t i=0;i<3;i++) { 251 | uint8_t state = (pattern >>(seqno*3+i)) & 1; //get pattern bit 252 | //since value is multiple of 25, we can calculate time based on pattern position 253 | //i=0 is green, 1=blue, 2=red, green ON ticks is calculated as 50*(0+1)-1*25 = 25 ticks 254 | uint8_t tick = 50*(i+1); 255 | if (state) 256 | tick -=25; 257 | PilotLamp(tick); 258 | resourceIsOn[i+2]=state; 259 | } 260 | seqno++; 261 | seqno%=num_patterns; 262 | } 263 | 264 | void PilotLamp(uint8_t count){ 265 | if (((tail+1)%PL_BUF_SIZE)!=head) { 266 | queue[tail]=count; 267 | tail++; 268 | tail=(tail%PL_BUF_SIZE); 269 | } 270 | } 271 | //ISR code sensitive to changes, test thoroughly before flying 272 | ISR(PL_ISR) { //the interrupt service routine 273 | static uint8_t state = 0; 274 | uint8_t h = head; 275 | uint8_t t = tail; 276 | if (state==0) { 277 | if (h!=t) { 278 | uint8_t c = queue[h]; 279 | PL_PIN_ON; 280 | PL_CHANNEL+=c; 281 | h = ((h+1) % PL_BUF_SIZE); 282 | head = h; 283 | state=1; 284 | } 285 | } else if (state==1) { 286 | PL_PIN_OFF; 287 | PL_CHANNEL+=PL_IDLE; 288 | state=0; 289 | } 290 | } 291 | #endif 292 | 293 | /********************************************************************/ 294 | /**** LED Handling ****/ 295 | /********************************************************************/ 296 | //Beware!! Is working with delays, do not use inflight! 297 | 298 | void blinkLED(uint8_t num, uint8_t ontime,uint8_t repeat) { 299 | uint8_t i,r; 300 | for (r=0;r= (LastToggleTime[resource] + pause))&& pulse != 0) { 327 | resourceIsOn[resource] = 1; 328 | toggleResource(resource,1); 329 | LastToggleTime[resource]=millis(); 330 | } else if ( (resourceIsOn[resource] && (millis() >= LastToggleTime[resource] + pulse) ) || (pulse==0 && resourceIsOn[resource]) ) { 331 | resourceIsOn[resource] = 0; 332 | toggleResource(resource,0); 333 | LastToggleTime[resource]=millis(); 334 | cycleDone[resource] = 1; 335 | } 336 | } 337 | 338 | void toggleResource(uint8_t resource, uint8_t activate){ 339 | switch(resource) { 340 | #if defined (BUZZER) 341 | case 1: 342 | if (activate == 1) {BUZZERPIN_ON;} 343 | else BUZZERPIN_OFF; 344 | break; 345 | #endif 346 | #if defined (PILOTLAMP) 347 | case 2: 348 | if (activate == 1) PilotLamp(PL_GRN_ON); 349 | else PilotLamp(PL_GRN_OFF); 350 | break; 351 | case 3: 352 | if (activate == 1) PilotLamp(PL_BLU_ON); 353 | else PilotLamp(PL_BLU_OFF); 354 | break; 355 | case 4: 356 | if (activate == 1) PilotLamp(PL_RED_ON); 357 | else PilotLamp(PL_RED_OFF); 358 | break; 359 | #endif 360 | case 0: 361 | default: 362 | if (activate == 1) {LEDPIN_ON;} 363 | else LEDPIN_OFF; 364 | break; 365 | } 366 | return; 367 | } 368 | 369 | 370 | /********************************************************************/ 371 | /**** LED Ring Handling ****/ 372 | /********************************************************************/ 373 | 374 | #if defined(LED_RING) 375 | 376 | #define LED_RING_ADDRESS 0xDA //7 bits 377 | 378 | void i2CLedRingState() { 379 | uint8_t b[10]; 380 | 381 | b[0]='M'; // MultiwII mode 382 | if (f.ARMED) { // Motors running = flying 383 | if(!(f.ANGLE_MODE||f.HORIZON_MODE)){ //ACRO 384 | b[0]= 'x'; 385 | } 386 | else if(f.GPS_HOME_MODE){ //RTH 387 | b[0]= 'w'; 388 | } 389 | else if(f.GPS_HOLD_MODE){//Position Hold 390 | b[0]= 'v'; 391 | } 392 | else if(f.HORIZON_MODE){ //HORIZON mode 393 | b[0]= 'y'; 394 | } 395 | else { 396 | b[0]= 'u'; // ANGLE mode 397 | } 398 | i2c_rep_start(LED_RING_ADDRESS); 399 | i2c_write(b[0]); 400 | i2c_stop(); 401 | } 402 | else if (!f.ACC_CALIBRATED) { // Multiwii not stable or uncalibrated 403 | b[0]= 't'; 404 | i2c_rep_start(LED_RING_ADDRESS); 405 | i2c_write(b[0]); 406 | i2c_stop(); 407 | } 408 | else { // Motors not running = on the ground 409 | b[0]= 's'; 410 | if (f.ANGLE_MODE) b[1]=1; 411 | else if (f.HORIZON_MODE) b[1]=2; 412 | else b[1]= 0; 413 | if (f.BARO_MODE) b[2]=1; 414 | else b[2]= 0; 415 | if (f.MAG_MODE) b[3]=1; 416 | else b[3]= 0; 417 | #if GPS 418 | if (rcOptions[BOXGPSHOME]) b[4]=2; 419 | else if (rcOptions[BOXGPSHOLD]) b[4]=1; 420 | else b[4]=0; 421 | #else 422 | b[4]=0; 423 | #endif 424 | b[5]=(180-heading)/2; // 1 unit = 2 degrees; 425 | b[6]=GPS_numSat; 426 | i2c_rep_start(LED_RING_ADDRESS); 427 | for(uint8_t i=0;i<7;i++){ 428 | i2c_write(b[i]); 429 | } 430 | i2c_stop(); 431 | } 432 | #if defined (VBAT) 433 | if (vbat < conf.vbatlevel_warn1){ // Uh oh - battery low 434 | i2c_rep_start(LED_RING_ADDRESS); 435 | i2c_write('r'); 436 | i2c_stop(); 437 | } 438 | # endif 439 | } 440 | 441 | void blinkLedRing() { 442 | uint8_t b[3]; 443 | b[0]='g'; 444 | b[1]= 10; 445 | b[2]= 10; 446 | i2c_rep_start(LED_RING_ADDRESS<<1); 447 | for(uint8_t i=0;i<3;i++) 448 | i2c_write(b[i]); 449 | i2c_stop(); 450 | } 451 | #endif 452 | 453 | /********************************************************************/ 454 | /**** LED flasher Handling ****/ 455 | /********************************************************************/ 456 | 457 | #if defined(LED_FLASHER) 458 | static uint8_t led_flasher_sequence = 0; 459 | /* if we load a specific sequence and do not want it change, set this flag */ 460 | static enum { 461 | LED_FLASHER_AUTO, 462 | LED_FLASHER_CUSTOM 463 | } led_flasher_control = LED_FLASHER_AUTO; 464 | 465 | void init_led_flasher() { 466 | #if defined(LED_FLASHER_DDR) 467 | LED_FLASHER_DDR |= (1<= 0 && sonarAlt <= LANDING_LIGHTS_AUTO_ALTITUDE && f.ARMED) 560 | #endif 561 | #if defined(LED_FLASHER_DDR) & defined(LANDING_LIGHTS_ADOPT_LED_FLASHER_PATTERN) 562 | || (led_flasher_on()) 563 | #endif 564 | ) { 565 | switch_landing_lights(1); 566 | } else { 567 | switch_landing_lights(0); 568 | } 569 | } 570 | #endif 571 | 572 | /********************************************************************/ 573 | /**** Variometer signaling ****/ 574 | /********************************************************************/ 575 | #ifdef VARIOMETER 576 | #define TRESHOLD_UP 50 // (m1) treshhold for up velocity 577 | #define TRESHOLD_DOWN 40 // (m1) treshhold for up velocity 578 | #define TRESHOLD_UP_MINUS_DOWN 10 // (m1) you compute: TRESHOLD_UP - TRESHOLD_DOWN 579 | #define ALTITUDE_INTERVAL 400 // (m2) in calls; interval to perodically observe altitude change 580 | #define DELTA_ALT_TRESHOLD 200 // (m2) in cm; treshold for delta altitude after ALTITUDE_INTERVAL 581 | #define DELTA_T 5 // (m2) divisor for delta_alt to compute vel 582 | #define SIGNAL_SCALE 4 // you compute: (50ms per beep / 5*3ms cycle time) 583 | #define SILENCE_M 200 // max duration of silence in calls 584 | #define SILENCE_SCALE 33 // vario scale: larger -> slower decay of silence 585 | #define SILENCE_A 6600 // you compute: SILENCE_M * SILENCE_SCALE 586 | #define DURATION_SUP 5 // sup duration of signal 587 | #define DURATION_SCALE 100 // vario scale: larger -> slower rise of length 588 | 589 | /* vario_signaling() gets called every 5th cycle (~2ms - 5ms) -> (~10ms - 25ms) 590 | * modulates silence duration between tones and tone duration 591 | * higher abs(vario) -> shorther silence & longer signal duration. 592 | * Utilize two methods for combined short and long term observation 593 | */ 594 | void vario_signaling() { 595 | static int16_t last_v = 0; 596 | static uint16_t silence = 0; 597 | static int16_t max_v = 0; 598 | static uint8_t max_up = 0; 599 | 600 | uint16_t s = 0; 601 | int16_t v = 0; 602 | 603 | /* method 1: use vario to follow short term up/down movement : */ 604 | #if (VARIOMETER == 1) || (VARIOMETER == 12) 605 | { 606 | uint8_t up = (vario > 0 ? 1 : 0 ); //, down = (vario < 0 ? 1 : 0 ); 607 | //int16_t v = abs(vario) - up * TRESHOLD_UP - down * TRESHOLD_DOWN; 608 | v = abs(vario) - up * (TRESHOLD_UP_MINUS_DOWN) - TRESHOLD_DOWN; 609 | if (silence>0) silence--; else silence = 0; 610 | if (v > 0) { 611 | // going up or down 612 | if (v > last_v) { 613 | // current speed greater than speed for last signal, 614 | // so shorten the remaining silence period 615 | s = (SILENCE_A) / (SILENCE_SCALE + v); 616 | if (silence > s) silence = s; 617 | } 618 | // remember interim max v 619 | if (v > max_v) { 620 | max_v = v; 621 | max_up = up; 622 | } 623 | } // end of (v>0) 624 | } 625 | #endif // end method 1 626 | /* method 2: use altitude to follow long term up/down movement : */ 627 | #if (VARIOMETER == 2) || (VARIOMETER == 12) 628 | { 629 | static uint16_t t = 0; 630 | if (!(t++ % ALTITUDE_INTERVAL)) { 631 | static int32_t last_BaroAlt = 0; 632 | int32_t delta_BaroAlt = BaroAlt - last_BaroAlt; 633 | if (abs(delta_BaroAlt) > DELTA_ALT_TRESHOLD) { 634 | // inject suitable values 635 | max_v = abs(delta_BaroAlt / DELTA_T); 636 | max_up = (delta_BaroAlt > 0 ? 1 : 0); 637 | silence = 0; 638 | } 639 | last_BaroAlt = BaroAlt; 640 | } 641 | } 642 | #endif // end method 2 643 | /* something to signal now? */ 644 | if ( (silence == 0) && (max_v > 0) ) { 645 | // create new signal 646 | uint16_t d = (DURATION_SUP * max_v)/(DURATION_SCALE + max_v); 647 | s = (SILENCE_A) / (SILENCE_SCALE + max_v); 648 | s+= d * SIGNAL_SCALE; 649 | vario_output(d, max_up); 650 | last_v = v; 651 | max_v = 0; 652 | max_up = 0; 653 | silence = s; 654 | } 655 | } // end of vario_signaling() 656 | 657 | void vario_output(uint16_t d, uint8_t up) { 658 | if (d == 0) return; 659 | #if defined(SUPPRESS_VARIOMETER_UP) 660 | if (up) return; 661 | #elif defined(SUPPRESS_VARIOMETER_DOWN) 662 | if (!up) return; 663 | #endif 664 | #ifdef VARIOMETER_SINGLE_TONE 665 | uint8_t s1 = 0x07; 666 | uint8_t d1 = d; 667 | #else 668 | uint8_t s1 = (up ? 0x05 : 0x07); 669 | uint8_t d1 = d/2; 670 | #endif 671 | if (d1<1) d1 = 1; 672 | for (uint8_t i=0; i //Auto-included by the Arduino core... but we need it sooner. 7 | #endif 8 | 9 | //RAW RC values will be store here 10 | #if defined(SBUS) 11 | volatile uint16_t rcValue[RC_CHANS] = {1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502}; // interval [1000;2000] 12 | #elif defined(SPEKTRUM) || defined(SERIAL_SUM_PPM) 13 | volatile uint16_t rcValue[RC_CHANS] = {1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502}; // interval [1000;2000] 14 | #else 15 | volatile uint16_t rcValue[RC_CHANS] = {1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502}; // interval [1000;2000] 16 | #endif 17 | 18 | #if defined(SERIAL_SUM_PPM) //Channel order for PPM SUM RX Configs 19 | static uint8_t rcChannel[RC_CHANS] = {SERIAL_SUM_PPM}; 20 | #elif defined(SBUS) //Channel order for SBUS RX Configs 21 | // for 16 + 2 Channels SBUS. The 10 extra channels 8->17 are not used by MultiWii, but it should be easy to integrate them. 22 | static uint8_t rcChannel[RC_CHANS] = {PITCH,YAW,THROTTLE,ROLL,AUX1,AUX2,AUX3,AUX4,8,9,10,11,12,13,14,15,16,17}; 23 | static uint16_t sbusIndex=0; 24 | #elif defined(SPEKTRUM) 25 | static uint8_t rcChannel[RC_CHANS] = {PITCH,YAW,THROTTLE,ROLL,AUX1,AUX2,AUX3,AUX4,8,9,10,11}; 26 | #else // Standard Channel order 27 | static uint8_t rcChannel[RC_CHANS] = {ROLLPIN, PITCHPIN, YAWPIN, THROTTLEPIN, AUX1PIN,AUX2PIN,AUX3PIN,AUX4PIN}; 28 | static uint8_t PCInt_RX_Pins[PCINT_PIN_COUNT] = {PCINT_RX_BITS}; // if this slowes the PCINT readings we can switch to a define for each pcint bit 29 | #endif 30 | 31 | #define FAILSAFE_DETECT_TRESHOLD 985 32 | 33 | /**************************************************************************************/ 34 | /*************** RX Pin Setup ********************/ 35 | /**************************************************************************************/ 36 | void configureReceiver() { 37 | /****************** Configure each rc pin for PCINT ***************************/ 38 | #if defined(STANDARD_RX) 39 | #if defined(MEGA) 40 | DDRK = 0; // defined PORTK as a digital port ([A8-A15] are consired as digital PINs and not analogical) 41 | #endif 42 | // PCINT activation 43 | for(uint8_t i = 0; i < PCINT_PIN_COUNT; i++){ // i think a for loop is ok for the init. 44 | PCINT_RX_PORT |= PCInt_RX_Pins[i]; 45 | PCINT_RX_MASK |= PCInt_RX_Pins[i]; 46 | } 47 | PCICR = PCIR_PORT_BIT; 48 | 49 | /************* atmega328P's Specific Aux2 Pin Setup *********************/ 50 | #if defined(PROMINI) 51 | #if defined(RCAUXPIN) 52 | PCICR |= (1 << 0) ; // PCINT activated also for PINS [D8-D13] on port B 53 | #if defined(RCAUXPIN8) 54 | PCMSK0 = (1 << 0); 55 | #endif 56 | #if defined(RCAUXPIN12) 57 | PCMSK0 = (1 << 4); 58 | #endif 59 | #endif 60 | #endif 61 | 62 | /*************** atmega32u4's Specific RX Pin Setup **********************/ 63 | #if defined(PROMICRO) 64 | //Trottle on pin 7 65 | DDRE &= ~(1 << 6); // pin 7 to input 66 | PORTE |= (1 << 6); // enable pullups 67 | EIMSK |= (1 << INT6); // enable interuppt 68 | EICRB |= (1 << ISC60); 69 | // Aux2 pin on PBO (D17/RXLED) 70 | #if defined(RCAUX2PIND17) 71 | DDRB &= ~(1 << 0); // set D17 to input 72 | #endif 73 | // Aux2 pin on PD2 (RX0) 74 | #if defined(RCAUX2PINRXO) 75 | DDRD &= ~(1 << 2); // RX to input 76 | PORTD |= (1 << 2); // enable pullups 77 | EIMSK |= (1 << INT2); // enable interuppt 78 | EICRA |= (1 << ISC20); 79 | #endif 80 | #endif 81 | 82 | /************************* Special RX Setup ********************************/ 83 | #endif 84 | // Init PPM SUM RX 85 | #if defined(SERIAL_SUM_PPM) 86 | PPM_PIN_INTERRUPT; 87 | #endif 88 | // Init Sektrum Satellite RX 89 | #if defined (SPEKTRUM) 90 | SerialOpen(SPEK_SERIAL_PORT,115200); 91 | #endif 92 | // Init SBUS RX 93 | #if defined(SBUS) 94 | SerialOpen(1,100000); 95 | #endif 96 | } 97 | 98 | /**************************************************************************************/ 99 | /*************** Standard RX Pins reading ********************/ 100 | /**************************************************************************************/ 101 | #if defined(STANDARD_RX) 102 | 103 | #if defined(FAILSAFE) && !defined(PROMICRO) 104 | // predefined PC pin block (thanks to lianj) - Version with failsafe 105 | #define RX_PIN_CHECK(pin_pos, rc_value_pos) \ 106 | if (mask & PCInt_RX_Pins[pin_pos]) { \ 107 | if (!(pin & PCInt_RX_Pins[pin_pos])) { \ 108 | dTime = cTime-edgeTime[pin_pos]; \ 109 | if (900FAILSAFE_DETECT_TRESHOLD) \ 114 | GoodPulses |= (1< we keep only 16 bits 146 | sei(); // re enable other interrupts at this point, the rest of this interrupt is not so time critical and can be interrupted safely 147 | PCintLast = pin; // we memorize the current state of all PINs [D0-D7] 148 | 149 | #if (PCINT_PIN_COUNT > 0) 150 | RX_PIN_CHECK(0,2); 151 | #endif 152 | #if (PCINT_PIN_COUNT > 1) 153 | RX_PIN_CHECK(1,4); 154 | #endif 155 | #if (PCINT_PIN_COUNT > 2) 156 | RX_PIN_CHECK(2,5); 157 | #endif 158 | #if (PCINT_PIN_COUNT > 3) 159 | RX_PIN_CHECK(3,6); 160 | #endif 161 | #if (PCINT_PIN_COUNT > 4) 162 | RX_PIN_CHECK(4,7); 163 | #endif 164 | #if (PCINT_PIN_COUNT > 5) 165 | RX_PIN_CHECK(5,0); 166 | #endif 167 | #if (PCINT_PIN_COUNT > 6) 168 | RX_PIN_CHECK(6,1); 169 | #endif 170 | #if (PCINT_PIN_COUNT > 7) 171 | RX_PIN_CHECK(7,3); 172 | #endif 173 | 174 | #if defined(FAILSAFE) && !defined(PROMICRO) 175 | if (GoodPulses==(1< 20) failsafeCnt -= 20; else failsafeCnt = 0; 178 | } 179 | #endif 180 | } 181 | /********************* atmega328P's Aux2 Pins *************************/ 182 | #if defined(PROMINI) 183 | #if defined(RCAUXPIN) 184 | /* this ISR is a simplification of the previous one for PROMINI on port D 185 | it's simplier because we know the interruption deals only with one PIN: 186 | bit 0 of PORT B, ie Arduino PIN 8 187 | or bit 4 of PORTB, ie Arduino PIN 12 188 | => no need to check which PIN has changed */ 189 | ISR(PCINT0_vect) { 190 | uint8_t pin; 191 | uint16_t cTime,dTime; 192 | static uint16_t edgeTime; 193 | 194 | pin = PINB; 195 | cTime = micros(); 196 | sei(); 197 | #if defined(RCAUXPIN8) 198 | if (!(pin & 1<<0)) { //indicates if the bit 0 of the arduino port [B0-B7] is not at a high state (so that we match here only descending PPM pulse) 199 | #endif 200 | #if defined(RCAUXPIN12) 201 | if (!(pin & 1<<4)) { //indicates if the bit 4 of the arduino port [B0-B7] is not at a high state (so that we match here only descending PPM pulse) 202 | #endif 203 | dTime = cTime-edgeTime; if (900FAILSAFE_DETECT_TRESHOLD) { // if Throttle value is higher than FAILSAFE_DETECT_TRESHOLD 222 | if(failsafeCnt > 20) failsafeCnt -= 20; else failsafeCnt = 0; // If pulse present on THROTTLE pin (independent from ardu version), clear FailSafe counter - added by MIS 223 | } 224 | #endif 225 | } 226 | }else last = now; 227 | } 228 | // Aux 2 229 | #if defined(RCAUX2PINRXO) 230 | ISR(INT2_vect){ 231 | static uint16_t now,diff; 232 | static uint16_t last = 0; 233 | now = micros(); 234 | if(!(PIND & (1<<2))){ 235 | diff = now - last; 236 | if(9003000) chan = 0; 272 | else { 273 | if(900FAILSAFE_DETECT_TRESHOLD) GoodPulses |= (1< 20) failsafeCnt -= 20; else failsafeCnt = 0; 280 | } 281 | #endif 282 | } 283 | chan++; 284 | } 285 | } 286 | #endif 287 | 288 | /**************************************************************************************/ 289 | /*************** SBUS RX Data ********************/ 290 | /**************************************************************************************/ 291 | #if defined(SBUS) 292 | void readSBus(){ 293 | #define SBUS_SYNCBYTE 0x0F // Not 100% sure: at the beginning of coding it was 0xF0 !!! 294 | static uint16_t sbus[25]={0}; 295 | while(SerialAvailable(1)){ 296 | int val = SerialRead(1); 297 | if(sbusIndex==0 && val != SBUS_SYNCBYTE) 298 | continue; 299 | sbus[sbusIndex++] = val; 300 | if(sbusIndex==25){ 301 | sbusIndex=0; 302 | rcValue[0] = ((sbus[1]|sbus[2]<< 8) & 0x07FF)/2+976; // Perhaps you may change the term "/2+976" -> center will be 1486 303 | rcValue[1] = ((sbus[2]>>3|sbus[3]<<5) & 0x07FF)/2+976; 304 | rcValue[2] = ((sbus[3]>>6|sbus[4]<<2|sbus[5]<<10) & 0x07FF)/2+976; 305 | rcValue[3] = ((sbus[5]>>1|sbus[6]<<7) & 0x07FF)/2+976; 306 | rcValue[4] = ((sbus[6]>>4|sbus[7]<<4) & 0x07FF)/2+976; 307 | rcValue[5] = ((sbus[7]>>7|sbus[8]<<1|sbus[9]<<9) & 0x07FF)/2+976; 308 | rcValue[6] = ((sbus[9]>>2|sbus[10]<<6) & 0x07FF)/2+976; 309 | rcValue[7] = ((sbus[10]>>5|sbus[11]<<3) & 0x07FF)/2+976; // & the other 8 + 2 channels if you need them 310 | //The following lines: If you need more than 8 channels, max 16 analog + 2 digital. Must comment the not needed channels! 311 | rcValue[8] = ((sbus[12]|sbus[13]<< 8) & 0x07FF)/2+976; 312 | rcValue[9] = ((sbus[13]>>3|sbus[14]<<5) & 0x07FF)/2+976; 313 | rcValue[10] = ((sbus[14]>>6|sbus[15]<<2|sbus[16]<<10) & 0x07FF)/2+976; 314 | rcValue[11] = ((sbus[16]>>1|sbus[17]<<7) & 0x07FF)/2+976; 315 | rcValue[12] = ((sbus[17]>>4|sbus[18]<<4) & 0x07FF)/2+976; 316 | rcValue[13] = ((sbus[18]>>7|sbus[19]<<1|sbus[20]<<9) & 0x07FF)/2+976; 317 | rcValue[14] = ((sbus[20]>>2|sbus[21]<<6) & 0x07FF)/2+976; 318 | rcValue[15] = ((sbus[21]>>5|sbus[22]<<3) & 0x07FF)/2+976; 319 | // now the two Digital-Channels 320 | if ((sbus[23]) & 0x0001) rcValue[16] = 2000; else rcValue[16] = 1000; 321 | if ((sbus[23] >> 1) & 0x0001) rcValue[17] = 2000; else rcValue[17] = 1000; 322 | 323 | // Failsafe: there is one Bit in the SBUS-protocol (Byte 25, Bit 4) whitch is the failsafe-indicator-bit 324 | #if defined(FAILSAFE) 325 | if (!((sbus[23] >> 3) & 0x0001)) 326 | {if(failsafeCnt > 20) failsafeCnt -= 20; else failsafeCnt = 0;} // clear FailSafe counter 327 | #endif 328 | } 329 | } 330 | } 331 | #endif 332 | 333 | 334 | /**************************************************************************************/ 335 | /*************** combine and sort the RX Datas ********************/ 336 | /**************************************************************************************/ 337 | #if defined(SPEKTRUM) 338 | void readSpektrum() { 339 | if ((!f.ARMED) && 340 | #if defined(FAILSAFE) || (SPEK_SERIAL_PORT != 0) 341 | (failsafeCnt > 5) && 342 | #endif 343 | ( SerialPeek(SPEK_SERIAL_PORT) == '$')) { 344 | while (SerialAvailable(SPEK_SERIAL_PORT)) { 345 | serialCom(); 346 | delay (10); 347 | } 348 | return; 349 | } //End of: Is it the GUI? 350 | while (SerialAvailable(SPEK_SERIAL_PORT) > SPEK_FRAME_SIZE) { // More than a frame? More bytes implies we weren't called for multiple frame times. We do not want to process 'old' frames in the buffer. 351 | for (uint8_t i = 0; i < SPEK_FRAME_SIZE; i++) {SerialRead(SPEK_SERIAL_PORT);} //Toss one full frame of bytes. 352 | } 353 | if (spekFrameFlags == 0x01) { //The interrupt handler saw at least one valid frame start since we were last here. 354 | if (SerialAvailable(SPEK_SERIAL_PORT) == SPEK_FRAME_SIZE) { //A complete frame? If not, we'll catch it next time we are called. 355 | SerialRead(SPEK_SERIAL_PORT); SerialRead(SPEK_SERIAL_PORT); //Eat the header bytes 356 | for (uint8_t b = 2; b < SPEK_FRAME_SIZE; b += 2) { 357 | uint8_t bh = SerialRead(SPEK_SERIAL_PORT); 358 | uint8_t bl = SerialRead(SPEK_SERIAL_PORT); 359 | uint8_t spekChannel = 0x0F & (bh >> SPEK_CHAN_SHIFT); 360 | if (spekChannel < RC_CHANS) rcValue[spekChannel] = 988 + ((((uint16_t)(bh & SPEK_CHAN_MASK) << 8) + bl) SPEK_DATA_SHIFT); 361 | } 362 | spekFrameFlags = 0x00; 363 | #if defined(FAILSAFE) 364 | if(failsafeCnt > 20) failsafeCnt -= 20; else failsafeCnt = 0; // Valid frame, clear FailSafe counter 365 | #endif 366 | } else { //Start flag is on, but not enough bytes means there is an incomplete frame in buffer. This could be OK, if we happened to be called in the middle of a frame. Or not, if it has been a while since the start flag was set. 367 | uint32_t spekInterval = (timer0_overflow_count << 8) * (64 / clockCyclesPerMicrosecond()) - spekTimeLast; 368 | if (spekInterval > 2500) {spekFrameFlags = 0;} //If it has been a while, make the interrupt handler start over. 369 | } 370 | } 371 | } 372 | #endif 373 | 374 | 375 | uint16_t readRawRC(uint8_t chan) { 376 | uint16_t data; 377 | #if defined(SPEKTRUM) 378 | readSpektrum(); 379 | if (chan < RC_CHANS) { 380 | data = rcValue[rcChannel[chan]]; 381 | } else data = 1500; 382 | #else 383 | uint8_t oldSREG; 384 | oldSREG = SREG; cli(); // Let's disable interrupts 385 | data = rcValue[rcChannel[chan]]; // Let's copy the data Atomically 386 | SREG = oldSREG; // Let's restore interrupt state 387 | #endif 388 | return data; // We return the value correctly copied when the IRQ's where disabled 389 | } 390 | 391 | /**************************************************************************************/ 392 | /*************** compute and Filter the RX data ********************/ 393 | /**************************************************************************************/ 394 | void computeRC() { 395 | static uint16_t rcData4Values[RC_CHANS][4], rcDataMean[RC_CHANS]; 396 | static uint8_t rc4ValuesIndex = 0; 397 | uint8_t chan,a; 398 | #if !(defined(RCSERIAL) || defined(OPENLRSv2MULTI)) // dont know if this is right here 399 | #if defined(SBUS) 400 | readSBus(); 401 | #endif 402 | rc4ValuesIndex++; 403 | if (rc4ValuesIndex == 4) rc4ValuesIndex = 0; 404 | for (chan = 0; chan < RC_CHANS; chan++) { 405 | #if defined(FAILSAFE) 406 | uint16_t rcval = readRawRC(chan); 407 | if(rcval>FAILSAFE_DETECT_TRESHOLD || chan > 3) { // update controls channel only if pulse is above FAILSAFE_DETECT_TRESHOLD 408 | rcData4Values[chan][rc4ValuesIndex] = rcval; 409 | } 410 | #else 411 | rcData4Values[chan][rc4ValuesIndex] = readRawRC(chan); 412 | #endif 413 | rcDataMean[chan] = 0; 414 | for (a=0;a<4;a++) rcDataMean[chan] += rcData4Values[chan][a]; 415 | rcDataMean[chan]= (rcDataMean[chan]+2)>>2; 416 | if ( rcDataMean[chan] < (uint16_t)rcData[chan] -3) rcData[chan] = rcDataMean[chan]+2; 417 | if ( rcDataMean[chan] > (uint16_t)rcData[chan] +3) rcData[chan] = rcDataMean[chan]-2; 418 | } 419 | #endif 420 | } 421 | 422 | 423 | /**************************************************************************************/ 424 | /*************** OPENLRS ********************/ 425 | /**************************************************************************************/ 426 | 427 | //note: this dont feels right in RX.pde 428 | 429 | #if defined(OPENLRSv2MULTI) 430 | // ********************************************************** 431 | // ****************** OpenLRS Rx Code ******************* 432 | // *** OpenLRS Designed by Melih Karakelle on 2010-2012 *** 433 | // ** an Arudino based RC Rx/Tx system with extra futures ** 434 | // ** This Source code licensed under GPL ** 435 | // ********************************************************** 436 | // Version Number : 1.11 437 | // Latest Code Update : 2012-03-25 438 | // Supported Hardware : OpenLRS Rx boards (store.flytron.com) 439 | // Project Forum : http://forum.flytron.com/viewforum.php?f=7 440 | // Google Code Page : http://code.google.com/p/openlrs/ 441 | // ********************************************************** 442 | // # PROJECT DEVELOPERS # 443 | // Melih Karakelle (http://www.flytron.com) (forum nick name: Flytron) 444 | // Jan-Dirk Schuitemaker (http://www.schuitemaker.org/) (forum nick name: CrashingDutchman) 445 | // Etienne Saint-Paul (http://www.gameseed.fr) (forum nick name: Etienne) 446 | // 447 | 448 | //######### TRANSMISSION VARIABLES ########## 449 | #define CARRIER_FREQUENCY 435000 // 435Mhz startup frequency 450 | #define FREQUENCY_HOPPING 1 // 1 = Enabled 0 = Disabled 451 | 452 | //###### HOPPING CHANNELS ####### 453 | //Select the hopping channels between 0-255 454 | // Default values are 13,54 and 23 for all transmitters and receivers, you should change it before your first flight for safety. 455 | //Frequency = CARRIER_FREQUENCY + (StepSize(60khz)* Channel_Number) 456 | static uint8_t hop_list[3] = {13,54,23}; 457 | 458 | //###### RF DEVICE ID HEADERS ####### 459 | // Change this 4 byte values for isolating your transmission, RF module accepts only data with same header 460 | static uint8_t RF_Header[4] = {'O','L','R','S'}; 461 | 462 | //########## Variables ################# 463 | 464 | static uint32_t last_hopping_time; 465 | static uint8_t RF_Rx_Buffer[17]; 466 | static uint16_t temp_int; 467 | static uint16_t Servo_Buffer[10] = {3000,3000,3000,3000,3000,3000,3000,3000}; //servo position values from RF 468 | static uint8_t hopping_channel = 1; 469 | 470 | void initOpenLRS(void) { 471 | pinMode(GREEN_LED_pin, OUTPUT); 472 | pinMode(RED_LED_pin, OUTPUT); 473 | 474 | //RF module pins 475 | pinMode(SDO_pin, INPUT); //SDO 476 | pinMode(SDI_pin, OUTPUT); //SDI 477 | pinMode(SCLK_pin, OUTPUT); //SCLK 478 | pinMode(IRQ_pin, INPUT); //IRQ 479 | pinMode(nSel_pin, OUTPUT); //nSEL 480 | checkPots(); // OpenLRS Multi board hardware pot check; 481 | } 482 | 483 | void Config_OpenLRS() { 484 | RF22B_init_parameter(); // Configure the RFM22B's registers 485 | frequency_configurator(CARRIER_FREQUENCY); // Calibrate the RFM22B to this frequency, frequency hopping starts from here. 486 | to_rx_mode(); 487 | #if (FREQUENCY_HOPPING==1) 488 | Hopping(); //Hop to the next frequency 489 | #endif 490 | } 491 | 492 | //############ MAIN LOOP ############## 493 | void Read_OpenLRS_RC() { 494 | uint8_t i,tx_data_length; 495 | uint8_t first_data = 0; 496 | 497 | if (_spi_read(0x0C)==0) {RF22B_init_parameter(); to_rx_mode(); }// detect the locked module and reboot 498 | if ((currentTime-last_hopping_time > 25000)) {//automatic hopping for clear channel when rf link down for 25ms. 499 | Red_LED_ON; 500 | last_hopping_time = currentTime; 501 | #if (FREQUENCY_HOPPING==1) 502 | Hopping(); //Hop to the next frequency 503 | #endif 504 | } 505 | if(nIRQ_0) { // RFM22B INT pin Enabled by received Data 506 | Red_LED_ON; 507 | send_read_address(0x7f); // Send the package read command 508 | for(i = 0; i<17; i++) {//read all buffer 509 | RF_Rx_Buffer[i] = read_8bit_data(); 510 | } 511 | rx_reset(); 512 | if (RF_Rx_Buffer[0] == 'S') {// servo control data 513 | for(i = 0; i<8; i++) {//Write into the Servo Buffer 514 | temp_int = (256*RF_Rx_Buffer[1+(2*i)]) + RF_Rx_Buffer[2+(2*i)]; 515 | if ((temp_int>1500) && (temp_int<4500)) Servo_Buffer[i] = temp_int/2; 516 | } 517 | rcData[ROLL] = Servo_Buffer[0]; 518 | rcData[PITCH] = Servo_Buffer[1]; 519 | rcData[THROTTLE] = Servo_Buffer[2]; 520 | rcData[YAW] = Servo_Buffer[3]; 521 | rcData[AUX1] = Servo_Buffer[4]; 522 | rcData[AUX2] = Servo_Buffer[5]; 523 | rcData[AUX3] = Servo_Buffer[6]; 524 | rcData[AUX4] = Servo_Buffer[7]; 525 | } 526 | #if (FREQUENCY_HOPPING==1) 527 | Hopping(); //Hop to the next frequency 528 | #endif 529 | delay(1); 530 | last_hopping_time = currentTime; 531 | Red_LED_OFF; 532 | } 533 | Red_LED_OFF; 534 | } 535 | 536 | // ********************************************************** 537 | // ** RFM22B/Si4432 control functions for OpenLRS ** 538 | // ** This Source code licensed under GPL ** 539 | // ********************************************************** 540 | // Latest Code Update : 2011-09-26 541 | // Supported Hardware : OpenLRS Tx/Rx boards (store.flytron.com) 542 | // Project Forum : http://forum.flytron.com/viewforum.php?f=7 543 | // Google Code Page : http://code.google.com/p/openlrs/ 544 | // ********************************************************** 545 | 546 | //***************************************************************************** 547 | //***************************************************************************** 548 | 549 | //-------------------------------------------------------------- 550 | void Write0( void ) { 551 | SCK_off; 552 | NOP(); 553 | SDI_off; 554 | NOP(); 555 | SCK_on; 556 | NOP(); 557 | } 558 | //-------------------------------------------------------------- 559 | void Write1( void ) { 560 | SCK_off; 561 | NOP(); 562 | SDI_on; 563 | NOP(); 564 | SCK_on; 565 | NOP(); 566 | } 567 | //-------------------------------------------------------------- 568 | void Write8bitcommand(uint8_t command) { // keep sel to low 569 | uint8_t n=8; 570 | nSEL_on; 571 | SCK_off; 572 | nSEL_off; 573 | while(n--) { 574 | if(command&0x80) 575 | Write1(); 576 | else 577 | Write0(); 578 | command = command << 1; 579 | } 580 | SCK_off; 581 | } 582 | 583 | //-------------------------------------------------------------- 584 | uint8_t _spi_read(uint8_t address) { 585 | uint8_t result; 586 | send_read_address(address); 587 | result = read_8bit_data(); 588 | nSEL_on; 589 | return(result); 590 | } 591 | 592 | //-------------------------------------------------------------- 593 | void _spi_write(uint8_t address, uint8_t data) { 594 | address |= 0x80; 595 | Write8bitcommand(address); 596 | send_8bit_data(data); 597 | nSEL_on; 598 | } 599 | 600 | 601 | //-------Defaults 38.400 baud---------------------------------------------- 602 | void RF22B_init_parameter(void) { 603 | ItStatus1 = _spi_read(0x03); // read status, clear interrupt 604 | ItStatus2 = _spi_read(0x04); 605 | _spi_write(0x06, 0x00); // no wakeup up, lbd, 606 | _spi_write(0x07, RF22B_PWRSTATE_READY); // disable lbd, wakeup timer, use internal 32768,xton = 1; in ready mode 607 | _spi_write(0x09, 0x7f); // c = 12.5p 608 | _spi_write(0x0a, 0x05); 609 | _spi_write(0x0b, 0x12); // gpio0 TX State 610 | _spi_write(0x0c, 0x15); // gpio1 RX State 611 | _spi_write(0x0d, 0xfd); // gpio 2 micro-controller clk output 612 | _spi_write(0x0e, 0x00); // gpio 0, 1,2 NO OTHER FUNCTION. 613 | _spi_write(0x70, 0x00); // disable manchest 614 | 615 | // 57.6Kbps data rate 616 | _spi_write(0x1c, 0x05); // case RATE_57.6K 617 | _spi_write(0x20, 0x45);// 0x20 calculate from the datasheet= 500*(1+2*down3_bypass)/(2^ndec*RB*(1+enmanch)) 618 | _spi_write(0x21, 0x01); // 0x21 , rxosr[10--8] = 0; stalltr = (default), ccoff[19:16] = 0; 619 | _spi_write(0x22, 0xD7); // 0x22 ncoff =5033 = 0x13a9 620 | _spi_write(0x23, 0xDC); // 0x23 621 | _spi_write(0x24, 0x03); // 0x24 622 | _spi_write(0x25, 0xB8); // 0x25 623 | _spi_write(0x2a, 0x1e); 624 | 625 | _spi_write(0x6e, 0x0E); //case RATE_57.6K 626 | _spi_write(0x6f, 0xBF); //case RATE_57.6K 627 | 628 | _spi_write(0x30, 0x8c); // enable packet handler, msb first, enable crc, 629 | 630 | _spi_write(0x32, 0xf3); // 0x32address enable for headere byte 0, 1,2,3, receive header check for byte 0, 1,2,3 631 | _spi_write(0x33, 0x42); // header 3, 2, 1,0 used for head length, fixed packet length, synchronize word length 3, 2, 632 | _spi_write(0x34, 0x07); // 7 default value or // 64 nibble = 32byte preamble 633 | _spi_write(0x36, 0x2d); // synchronize word 634 | _spi_write(0x37, 0xd4); 635 | _spi_write(0x38, 0x00); 636 | _spi_write(0x39, 0x00); 637 | _spi_write(0x3a, RF_Header[0]); // tx header 638 | _spi_write(0x3b, RF_Header[1]); 639 | _spi_write(0x3c, RF_Header[2]); 640 | _spi_write(0x3d, RF_Header[3]); 641 | _spi_write(0x3e, 17); // total tx 17 byte 642 | 643 | //RX HEADER 644 | _spi_write(0x3f, RF_Header[0]); // check hearder 645 | _spi_write(0x40, RF_Header[1]); 646 | _spi_write(0x41, RF_Header[2]); 647 | _spi_write(0x42, RF_Header[3]); 648 | _spi_write(0x43, 0xff); // all the bit to be checked 649 | _spi_write(0x44, 0xff); // all the bit to be checked 650 | _spi_write(0x45, 0xff); // all the bit to be checked 651 | _spi_write(0x46, 0xff); // all the bit to be checked 652 | 653 | _spi_write(0x6d, 0x07); // 7 set power max power 654 | _spi_write(0x79, 0x00); // no hopping 655 | _spi_write(0x7a, 0x06); // 60khz step size (10khz x value) // no hopping 656 | 657 | _spi_write(0x71, 0x23); // Gfsk, fd[8] =0, no invert for Tx/Rx data, fifo mode, txclk -->gpio 658 | //_spi_write(0x72, 0x1F); // frequency deviation setting to 19.6khz (for 38.4kbps) 659 | _spi_write(0x72, 0x2E); // frequency deviation setting to 28.8khz(for 57.6kbps) 660 | _spi_write(0x73, 0x00); 661 | _spi_write(0x74, 0x00); // no offset 662 | 663 | //band 435.000 664 | _spi_write(0x75, 0x53); 665 | _spi_write(0x76, 0x7D); 666 | _spi_write(0x77, 0x00); 667 | } 668 | 669 | //-------------------------------------------------------------- 670 | void send_read_address(uint8_t i) { 671 | i &= 0x7f; 672 | Write8bitcommand(i); 673 | } 674 | //-------------------------------------------------------------- 675 | void send_8bit_data(uint8_t i) { 676 | uint8_t n = 8; 677 | SCK_off; 678 | while(n--) { 679 | if(i&0x80) 680 | Write1(); 681 | else 682 | Write0(); 683 | i = i << 1; 684 | } 685 | SCK_off; 686 | } 687 | //-------------------------------------------------------------- 688 | 689 | uint8_t read_8bit_data(void) { 690 | uint8_t Result, i; 691 | 692 | SCK_off; 693 | Result=0; 694 | for(i=0;i<8;i++) { //read fifo data byte 695 | Result=Result<<1; 696 | SCK_on; 697 | NOP(); 698 | if(SDO_1) { 699 | Result|=1; 700 | } 701 | SCK_off; 702 | NOP(); 703 | } 704 | return(Result); 705 | } 706 | //-------------------------------------------------------------- 707 | 708 | //----------------------------------------------------------------------- 709 | void rx_reset(void) { 710 | _spi_write(0x07, RF22B_PWRSTATE_READY); 711 | _spi_write(0x7e, 36); // threshold for rx almost full, interrupt when 1 byte received 712 | _spi_write(0x08, 0x03); //clear fifo disable multi packet 713 | _spi_write(0x08, 0x00); // clear fifo, disable multi packet 714 | _spi_write(0x07,RF22B_PWRSTATE_RX ); // to rx mode 715 | _spi_write(0x05, RF22B_Rx_packet_received_interrupt); 716 | ItStatus1 = _spi_read(0x03); //read the Interrupt Status1 register 717 | ItStatus2 = _spi_read(0x04); 718 | } 719 | //----------------------------------------------------------------------- 720 | 721 | void to_rx_mode(void) { 722 | to_ready_mode(); 723 | delay(50); 724 | rx_reset(); 725 | NOP(); 726 | } 727 | 728 | 729 | //-------------------------------------------------------------- 730 | void to_ready_mode(void) { 731 | ItStatus1 = _spi_read(0x03); 732 | ItStatus2 = _spi_read(0x04); 733 | _spi_write(0x07, RF22B_PWRSTATE_READY); 734 | } 735 | //-------------------------------------------------------------- 736 | void to_sleep_mode(void) { 737 | // TXEN = RXEN = 0; 738 | //LED_RED = 0; 739 | _spi_write(0x07, RF22B_PWRSTATE_READY); 740 | 741 | ItStatus1 = _spi_read(0x03); //read the Interrupt Status1 register 742 | ItStatus2 = _spi_read(0x04); 743 | _spi_write(0x07, RF22B_PWRSTATE_POWERDOWN); 744 | } 745 | //-------------------------------------------------------------- 746 | 747 | void frequency_configurator(uint32_t frequency) { 748 | // frequency formulation from Si4432 chip's datasheet 749 | // original formulation is working with mHz values and floating numbers, I replaced them with kHz values. 750 | frequency = frequency / 10; 751 | frequency = frequency - 24000; 752 | frequency = frequency - 19000; // 19 for 430-439.9 MHz band from datasheet 753 | frequency = frequency * 64; // this is the Nominal Carrier Frequency (fc) value for register setting 754 | 755 | uint8_t byte0 = (uint8_t) frequency; 756 | uint8_t byte1 = (uint8_t) (frequency >> 8); 757 | 758 | _spi_write(0x76, byte1); 759 | _spi_write(0x77, byte0); 760 | } 761 | 762 | //############# FREQUENCY HOPPING FUNCTIONS ################# 763 | #if (FREQUENCY_HOPPING==1) 764 | void Hopping(void) { 765 | hopping_channel++; 766 | if (hopping_channel>2) hopping_channel = 0; 767 | _spi_write(0x79, hop_list[hopping_channel]); 768 | } 769 | #endif 770 | 771 | void checkPots() { 772 | ////Flytron OpenLRS Multi Pots 773 | pot_P = analogRead(7); 774 | pot_I = analogRead(6); 775 | 776 | pot_P = pot_P - 512; 777 | pot_I = pot_I - 512; 778 | 779 | pot_P = pot_P / 25; //+-20 780 | pot_I = pot_I / 25; //+-20 781 | } 782 | #endif 783 | 784 | #if defined(SPEK_BIND) // Bind Support 785 | void spekBind() { 786 | pinMode(SPEK_BIND_DATA, INPUT); // Data line from sat 787 | digitalWrite(SPEK_BIND_DATA,LOW); // Turn off internal Pull Up resistor 788 | 789 | pinMode(SPEK_BIND_GROUND, INPUT); 790 | digitalWrite(SPEK_BIND_GROUND,LOW); 791 | pinMode(SPEK_BIND_GROUND, OUTPUT); 792 | digitalWrite(SPEK_BIND_GROUND,LOW); 793 | 794 | pinMode(SPEK_BIND_POWER, INPUT); 795 | digitalWrite(SPEK_BIND_POWER,LOW); 796 | pinMode(SPEK_BIND_POWER,OUTPUT); 797 | 798 | while(1) { //Do not return. User presses reset button to return to normal. 799 | blinkLED(4,255,1); 800 | digitalWrite(SPEK_BIND_POWER,LOW); // Power off sat 801 | pinMode(SPEK_BIND_DATA, OUTPUT); 802 | digitalWrite(SPEK_BIND_DATA,LOW); 803 | delay(1000); 804 | blinkLED(4,255,1); 805 | 806 | digitalWrite(SPEK_BIND_POWER,HIGH); // Power on sat 807 | delay(10); 808 | digitalWrite(SPEK_BIND_DATA,HIGH); 809 | delay(60); // Keep data pin steady for 20 to 120ms after power up 810 | 811 | noInterrupts(); 812 | for (byte i = 0; i < SPEK_BIND_PULSES; i++) { 813 | digitalWrite(SPEK_BIND_DATA,LOW); 814 | delayMicroseconds(118); 815 | digitalWrite(SPEK_BIND_DATA,HIGH); 816 | delayMicroseconds(122); 817 | } 818 | interrupts(); 819 | delay(60000); //Allow one full minute to bind, then try again. 820 | } 821 | } 822 | #endif 823 | 824 | 825 | 826 | -------------------------------------------------------------------------------- /Serial.ino: -------------------------------------------------------------------------------- 1 | #if defined(MEGA) 2 | #define UART_NUMBER 4 3 | #elif defined(PROMICRO) 4 | #define UART_NUMBER 2 5 | #else 6 | #define UART_NUMBER 1 7 | #endif 8 | #if defined(GPS_SERIAL) 9 | #define RX_BUFFER_SIZE 256 // 256 RX buffer is needed for GPS communication (64 or 128 was too short) 10 | #else 11 | #define RX_BUFFER_SIZE 256 12 | #endif 13 | #define TX_BUFFER_SIZE 128 14 | #define INBUF_SIZE 64 15 | 16 | static volatile uint8_t serialHeadRX[UART_NUMBER],serialTailRX[UART_NUMBER]; 17 | static uint8_t serialBufferRX[RX_BUFFER_SIZE][UART_NUMBER]; 18 | static volatile uint8_t serialHeadTX[UART_NUMBER],serialTailTX[UART_NUMBER]; 19 | static uint8_t serialBufferTX[TX_BUFFER_SIZE][UART_NUMBER]; 20 | static uint8_t inBuf[INBUF_SIZE][UART_NUMBER]; 21 | 22 | #define BIND_CAPABLE 0; //Used for Spektrum today; can be used in the future for any RX type that needs a bind and has a MultiWii module. 23 | #if defined(SPEK_BIND) 24 | #define BIND_CAPABLE 1; 25 | #endif 26 | // Capability is bit flags; next defines should be 2, 4, 8... 27 | 28 | const uint32_t PROGMEM capability = 0+BIND_CAPABLE; 29 | 30 | #ifdef DEBUGMSG 31 | #define DEBUG_MSG_BUFFER_SIZE 128 32 | static char debug_buf[DEBUG_MSG_BUFFER_SIZE]; 33 | static uint8_t head_debug; 34 | static uint8_t tail_debug; 35 | #endif 36 | 37 | // Multiwii Serial Protocol 0 38 | #define MSP_VERSION 0 39 | 40 | //to multiwii developpers/committers : do not add new MSP messages without a proper argumentation/agreement on the forum 41 | #define MSP_IDENT 100 //out message multitype + multiwii version + protocol version + capability variable 42 | #define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number 43 | #define MSP_RAW_IMU 102 //out message 9 DOF 44 | #define MSP_SERVO 103 //out message 8 servos 45 | #define MSP_MOTOR 104 //out message 8 motors 46 | #define MSP_RC 105 //out message 8 rc chan and more 47 | #define MSP_RAW_GPS 106 //out message fix, numsat, lat, lon, alt, speed, ground course 48 | #define MSP_COMP_GPS 107 //out message distance home, direction home 49 | #define MSP_ATTITUDE 108 //out message 2 angles 1 heading 50 | #define MSP_ALTITUDE 109 //out message altitude, variometer 51 | #define MSP_ANALOG 110 //out message vbat, powermetersum, rssi if available on RX 52 | #define MSP_RC_TUNING 111 //out message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID 53 | #define MSP_PID 112 //out message P I D coeff (9 are used currently) 54 | #define MSP_BOX 113 //out message BOX setup (number is dependant of your setup) 55 | #define MSP_MISC 114 //out message powermeter trig 56 | #define MSP_MOTOR_PINS 115 //out message which pins are in use for motors & servos, for GUI 57 | #define MSP_BOXNAMES 116 //out message the aux switch names 58 | #define MSP_PIDNAMES 117 //out message the PID names 59 | #define MSP_WP 118 //out message get a WP, WP# is in the payload, returns (WP#, lat, lon, alt, flags) WP#0-home, WP#16-poshold 60 | #define MSP_BOXIDS 119 //out message get the permanent IDs associated to BOXes 61 | 62 | #if defined(HEX_NANO) 63 | #define MSP_SET_RAW_RC_TINY 150 //in message 4 rc chan 64 | #define MSP_ARM 151 65 | #define MSP_DISARM 152 66 | #define MSP_TRIM_UP 153 67 | #define MSP_TRIM_DOWN 154 68 | #define MSP_TRIM_LEFT 155 69 | #define MSP_TRIM_RIGHT 156 70 | #define MSP_TRIM_UP_FAST 157 71 | #define MSP_TRIM_DOWN_FAST 158 72 | #define MSP_TRIM_LEFT_FAST 159 73 | #define MSP_TRIM_RIGHT_FAST 160 74 | 75 | #define MSP_READ_TEST_PARAM 189 76 | #define MSP_SET_TEST_PARAM 190 77 | 78 | #define MSP_READ_TEST_PARAM 189 79 | #define MSP_HEX_NANO 199 80 | #endif 81 | 82 | #define MSP_SET_RAW_RC 200 //in message 8 rc chan 83 | #define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed 84 | #define MSP_SET_PID 202 //in message P I D coeff (9 are used currently) 85 | #define MSP_SET_BOX 203 //in message BOX setup (number is dependant of your setup) 86 | #define MSP_SET_RC_TUNING 204 //in message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID 87 | #define MSP_ACC_CALIBRATION 205 //in message no param 88 | #define MSP_MAG_CALIBRATION 206 //in message no param 89 | #define MSP_SET_MISC 207 //in message powermeter trig + 8 free for future use 90 | #define MSP_RESET_CONF 208 //in message no param 91 | #define MSP_SET_WP 209 //in message sets a given WP (WP#,lat, lon, alt, flags) 92 | #define MSP_SELECT_SETTING 210 //in message Select Setting Number (0-2) 93 | #define MSP_SET_HEAD 211 //in message define a new heading hold direction 94 | 95 | #define MSP_BIND 240 //in message no param 96 | 97 | #define MSP_EEPROM_WRITE 250 //in message no param 98 | 99 | #define MSP_DEBUGMSG 253 //out message debug string buffer 100 | #define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4 101 | 102 | static uint8_t checksum[UART_NUMBER]; 103 | static uint8_t indRX[UART_NUMBER]; 104 | static uint8_t cmdMSP[UART_NUMBER]; 105 | 106 | #if defined(PROMINI) 107 | #define CURRENTPORT 0 108 | #else 109 | static uint8_t CURRENTPORT=0; 110 | #endif 111 | 112 | uint32_t read32() { 113 | uint32_t t = read16(); 114 | t+= (uint32_t)read16()<<16; 115 | return t; 116 | } 117 | uint16_t read16() { 118 | uint16_t t = read8(); 119 | t+= (uint16_t)read8()<<8; 120 | return t; 121 | } 122 | uint8_t read8() { 123 | return inBuf[indRX[CURRENTPORT]++][CURRENTPORT]&0xff; 124 | } 125 | 126 | void headSerialResponse(uint8_t err, uint8_t s) { 127 | serialize8('$'); 128 | serialize8('M'); 129 | serialize8(err ? '!' : '>'); 130 | checksum[CURRENTPORT] = 0; // start calculating a new checksum 131 | serialize8(s); 132 | serialize8(cmdMSP[CURRENTPORT]); 133 | } 134 | 135 | void headSerialReply(uint8_t s) { 136 | headSerialResponse(0, s); 137 | } 138 | 139 | void inline headSerialError(uint8_t s) { 140 | headSerialResponse(1, s); 141 | } 142 | 143 | void tailSerialReply() { 144 | serialize8(checksum[CURRENTPORT]);UartSendData(); 145 | } 146 | 147 | void serializeNames(PGM_P s) { 148 | for (PGM_P c = s; pgm_read_byte(c); c++) { 149 | serialize8(pgm_read_byte(c)); 150 | } 151 | } 152 | 153 | void serialCom() { 154 | uint8_t c,n; 155 | static uint8_t offset[UART_NUMBER]; 156 | static uint8_t dataSize[UART_NUMBER]; 157 | static enum _serial_state { 158 | IDLE, 159 | HEADER_START, 160 | HEADER_M, 161 | HEADER_ARROW, 162 | HEADER_SIZE, 163 | HEADER_CMD, 164 | } c_state[UART_NUMBER];// = IDLE; 165 | 166 | for(n=0;n 1) 180 | #define SPEK_COND && (SPEK_SERIAL_PORT != CURRENTPORT) 181 | #endif 182 | while (SerialAvailable(CURRENTPORT) GPS_COND SPEK_COND) { 183 | uint8_t bytesTXBuff = ((uint8_t)(serialHeadTX[CURRENTPORT]-serialTailTX[CURRENTPORT]))%TX_BUFFER_SIZE; // indicates the number of occupied bytes in TX buffer 184 | if (bytesTXBuff > TX_BUFFER_SIZE - 50 ) return; // ensure there is enough free TX buffer to go further (50 bytes margin) 185 | c = SerialRead(CURRENTPORT); 186 | #ifdef SUPPRESS_ALL_SERIAL_MSP 187 | // no MSP handling, so go directly 188 | evaluateOtherData(c); 189 | #else 190 | // regular data handling to detect and handle MSP and other data 191 | if (c_state[CURRENTPORT] == IDLE) { 192 | c_state[CURRENTPORT] = (c=='$') ? HEADER_START : IDLE; 193 | if (c_state[CURRENTPORT] == IDLE) evaluateOtherData(c); // evaluate all other incoming serial data 194 | } else if (c_state[CURRENTPORT] == HEADER_START) { 195 | c_state[CURRENTPORT] = (c=='M') ? HEADER_M : IDLE; 196 | } else if (c_state[CURRENTPORT] == HEADER_M) { 197 | c_state[CURRENTPORT] = (c=='<') ? HEADER_ARROW : IDLE; 198 | } else if (c_state[CURRENTPORT] == HEADER_ARROW) { 199 | if (c > INBUF_SIZE) { // now we are expecting the payload size 200 | c_state[CURRENTPORT] = IDLE; 201 | continue; 202 | } 203 | dataSize[CURRENTPORT] = c; 204 | offset[CURRENTPORT] = 0; 205 | checksum[CURRENTPORT] = 0; 206 | indRX[CURRENTPORT] = 0; 207 | checksum[CURRENTPORT] ^= c; 208 | c_state[CURRENTPORT] = HEADER_SIZE; // the command is to follow 209 | } else if (c_state[CURRENTPORT] == HEADER_SIZE) { 210 | cmdMSP[CURRENTPORT] = c; 211 | checksum[CURRENTPORT] ^= c; 212 | c_state[CURRENTPORT] = HEADER_CMD; 213 | } else if (c_state[CURRENTPORT] == HEADER_CMD && offset[CURRENTPORT] < dataSize[CURRENTPORT]) { 214 | checksum[CURRENTPORT] ^= c; 215 | inBuf[offset[CURRENTPORT]++][CURRENTPORT] = c; 216 | } else if (c_state[CURRENTPORT] == HEADER_CMD && offset[CURRENTPORT] >= dataSize[CURRENTPORT]) { 217 | if (checksum[CURRENTPORT] == c) { // compare calculated and transferred checksum 218 | evaluateCommand(); // we got a valid packet, evaluate it 219 | } 220 | c_state[CURRENTPORT] = IDLE; 221 | } 222 | #endif // SUPPRESS_ALL_SERIAL_MSP 223 | } 224 | } 225 | } 226 | #ifndef SUPPRESS_ALL_SERIAL_MSP 227 | void evaluateCommand() { 228 | #if defined(HEX_NANO) 229 | unsigned char auxChannels; 230 | unsigned char aux; 231 | #endif 232 | 233 | switch(cmdMSP[CURRENTPORT]) { 234 | case MSP_SET_RAW_RC: 235 | for(uint8_t i=0;i<8;i++) { 236 | rcData[i] = read16(); 237 | } 238 | 239 | failsafeCnt = 0; 240 | 241 | headSerialReply(0); 242 | break; 243 | 244 | #if defined(HEX_NANO) 245 | case MSP_READ_TEST_PARAM: 246 | headSerialReply(12); 247 | 248 | blinkLED(15,20,1); 249 | 250 | paramList[0] = alpha * 250.f; 251 | paramList[1] = conf.P8[PIDALT] * 250.f / 200; 252 | paramList[2] = conf.I8[PIDALT]; 253 | paramList[3] = conf.D8[PIDALT] * 250.f / 100; 254 | 255 | for(int idx = 0; idx < 12; idx++){ 256 | serialize8(paramList[idx]); 257 | } 258 | 259 | break; 260 | case MSP_SET_TEST_PARAM: 261 | for(int idx = 0; idx < 12; idx++){ 262 | paramList[idx] = read8(); 263 | } 264 | 265 | blinkLED(15,20,1); 266 | 267 | alpha = paramList[0] / 250.f; 268 | conf.P8[PIDALT] = paramList[1] / 250.f * 200; //0~200 269 | conf.I8[PIDALT] = paramList[2]; //0~250 270 | conf.D8[PIDALT] = paramList[3] / 250.f * 100; //0~100 271 | writeParams(0); 272 | return; 273 | break; 274 | case MSP_SET_RAW_RC_TINY: 275 | for(uint8_t i = 0;i < 4;i++) { 276 | serialRcValue[i] = 1000 + read8() * 4; 277 | } 278 | 279 | auxChannels = read8(); 280 | 281 | aux = (auxChannels & 0xc0) >> 6; 282 | 283 | if(aux == 0){ 284 | serialRcValue[4] = 1000; 285 | } 286 | else if(aux == 1){ 287 | serialRcValue[4] = 1500; 288 | } 289 | else{ 290 | serialRcValue[4] = 2000; 291 | } 292 | 293 | 294 | aux = (auxChannels & 0x30) >> 4; 295 | 296 | if(aux == 0){ 297 | serialRcValue[5] = 1000; 298 | } 299 | else if(aux == 1){ 300 | serialRcValue[5] = 1500; 301 | } 302 | else{ 303 | serialRcValue[5] = 2000; 304 | } 305 | 306 | 307 | aux = (auxChannels & 0x0c) >> 2; 308 | 309 | if(aux == 0){ 310 | serialRcValue[6] = 1000; 311 | } 312 | else if(aux == 1){ 313 | serialRcValue[6] = 1500; 314 | } 315 | else{ 316 | serialRcValue[6] = 2000; 317 | } 318 | 319 | aux = (auxChannels & 0x03); 320 | 321 | if(aux == 0){ 322 | serialRcValue[7] = 1000; 323 | } 324 | else if(aux == 1){ 325 | serialRcValue[7] = 1500; 326 | } 327 | else{ 328 | serialRcValue[7] = 2000; 329 | } 330 | 331 | failsafeCnt = 0; 332 | 333 | 334 | return; 335 | 336 | /* 337 | headSerialReply(8); 338 | 339 | for(uint8_t i = 0; i < 4; i++) { 340 | serialize16(serialRcValue[i]); 341 | }*/ 342 | break; 343 | case MSP_ARM: 344 | go_arm(); 345 | break; 346 | case MSP_DISARM: 347 | go_disarm(); 348 | break; 349 | case MSP_TRIM_UP: 350 | if(conf.angleTrim[PITCH] < 120){ 351 | conf.angleTrim[PITCH]+=1; 352 | writeParams(1); 353 | #if defined(LED_RING) 354 | blinkLedRing(); 355 | #endif 356 | } 357 | break; 358 | case MSP_TRIM_DOWN: 359 | if(conf.angleTrim[PITCH] > -120){ 360 | conf.angleTrim[PITCH]-=1; 361 | writeParams(1); 362 | #if defined(LED_RING) 363 | blinkLedRing(); 364 | #endif 365 | } 366 | break; 367 | case MSP_TRIM_LEFT: 368 | if(conf.angleTrim[ROLL] > -120){ 369 | conf.angleTrim[ROLL]-=1; 370 | writeParams(1); 371 | #if defined(LED_RING) 372 | blinkLedRing(); 373 | #endif 374 | } 375 | break; 376 | case MSP_TRIM_RIGHT: 377 | if(conf.angleTrim[ROLL] < 120){ 378 | conf.angleTrim[ROLL]+=1; 379 | writeParams(1); 380 | #if defined(LED_RING) 381 | blinkLedRing(); 382 | #endif 383 | } 384 | break; 385 | case MSP_TRIM_UP_FAST: 386 | if(conf.angleTrim[PITCH] < 120){ 387 | conf.angleTrim[PITCH]+=10; 388 | writeParams(1); 389 | #if defined(LED_RING) 390 | blinkLedRing(); 391 | #endif 392 | } 393 | break; 394 | case MSP_TRIM_DOWN_FAST: 395 | if(conf.angleTrim[PITCH] > -120){ 396 | conf.angleTrim[PITCH]-=10; 397 | writeParams(1); 398 | #if defined(LED_RING) 399 | blinkLedRing(); 400 | #endif 401 | } 402 | break; 403 | case MSP_TRIM_LEFT_FAST: 404 | if(conf.angleTrim[ROLL] > -120){ 405 | conf.angleTrim[ROLL]-=10; 406 | writeParams(1); 407 | #if defined(LED_RING) 408 | blinkLedRing(); 409 | #endif 410 | } 411 | break; 412 | case MSP_TRIM_RIGHT_FAST: 413 | if(conf.angleTrim[ROLL] < 120){ 414 | conf.angleTrim[ROLL]+=10; 415 | writeParams(1); 416 | #if defined(LED_RING) 417 | blinkLedRing(); 418 | #endif 419 | } 420 | break; 421 | case MSP_HEX_NANO: 422 | headSerialReply(14); 423 | serialize8(flightState); 424 | serialize16(absolutedAccZ); 425 | //serialize32(EstAlt); 426 | serialize16((int16_t)EstAlt); 427 | for(uint8_t i=0;i<2;i++) serialize16(angle[i]); 428 | serialize16((int16_t)AltHold); 429 | serialize8(vbat); 430 | serialize8((int8_t)(conf.angleTrim[PITCH])); 431 | serialize8((int8_t)(conf.angleTrim[ROLL])); 432 | break; 433 | #endif 434 | #if GPS 435 | case MSP_SET_RAW_GPS: 436 | f.GPS_FIX = read8(); 437 | GPS_numSat = read8(); 438 | GPS_coord[LAT] = read32(); 439 | GPS_coord[LON] = read32(); 440 | GPS_altitude = read16(); 441 | GPS_speed = read16(); 442 | GPS_update |= 2; // New data signalisation to GPS functions 443 | headSerialReply(0); 444 | break; 445 | #endif 446 | case MSP_SET_PID: 447 | for(uint8_t i=0;i2) global_conf.currentSet = 0; 481 | writeGlobalSet(0); 482 | readEEPROM(); 483 | } 484 | headSerialReply(0); 485 | break; 486 | #endif 487 | case MSP_SET_HEAD: 488 | magHold = read16(); 489 | headSerialReply(0); 490 | break; 491 | case MSP_IDENT: 492 | headSerialReply(7); 493 | serialize8(VERSION); // multiwii version 494 | serialize8(MULTITYPE); // type of multicopter 495 | serialize8(MSP_VERSION); // MultiWii Serial Protocol Version 496 | serialize32(pgm_read_dword(&(capability))); // "capability" 497 | break; 498 | case MSP_STATUS: 499 | headSerialReply(11); 500 | serialize16(cycleTime); 501 | serialize16(i2c_errors_count); 502 | serialize16(ACC|BARO<<1|MAG<<2|GPS<<3|SONAR<<4); 503 | serialize32( 504 | #if ACC 505 | f.ANGLE_MODE< 16) size = 16; 742 | headSerialReply(size); 743 | debugmsg_serialize(size); 744 | } 745 | break; 746 | #endif 747 | default: // we do not know how to handle the (valid) message, indicate error MSP $M! 748 | headSerialError(0); 749 | break; 750 | } 751 | tailSerialReply(); 752 | } 753 | #endif // SUPPRESS_ALL_SERIAL_MSP 754 | 755 | // evaluate all other incoming serial data 756 | void evaluateOtherData(uint8_t sr) { 757 | #ifndef SUPPRESS_OTHER_SERIAL_COMMANDS 758 | switch (sr) { 759 | // Note: we may receive weird characters here which could trigger unwanted features during flight. 760 | // this could lead to a crash easily. 761 | // Please use if (!f.ARMED) where neccessary 762 | #ifdef LCD_CONF 763 | case 's': 764 | case 'S': 765 | if (!f.ARMED) configurationLoop(); 766 | break; 767 | #endif 768 | #ifdef LOG_PERMANENT_SHOW_AT_L 769 | case 'L': 770 | if (!f.ARMED) dumpPLog(1); 771 | break; 772 | #endif 773 | #if defined(LCD_TELEMETRY) && defined(LCD_TEXTSTAR) 774 | case 'A': // button A press 775 | toggle_telemetry(1); 776 | break; 777 | case 'B': // button B press 778 | toggle_telemetry(2); 779 | break; 780 | case 'C': // button C press 781 | toggle_telemetry(3); 782 | break; 783 | case 'D': // button D press 784 | toggle_telemetry(4); 785 | break; 786 | case 'a': // button A release 787 | case 'b': // button B release 788 | case 'c': // button C release 789 | case 'd': // button D release 790 | break; 791 | #endif 792 | #ifdef LCD_TELEMETRY 793 | case '0': 794 | case '1': 795 | case '2': 796 | case '3': 797 | case '4': 798 | case '5': 799 | case '6': 800 | case '7': 801 | case '8': 802 | case '9': 803 | #if defined(LOG_VALUES) || defined(DEBUG) 804 | case 'R': 805 | #endif 806 | #ifdef DEBUG 807 | case 'F': 808 | #endif 809 | toggle_telemetry(sr); 810 | break; 811 | #endif // LCD_TELEMETRY 812 | } 813 | #endif // SUPPRESS_OTHER_SERIAL_COMMANDS 814 | } 815 | 816 | // ******************************************************* 817 | // For Teensy 2.0, these function emulate the API used for ProMicro 818 | // it cant have the same name as in the arduino API because it wont compile for the promini (eaven if it will be not compiled) 819 | // ******************************************************* 820 | #if defined(TEENSY20) 821 | unsigned char T_USB_Available(){ 822 | int n = Serial.available(); 823 | if (n > 255) n = 255; 824 | return n; 825 | } 826 | #endif 827 | 828 | // ******************************************************* 829 | // Interrupt driven UART transmitter - using a ring buffer 830 | // ******************************************************* 831 | 832 | void serialize32(uint32_t a) { 833 | serialize8((a ) & 0xFF); 834 | serialize8((a>> 8) & 0xFF); 835 | serialize8((a>>16) & 0xFF); 836 | serialize8((a>>24) & 0xFF); 837 | } 838 | 839 | void serialize16(int16_t a) { 840 | serialize8((a ) & 0xFF); 841 | serialize8((a>>8) & 0xFF); 842 | } 843 | 844 | void serialize8(uint8_t a) { 845 | uint8_t t = serialHeadTX[CURRENTPORT]; 846 | if (++t >= TX_BUFFER_SIZE) t = 0; 847 | serialBufferTX[t][CURRENTPORT] = a; 848 | checksum[CURRENTPORT] ^= a; 849 | serialHeadTX[CURRENTPORT] = t; 850 | } 851 | 852 | #if defined(PROMINI) || defined(MEGA) 853 | #if defined(PROMINI) 854 | ISR(USART_UDRE_vect) { // Serial 0 on a PROMINI 855 | #endif 856 | #if defined(MEGA) 857 | ISR(USART0_UDRE_vect) { // Serial 0 on a MEGA 858 | #endif 859 | uint8_t t = serialTailTX[0]; 860 | if (serialHeadTX[0] != t) { 861 | if (++t >= TX_BUFFER_SIZE) t = 0; 862 | UDR0 = serialBufferTX[t][0]; // Transmit next byte in the ring 863 | serialTailTX[0] = t; 864 | } 865 | if (t == serialHeadTX[0]) UCSR0B &= ~(1<= TX_BUFFER_SIZE) t = 0; 873 | UDR1 = serialBufferTX[t][1]; // Transmit next byte in the ring 874 | serialTailTX[1] = t; 875 | } 876 | if (t == serialHeadTX[1]) UCSR1B &= ~(1<= TX_BUFFER_SIZE) t = 0; 884 | UDR2 = serialBufferTX[t][2]; 885 | serialTailTX[2] = t; 886 | } 887 | if (t == serialHeadTX[2]) UCSR2B &= ~(1<= TX_BUFFER_SIZE) t = 0; 893 | UDR3 = serialBufferTX[t][3]; 894 | serialTailTX[3] = t; 895 | } 896 | if (t == serialHeadTX[3]) UCSR3B &= ~(1<= TX_BUFFER_SIZE) serialTailTX[0] = 0; 909 | #if !defined(TEENSY20) 910 | USB_Send(USB_CDC_TX,serialBufferTX[serialTailTX[0]],1); 911 | #else 912 | Serial.write(serialBufferTX[serialTailTX[0]],1); 913 | #endif 914 | } 915 | break; 916 | case 1: UCSR1B |= (1<> 8; 937 | uint8_t l = ((F_CPU / 4 / baud -1) / 2); 938 | switch (port) { 939 | #if defined(PROMINI) 940 | case 0: UCSR0A = (1<= 100) && !defined(TEENSY20) 944 | case 0: UDIEN &= ~(1< 5000) { //Potential start of a Spektrum frame, they arrive every 11 or every 22 ms. Mark it, and clear the buffer. 983 | serialTailRX[portnum] = 0; 984 | serialHeadRX[portnum] = 0; 985 | spekFrameFlags = 0x01; 986 | } 987 | cli(); 988 | } 989 | } 990 | #endif 991 | 992 | uint8_t h = serialHeadRX[portnum]; 993 | if (++h >= RX_BUFFER_SIZE) h = 0; 994 | if (h == serialTailRX[portnum]) return; // we did not bite our own tail? 995 | serialBufferRX[serialHeadRX[portnum]][portnum] = data; 996 | serialHeadRX[portnum] = h; 997 | } 998 | 999 | #if defined(PROMINI) 1000 | ISR(USART_RX_vect) { store_uart_in_buf(UDR0, 0); } 1001 | #endif 1002 | #if defined(PROMICRO) 1003 | ISR(USART1_RX_vect) { store_uart_in_buf(UDR1, 1); } 1004 | #endif 1005 | #if defined(MEGA) 1006 | ISR(USART0_RX_vect) { store_uart_in_buf(UDR0, 0); } 1007 | ISR(USART1_RX_vect) { store_uart_in_buf(UDR1, 1); } 1008 | ISR(USART2_RX_vect) { store_uart_in_buf(UDR2, 2); } 1009 | ISR(USART3_RX_vect) { store_uart_in_buf(UDR3, 3); } 1010 | #endif 1011 | 1012 | uint8_t SerialRead(uint8_t port) { 1013 | #if defined(PROMICRO) 1014 | #if defined(TEENSY20) 1015 | if(port == 0) return Serial.read(); 1016 | #else 1017 | #if (ARDUINO >= 100) 1018 | if(port == 0) USB_Flush(USB_CDC_TX); 1019 | #endif 1020 | if(port == 0) return USB_Recv(USB_CDC_RX); 1021 | #endif 1022 | #endif 1023 | uint8_t t = serialTailRX[port]; 1024 | uint8_t c = serialBufferRX[t][port]; 1025 | if (serialHeadRX[port] != t) { 1026 | if (++t >= RX_BUFFER_SIZE) t = 0; 1027 | serialTailRX[port] = t; 1028 | } 1029 | return c; 1030 | } 1031 | 1032 | #if defined(SPEKTRUM) 1033 | uint8_t SerialPeek(uint8_t port) { 1034 | uint8_t c = serialBufferRX[serialTailRX[port]][port]; 1035 | if ((serialHeadRX[port] != serialTailRX[port])) return c; else return 0; 1036 | } 1037 | #endif 1038 | 1039 | uint8_t SerialAvailable(uint8_t port) { 1040 | #if defined(PROMICRO) 1041 | #if !defined(TEENSY20) 1042 | if(port == 0) return USB_Available(USB_CDC_RX); 1043 | #else 1044 | if(port == 0) return T_USB_Available(); 1045 | #endif 1046 | #endif 1047 | return (serialHeadRX[port] - serialTailRX[port])%RX_BUFFER_SIZE; 1048 | } 1049 | 1050 | void SerialWrite(uint8_t port,uint8_t c){ 1051 | #if !defined(PROMINI) 1052 | CURRENTPORT=port; 1053 | #endif 1054 | serialize8(c);UartSendData(); 1055 | } 1056 | 1057 | #ifdef DEBUGMSG 1058 | void debugmsg_append_str(const char *str) { 1059 | while(*str) { 1060 | debug_buf[head_debug++] = *str++; 1061 | if (head_debug == DEBUG_MSG_BUFFER_SIZE) { 1062 | head_debug = 0; 1063 | } 1064 | } 1065 | } 1066 | 1067 | static uint8_t debugmsg_available() { 1068 | if (head_debug >= tail_debug) { 1069 | return head_debug-tail_debug; 1070 | } else { 1071 | return head_debug + (DEBUG_MSG_BUFFER_SIZE-tail_debug); 1072 | } 1073 | } 1074 | 1075 | static void debugmsg_serialize(uint8_t l) { 1076 | for (uint8_t i=0; i 9 | */ 10 | 11 | #include 12 | 13 | #include "config.h" 14 | #include "def.h" 15 | 16 | 17 | #include 18 | #define VERSION 220 19 | 20 | #if defined(HEX_NANO) 21 | volatile uint16_t serialRcValue[RC_CHANS] = {1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502}; 22 | float alpha = 0.95; 23 | uint8_t paramList[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; 24 | int16_t absolutedAccZ = 0; 25 | uint8_t flightState = 0; 26 | #endif 27 | 28 | /*********** RC alias *****************/ 29 | enum rc { 30 | ROLL, 31 | PITCH, 32 | YAW, 33 | THROTTLE, 34 | AUX1, 35 | AUX2, 36 | AUX3, 37 | AUX4 38 | }; 39 | 40 | enum pid { 41 | PIDROLL, 42 | PIDPITCH, 43 | PIDYAW, 44 | PIDALT, 45 | PIDPOS, 46 | PIDPOSR, 47 | PIDNAVR, 48 | PIDLEVEL, 49 | PIDMAG, 50 | PIDVEL, // not used currently 51 | PIDITEMS 52 | }; 53 | 54 | const char pidnames[] PROGMEM = 55 | "ROLL;" 56 | "PITCH;" 57 | "YAW;" 58 | "ALT;" 59 | "Pos;" 60 | "PosR;" 61 | "NavR;" 62 | "LEVEL;" 63 | "MAG;" 64 | "VEL;" 65 | ; 66 | 67 | enum box { 68 | BOXARM, 69 | #if ACC 70 | BOXANGLE, 71 | BOXHORIZON, 72 | #endif 73 | #if BARO && (!defined(SUPPRESS_BARO_ALTHOLD)) 74 | BOXBARO, 75 | #endif 76 | #ifdef VARIOMETER 77 | BOXVARIO, 78 | #endif 79 | #if MAG 80 | BOXMAG, 81 | BOXHEADFREE, 82 | BOXHEADADJ, // acquire heading for HEADFREE mode 83 | #endif 84 | #if defined(SERVO_TILT) || defined(GIMBAL) || defined(SERVO_MIX_TILT) 85 | BOXCAMSTAB, 86 | #endif 87 | #if defined(CAMTRIG) 88 | BOXCAMTRIG, 89 | #endif 90 | #if GPS 91 | BOXGPSHOME, 92 | BOXGPSHOLD, 93 | #endif 94 | #if defined(FIXEDWING) || defined(HELICOPTER) 95 | BOXPASSTHRU, 96 | #endif 97 | #if defined(BUZZER) 98 | BOXBEEPERON, 99 | #endif 100 | #if defined(LED_FLASHER) 101 | BOXLEDMAX, // we want maximum illumination 102 | BOXLEDLOW, // low/no lights 103 | #endif 104 | #if defined(LANDING_LIGHTS_DDR) 105 | BOXLLIGHTS, // enable landing lights at any altitude 106 | #endif 107 | #ifdef INFLIGHT_ACC_CALIBRATION 108 | BOXCALIB, 109 | #endif 110 | #ifdef GOVERNOR_P 111 | BOXGOV, 112 | #endif 113 | #ifdef OSD_SWITCH 114 | BOXOSD, 115 | #endif 116 | CHECKBOXITEMS 117 | }; 118 | 119 | const char boxnames[] PROGMEM = // names for dynamic generation of config GUI 120 | "ARM;" 121 | #if ACC 122 | "ANGLE;" 123 | "HORIZON;" 124 | #endif 125 | #if BARO && (!defined(SUPPRESS_BARO_ALTHOLD)) 126 | "BARO;" 127 | #endif 128 | #ifdef VARIOMETER 129 | "VARIO;" 130 | #endif 131 | #if MAG 132 | "MAG;" 133 | "HEADFREE;" 134 | "HEADADJ;" 135 | #endif 136 | #if defined(SERVO_TILT) || defined(GIMBAL)|| defined(SERVO_MIX_TILT) 137 | "CAMSTAB;" 138 | #endif 139 | #if defined(CAMTRIG) 140 | "CAMTRIG;" 141 | #endif 142 | #if GPS 143 | "GPS HOME;" 144 | "GPS HOLD;" 145 | #endif 146 | #if defined(FIXEDWING) || defined(HELICOPTER) 147 | "PASSTHRU;" 148 | #endif 149 | #if defined(BUZZER) 150 | "BEEPER;" 151 | #endif 152 | #if defined(LED_FLASHER) 153 | "LEDMAX;" 154 | "LEDLOW;" 155 | #endif 156 | #if defined(LANDING_LIGHTS_DDR) 157 | "LLIGHTS;" 158 | #endif 159 | #ifdef INFLIGHT_ACC_CALIBRATION 160 | "CALIB;" 161 | #endif 162 | #ifdef GOVERNOR_P 163 | "GOVERNOR;" 164 | #endif 165 | #ifdef OSD_SWITCH 166 | "OSD SW;" 167 | #endif 168 | ; 169 | 170 | const uint8_t boxids[] PROGMEM = {// permanent IDs associated to boxes. This way, you can rely on an ID number to identify a BOX function. 171 | 0, //"ARM;" 172 | #if ACC 173 | 1, //"ANGLE;" 174 | 2, //"HORIZON;" 175 | #endif 176 | #if BARO && (!defined(SUPPRESS_BARO_ALTHOLD)) 177 | 3, //"BARO;" 178 | #endif 179 | #ifdef VARIOMETER 180 | 4, //"VARIO;" 181 | #endif 182 | #if MAG 183 | 5, //"MAG;" 184 | 6, //"HEADFREE;" 185 | 7, //"HEADADJ;" 186 | #endif 187 | #if defined(SERVO_TILT) || defined(GIMBAL)|| defined(SERVO_MIX_TILT) 188 | 8, //"CAMSTAB;" 189 | #endif 190 | #if defined(CAMTRIG) 191 | 9, //"CAMTRIG;" 192 | #endif 193 | #if GPS 194 | 10, //"GPS HOME;" 195 | 11, //"GPS HOLD;" 196 | #endif 197 | #if defined(FIXEDWING) || defined(HELICOPTER) 198 | 12, //"PASSTHRU;" 199 | #endif 200 | #if defined(BUZZER) 201 | 13, //"BEEPER;" 202 | #endif 203 | #if defined(LED_FLASHER) 204 | 14, //"LEDMAX;" 205 | 15, //"LEDLOW;" 206 | #endif 207 | #if defined(LANDING_LIGHTS_DDR) 208 | 16, //"LLIGHTS;" 209 | #endif 210 | #ifdef INFLIGHT_ACC_CALIBRATION 211 | 17, //"CALIB;" 212 | #endif 213 | #ifdef GOVERNOR_P 214 | 18, //"GOVERNOR;" 215 | #endif 216 | #ifdef OSD_SWITCH 217 | 19, //"OSD_SWITCH;" 218 | #endif 219 | }; 220 | 221 | 222 | static uint32_t currentTime = 0; 223 | static uint16_t previousTime = 0; 224 | static uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop 225 | static uint16_t calibratingA = 0; // the calibration is done in the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode. 226 | static uint16_t calibratingB = 0; // baro calibration = get new ground pressure value 227 | static uint16_t calibratingG; 228 | static uint16_t acc_1G; // this is the 1G measured acceleration 229 | static uint16_t acc_25deg; 230 | static int16_t gyroADC[3],accADC[3],accSmooth[3],magADC[3]; 231 | static int16_t heading,magHold,headFreeModeHold; // [-180;+180] 232 | static uint8_t vbat; // battery voltage in 0.1V steps 233 | static uint8_t vbatMin = VBATNOMINAL; // lowest battery voltage in 0.1V steps 234 | static uint8_t rcOptions[CHECKBOXITEMS]; 235 | static int32_t BaroAlt,EstAlt,AltHold; // in cm 236 | static int16_t BaroPID = 0; 237 | static int16_t errorAltitudeI = 0; 238 | static int16_t vario = 0; // variometer in cm/s 239 | 240 | #if defined(ARMEDTIMEWARNING) 241 | static uint32_t ArmedTimeWarningMicroSeconds = 0; 242 | #endif 243 | 244 | static int16_t debug[4]; 245 | static int16_t sonarAlt; //to think about the unit 246 | 247 | //------------------------------------------------------------------------- 248 | // Calibration flag value 249 | uint8_t calibration_flag; 250 | //------------------------------------------------------------------------- 251 | 252 | struct flags_struct { 253 | uint8_t OK_TO_ARM :1 ; 254 | uint8_t ARMED :1 ; 255 | uint8_t I2C_INIT_DONE :1 ; // For i2c gps we have to now when i2c init is done, so we can update parameters to the i2cgps from eeprom (at startup it is done in setup()) 256 | uint8_t ACC_CALIBRATED :1 ; 257 | uint8_t NUNCHUKDATA :1 ; 258 | uint8_t ANGLE_MODE :1 ; 259 | uint8_t HORIZON_MODE :1 ; 260 | uint8_t MAG_MODE :1 ; 261 | uint8_t BARO_MODE :1 ; 262 | uint8_t GPS_HOME_MODE :1 ; 263 | uint8_t GPS_HOLD_MODE :1 ; 264 | uint8_t HEADFREE_MODE :1 ; 265 | uint8_t PASSTHRU_MODE :1 ; 266 | uint8_t GPS_FIX :1 ; 267 | uint8_t GPS_FIX_HOME :1 ; 268 | uint8_t SMALL_ANGLES_25 :1 ; 269 | uint8_t CALIBRATE_MAG :1 ; 270 | uint8_t VARIO_MODE :1; 271 | } f; 272 | 273 | //for log 274 | #if defined(LOG_VALUES) || defined(LCD_TELEMETRY) 275 | static uint16_t cycleTimeMax = 0; // highest ever cycle timen 276 | static uint16_t cycleTimeMin = 65535; // lowest ever cycle timen 277 | static uint16_t powerMax = 0; // highest ever current; 278 | static int32_t BAROaltMax; // maximum value 279 | #endif 280 | #if defined(LOG_VALUES) || defined(LCD_TELEMETRY) || defined(ARMEDTIMEWARNING) || defined(LOG_PERMANENT) 281 | static uint32_t armedTime = 0; 282 | #endif 283 | 284 | static int16_t i2c_errors_count = 0; 285 | static int16_t annex650_overrun_count = 0; 286 | 287 | // ********************** 288 | //Automatic ACC Offset Calibration 289 | // ********************** 290 | #if defined(INFLIGHT_ACC_CALIBRATION) 291 | static uint16_t InflightcalibratingA = 0; 292 | static int16_t AccInflightCalibrationArmed; 293 | static uint16_t AccInflightCalibrationMeasurementDone = 0; 294 | static uint16_t AccInflightCalibrationSavetoEEProm = 0; 295 | static uint16_t AccInflightCalibrationActive = 0; 296 | #endif 297 | 298 | // ********************** 299 | // power meter 300 | // ********************** 301 | #if defined(POWERMETER) 302 | #define PMOTOR_SUM 8 // index into pMeter[] for sum 303 | static uint32_t pMeter[PMOTOR_SUM + 1]; // we use [0:7] for eight motors,one extra for sum 304 | static uint8_t pMeterV; // dummy to satisfy the paramStruct logic in ConfigurationLoop() 305 | static uint32_t pAlarm; // we scale the eeprom value from [0:255] to this value we can directly compare to the sum in pMeter[6] 306 | static uint16_t powerValue = 0; // last known current 307 | #endif 308 | static uint16_t intPowerMeterSum, intPowerTrigger1; 309 | 310 | // ********************** 311 | // telemetry 312 | // ********************** 313 | #if defined(LCD_TELEMETRY) 314 | static uint8_t telemetry = 0; 315 | static uint8_t telemetry_auto = 0; 316 | #endif 317 | #ifdef LCD_TELEMETRY_STEP 318 | static char telemetryStepSequence [] = LCD_TELEMETRY_STEP; 319 | static uint8_t telemetryStepIndex = 0; 320 | #endif 321 | 322 | // ****************** 323 | // rc functions 324 | // ****************** 325 | #define MINCHECK 1100 326 | #define MAXCHECK 1900 327 | 328 | #define ROL_LO (1<<(2*ROLL)) 329 | #define ROL_CE (3<<(2*ROLL)) 330 | #define ROL_HI (2<<(2*ROLL)) 331 | #define PIT_LO (1<<(2*PITCH)) 332 | #define PIT_CE (3<<(2*PITCH)) 333 | #define PIT_HI (2<<(2*PITCH)) 334 | #define YAW_LO (1<<(2*YAW)) 335 | #define YAW_CE (3<<(2*YAW)) 336 | #define YAW_HI (2<<(2*YAW)) 337 | #define THR_LO (1<<(2*THROTTLE)) 338 | #define THR_CE (3<<(2*THROTTLE)) 339 | #define THR_HI (2<<(2*THROTTLE)) 340 | 341 | static int16_t failsafeEvents = 0; 342 | volatile int16_t failsafeCnt = 0; 343 | 344 | static int16_t rcData[RC_CHANS]; // interval [1000;2000] 345 | static int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW 346 | static int16_t lookupPitchRollRC[6];// lookup table for expo & RC rate PITCH+ROLL 347 | static int16_t lookupThrottleRC[11];// lookup table for expo & mid THROTTLE 348 | static uint16_t rssi; // range: [0;1023] 349 | 350 | #if defined(SPEKTRUM) 351 | volatile uint8_t spekFrameFlags; 352 | volatile uint32_t spekTimeLast; 353 | #endif 354 | 355 | #if defined(OPENLRSv2MULTI) 356 | static uint8_t pot_P,pot_I; // OpenLRS onboard potentiometers for P and I trim or other usages 357 | #endif 358 | 359 | // ************** 360 | // gyro+acc IMU 361 | // ************** 362 | static int16_t gyroData[3] = {0,0,0}; 363 | static int16_t gyroZero[3] = {0,0,0}; 364 | static int16_t angle[2] = {0,0}; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 365 | 366 | // ************************* 367 | // motor and servo functions 368 | // ************************* 369 | static int16_t axisPID[3]; 370 | static int16_t motor[NUMBER_MOTOR]; 371 | #if defined(SERVO) 372 | static int16_t servo[8] = {1500,1500,1500,1500,1500,1500,1500,1500}; 373 | #endif 374 | 375 | // ************************ 376 | // EEPROM Layout definition 377 | // ************************ 378 | static uint8_t dynP8[3], dynD8[3]; 379 | 380 | static struct { 381 | uint8_t currentSet; 382 | int16_t accZero[3]; 383 | int16_t magZero[3]; 384 | uint8_t checksum; // MUST BE ON LAST POSITION OF STRUCTURE ! 385 | } global_conf; 386 | 387 | 388 | static struct { 389 | uint8_t checkNewConf; 390 | uint8_t P8[PIDITEMS], I8[PIDITEMS], D8[PIDITEMS]; 391 | uint8_t rcRate8; 392 | uint8_t rcExpo8; 393 | uint8_t rollPitchRate; 394 | uint8_t yawRate; 395 | uint8_t dynThrPID; 396 | uint8_t thrMid8; 397 | uint8_t thrExpo8; 398 | int16_t angleTrim[2]; 399 | uint16_t activate[CHECKBOXITEMS]; 400 | uint8_t powerTrigger1; 401 | #ifdef FLYING_WING 402 | uint16_t wing_left_mid; 403 | uint16_t wing_right_mid; 404 | #endif 405 | #ifdef TRI 406 | uint16_t tri_yaw_middle; 407 | #endif 408 | #if defined HELICOPTER || defined(AIRPLANE)|| defined(SINGLECOPTER)|| defined(DUALCOPTER) 409 | int16_t servoTrim[8]; 410 | #endif 411 | #if defined(GYRO_SMOOTHING) 412 | uint8_t Smoothing[3]; 413 | #endif 414 | #if defined (FAILSAFE) 415 | int16_t failsafe_throttle; 416 | #endif 417 | #ifdef VBAT 418 | uint8_t vbatscale; 419 | uint8_t vbatlevel_warn1; 420 | uint8_t vbatlevel_warn2; 421 | uint8_t vbatlevel_crit; 422 | uint8_t no_vbat; 423 | #endif 424 | #ifdef POWERMETER 425 | uint16_t psensornull; 426 | uint16_t pleveldivsoft; 427 | uint16_t pleveldiv; 428 | uint8_t pint2ma; 429 | #endif 430 | #ifdef CYCLETIME_FIXATED 431 | uint16_t cycletime_fixated; 432 | #endif 433 | #ifdef MMGYRO 434 | uint8_t mmgyro; 435 | #endif 436 | #ifdef ARMEDTIMEWARNING 437 | uint16_t armedtimewarning; 438 | #endif 439 | int16_t minthrottle; 440 | #ifdef GOVERNOR_P 441 | int16_t governorP; 442 | int16_t governorD; 443 | int8_t governorR; 444 | #endif 445 | uint8_t checksum; // MUST BE ON LAST POSITION OF CONF STRUCTURE ! 446 | } conf; 447 | 448 | #ifdef LOG_PERMANENT 449 | static struct { 450 | uint16_t arm; // #arm events 451 | uint16_t disarm; // #disarm events 452 | uint16_t start; // #powercycle/reset/initialize events 453 | uint32_t armed_time ; // copy of armedTime @ disarm 454 | uint32_t lifetime; // sum (armed) lifetime in seconds 455 | uint16_t failsafe; // #failsafe state @ disarm 456 | uint16_t i2c; // #i2c errs state @ disarm 457 | uint8_t running; // toggle on arm & disarm to monitor for clean shutdown vs. powercut 458 | uint8_t checksum; // MUST BE ON LAST POSITION OF CONF STRUCTURE ! 459 | } plog; 460 | #endif 461 | 462 | // ********************** 463 | // GPS common variables 464 | // ********************** 465 | static int32_t GPS_coord[2]; 466 | static int32_t GPS_home[2]; 467 | static int32_t GPS_hold[2]; 468 | static uint8_t GPS_numSat; 469 | static uint16_t GPS_distanceToHome; // distance to home - unit: meter 470 | static int16_t GPS_directionToHome; // direction to home - unit: degree 471 | static uint16_t GPS_altitude; // GPS altitude - unit: meter 472 | static uint16_t GPS_speed; // GPS speed - unit: cm/s 473 | static uint8_t GPS_update = 0; // a binary toogle to distinct a GPS position update 474 | static int16_t GPS_angle[2] = { 0, 0}; // the angles that must be applied for GPS correction 475 | static uint16_t GPS_ground_course = 0; // - unit: degree*10 476 | static uint8_t GPS_Present = 0; // Checksum from Gps serial 477 | static uint8_t GPS_Enable = 0; 478 | 479 | #define LAT 0 480 | #define LON 1 481 | // The desired bank towards North (Positive) or South (Negative) : latitude 482 | // The desired bank towards East (Positive) or West (Negative) : longitude 483 | static int16_t nav[2]; 484 | static int16_t nav_rated[2]; //Adding a rate controller to the navigation to make it smoother 485 | 486 | // default POSHOLD control gains 487 | #define POSHOLD_P .11 488 | #define POSHOLD_I 0.0 489 | #define POSHOLD_IMAX 20 // degrees 490 | 491 | #define POSHOLD_RATE_P 2.0 492 | #define POSHOLD_RATE_I 0.08 // Wind control 493 | #define POSHOLD_RATE_D 0.045 // try 2 or 3 for POSHOLD_RATE 1 494 | #define POSHOLD_RATE_IMAX 20 // degrees 495 | 496 | // default Navigation PID gains 497 | #define NAV_P 1.4 498 | #define NAV_I 0.20 // Wind control 499 | #define NAV_D 0.08 // 500 | #define NAV_IMAX 20 // degrees 501 | 502 | ///////////////////////////////////////////////////////////////////////////////////////////////////////////// 503 | // Serial GPS only variables 504 | //navigation mode 505 | #define NAV_MODE_NONE 0 506 | #define NAV_MODE_POSHOLD 1 507 | #define NAV_MODE_WP 2 508 | static uint8_t nav_mode = NAV_MODE_NONE; // Navigation mode 509 | 510 | static uint8_t alarmArray[16]; // array 511 | 512 | #if BARO 513 | static int32_t baroPressure; 514 | static int32_t baroTemperature; 515 | static int32_t baroPressureSum; 516 | #endif 517 | 518 | void annexCode() { // this code is excetuted at each loop and won't interfere with control loop if it lasts less than 650 microseconds 519 | static uint32_t calibratedAccTime; 520 | uint16_t tmp,tmp2; 521 | uint8_t axis,prop1,prop2; 522 | 523 | #define BREAKPOINT 1500 524 | // PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value 525 | if (rcData[THROTTLE]DEADBAND) { tmp -= DEADBAND; } 539 | else { tmp=0; } 540 | #endif 541 | if(axis!=2) { //ROLL & PITCH 542 | tmp2 = tmp/100; 543 | rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp-tmp2*100) * (lookupPitchRollRC[tmp2+1]-lookupPitchRollRC[tmp2]) / 100; 544 | prop1 = 100-(uint16_t)conf.rollPitchRate*tmp/500; 545 | prop1 = (uint16_t)prop1*prop2/100; 546 | } else { // YAW 547 | rcCommand[axis] = tmp; 548 | prop1 = 100-(uint16_t)conf.yawRate*tmp/500; 549 | } 550 | dynP8[axis] = (uint16_t)conf.P8[axis]*prop1/100; 551 | dynD8[axis] = (uint16_t)conf.D8[axis]*prop1/100; 552 | if (rcData[axis] [0;1000] 556 | tmp2 = tmp/100; 557 | rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp-tmp2*100) * (lookupThrottleRC[tmp2+1]-lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [conf.minthrottle;MAXTHROTTLE] 558 | 559 | if(f.HEADFREE_MODE) { //to optimize 560 | float radDiff = (heading - headFreeModeHold) * 0.0174533f; // where PI/180 ~= 0.0174533 561 | float cosDiff = cos(radDiff); 562 | float sinDiff = sin(radDiff); 563 | int16_t rcCommand_PITCH = rcCommand[PITCH]*cosDiff + rcCommand[ROLL]*sinDiff; 564 | rcCommand[ROLL] = rcCommand[ROLL]*cosDiff - rcCommand[PITCH]*sinDiff; 565 | rcCommand[PITCH] = rcCommand_PITCH; 566 | } 567 | 568 | #if defined(POWERMETER_HARD) 569 | uint16_t pMeterRaw; // used for current reading 570 | static uint16_t psensorTimer = 0; 571 | if (! (++psensorTimer % PSENSORFREQ)) { 572 | pMeterRaw = analogRead(PSENSORPIN); 573 | //lcdprint_int16(pMeterRaw); LCDcrlf(); 574 | powerValue = ( conf.psensornull > pMeterRaw ? conf.psensornull - pMeterRaw : pMeterRaw - conf.psensornull); // do not use abs(), it would induce implicit cast to uint and overrun 575 | if ( powerValue < 333) { // only accept reasonable values. 333 is empirical 576 | #ifdef LCD_TELEMETRY 577 | if (powerValue > powerMax) powerMax = powerValue; 578 | #endif 579 | } else { 580 | powerValue = 333; 581 | } 582 | pMeter[PMOTOR_SUM] += (uint32_t) powerValue; 583 | } 584 | #endif 585 | #if defined(BUZZER) 586 | #if defined(VBAT) 587 | static uint8_t vbatTimer = 0; 588 | static uint8_t ind = 0; 589 | uint16_t vbatRaw = 0; 590 | static uint16_t vbatRawArray[8]; 591 | if (! (++vbatTimer % VBATFREQ)) { 592 | vbatRawArray[(ind++)%8] = analogRead(V_BATPIN); 593 | for (uint8_t i=0;i<8;i++) vbatRaw += vbatRawArray[i]; 594 | vbat = (vbatRaw*2) / conf.vbatscale; // result is Vbatt in 0.1V steps 595 | } 596 | #endif 597 | alarmHandler(); // external buzzer routine that handles buzzer events globally now 598 | #endif 599 | 600 | #if defined(RX_RSSI) 601 | static uint8_t sig = 0; 602 | uint16_t rssiRaw = 0; 603 | static uint16_t rssiRawArray[8]; 604 | rssiRawArray[(sig++)%8] = analogRead(RX_RSSI_PIN); 605 | for (uint8_t i=0;i<8;i++) rssiRaw += rssiRawArray[i]; 606 | rssi = rssiRaw / 8; 607 | #endif 608 | 609 | if ( (calibratingA>0 && ACC ) || (calibratingG>0) ) { // Calibration phasis 610 | LEDPIN_TOGGLE; 611 | } else { 612 | if (f.ACC_CALIBRATED) {LEDPIN_OFF;} 613 | if (f.ARMED) {LEDPIN_ON;} 614 | } 615 | 616 | #if defined(LED_RING) 617 | static uint32_t LEDTime; 618 | if ( currentTime > LEDTime ) { 619 | LEDTime = currentTime + 50000; 620 | i2CLedRingState(); 621 | } 622 | #endif 623 | 624 | #if defined(LED_FLASHER) 625 | auto_switch_led_flasher(); 626 | #endif 627 | 628 | if ( currentTime > calibratedAccTime ) { 629 | if (! f.SMALL_ANGLES_25) { 630 | // the multi uses ACC and is not calibrated or is too much inclinated 631 | f.ACC_CALIBRATED = 0; 632 | LEDPIN_TOGGLE; 633 | calibratedAccTime = currentTime + 100000; 634 | } else { 635 | f.ACC_CALIBRATED = 1; 636 | } 637 | } 638 | 639 | #if !(defined(SPEKTRUM) && defined(PROMINI)) //Only one serial port on ProMini. Skip serial com if Spektrum Sat in use. Note: Spek code will auto-call serialCom if GUI data detected on serial0. 640 | #if defined(GPS_PROMINI) 641 | if(GPS_Enable == 0) {serialCom();} 642 | #else 643 | serialCom(); 644 | #endif 645 | #endif 646 | 647 | #if defined(POWERMETER) 648 | intPowerMeterSum = (pMeter[PMOTOR_SUM]/conf.pleveldiv); 649 | intPowerTrigger1 = conf.powerTrigger1 * PLEVELSCALE; 650 | #endif 651 | 652 | #ifdef LCD_TELEMETRY_AUTO 653 | static char telemetryAutoSequence [] = LCD_TELEMETRY_AUTO; 654 | static uint8_t telemetryAutoIndex = 0; 655 | static uint16_t telemetryAutoTimer = 0; 656 | if ( (telemetry_auto) && (! (++telemetryAutoTimer % LCD_TELEMETRY_AUTO_FREQ) ) ){ 657 | telemetry = telemetryAutoSequence[++telemetryAutoIndex % strlen(telemetryAutoSequence)]; 658 | LCDclear(); // make sure to clear away remnants 659 | } 660 | #endif 661 | #ifdef LCD_TELEMETRY 662 | static uint16_t telemetryTimer = 0; 663 | if (! (++telemetryTimer % LCD_TELEMETRY_FREQ)) { 664 | #if (LCD_TELEMETRY_DEBUG+0 > 0) 665 | telemetry = LCD_TELEMETRY_DEBUG; 666 | #endif 667 | if (telemetry) lcd_telemetry(); 668 | } 669 | #endif 670 | 671 | #if GPS & defined(GPS_LED_INDICATOR) // modified by MIS to use STABLEPIN LED for number of sattelites indication 672 | static uint32_t GPSLEDTime; // - No GPS FIX -> LED blink at speed of incoming GPS frames 673 | static uint8_t blcnt; // - Fix and sat no. bellow 5 -> LED off 674 | if(currentTime > GPSLEDTime) { // - Fix and sat no. >= 5 -> LED blinks, one blink for 5 sat, two blinks for 6 sat, three for 7 ... 675 | if(f.GPS_FIX && GPS_numSat >= 5) { 676 | if(++blcnt > 2*GPS_numSat) blcnt = 0; 677 | GPSLEDTime = currentTime + 150000; 678 | if(blcnt >= 10 && ((blcnt%2) == 0)) {STABLEPIN_ON;} else {STABLEPIN_OFF;} 679 | }else{ 680 | if((GPS_update == 1) && !f.GPS_FIX) {STABLEPIN_ON;} else {STABLEPIN_OFF;} 681 | blcnt = 0; 682 | } 683 | } 684 | #endif 685 | 686 | #if defined(LOG_VALUES) && (LOG_VALUES >= 2) 687 | if (cycleTime > cycleTimeMax) cycleTimeMax = cycleTime; // remember highscore 688 | if (cycleTime < cycleTimeMin) cycleTimeMin = cycleTime; // remember lowscore 689 | #endif 690 | if (f.ARMED) { 691 | #if defined(LCD_TELEMETRY) || defined(ARMEDTIMEWARNING) || defined(LOG_PERMANENT) 692 | armedTime += (uint32_t)cycleTime; 693 | #endif 694 | #if defined(VBAT) 695 | if ( (vbat > conf.no_vbat) && (vbat < vbatMin) ) vbatMin = vbat; 696 | #endif 697 | #ifdef LCD_TELEMETRY 698 | #if BARO 699 | if ( (BaroAlt > BAROaltMax) ) BAROaltMax = BaroAlt; 700 | #endif 701 | #endif 702 | } 703 | } 704 | 705 | void system_init(void) 706 | { 707 | #if !defined(GPS_PROMINI) 708 | SerialOpen(0,SERIAL0_COM_SPEED); 709 | #if defined(PROMICRO) 710 | SerialOpen(1,SERIAL1_COM_SPEED); 711 | #endif 712 | #if defined(MEGA) 713 | SerialOpen(1,SERIAL1_COM_SPEED); 714 | SerialOpen(2,SERIAL2_COM_SPEED); 715 | SerialOpen(3,SERIAL3_COM_SPEED); 716 | #endif 717 | #endif 718 | LEDPIN_PINMODE; 719 | POWERPIN_PINMODE; 720 | BUZZERPIN_PINMODE; 721 | STABLEPIN_PINMODE; 722 | POWERPIN_OFF; 723 | initOutput(); 724 | #ifdef MULTIPLE_CONFIGURATION_PROFILES 725 | for(global_conf.currentSet=0; global_conf.currentSet<3; global_conf.currentSet++) { // check all settings integrity 726 | readEEPROM(); 727 | } 728 | #else 729 | global_conf.currentSet=0; 730 | readEEPROM(); 731 | #endif 732 | readGlobalSet(); 733 | readEEPROM(); // load current setting data 734 | blinkLED(2,40,global_conf.currentSet+1); 735 | configureReceiver(); 736 | #if defined (PILOTLAMP) 737 | PL_INIT; 738 | #endif 739 | #if defined(OPENLRSv2MULTI) 740 | initOpenLRS(); 741 | #endif 742 | initSensors(); 743 | #if defined(I2C_GPS) || defined(GPS_SERIAL) || defined(GPS_FROM_OSD) 744 | GPS_set_pids(); 745 | #endif 746 | previousTime = micros(); 747 | #if defined(GIMBAL) 748 | calibratingA = 512; 749 | #endif 750 | calibratingG = 512; 751 | calibratingB = 200; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles 752 | #if defined(POWERMETER) 753 | for(uint8_t i=0;i<=PMOTOR_SUM;i++) 754 | pMeter[i]=0; 755 | #endif 756 | /************************************/ 757 | #if defined(GPS_SERIAL) 758 | GPS_SerialInit(); 759 | for(uint8_t i=0;i<=5;i++){ 760 | GPS_NewData(); 761 | LEDPIN_ON 762 | delay(20); 763 | LEDPIN_OFF 764 | delay(80); 765 | } 766 | if(!GPS_Present){ 767 | SerialEnd(GPS_SERIAL); 768 | SerialOpen(0,SERIAL0_COM_SPEED); 769 | } 770 | #if !defined(GPS_PROMINI) 771 | GPS_Present = 1; 772 | #endif 773 | GPS_Enable = GPS_Present; 774 | #endif 775 | /************************************/ 776 | 777 | #if defined(I2C_GPS) || defined(TINY_GPS) || defined(GPS_FROM_OSD) 778 | GPS_Enable = 1; 779 | #endif 780 | 781 | #if defined(LCD_ETPP) || defined(LCD_LCD03) || defined(OLED_I2C_128x64) || defined(LCD_TELEMETRY_STEP) 782 | initLCD(); 783 | #endif 784 | #ifdef LCD_TELEMETRY_DEBUG 785 | telemetry_auto = 1; 786 | #endif 787 | #ifdef LCD_CONF_DEBUG 788 | configurationLoop(); 789 | #endif 790 | #ifdef LANDING_LIGHTS_DDR 791 | init_landing_lights(); 792 | #endif 793 | ADCSRA |= _BV(ADPS2) ; ADCSRA &= ~_BV(ADPS1); ADCSRA &= ~_BV(ADPS0); // this speeds up analogRead without loosing too much resolution: http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1208715493/11 794 | #if defined(LED_FLASHER) 795 | init_led_flasher(); 796 | led_flasher_set_sequence(LED_FLASHER_SEQUENCE); 797 | #endif 798 | f.SMALL_ANGLES_25=1; // important for gyro only conf 799 | #ifdef LOG_PERMANENT 800 | // read last stored set 801 | readPLog(); 802 | plog.lifetime += plog.armed_time / 1000000; 803 | plog.start++; // #powercycle/reset/initialize events 804 | // dump plog data to terminal 805 | #ifdef LOG_PERMANENT_SHOW_AT_STARTUP 806 | dumpPLog(0); 807 | #endif 808 | plog.armed_time = 0; // lifetime in seconds 809 | //plog.running = 0; // toggle on arm & disarm to monitor for clean shutdown vs. powercut 810 | #endif 811 | 812 | debugmsg_append_str("initialization completed\n"); 813 | } 814 | 815 | 816 | void setup() 817 | { 818 | //------------------------------------------------------------------ 819 | calibration_flag = 0; 820 | //------------------------------------------------------------------ 821 | system_init(); 822 | } 823 | 824 | //------------------------------------------------------------------ 825 | void calibration_fun(void) 826 | { 827 | calibratingG = 512; 828 | #if GPS 829 | GPS_reset_home_position(); 830 | #endif 831 | #if BARO 832 | calibratingB = 10; // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking) 833 | #endif 834 | } 835 | //------------------------------------------------------------------ 836 | 837 | // Unlock function 838 | void go_arm() { 839 | 840 | if(calibratingG == 0 && f.ACC_CALIBRATED 841 | #if defined(FAILSAFE) 842 | // && failsafeCnt < 2 843 | #endif 844 | ) { 845 | if(!f.ARMED) { // arm now! 846 | f.ARMED = 1; 847 | headFreeModeHold = heading; 848 | #if defined(VBAT) 849 | if (vbat > conf.no_vbat) vbatMin = vbat; 850 | #endif 851 | #ifdef LCD_TELEMETRY // reset some values when arming 852 | #if BARO 853 | BAROaltMax = BaroAlt; 854 | #endif 855 | #endif 856 | #ifdef LOG_PERMANENT 857 | plog.arm++; // #arm events 858 | plog.running = 1; // toggle on arm & disarm to monitor for clean shutdown vs. powercut 859 | // write now. 860 | writePLog(); 861 | #endif 862 | } 863 | } else if(!f.ARMED) { 864 | blinkLED(2,255,1); 865 | alarmArray[8] = 1; 866 | } 867 | 868 | //------------------------------------------------------------------ 869 | if (calibration_flag == 0) { 870 | calibration_flag = 1; 871 | calibration_fun(); 872 | } 873 | //------------------------------------------------------------------ 874 | } 875 | 876 | // Unlock function 877 | void go_disarm() { 878 | if (f.ARMED) { 879 | f.ARMED = 0; 880 | #ifdef LOG_PERMANENT 881 | plog.disarm++; // #disarm events 882 | plog.armed_time = armedTime ; // lifetime in seconds 883 | if (failsafeEvents) plog.failsafe++; // #acitve failsafe @ disarm 884 | if (i2c_errors_count > 10) plog.i2c++; // #i2c errs @ disarm 885 | plog.running = 0; // toggle @ arm & disarm to monitor for clean shutdown vs. powercut 886 | // write now. 887 | writePLog(); 888 | #endif 889 | } 890 | } 891 | void servos2Neutral() { 892 | #ifdef TRI 893 | servo[5] = 1500; // we center the yaw servo in conf mode 894 | writeServos(); 895 | #endif 896 | #ifdef FLYING_WING 897 | servo[0] = conf.wing_left_mid; 898 | servo[1] = conf.wing_right_mid; 899 | writeServos(); 900 | #endif 901 | #ifdef AIRPLANE 902 | for(uint8_t i = 4; i<7 ;i++) servo[i] = 1500; 903 | writeServos(); 904 | #endif 905 | #ifdef HELICOPTER 906 | servo[5] = YAW_CENTER; 907 | servo[3] = servo[4] = servo[6] = 1500; 908 | writeServos(); 909 | #endif 910 | } 911 | 912 | // ******** Main Loop ********* 913 | void loop () { 914 | static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors 915 | static uint8_t rcSticks; // this hold sticks position for command combos 916 | uint8_t axis,i; 917 | int16_t error,errorAngle; 918 | int16_t delta,deltaSum; 919 | int16_t PTerm,ITerm,DTerm; 920 | int16_t PTermACC = 0 , ITermACC = 0 , PTermGYRO = 0 , ITermGYRO = 0; 921 | static int16_t lastGyro[3] = {0,0,0}; 922 | static int16_t delta1[3],delta2[3]; 923 | static int16_t errorGyroI[3] = {0,0,0}; 924 | static int16_t errorAngleI[2] = {0,0}; 925 | static uint32_t rcTime = 0; 926 | static int16_t initialThrottleHold; 927 | static uint32_t timestamp_fixated = 0; 928 | 929 | #if defined(SPEKTRUM) 930 | if (spekFrameFlags == 0x01) readSpektrum(); 931 | #endif 932 | #if defined(OPENLRSv2MULTI) 933 | Read_OpenLRS_RC(); 934 | #endif 935 | 936 | #if defined(HEX_NANO) 937 | serialCom(); 938 | #endif 939 | 940 | if (currentTime > rcTime ) { // 50Hz 941 | rcTime = currentTime + 20000; 942 | computeRC(); 943 | // Failsafe routine - added by MIS 944 | #if defined(HEX_NANO) 945 | rcData[0] = serialRcValue[0]; 946 | rcData[1] = serialRcValue[1]; 947 | rcData[2] = serialRcValue[2]; 948 | rcData[3] = serialRcValue[3]; 949 | rcData[4] = serialRcValue[4]; 950 | rcData[5] = serialRcValue[5]; 951 | rcData[6] = serialRcValue[6]; 952 | rcData[7] = serialRcValue[7]; 953 | #endif 954 | 955 | #if defined(FAILSAFE) 956 | if ( failsafeCnt > (5*FAILSAFE_DELAY) && f.ARMED) { // Stabilize, and set Throttle to specified level 957 | for(i=0; i<3; i++) rcData[i] = MIDRC; // after specified guard time after RC signal is lost (in 0.1sec) 958 | 959 | if(rcData[THROTTLE] < conf.failsafe_throttle){ 960 | rcData[THROTTLE] = MINTHROTTLE; 961 | } 962 | else{ 963 | rcData[THROTTLE] = conf.failsafe_throttle; 964 | } 965 | 966 | if (failsafeCnt > 5*(FAILSAFE_DELAY+FAILSAFE_OFF_DELAY)) { // Turn OFF motors after specified Time (in 0.1sec) 967 | go_disarm(); // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents 968 | f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm 969 | } 970 | failsafeEvents++; 971 | } 972 | if ( failsafeCnt > (5*FAILSAFE_DELAY) && !f.ARMED) { //Turn of "Ok To arm to prevent the motors from spinning after repowering the RX with low throttle and aux to arm 973 | go_disarm(); // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents 974 | f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm 975 | } 976 | failsafeCnt++; 977 | #endif 978 | // end of failsafe routine - next change is made with RcOptions setting 979 | 980 | // ------------------ STICKS COMMAND HANDLER -------------------- 981 | // checking sticks positions 982 | uint8_t stTmp = 0; 983 | for(i=0;i<4;i++) { 984 | stTmp >>= 2; 985 | if(rcData[i] > MINCHECK) stTmp |= 0x80; // check for MIN 986 | if(rcData[i] < MAXCHECK) stTmp |= 0x40; // check for MAX 987 | } 988 | if(stTmp == rcSticks) { 989 | if(rcDelayCommand<250) rcDelayCommand++; 990 | } else rcDelayCommand = 0; 991 | rcSticks = stTmp; 992 | 993 | // perform actions 994 | if (rcData[THROTTLE] <= MINCHECK) { // THROTTLE at minimum 995 | errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0; errorGyroI[YAW] = 0; 996 | errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0; 997 | if (conf.activate[BOXARM] > 0) { // Arming/Disarming via ARM BOX 998 | if ( rcOptions[BOXARM] && f.OK_TO_ARM ) 999 | go_arm(); 1000 | else if (f.ARMED) 1001 | go_disarm(); 1002 | } 1003 | } 1004 | if(rcDelayCommand == 20) { 1005 | if(f.ARMED) { // actions during armed 1006 | #ifdef ALLOW_ARM_DISARM_VIA_TX_YAW 1007 | if (conf.activate[BOXARM] == 0 && rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) go_disarm(); // Disarm via YAW 1008 | #endif 1009 | #ifdef ALLOW_ARM_DISARM_VIA_TX_ROLL 1010 | if (conf.activate[BOXARM] == 0 && rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO) go_disarm(); // Disarm via ROLL 1011 | #endif 1012 | } else { // actions during not armed 1013 | i=0; 1014 | if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) { // GYRO calibration 1015 | //------------------------------------------------------------ 1016 | calibration_fun(); 1017 | //------------------------------------------------------------ 1018 | } 1019 | #if defined(INFLIGHT_ACC_CALIBRATION) 1020 | else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI) { // Inflight ACC calibration START/STOP 1021 | if (AccInflightCalibrationMeasurementDone){ // trigger saving into eeprom after landing 1022 | AccInflightCalibrationMeasurementDone = 0; 1023 | AccInflightCalibrationSavetoEEProm = 1; 1024 | }else{ 1025 | AccInflightCalibrationArmed = !AccInflightCalibrationArmed; 1026 | #if defined(BUZZER) 1027 | if (AccInflightCalibrationArmed) alarmArray[0]=2; else alarmArray[0]=3; 1028 | #endif 1029 | } 1030 | } 1031 | #endif 1032 | #ifdef MULTIPLE_CONFIGURATION_PROFILES 1033 | if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) i=1; // ROLL left -> Profile 1 1034 | else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) i=2; // PITCH up -> Profile 2 1035 | else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) i=3; // ROLL right -> Profile 3 1036 | if(i) { 1037 | global_conf.currentSet = i-1; 1038 | writeGlobalSet(0); 1039 | readEEPROM(); 1040 | blinkLED(2,40,i); 1041 | alarmArray[0] = i; 1042 | } 1043 | #endif 1044 | if (rcSticks == THR_LO + YAW_HI + PIT_HI + ROL_CE) { // Enter LCD config 1045 | #if defined(LCD_CONF) 1046 | configurationLoop(); // beginning LCD configuration 1047 | #endif 1048 | previousTime = micros(); 1049 | } 1050 | #ifdef ALLOW_ARM_DISARM_VIA_TX_YAW 1051 | else if (conf.activate[BOXARM] == 0 && rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) go_arm(); // Arm via YAW 1052 | #endif 1053 | #ifdef ALLOW_ARM_DISARM_VIA_TX_ROLL 1054 | else if (conf.activate[BOXARM] == 0 && rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI) go_arm(); // Arm via ROLL 1055 | #endif 1056 | #ifdef LCD_TELEMETRY_AUTO 1057 | else if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) { // Auto telemetry ON/OFF 1058 | if (telemetry_auto) { 1059 | telemetry_auto = 0; 1060 | telemetry = 0; 1061 | } else 1062 | telemetry_auto = 1; 1063 | } 1064 | #endif 1065 | #ifdef LCD_TELEMETRY_STEP 1066 | else if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) { // Telemetry next step 1067 | telemetry = telemetryStepSequence[++telemetryStepIndex % strlen(telemetryStepSequence)]; 1068 | #ifdef OLED_I2C_128x64 1069 | if (telemetry != 0) i2c_OLED_init(); 1070 | #endif 1071 | LCDclear(); 1072 | } 1073 | #endif 1074 | else if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) calibratingA=512; // throttle=max, yaw=left, pitch=min 1075 | else if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) f.CALIBRATE_MAG = 1; // throttle=max, yaw=right, pitch=min 1076 | i=0; 1077 | if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) {conf.angleTrim[PITCH]+=2; i=1;} 1078 | else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {conf.angleTrim[PITCH]-=2; i=1;} 1079 | else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {conf.angleTrim[ROLL] +=2; i=1;} 1080 | else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {conf.angleTrim[ROLL] -=2; i=1;} 1081 | if (i) { 1082 | writeParams(1); 1083 | rcDelayCommand = 0; // allow autorepetition 1084 | #if defined(LED_RING) 1085 | blinkLedRing(); 1086 | #endif 1087 | } 1088 | } 1089 | } 1090 | #if defined(LED_FLASHER) 1091 | led_flasher_autoselect_sequence(); 1092 | #endif 1093 | 1094 | #if defined(INFLIGHT_ACC_CALIBRATION) 1095 | if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > MINCHECK && !rcOptions[BOXARM] ){ // Copter is airborne and you are turning it off via boxarm : start measurement 1096 | InflightcalibratingA = 50; 1097 | AccInflightCalibrationArmed = 0; 1098 | } 1099 | if (rcOptions[BOXCALIB]) { // Use the Calib Option to activate : Calib = TRUE Meausrement started, Land and Calib = 0 measurement stored 1100 | if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone){ 1101 | InflightcalibratingA = 50; 1102 | } 1103 | }else if(AccInflightCalibrationMeasurementDone && !f.ARMED){ 1104 | AccInflightCalibrationMeasurementDone = 0; 1105 | AccInflightCalibrationSavetoEEProm = 1; 1106 | } 1107 | #endif 1108 | 1109 | uint16_t auxState = 0; 1110 | for(i=0;i<4;i++) 1111 | auxState |= (rcData[AUX1+i]<1300)<<(3*i) | (13001700)<<(3*i+2); 1112 | for(i=0;i0; 1114 | 1115 | // note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAFE_DELAY is always false 1116 | #if ACC 1117 | if ( rcOptions[BOXANGLE] || (failsafeCnt > 5*FAILSAFE_DELAY) ) { 1118 | // bumpless transfer to Level mode 1119 | if (!f.ANGLE_MODE) { 1120 | errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0; 1121 | f.ANGLE_MODE = 1; 1122 | } 1123 | } else { 1124 | // failsafe support 1125 | f.ANGLE_MODE = 0; 1126 | } 1127 | if ( rcOptions[BOXHORIZON] ) { 1128 | f.ANGLE_MODE = 0; 1129 | if (!f.HORIZON_MODE) { 1130 | errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0; 1131 | f.HORIZON_MODE = 1; 1132 | } 1133 | } else { 1134 | f.HORIZON_MODE = 0; 1135 | } 1136 | #endif 1137 | 1138 | if (rcOptions[BOXARM] == 0) f.OK_TO_ARM = 1; 1139 | #if !defined(GPS_LED_INDICATOR) 1140 | if (f.ANGLE_MODE || f.HORIZON_MODE) {STABLEPIN_ON;} else {STABLEPIN_OFF;} 1141 | #endif 1142 | 1143 | #if BARO 1144 | #if (!defined(SUPPRESS_BARO_ALTHOLD)) 1145 | if (rcOptions[BOXBARO]) { 1146 | if (!f.BARO_MODE) { 1147 | f.BARO_MODE = 1; 1148 | AltHold = EstAlt; 1149 | initialThrottleHold = rcCommand[THROTTLE]; 1150 | errorAltitudeI = 0; 1151 | BaroPID=0; 1152 | } 1153 | } else { 1154 | f.BARO_MODE = 0; 1155 | } 1156 | #endif 1157 | #ifdef VARIOMETER 1158 | if (rcOptions[BOXVARIO]) { 1159 | if (!f.VARIO_MODE) { 1160 | f.VARIO_MODE = 1; 1161 | } 1162 | } else { 1163 | f.VARIO_MODE = 0; 1164 | } 1165 | #endif 1166 | #endif 1167 | #if MAG 1168 | if (rcOptions[BOXMAG]) { 1169 | if (!f.MAG_MODE) { 1170 | f.MAG_MODE = 1; 1171 | magHold = heading; 1172 | } 1173 | } else { 1174 | f.MAG_MODE = 0; 1175 | } 1176 | if (rcOptions[BOXHEADFREE]) { 1177 | if (!f.HEADFREE_MODE) { 1178 | f.HEADFREE_MODE = 1; 1179 | } 1180 | } else { 1181 | f.HEADFREE_MODE = 0; 1182 | } 1183 | if (rcOptions[BOXHEADADJ]) { 1184 | headFreeModeHold = heading; // acquire new heading 1185 | } 1186 | #endif 1187 | 1188 | #if GPS 1189 | static uint8_t GPSNavReset = 1; 1190 | if (f.GPS_FIX && GPS_numSat >= 5 ) { 1191 | if (rcOptions[BOXGPSHOME]) { // if both GPS_HOME & GPS_HOLD are checked => GPS_HOME is the priority 1192 | if (!f.GPS_HOME_MODE) { 1193 | f.GPS_HOME_MODE = 1; 1194 | f.GPS_HOLD_MODE = 0; 1195 | GPSNavReset = 0; 1196 | #if defined(I2C_GPS) 1197 | GPS_I2C_command(I2C_GPS_COMMAND_START_NAV,0); //waypoint zero 1198 | #else // SERIAL 1199 | GPS_set_next_wp(&GPS_home[LAT],&GPS_home[LON]); 1200 | nav_mode = NAV_MODE_WP; 1201 | #endif 1202 | } 1203 | } else { 1204 | f.GPS_HOME_MODE = 0; 1205 | if (rcOptions[BOXGPSHOLD] && abs(rcCommand[ROLL])< AP_MODE && abs(rcCommand[PITCH]) < AP_MODE) { 1206 | if (!f.GPS_HOLD_MODE) { 1207 | f.GPS_HOLD_MODE = 1; 1208 | GPSNavReset = 0; 1209 | #if defined(I2C_GPS) 1210 | GPS_I2C_command(I2C_GPS_COMMAND_POSHOLD,0); 1211 | #else 1212 | GPS_hold[LAT] = GPS_coord[LAT]; 1213 | GPS_hold[LON] = GPS_coord[LON]; 1214 | GPS_set_next_wp(&GPS_hold[LAT],&GPS_hold[LON]); 1215 | nav_mode = NAV_MODE_POSHOLD; 1216 | #endif 1217 | } 1218 | } else { 1219 | f.GPS_HOLD_MODE = 0; 1220 | // both boxes are unselected here, nav is reset if not already done 1221 | if (GPSNavReset == 0 ) { 1222 | GPSNavReset = 1; 1223 | GPS_reset_nav(); 1224 | } 1225 | } 1226 | } 1227 | } else { 1228 | f.GPS_HOME_MODE = 0; 1229 | f.GPS_HOLD_MODE = 0; 1230 | #if !defined(I2C_GPS) 1231 | nav_mode = NAV_MODE_NONE; 1232 | #endif 1233 | } 1234 | #endif 1235 | 1236 | #if defined(FIXEDWING) || defined(HELICOPTER) 1237 | if (rcOptions[BOXPASSTHRU]) {f.PASSTHRU_MODE = 1;} 1238 | else {f.PASSTHRU_MODE = 0;} 1239 | #endif 1240 | 1241 | } else { // not in rc loop 1242 | static uint8_t taskOrder=0; // never call all functions in the same loop, to avoid high delay spikes 1243 | if(taskOrder>4) taskOrder-=5; 1244 | switch (taskOrder) { 1245 | case 0: 1246 | taskOrder++; 1247 | #if MAG 1248 | if (Mag_getADC()) break; // max 350 碌s (HMC5883) // only break when we actually did something 1249 | #endif 1250 | case 1: 1251 | taskOrder++; 1252 | #if BARO 1253 | if (Baro_update() != 0 ) break; 1254 | #endif 1255 | case 2: 1256 | taskOrder++; 1257 | #if BARO 1258 | if (getEstimatedAltitude() !=0) break; 1259 | #endif 1260 | case 3: 1261 | taskOrder++; 1262 | #if GPS 1263 | if(GPS_Enable) GPS_NewData(); 1264 | break; 1265 | #endif 1266 | case 4: 1267 | taskOrder++; 1268 | #if SONAR 1269 | Sonar_update();debug[2] = sonarAlt; 1270 | #endif 1271 | #ifdef LANDING_LIGHTS_DDR 1272 | auto_switch_landing_lights(); 1273 | #endif 1274 | #ifdef VARIOMETER 1275 | if (f.VARIO_MODE) vario_signaling(); 1276 | #endif 1277 | break; 1278 | } 1279 | } 1280 | 1281 | computeIMU(); 1282 | // Measure loop rate just afer reading the sensors 1283 | currentTime = micros(); 1284 | cycleTime = currentTime - previousTime; 1285 | previousTime = currentTime; 1286 | 1287 | #ifdef CYCLETIME_FIXATED 1288 | if (conf.cycletime_fixated) { 1289 | if ((micros()-timestamp_fixated)>conf.cycletime_fixated) { 1290 | } else { 1291 | while((micros()-timestamp_fixated)= ACROTRAINER_MODE ) { 1302 | f.ANGLE_MODE=0; 1303 | f.HORIZON_MODE=0; 1304 | f.MAG_MODE=0; 1305 | f.BARO_MODE=0; 1306 | f.GPS_HOME_MODE=0; 1307 | f.GPS_HOLD_MODE=0; 1308 | } 1309 | } 1310 | #endif 1311 | 1312 | //*********************************** 1313 | 1314 | #if MAG 1315 | if (abs(rcCommand[YAW]) <70 && f.MAG_MODE) { 1316 | int16_t dif = heading - magHold; 1317 | if (dif <= - 180) dif += 360; 1318 | if (dif >= + 180) dif -= 360; 1319 | if ( f.SMALL_ANGLES_25 ) rcCommand[YAW] -= dif*conf.P8[PIDMAG]>>5; 1320 | } else magHold = heading; 1321 | #endif 1322 | 1323 | #if BARO && (!defined(SUPPRESS_BARO_ALTHOLD)) 1324 | if (f.BARO_MODE) { 1325 | static uint8_t isAltHoldChanged = 0; 1326 | #if defined(ALTHOLD_FAST_THROTTLE_CHANGE) 1327 | if (abs(rcCommand[THROTTLE]-initialThrottleHold) > ALT_HOLD_THROTTLE_NEUTRAL_ZONE) { 1328 | //errorAltitudeI = 0; 1329 | isAltHoldChanged = 1; 1330 | rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -ALT_HOLD_THROTTLE_NEUTRAL_ZONE : ALT_HOLD_THROTTLE_NEUTRAL_ZONE; 1331 | // initialThrottleHold += (rcCommand[THROTTLE] > initialThrottleHold) ? -ALT_HOLD_THROTTLE_NEUTRAL_ZONE : ALT_HOLD_THROTTLE_NEUTRAL_ZONE;; //++hex nano 1332 | } else { 1333 | if (isAltHoldChanged) { 1334 | AltHold = EstAlt; 1335 | isAltHoldChanged = 0; 1336 | } 1337 | rcCommand[THROTTLE] = initialThrottleHold + BaroPID; 1338 | } 1339 | #else 1340 | static int16_t AltHoldCorr = 0; 1341 | if (abs(rcCommand[THROTTLE]-initialThrottleHold)>ALT_HOLD_THROTTLE_NEUTRAL_ZONE) { 1342 | // Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms) 1343 | AltHoldCorr+= rcCommand[THROTTLE] - initialThrottleHold; 1344 | if(abs(AltHoldCorr) > 500) { 1345 | AltHold += AltHoldCorr/500; 1346 | AltHoldCorr %= 500; 1347 | } 1348 | //errorAltitudeI = 0; 1349 | isAltHoldChanged = 1; 1350 | } else if (isAltHoldChanged) { 1351 | AltHold = EstAlt; 1352 | isAltHoldChanged = 0; 1353 | } 1354 | rcCommand[THROTTLE] = initialThrottleHold + BaroPID; 1355 | #endif 1356 | } 1357 | #endif 1358 | #if GPS 1359 | if ( (f.GPS_HOME_MODE || f.GPS_HOLD_MODE) && f.GPS_FIX_HOME ) { 1360 | float sin_yaw_y = sin(heading*0.0174532925f); 1361 | float cos_yaw_x = cos(heading*0.0174532925f); 1362 | #if defined(NAV_SLEW_RATE) 1363 | nav_rated[LON] += constrain(wrap_18000(nav[LON]-nav_rated[LON]),-NAV_SLEW_RATE,NAV_SLEW_RATE); 1364 | nav_rated[LAT] += constrain(wrap_18000(nav[LAT]-nav_rated[LAT]),-NAV_SLEW_RATE,NAV_SLEW_RATE); 1365 | GPS_angle[ROLL] = (nav_rated[LON]*cos_yaw_x - nav_rated[LAT]*sin_yaw_y) /10; 1366 | GPS_angle[PITCH] = (nav_rated[LON]*sin_yaw_y + nav_rated[LAT]*cos_yaw_x) /10; 1367 | #else 1368 | GPS_angle[ROLL] = (nav[LON]*cos_yaw_x - nav[LAT]*sin_yaw_y) /10; 1369 | GPS_angle[PITCH] = (nav[LON]*sin_yaw_y + nav[LAT]*cos_yaw_x) /10; 1370 | #endif 1371 | } else { 1372 | GPS_angle[ROLL] = 0; 1373 | GPS_angle[PITCH] = 0; 1374 | } 1375 | #endif 1376 | 1377 | //**** PITCH & ROLL & YAW PID **** 1378 | int16_t prop; 1379 | prop = min(max(abs(rcCommand[PITCH]),abs(rcCommand[ROLL])),500); // range [0;500] 1380 | 1381 | for(axis=0;axis<3;axis++) { 1382 | if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis<2 ) { // MODE relying on ACC 1383 | // 50 degrees max inclination 1384 | errorAngle = constrain((rcCommand[axis]<<1) + GPS_angle[axis],-500,+500) - angle[axis] + conf.angleTrim[axis]; //16 bits is ok here 1385 | PTermACC = ((int32_t)errorAngle*conf.P8[PIDLEVEL])>>7; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result 1386 | PTermACC = constrain(PTermACC,-conf.D8[PIDLEVEL]*5,+conf.D8[PIDLEVEL]*5); 1387 | 1388 | errorAngleI[axis] = constrain(errorAngleI[axis]+errorAngle,-10000,+10000); // WindUp //16 bits is ok here 1389 | ITermACC = ((int32_t)errorAngleI[axis]*conf.I8[PIDLEVEL])>>12; // 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result 1390 | } 1391 | if ( !f.ANGLE_MODE || f.HORIZON_MODE || axis == 2 ) { // MODE relying on GYRO or YAW axis 1392 | if (abs(rcCommand[axis])<500) error = (rcCommand[axis]<<6)/conf.P8[axis] ; // 16 bits is needed for calculation: 500*64 = 32000 16 bits is ok for result if P8>5 (P>0.5) 1393 | else error = ((int32_t)rcCommand[axis]<<6)/conf.P8[axis] ; // 32 bits is needed for calculation 1394 | 1395 | error -= gyroData[axis]; 1396 | 1397 | PTermGYRO = rcCommand[axis]; 1398 | 1399 | errorGyroI[axis] = constrain(errorGyroI[axis]+error,-16000,+16000); // WindUp 16 bits is ok here 1400 | if (abs(gyroData[axis])>640) errorGyroI[axis] = 0; 1401 | ITermGYRO = ((errorGyroI[axis]>>7)*conf.I8[axis])>>6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000 1402 | } 1403 | if ( f.HORIZON_MODE && axis<2) { 1404 | PTerm = ((int32_t)PTermACC*(512-prop) + (int32_t)PTermGYRO*prop)>>9; // the real factor should be 500, but 512 is ok 1405 | ITerm = ((int32_t)ITermACC*(512-prop) + (int32_t)ITermGYRO*prop)>>9; 1406 | } else { 1407 | if ( f.ANGLE_MODE && axis<2) { 1408 | PTerm = PTermACC; 1409 | ITerm = ITermACC; 1410 | } else { 1411 | PTerm = PTermGYRO; 1412 | ITerm = ITermGYRO; 1413 | } 1414 | } 1415 | 1416 | PTerm -= ((int32_t)gyroData[axis]*dynP8[axis])>>6; // 32 bits is needed for calculation 1417 | 1418 | delta = gyroData[axis] - lastGyro[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 1419 | lastGyro[axis] = gyroData[axis]; 1420 | deltaSum = delta1[axis]+delta2[axis]+delta; 1421 | delta2[axis] = delta1[axis]; 1422 | delta1[axis] = delta; 1423 | 1424 | DTerm = ((int32_t)deltaSum*dynD8[axis])>>5; // 32 bits is needed for calculation 1425 | 1426 | axisPID[axis] = PTerm + ITerm - DTerm; 1427 | } 1428 | 1429 | mixTable(); 1430 | writeServos(); 1431 | writeMotors(); 1432 | } 1433 | 1434 | --------------------------------------------------------------------------------