// evaluate all other incoming serial data
void evaluateOtherData(uint8_t sr) {
#ifndef SUPPRESS_OTHER_SERIAL_COMMANDS
switch (sr) {
// Note: we may receive weird characters here which could trigger unwanted features during flight.
// this could lead to a crash easily.
// Please use if (!f.ARMED) where neccessary
#ifdef LCD_CONF
case 's':
case 'S':
if (!f.ARMED) configurationLoop();
break;
#endif
#ifdef LOG_PERMANENT_SHOW_AT_L
case 'L':
if (!f.ARMED) dumpPLog(1);
break;
#endif
#if defined(LCD_TELEMETRY) && defined(LCD_TEXTSTAR)
case 'A': // button A press
toggle_telemetry(1);
break;
case 'B': // button B press
toggle_telemetry(2);
break;
case 'C': // button C press
toggle_telemetry(3);
break;
case 'D': // button D press
toggle_telemetry(4);
break;
case 'a': // button A release
case 'b': // button B release
case 'c': // button C release
case 'd': // button D release
break;
#endif
#ifdef LCD_TELEMETRY
case '0':
case '1':
case '2':
case '3':
case '4':
case '5':
case '6':
case '7':
case '8':
case '9':
#ifndef SUPPRESS_TELEMETRY_PAGE_R
case 'R':
#endif
#if defined(DEBUG) || defined(DEBUG_FREE)
case 'F':
#endif
toggle_telemetry(sr);
break;
#endif // LCD_TELEMETRY
}
#endif // SUPPRESS_OTHER_SERIAL_COMMANDS
}
/*******************************************************************************
** Main Function main()
*******************************************************************************/
int main (void)
{
/* Basic chip initialization is taken care of in SystemInit() called
* from the startup code. SystemInit() and chip settings are defined
* in the CMSIS system_<part family>.c file.
*/
setup();
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
uint8_t axis,i;
int16_t delta,deltaSum;
int16_t PTerm,ITerm,DTerm;
static int16_t lastGyro[3] = {0,0,0};
static int16_t delta1[3],delta2[3];
static int16_t errorAngleI[2] = {0,0};
static uint32_t rcTime = 0;
static int16_t initialThrottleHold;
int16_t error,errorAngle =0;
#if PIDcontroller ==1
static int16_t errorGyroI[3] = {0,0,0};
#elif PIDcontroller == 2
int32_t AngleRateTmp = 0, RateError = 0, AngleITerm = 0;
static int32_t errorGyroI[3] = {0,0,0};
#endif
#define RC_FREQ 50
while(1)
{
#ifdef LCD_TELEMETRY_STEP
static char telemetryStepSequence [] = LCD_TELEMETRY_STEP;
static uint8_t telemetryStepIndex = 0;
#endif
#if defined(SPEKTRUM)
if (rcFrameComplete) computeRC();
#endif
#if defined(OPENLRSv2MULTI)
Read_OpenLRS_RC();
#endif
if ((currentTime > rcTime) || rcTime > (currentTime + 20000)) { // 50Hz + additional check to ensure rcTime can reach currentTime
rcTime = currentTime + 20000;
computeRC();
//reset watchdog timer in RC loop to ensure user is not locked out of control
feedWatchDog();
// Failsafe routine - added by MIS
#if defined(FAILSAFE)
if ( failsafeCnt > (5*FAILSAVE_DELAY) && f.ARMED) { // Stabilize, and set Throttle to specified level
for(i=0; i<3; i++) rcData[i] = MIDRC; // after specified guard time after RC signal is lost (in 0.1sec)
rcData[THROTTLE] = FAILSAVE_THROTTLE;
if (failsafeCnt > 5*(FAILSAVE_DELAY+FAILSAVE_OFF_DELAY)) { // Turn OFF motors after specified Time (in 0.1sec)
f.ARMED = 0; // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
}
failsafeEvents++;
}
if ( failsafeCnt > (5*FAILSAVE_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
f.ARMED = 0; // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
}
failsafeCnt++;
#endif
// end of failsave routine - next change is made with RcOptions setting
if (rcData[THROTTLE] < MINCHECK) {
errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0; errorGyroI[YAW] = 0;
errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
rcDelayCommand++;
if (rcData[YAW] < MINCHECK && rcData[PITCH] < MINCHECK && !f.ARMED) {
if (rcDelayCommand == 20) {
calibratingG=400;
#if GPS
GPS_reset_home_position();
#endif
}
} else if (rcData[YAW] > MAXCHECK && rcData[PITCH] > MAXCHECK && !f.ARMED) {
if (rcDelayCommand == 20) {
#ifdef TRI
servo[5] = 1500; // we center the yaw servo in conf mode
writeServos();
#endif
#ifdef FLYING_WING
servo[0] = conf.wing_left_mid;
servo[1] = conf.wing_right_mid;
writeServos();
#endif
#ifdef AIRPLANE
for(i = 4; i<7 ;i++) servo[i] = 1500;
writeServos();
#endif
#if defined(LCD_CONF)
configurationLoop(); // beginning LCD configuration
#endif
previousTime = micros();
}
}
#if defined(INFLIGHT_ACC_CALIBRATION)
else if (!f.ARMED && rcData[YAW] < MINCHECK && rcData[PITCH] > MAXCHECK && rcData[ROLL] > MAXCHECK){
if (rcDelayCommand == 20){
if (AccInflightCalibrationMeasurementDone){ // trigger saving into eeprom after landing
AccInflightCalibrationMeasurementDone = 0;
AccInflightCalibrationSavetoEEProm = 1;
}else{
AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
#if defined(BUZZER)
if (AccInflightCalibrationArmed){
toggleBeep = 2;
} else {
toggleBeep = 3;
}
#endif
}
}
}
#endif
else if (conf.activate[BOXARM] > 0) {
if ( rcOptions[BOXARM] && f.OK_TO_ARM
#if defined(FAILSAFE)
&& failsafeCnt <= 1
#endif
) {
f.ARMED = 1;
headFreeModeHold = heading;
} else if (f.ARMED) f.ARMED = 0;
rcDelayCommand = 0;
#ifdef ALLOW_ARM_DISARM_VIA_TX_YAW
} else if ( (rcData[YAW] < MINCHECK ) && f.ARMED) {
if (rcDelayCommand == 20) f.ARMED = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged
} else if ( (rcData[YAW] > MAXCHECK ) && rcData[PITCH] < MAXCHECK && !f.ARMED && calibratingG == 0 && f.ACC_CALIBRATED) {
if (rcDelayCommand == 20) {
f.ARMED = 1;
headFreeModeHold = heading;
}
#endif
#ifdef ALLOW_ARM_DISARM_VIA_TX_ROLL
} else if ( (rcData[ROLL] < MINCHECK) && f.ARMED) {
if (rcDelayCommand == 20) f.ARMED = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged
} else if ( (rcData[ROLL] > MAXCHECK) && rcData[PITCH] < MAXCHECK && !f.ARMED && calibratingG == 0 && f.ACC_CALIBRATED) {
if (rcDelayCommand == 20) {
f.ARMED = 1;
headFreeModeHold = heading;
}
#endif
#ifdef LCD_TELEMETRY_AUTO
} else if (rcData[ROLL] < MINCHECK && rcData[PITCH] > MAXCHECK && !f.ARMED) {
if (rcDelayCommand == 20) {
if (telemetry_auto) {
telemetry_auto = 0;
telemetry = 0;
} else
telemetry_auto = 1;
}
#endif
#ifdef LCD_TELEMETRY_STEP
} else if (rcData[ROLL] > MAXCHECK && rcData[PITCH] > MAXCHECK && !f.ARMED) {
if (rcDelayCommand == 20) {
telemetry = telemetryStepSequence[++telemetryStepIndex % strlen(telemetryStepSequence)];
LCDclear(); // make sure to clear away remnants
}
#endif
} else
rcDelayCommand = 0;
} else if (rcData[THROTTLE] > MAXCHECK && !f.ARMED) {
if (rcData[YAW] < MINCHECK && rcData[PITCH] < MINCHECK) { // throttle=max, yaw=left, pitch=min
if (rcDelayCommand == 20) calibratingA=400;
rcDelayCommand++;
} else if (rcData[YAW] > MAXCHECK && rcData[PITCH] < MINCHECK) { // throttle=max, yaw=right, pitch=min
if (rcDelayCommand == 20) f.CALIBRATE_MAG = 1; // MAG calibration request
rcDelayCommand++;
} else if (rcData[PITCH] > MAXCHECK) {
conf.angleTrim[PITCH]+=2;writeParams(1);
#if defined(LED_RING)
blinkLedRing();
#endif
} else if (rcData[PITCH] < MINCHECK) {
conf.angleTrim[PITCH]-=2;writeParams(1);
#if defined(LED_RING)
blinkLedRing();
#endif
} else if (rcData[ROLL] > MAXCHECK) {
conf.angleTrim[ROLL]+=2;writeParams(1);
#if defined(LED_RING)
blinkLedRing();
#endif
} else if (rcData[ROLL] < MINCHECK) {
conf.angleTrim[ROLL]-=2;writeParams(1);
#if defined(LED_RING)
blinkLedRing();
#endif
} else {
rcDelayCommand = 0;
}
}
#if defined(LED_FLASHER)
led_flasher_autoselect_sequence();
#endif
#if defined(INFLIGHT_ACC_CALIBRATION)
if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > MINCHECK && !rcOptions[BOXARM] ){ // Copter is airborne and you are turning it off via boxarm : start measurement
InflightcalibratingA = 50;
AccInflightCalibrationArmed = 0;
}
if (rcOptions[BOXPASSTHRU]) { // Use the Passthru Option to activate : Passthru = TRUE Meausrement started, Land and passtrhu = 0 measurement stored
if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone){
InflightcalibratingA = 50;
}
}else if(AccInflightCalibrationMeasurementDone && !f.ARMED){
AccInflightCalibrationMeasurementDone = 0;
AccInflightCalibrationSavetoEEProm = 1;
}
#endif
uint16_t auxState = 0;
for(i=0;i<4;i++)
auxState |= (rcData[AUX1+i]<1300)<<(3*i) | (1300<rcData[AUX1+i] && rcData[AUX1+i]<1700)<<(3*i+1) | (rcData[AUX1+i]>1700)<<(3*i+2);
for(i=0;i<CHECKBOXITEMS;i++)
rcOptions[i] = (auxState & conf.activate[i])>0;
// note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false
if (( rcOptions[BOXACC] || (failsafeCnt > 5*FAILSAVE_DELAY) ) && ACC ) {
// bumpless transfer to Level mode
if (!f.ACC_MODE) {
errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
f.ACC_MODE = 1;
}
} else {
// failsafe support
f.ACC_MODE = 0;
}
if (rcOptions[BOXARM] == 0) f.OK_TO_ARM = 1;
//if (f.ACC_MODE) {STABLEPIN_ON;} else {STABLEPIN_OFF;}
#if BARO
if (rcOptions[BOXBARO]) {
if (!f.BARO_MODE) {
f.BARO_MODE = 1;
AltHold = EstAlt;
initialThrottleHold = rcCommand[THROTTLE];
errorAltitudeI = 0;
BaroPID=0;
}
} else {
f.BARO_MODE = 0;
}
#endif
#if MAG
if (rcOptions[BOXMAG]) {
if (!f.MAG_MODE) {
f.MAG_MODE = 1;
magHold = heading;
}
} else {
f.MAG_MODE = 0;
}
if (rcOptions[BOXHEADFREE]) {
if (!f.HEADFREE_MODE) {
f.HEADFREE_MODE = 1;
}
} else {
f.HEADFREE_MODE = 0;
}
if (rcOptions[BOXHEADADJ]) {
headFreeModeHold = heading; // acquire new heading
}
#endif
#if GPS
#if defined(I2C_NAV)
static uint8_t GPSNavReset = 1;
if (f.GPS_FIX && GPS_numSat >= 5 ) {
if (!rcOptions[BOXGPSHOME] && !rcOptions[BOXGPSHOLD] )
{ //Both boxes are unselected
if (GPSNavReset == 0 ) {
GPSNavReset = 1;
GPS_reset_nav();
}
}
if (rcOptions[BOXGPSHOME]) {
if (!f.GPS_HOME_MODE) {
f.GPS_HOME_MODE = 1;
GPSNavReset = 0;
GPS_I2C_command(I2C_GPS_COMMAND_START_NAV,0); //waypoint zero
}
} else {
f.GPS_HOME_MODE = 0;
}
if (rcOptions[BOXGPSHOLD]) {
if (!f.GPS_HOLD_MODE & !f.GPS_HOME_MODE) {
f.GPS_HOLD_MODE = 1;
GPSNavReset = 0;
GPS_I2C_command(I2C_GPS_COMMAND_POSHOLD,0);
}
} else {
f.GPS_HOLD_MODE = 0;
}
}
#endif
#if defined(GPS_SERIAL) || defined(TINY_GPS) || defined(GPS_FROM_OSD)||defined(I2C_GPS)
if (f.GPS_FIX && GPS_numSat >= 5 ) {
if (rcOptions[BOXGPSHOME]) {
if (!f.GPS_HOME_MODE) {
f.GPS_HOME_MODE = 1;
GPS_set_next_wp(&GPS_home[LAT],&GPS_home[LON]);
nav_mode = NAV_MODE_WP;
}
} else {
f.GPS_HOME_MODE = 0;
}
if (rcOptions[BOXGPSHOLD]) {
if (!f.GPS_HOLD_MODE) {
f.GPS_HOLD_MODE = 1;
GPS_hold[LAT] = GPS_coord[LAT];
GPS_hold[LON] = GPS_coord[LON];
GPS_set_next_wp(&GPS_hold[LAT],&GPS_hold[LON]);
nav_mode = NAV_MODE_POSHOLD;
}
} else {
f.GPS_HOLD_MODE = 0;
}
}
#endif
#endif
#ifdef FLOW
if(rcOptions[BOXFLOWHOLD])
{
if (!f.FLOW_HOLD_MODE)
{
f.FLOW_HOLD_MODE = 1;
GPS_hold[LAT] = GPS_coord[LAT];
GPS_hold[LON] = GPS_coord[LON];
flow_set_next_wp(&GPS_hold[LAT],&GPS_hold[LON]);
nav_mode = NAV_MODE_POSHOLD;
}
//debug[1] = 1;
}
else
{
f.FLOW_HOLD_MODE = 0;
//debug[1] = 0;
}
#endif
if (rcOptions[BOXPASSTHRU]) {f.PASSTHRU_MODE = 1;}
else {f.PASSTHRU_MODE = 0;}
#ifdef FIXEDWING
f.HEADFREE_MODE = 0;
#endif
} else { // not in rc loop
static uint8_t taskOrder=0; // never call all functions in the same loop, to avoid high delay spikes
if(taskOrder>4) taskOrder-=5;
switch (taskOrder) {
case 0:
taskOrder++;
#if MAG
if (Mag_getADC()) break; // max 350 µs (HMC5883) // only break when we actually did something
#endif
case 1:
taskOrder++;
#if BARO
if (Baro_update() != 0 ) break;
#endif
case 2:
taskOrder++;
#if BARO
if (getEstimatedAltitude() !=0) break;
#endif
case 3:
taskOrder++;
if(flowUpdate() !=0) break;
#if GPS
if(GPS_Enable) GPS_NewData();
break;
#endif
case 4:
taskOrder++;
#if SONAR
#if !defined(MAXSONAR_PWM) && !defined(FLOW)
Sonar_update();
#endif
//debug[2] = sonarAlt;
#endif
#ifdef LANDING_LIGHTS_DDR
auto_switch_landing_lights();
#endif
#ifdef VARIOMETER
if (f.VARIO_MODE) vario_signaling();
#endif
break;
}
}
computeIMU();
// Measure loop rate just afer reading the sensors
currentTime = micros();
cycleTime = currentTime - previousTime;
previousTime = currentTime;
//delayMs(500);
//if(cycleTime > 3150)
// debug[0]++;
#if MAG
if (fabs(rcCommand[YAW]) <70 && f.MAG_MODE) {
int16_t dif = heading - magHold;
if (dif <= - 180) dif += 360;
if (dif >= + 180) dif -= 360;
if ( f.SMALL_ANGLES_25 ) rcCommand[YAW] -= dif*conf.P8[PIDMAG]/30; // 18 deg
} else magHold = heading;
#endif
#if BARO
if (f.BARO_MODE) {
if (fabs(rcCommand[THROTTLE]-initialThrottleHold)>ALT_HOLD_THROTTLE_NEUTRAL_ZONE) {
f.BARO_MODE = 0; // so that a new althold reference is defined
}
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
}
#endif
#if GPS
if ( (!f.GPS_HOME_MODE && !f.GPS_HOLD_MODE) || !f.GPS_FIX_HOME ) {
GPS_reset_nav(); // If GPS is not activated. Reset nav loops and all nav related parameters
GPS_angle[ROLL] = 0;
GPS_angle[PITCH] = 0;
} else {
float sin_yaw_y = sinf(heading*0.0174532925f);
float cos_yaw_x = cosf(heading*0.0174532925f);
#if defined(NAV_SLEW_RATE)
nav_rated[LON] += constrain(wrap_18000(nav[LON]-nav_rated[LON]),-NAV_SLEW_RATE,NAV_SLEW_RATE);
nav_rated[LAT] += constrain(wrap_18000(nav[LAT]-nav_rated[LAT]),-NAV_SLEW_RATE,NAV_SLEW_RATE);
GPS_angle[ROLL] = (nav_rated[LON]*cos_yaw_x - nav_rated[LAT]*sin_yaw_y) /10;
GPS_angle[PITCH] = (nav_rated[LON]*sin_yaw_y + nav_rated[LAT]*cos_yaw_x) /10;
#else
GPS_angle[ROLL] = (nav[LON]*cos_yaw_x - nav[LAT]*sin_yaw_y) /10;
GPS_angle[PITCH] = (nav[LON]*sin_yaw_y + nav[LAT]*cos_yaw_x) /10;
#endif
}
//debug[2] = GPS_angle[ROLL];
//debug[3] = GPS_angle[PITCH];
#endif
#ifdef FLOW
if (!f.FLOW_HOLD_MODE) {
flow_reset_nav(); // If GPS is not activated. Reset nav loops and all nav related parameters
GPS_angle[ROLL] = 0;
GPS_angle[PITCH] = 0;
} else {
float sin_yaw_y = sinf(heading*0.0174532925f);
float cos_yaw_x = cosf(heading*0.0174532925f);
#if defined(NAV_SLEW_RATE)
nav_rated[LON] += constrain(wrap_18000(nav[LON]-nav_rated[LON]),-NAV_SLEW_RATE,NAV_SLEW_RATE);
nav_rated[LAT] += constrain(wrap_18000(nav[LAT]-nav_rated[LAT]),-NAV_SLEW_RATE,NAV_SLEW_RATE);
GPS_angle[ROLL] = (nav_rated[LON]*cos_yaw_x - nav_rated[LAT]*sin_yaw_y) /10;
GPS_angle[PITCH] = (nav_rated[LON]*sin_yaw_y + nav_rated[LAT]*cos_yaw_x) /10;
#else
GPS_angle[ROLL] = (nav[LON]*cos_yaw_x - nav[LAT]*sin_yaw_y) /10;
GPS_angle[PITCH] = (nav[LON]*sin_yaw_y + nav[LAT]*cos_yaw_x) /10;
#endif
}
//debug[2] = GPS_angle[ROLL];
//debug[3] = GPS_angle[PITCH];
#endif
#if PIDcontroller == 1
//**** PITCH & ROLL & YAW PID ****
for(axis=0;axis<3;axis++) {
if (f.ACC_MODE && axis<2 ) { //LEVEL MODE
// 50 degrees max inclination
errorAngle = constrain(2*rcCommand[axis] + GPS_angle[axis],-500,+500) - angle[axis] + conf.angleTrim[axis]; //16 bits is ok here
#ifdef LEVEL_PDF
PTerm = -(int32_t)angle[axis]*conf.P8[PIDLEVEL]/100 ;
#else
PTerm = (int32_t)errorAngle*conf.P8[PIDLEVEL]/100 ; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
#endif
PTerm = constrain(PTerm,-conf.D8[PIDLEVEL]*5,+conf.D8[PIDLEVEL]*5);
errorAngleI[axis] = constrain(errorAngleI[axis]+errorAngle,-10000,+10000); // WindUp //16 bits is ok here
ITerm = ((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
} else { //ACRO MODE or YAW axis
if (fabs(rcCommand[axis])<350) error = rcCommand[axis]*10*8/conf.P8[axis] ; // 16 bits is needed for calculation: 350*10*8 = 28000 16 bits is ok for result if P8>2 (P>0.2)
else error = (int32_t)rcCommand[axis]*10*8/conf.P8[axis] ; // 32 bits is needed for calculation: 500*5*10*8 = 200000 16 bits is ok for result if P8>2 (P>0.2)
error -= gyroData[axis];
PTerm = rcCommand[axis];
errorGyroI[axis] = constrain(errorGyroI[axis]+error,-16000,+16000); // WindUp 16 bits is ok here
if (fabs(gyroData[axis])>640) errorGyroI[axis] = 0;
ITerm = (errorGyroI[axis]/125*conf.I8[axis])>>6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
}
if (fabs(gyroData[axis])<160) PTerm -= gyroData[axis]*dynP8[axis]/10/8; // 16 bits is needed for calculation 160*200 = 32000 16 bits is ok for result
else PTerm -= (int32_t)gyroData[axis]*dynP8[axis]/10/8; // 32 bits is needed for calculation
delta = gyroData[axis] - lastGyro[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyro[axis] = gyroData[axis];
deltaSum = delta1[axis]+delta2[axis]+delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
if (fabs(deltaSum)<640) DTerm = (deltaSum*dynD8[axis])>>5; // 16 bits is needed for calculation 640*50 = 32000 16 bits is ok for result
else DTerm = ((int32_t)deltaSum*dynD8[axis])>>5; // 32 bits is needed for calculation
// evaluate all other incoming serial data
void evaluateOtherData(uint8_t sr) {
#ifndef SUPPRESS_OTHER_SERIAL_COMMANDS
#if GPS
#if !defined(I2C_GPS)
// on the GPS port, we must avoid interpreting incoming values for other commands because there is no
// protocol protection as is with MSP commands
// doing so with single chars would be prone to error.
if (CURRENTPORT == GPS_SERIAL) return;
#endif
#endif
switch (sr) {
// Note: we may receive weird characters here which could trigger unwanted features during flight.
// this could lead to a crash easily.
// Please use if (!f.ARMED) where neccessary
#ifdef LCD_CONF
case 's':
case 'S':
if (!f.ARMED) configurationLoop();
break;
#endif
#ifdef LOG_PERMANENT_SHOW_AT_L
case 'L':
if (!f.ARMED) dumpPLog(1);
break;
#endif
#if defined(LCD_TELEMETRY) && defined(LCD_TEXTSTAR)
case 'A': // button A press
toggle_telemetry(1);
break;
case 'B': // button B press
toggle_telemetry(2);
break;
case 'C': // button C press
toggle_telemetry(3);
break;
case 'D': // button D press
toggle_telemetry(4);
break;
case 'a': // button A release
case 'b': // button B release
case 'c': // button C release
case 'd': // button D release
break;
#endif
#ifdef LCD_TELEMETRY
case '0':
case '1':
case '2':
case '3':
case '4':
case '5':
case '6':
case '7':
case '8':
case '9':
#ifndef SUPPRESS_TELEMETRY_PAGE_R
case 'R':
#endif
#if defined(DEBUG) || defined(DEBUG_FREE)
case 'F':
#endif
toggle_telemetry(sr);
break;
#endif // LCD_TELEMETRY
}
#endif // SUPPRESS_OTHER_SERIAL_COMMANDS
}
void setup() {
#if !defined(GPS_PROMINI)
UARTInit(115200);
#endif
LEDPIN_PINMODE;
SHIELDLED_PINMODE;
//POWERPIN_PINMODE;
//BUZZERPIN_PINMODE;
//STABLEPIN_PINMODE;
//POWERPIN_OFF;
/* Initialize GPIO (sets up clock) */
GPIOInit();
init_microsec();
enable_microsec();
init_timer16PWM();
enable_PWMtimer();
/******** special version of MultiWii to calibrate all attached ESCs ************/
#if defined(ESC_CALIB_CANNOT_FLY)
writeAllMotors(ESC_CALIB_HIGH);
delayMs(3000);
writeAllMotors(ESC_CALIB_LOW);
delayMs(500);
while (1) {
delayMs(5000);
blinkLED(2,20, 2);
}
// statement never reached
#endif
writeAllMotors(MINCOMMAND);
delayMs(300);
readEEPROM();
checkFirstTime();
configureReceiver();
#if defined(OPENLRSv2MULTI)
initOpenLRS();
#endif
initSensors();
#if defined(I2C_GPS) || defined(GPS_SERIAL) || defined(GPS_FROM_OSD)||defined(I2C_NAV)
GPS_set_pids();
#endif
previousTime = micros();
#if defined(GIMBAL)
calibratingA = 400;
#endif
calibratingG = 400;
calibratingB = 200; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
#if defined(POWERMETER)
for(uint8_t i=0;i<=PMOTOR_SUM;i++)
pMeter[i]=0;
#endif
#if defined(ARMEDTIMEWARNING)
ArmedTimeWarningMicroSeconds = (ARMEDTIMEWARNING *1000000);
#endif
/************************************/
#if defined(GPS_SERIAL)
SerialOpen(GPS_SERIAL,GPS_BAUD);
delay(400);
for(uint8_t i=0;i<=5;i++){
GPS_NewData();
LEDPIN_ON
delay(20);
LEDPIN_OFF
delay(80);
}
if(!GPS_Present){
SerialEnd(GPS_SERIAL);
SerialOpen(0,SERIAL_COM_SPEED);
}
#if !defined(GPS_PROMINI)
GPS_Present = 1;
#endif
GPS_Enable = GPS_Present;
#endif
/************************************/
#if defined(I2C_GPS) || defined(TINY_GPS) || defined(GPS_FROM_OSD)|| defined(I2C_NAV)
GPS_Enable = 1;
#endif
#if defined(LCD_ETPP) || defined(LCD_LCD03) || defined(OLED_I2C_128x64)
initLCD();
#endif
#ifdef LCD_TELEMETRY_DEBUG
telemetry_auto = 1;
#endif
#ifdef LCD_CONF_DEBUG
configurationLoop();
#endif
#ifdef LANDING_LIGHTS_DDR
init_landing_lights();
#endif
#if defined(LED_FLASHER)
init_led_flasher();
led_flasher_set_sequence(LED_FLASHER_SEQUENCE);
#endif
f.SMALL_ANGLES_25=1; // important for gyro only conf
//initialise median filter structures
#ifdef MEDFILTER
#ifdef SONAR
initMedianFilter(&SonarFilter, 5);
#endif
#endif
initWatchDog();
}