예제 #1
0
파일: AP_GPS.c 프로젝트: BobLiu20/AirPilot
static void Set_nextwp(void)				 //返回航点完成标志位,完成为1
{
	
	if(Waypoint[wp][0]!= 0)		   
	{
		GPS_set_next_wp(&Waypoint[wp][LAT], &Waypoint[wp][LON]);
		wp++;				//wp总是指向下一个航点  非当前目的航点号
	}
}
예제 #2
0
void updateGpsWaypointsAndMode(void)
{
    static uint8_t GPSNavReset = 1;

    if (STATE(GPS_FIX) && GPS_numSat >= 5) {
        // if both GPS_HOME & GPS_HOLD are checked => GPS_HOME is the priority
        if (rcOptions[BOXGPSHOME]) {
            if (!STATE(GPS_HOME_MODE)) {
                ENABLE_FLIGHT_MODE(GPS_HOME_MODE);
                DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);
                GPSNavReset = 0;
                GPS_set_next_wp(&GPS_home[LAT], &GPS_home[LON]);
                nav_mode = NAV_MODE_WP;
            }
        } else {
            DISABLE_STATE(GPS_HOME_MODE);

            if (rcOptions[BOXGPSHOLD] && areSticksInApModePosition(gpsProfile->ap_mode)) {
                if (!FLIGHT_MODE(GPS_HOLD_MODE)) {
                    ENABLE_STATE(GPS_HOLD_MODE);
                    GPSNavReset = 0;
                    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 {
                DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);
                // both boxes are unselected here, nav is reset if not already done
                if (GPSNavReset == 0) {
                    GPSNavReset = 1;
                    GPS_reset_nav();
                }
            }
        }
    } else {
        DISABLE_FLIGHT_MODE(GPS_HOME_MODE);
        DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);
        nav_mode = NAV_MODE_NONE;
    }
}
void evaluateCommand() {
  uint32_t tmp=0; 

  switch(cmdMSP[CURRENTPORT]) {
   case MSP_SET_RAW_RC:
     s_struct_w((uint8_t*)&rcSerial,16);
     rcSerialCount = 1000; // 1s transition 
     break;
   #if GPS
   case MSP_SET_RAW_GPS:
     f.GPS_FIX = read8();
     GPS_numSat = read8();
     GPS_coord[LAT] = read32();
     GPS_coord[LON] = read32();
     GPS_altitude = read16();
     GPS_speed = read16();
     GPS_update |= 2;              // New data signalisation to GPS functions
     headSerialReply(0);
     break;
   #endif
   case MSP_SET_PID:
     s_struct_w((uint8_t*)&conf.pid[0].P8,3*PIDITEMS);
     break;
   case MSP_SET_BOX:
     s_struct_w((uint8_t*)&conf.activate[0],CHECKBOXITEMS*2);
     break;
   case MSP_SET_RC_TUNING:
     s_struct_w((uint8_t*)&conf.rcRate8,7);
     break;
   #if !defined(DISABLE_SETTINGS_TAB)
   case MSP_SET_MISC:
     struct {
       uint16_t a,b,c,d,e,f;
       uint32_t g;
       uint16_t h;
       uint8_t  i,j,k,l;
     } set_misc;
     s_struct_w((uint8_t*)&set_misc,22);
     #if defined(POWERMETER)
       conf.powerTrigger1 = set_misc.a / PLEVELSCALE;
     #endif
     conf.minthrottle = set_misc.b;
     #ifdef FAILSAFE 
       conf.failsafe_throttle = set_misc.e;
     #endif  
     #if MAG
       conf.mag_declination = set_misc.h;
     #endif
     #if defined(VBAT)
       conf.vbatscale        = set_misc.i;
       conf.vbatlevel_warn1  = set_misc.j;
       conf.vbatlevel_warn2  = set_misc.k;
       conf.vbatlevel_crit   = set_misc.l;
     #endif
     break;
   case MSP_MISC:
     struct {
       uint16_t a,b,c,d,e,f;
       uint32_t g;
       uint16_t h;
       uint8_t  i,j,k,l;
     } misc;
     misc.a = intPowerTrigger1;
     misc.b = conf.minthrottle;
     misc.c = MAXTHROTTLE;
     misc.d = MINCOMMAND;
     #ifdef FAILSAFE 
       misc.e = conf.failsafe_throttle;
     #else  
       misc.e = 0;
     #endif
     #ifdef LOG_PERMANENT
       misc.f = plog.arm;
       misc.g = plog.lifetime + (plog.armed_time / 1000000); // <- computation must be moved outside from serial
     #else
       misc.f = 0; misc.g =0;
     #endif
     #if MAG
       misc.h = conf.mag_declination;
     #else
       misc.h = 0;
     #endif
     #ifdef VBAT
       misc.i = conf.vbatscale;
       misc.j = conf.vbatlevel_warn1;
       misc.k = conf.vbatlevel_warn2;
       misc.l = conf.vbatlevel_crit;
     #else
       misc.i = 0;misc.j = 0;misc.k = 0;misc.l = 0;
     #endif
     s_struct((uint8_t*)&misc,22);
     break;
   #endif
   #if defined (DYNBALANCE)
     case MSP_SET_MOTOR:
       s_struct_w((uint8_t*)&motor,16);
     break;
   #endif
   #ifdef MULTIPLE_CONFIGURATION_PROFILES
   case MSP_SELECT_SETTING:
     if(!f.ARMED) {
       global_conf.currentSet = read8();
       if(global_conf.currentSet>2) global_conf.currentSet = 0;
       writeGlobalSet(0);
       readEEPROM();
     }
     headSerialReply(0);
     break;
   #endif
   case MSP_SET_HEAD:
     s_struct_w((uint8_t*)&magHold,2);
     break;
   case MSP_IDENT:
     struct {
       uint8_t v,t,msp_v;
       uint32_t cap;
     } id;
     id.v     = VERSION;
     id.t     = MULTITYPE;
     id.msp_v = MSP_VERSION;
     id.cap   = capability|DYNBAL<<2|FLAP<<3;
     s_struct((uint8_t*)&id,7);
     break;
   case MSP_STATUS:
     struct {
       uint16_t cycleTime,i2c_errors_count,sensor;
       uint32_t flag;
       uint8_t set;
     } st;
     st.cycleTime        = cycleTime;
     st.i2c_errors_count = i2c_errors_count;
     st.sensor           = ACC|BARO<<1|MAG<<2|GPS<<3|SONAR<<4;
     #if ACC
       if(f.ANGLE_MODE)   tmp |= 1<<BOXANGLE;
       if(f.HORIZON_MODE) tmp |= 1<<BOXHORIZON;
     #endif
     #if BARO && (!defined(SUPPRESS_BARO_ALTHOLD))
       if(f.BARO_MODE) tmp |= 1<<BOXBARO;
     #endif
     #if MAG
       if(f.MAG_MODE) tmp |= 1<<BOXMAG;
       #if !defined(FIXEDWING)
         if(f.HEADFREE_MODE)       tmp |= 1<<BOXHEADFREE;
         if(rcOptions[BOXHEADADJ]) tmp |= 1<<BOXHEADADJ;
       #endif
     #endif
     #if defined(SERVO_TILT) || defined(GIMBAL)|| defined(SERVO_MIX_TILT)
       if(rcOptions[BOXCAMSTAB]) tmp |= 1<<BOXCAMSTAB;
     #endif
     #if defined(CAMTRIG)
       if(rcOptions[BOXCAMTRIG]) tmp |= 1<<BOXCAMTRIG;
     #endif
     #if GPS
       if(f.GPS_HOME_MODE) tmp |= 1<<BOXGPSHOME; 
       if(f.GPS_HOLD_MODE) tmp |= 1<<BOXGPSHOLD;
     #endif
     #if defined(FIXEDWING) || defined(HELICOPTER)
       if(f.PASSTHRU_MODE) tmp |= 1<<BOXPASSTHRU;
     #endif
     #if defined(BUZZER)
       if(rcOptions[BOXBEEPERON]) tmp |= 1<<BOXBEEPERON;
     #endif
     #if defined(LED_FLASHER)
       if(rcOptions[BOXLEDMAX]) tmp |= 1<<BOXLEDMAX;
       if(rcOptions[BOXLEDLOW]) tmp |= 1<<BOXLEDLOW;
     #endif
     #if defined(LANDING_LIGHTS_DDR)
       if(rcOptions[BOXLLIGHTS]) tmp |= 1<<BOXLLIGHTS;
     #endif
     #if defined(VARIOMETER)
       if(rcOptions[BOXVARIO]) tmp |= 1<<BOXVARIO;
     #endif
     #if defined(INFLIGHT_ACC_CALIBRATION)
       if(rcOptions[BOXCALIB]) tmp |= 1<<BOXCALIB;
     #endif
     #if defined(GOVERNOR_P)
       if(rcOptions[BOXGOV]) tmp |= 1<<BOXGOV;
     #endif
     #if defined(OSD_SWITCH)
       if(rcOptions[BOXOSD]) tmp |= 1<<BOXOSD;
     #endif
     if(f.ARMED) tmp |= 1<<BOXARM;
     st.flag             = tmp;
     st.set              = global_conf.currentSet;
     s_struct((uint8_t*)&st,11);
     break;
   case MSP_RAW_IMU:
     #if defined(DYNBALANCE)
       for(uint8_t axis=0;axis<3;axis++) {imu.gyroData[axis]=imu.gyroADC[axis];imu.accSmooth[axis]= imu.accADC[axis];} // Send the unfiltered Gyro & Acc values to gui.
     #endif 
     s_struct((uint8_t*)&imu,18);
     break;
   case MSP_SERVO:
     s_struct((uint8_t*)&servo,16);
     break;
   case MSP_SERVO_CONF:
     s_struct((uint8_t*)&conf.servoConf[0].min,56); // struct servo_conf_ is 7 bytes length: min:2 / max:2 / middle:2 / rate:1    ----     8 servo =>  8x7 = 56
     break;
   case MSP_SET_SERVO_CONF:
     s_struct_w((uint8_t*)&conf.servoConf[0].min,56);
     break;
   case MSP_MOTOR:
     s_struct((uint8_t*)&motor,16);
     break;
   case MSP_RC:
     s_struct((uint8_t*)&rcData,RC_CHANS*2);
     break;
   #if GPS
   case MSP_RAW_GPS:
     headSerialReply(16);
     serialize8(f.GPS_FIX);
     serialize8(GPS_numSat);
     serialize32(GPS_coord[LAT]);
     serialize32(GPS_coord[LON]);
     serialize16(GPS_altitude);
     serialize16(GPS_speed);
     serialize16(GPS_ground_course);        // added since r1172
     break;
   case MSP_COMP_GPS:
     headSerialReply(5);
     serialize16(GPS_distanceToHome);
     serialize16(GPS_directionToHome);
     serialize8(GPS_update & 1);
     break;
   #endif
   case MSP_ATTITUDE:
     s_struct((uint8_t*)&att,6);
     break;
   case MSP_ALTITUDE:
     s_struct((uint8_t*)&alt,6);
     break;
   case MSP_ANALOG:
     s_struct((uint8_t*)&analog,7);
     break;
   case MSP_RC_TUNING:
     s_struct((uint8_t*)&conf.rcRate8,7);
     break;
   case MSP_PID:
     s_struct((uint8_t*)&conf.pid[0].P8,3*PIDITEMS);
     break;
   case MSP_PIDNAMES:
     serializeNames(pidnames);
     break;
   case MSP_BOX:
     s_struct((uint8_t*)&conf.activate[0],2*CHECKBOXITEMS);
     break;
   case MSP_BOXNAMES:
     serializeNames(boxnames);
     break;
   case MSP_BOXIDS:
     headSerialReply(CHECKBOXITEMS);
     for(uint8_t i=0;i<CHECKBOXITEMS;i++) {
       serialize8(pgm_read_byte(&(boxids[i])));
     }
     break;
   case MSP_MOTOR_PINS:
     s_struct((uint8_t*)&PWM_PIN,8);
     break;
   #if defined(USE_MSP_WP)    
   case MSP_WP:
     {
       int32_t lat = 0,lon = 0;
       uint8_t wp_no = read8();        //get the wp number  
       headSerialReply(18);
       if (wp_no == 0) {
         lat = GPS_home[LAT];
         lon = GPS_home[LON];
       } else if (wp_no == 16) {
         lat = GPS_hold[LAT];
         lon = GPS_hold[LON];
       }
       serialize8(wp_no);
       serialize32(lat);
       serialize32(lon);
       serialize32(AltHold);           //altitude (cm) will come here -- temporary implementation to test feature with apps
       serialize16(0);                 //heading  will come here (deg)
       serialize16(0);                 //time to stay (ms) will come here 
       serialize8(0);                  //nav flag will come here
     }
     break;
   case MSP_SET_WP:
     {
       int32_t lat = 0,lon = 0,alt = 0;
       uint8_t wp_no = read8();        //get the wp number
       lat = read32();
       lon = read32();
       alt = read32();                 // to set altitude (cm)
       read16();                       // future: to set heading (deg)
       read16();                       // future: to set time to stay (ms)
       read8();                        // future: to set nav flag
       if (wp_no == 0) {
         GPS_home[LAT] = lat;
         GPS_home[LON] = lon;
         f.GPS_HOME_MODE = 0;          // with this flag, GPS_set_next_wp will be called in the next loop -- OK with SERIAL GPS / OK with I2C GPS
         f.GPS_FIX_HOME  = 1;
         if (alt != 0) AltHold = alt;  // temporary implementation to test feature with apps
       } else if (wp_no == 16) {       // OK with SERIAL GPS  --  NOK for I2C GPS / needs more code dev in order to inject GPS coord inside I2C GPS
         GPS_hold[LAT] = lat;
         GPS_hold[LON] = lon;
         if (alt != 0) AltHold = alt;  // temporary implementation to test feature with apps
         #if !defined(I2C_GPS)
           nav_mode      = NAV_MODE_WP;
           GPS_set_next_wp(&GPS_hold[LAT],&GPS_hold[LON]);
         #endif
       }
     }
     headSerialReply(0);
     break;
   #endif
   case MSP_RESET_CONF:
     if(!f.ARMED) LoadDefaults();
     headSerialReply(0);
     break;
   case MSP_ACC_CALIBRATION:
     if(!f.ARMED) calibratingA=512;
     headSerialReply(0);
     break;
   case MSP_MAG_CALIBRATION:
     if(!f.ARMED) f.CALIBRATE_MAG = 1;
     headSerialReply(0);
     break;
   #if defined(SPEK_BIND)
   case MSP_BIND:
     spekBind();  
     headSerialReply(0);
     break;
   #endif
   case MSP_EEPROM_WRITE:
     writeParams(0);
     headSerialReply(0);
     break;
   case MSP_DEBUG:
     s_struct((uint8_t*)&debug,8);
     break;
   #ifdef DEBUGMSG
   case MSP_DEBUGMSG:
     {
       uint8_t size = debugmsg_available();
       if (size > 16) size = 16;
       headSerialReply(size);
       debugmsg_serialize(size);
     }
     break;
   #endif
   default:  // we do not know how to handle the (valid) message, indicate error MSP $M!
     headSerialError(0);
     break;
  }
  tailSerialReply();
}
예제 #4
0
파일: mw.c 프로젝트: trigrass2/tmr
void loop(void)
{
    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 error, errorAngle;
    int16_t delta, deltaSum;
    int16_t PTerm, ITerm, PTermACC, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm;
    static int16_t lastGyro[3] = { 0, 0, 0 };
    static int16_t delta1[3], delta2[3];
    static int16_t errorGyroI[3] = { 0, 0, 0 };
    static int16_t errorAngleI[2] = { 0, 0 };
    static uint32_t rcTime = 0;
    static int16_t initialThrottleHold;
    static uint32_t loopTime;
    uint16_t auxState = 0;
    int16_t prop;

    // this will return false if spektrum is disabled. shrug.
    if (spektrumFrameComplete())
        computeRC();

    if ((int32_t)(currentTime - rcTime) >= 0) { // 50Hz
        rcTime = currentTime + 20000;
        // TODO clean this up. computeRC should handle this check
        if (!feature(FEATURE_SPEKTRUM))
            computeRC();

        // Failsafe routine
        if (feature(FEATURE_FAILSAFE)) {
            if (failsafeCnt > (5 * cfg.failsafe_delay) && f.ARMED) { // Stabilize, and set Throttle to specified level
                for (i = 0; i < 3; i++)
                    rcData[i] = cfg.midrc;      // after specified guard time after RC signal is lost (in 0.1sec)
                rcData[THROTTLE] = cfg.failsafe_throttle;
                if (failsafeCnt > 5 * (cfg.failsafe_delay + cfg.failsafe_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 * cfg.failsafe_delay) && !f.ARMED) {  // Turn off "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++;
        }

        if (rcData[THROTTLE] < cfg.mincheck) {
            errorGyroI[ROLL] = 0;
            errorGyroI[PITCH] = 0;
            errorGyroI[YAW] = 0;
            errorAngleI[ROLL] = 0;
            errorAngleI[PITCH] = 0;
            rcDelayCommand++;
            if (rcData[YAW] < cfg.mincheck && rcData[PITCH] < cfg.mincheck && !f.ARMED) {
                if (rcDelayCommand == 20) {
                    calibratingG = 1000;
                    if (feature(FEATURE_GPS))
                        GPS_reset_home_position();
                }
            } else if (feature(FEATURE_INFLIGHT_ACC_CAL) && (!f.ARMED && rcData[YAW] < cfg.mincheck && rcData[PITCH] > cfg.maxcheck && rcData[ROLL] > cfg.maxcheck)) {
                if (rcDelayCommand == 20) {
                    if (AccInflightCalibrationMeasurementDone) {        // trigger saving into eeprom after landing
                        AccInflightCalibrationMeasurementDone = 0;
                        AccInflightCalibrationSavetoEEProm = 1;
                    } else {
                        AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
                        if (AccInflightCalibrationArmed) {
                            toggleBeep = 2;
                        } else {
                            toggleBeep = 3;
                        }
                    }
                }
            } else if (cfg.activate[BOXARM] > 0) {
                if (rcOptions[BOXARM] && f.OK_TO_ARM) {
                    // TODO: feature(FEATURE_FAILSAFE) && failsafeCnt == 0
                    f.ARMED = 1;
                    headFreeModeHold = heading;
                } else if (f.ARMED)
                    f.ARMED = 0;
                rcDelayCommand = 0;
            } else if ((rcData[YAW] < cfg.mincheck || (cfg.retarded_arm == 1 && rcData[ROLL] < cfg.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] > cfg.maxcheck || (rcData[ROLL] > cfg.maxcheck && cfg.retarded_arm == 1)) && rcData[PITCH] < cfg.maxcheck && !f.ARMED && calibratingG == 0 && f.ACC_CALIBRATED) {
                if (rcDelayCommand == 20) {
                    f.ARMED = 1;
                    headFreeModeHold = heading;
                }
            } else
                rcDelayCommand = 0;
        } else if (rcData[THROTTLE] > cfg.maxcheck && !f.ARMED) {
            if (rcData[YAW] < cfg.mincheck && rcData[PITCH] < cfg.mincheck) {   // throttle=max, yaw=left, pitch=min
                if (rcDelayCommand == 20)
                    calibratingA = 400;
                rcDelayCommand++;
            } else if (rcData[YAW] > cfg.maxcheck && rcData[PITCH] < cfg.mincheck) {    // throttle=max, yaw=right, pitch=min
                if (rcDelayCommand == 20)
                    f.CALIBRATE_MAG = 1;   // MAG calibration request
                rcDelayCommand++;
            } else if (rcData[PITCH] > cfg.maxcheck) {
                cfg.angleTrim[PITCH] += 2;
                writeParams(1);
#ifdef LEDRING
                if (feature(FEATURE_LED_RING))
                    ledringBlink();
#endif
            } else if (rcData[PITCH] < cfg.mincheck) {
                cfg.angleTrim[PITCH] -= 2;
                writeParams(1);
#ifdef LEDRING
                if (feature(FEATURE_LED_RING))
                    ledringBlink();
#endif
            } else if (rcData[ROLL] > cfg.maxcheck) {
                cfg.angleTrim[ROLL] += 2;
                writeParams(1);
#ifdef LEDRING
                if (feature(FEATURE_LED_RING))
                    ledringBlink();
#endif
            } else if (rcData[ROLL] < cfg.mincheck) {
                cfg.angleTrim[ROLL] -= 2;
                writeParams(1);
#ifdef LEDRING
                if (feature(FEATURE_LED_RING))
                    ledringBlink();
#endif
            } else {
                rcDelayCommand = 0;
            }
        }

        if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
            if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > cfg.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;
            }
        }

        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 & cfg.activate[i]) > 0;

        // note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false
        if ((rcOptions[BOXANGLE] || (failsafeCnt > 5 * cfg.failsafe_delay)) && (sensors(SENSOR_ACC))) {
            // bumpless transfer to Level mode
            if (!f.ANGLE_MODE) {
                errorAngleI[ROLL] = 0;
                errorAngleI[PITCH] = 0;
                f.ANGLE_MODE = 1;
            }
        } else {
            f.ANGLE_MODE = 0;        // failsave support
        }

        if (rcOptions[BOXHORIZON]) {
            if (!f.HORIZON_MODE) {
                errorAngleI[ROLL] = 0;
                errorAngleI[PITCH] = 0;
                f.HORIZON_MODE = 1;
            }
        } else {
            f.HORIZON_MODE = 0;
        }

        if ((rcOptions[BOXARM]) == 0)
            f.OK_TO_ARM = 1;
        if (f.ANGLE_MODE || f.HORIZON_MODE) {
            LED1_ON;
        } else {
            LED1_OFF;
        }

#ifdef BARO
        if (sensors(SENSOR_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

#ifdef  MAG
        if (sensors(SENSOR_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 (sensors(SENSOR_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;
                }
            }
        }

        if (rcOptions[BOXPASSTHRU]) {
            f.PASSTHRU_MODE = 1;
        } else {
            f.PASSTHRU_MODE = 0;
        }

        if (cfg.mixerConfiguration == MULTITYPE_FLYING_WING || cfg.mixerConfiguration == MULTITYPE_AIRPLANE) {
            f.HEADFREE_MODE = 0;
        }
    } else {                    // not in rc loop
        static int8_t taskOrder = 0;    // never call all function in the same loop, to avoid high delay spikes
        switch (taskOrder++ % 4) {
        case 0:
#ifdef MAG
            if (sensors(SENSOR_MAG))
                Mag_getADC();
#endif
            break;
        case 1:
#ifdef BARO
            if (sensors(SENSOR_BARO))
                Baro_update();
#endif
            break;
        case 2:
#ifdef BARO
            if (sensors(SENSOR_BARO))
                getEstimatedAltitude();
#endif
            break;
        case 3:
#ifdef SONAR
            if (sensors(SENSOR_SONAR)) {
                Sonar_update();
                debug[2] = sonarAlt;
            }
#endif
            break;
        default:
            taskOrder = 0;
            break;
        }
    }

    currentTime = micros();
    if (cfg.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
        loopTime = currentTime + cfg.looptime;

        computeIMU();
        // Measure loop rate just afer reading the sensors
        currentTime = micros();
        cycleTime = (int32_t)(currentTime - previousTime);
        previousTime = currentTime;
#ifdef MPU6050_DMP
        mpu6050DmpLoop();
#endif

#ifdef MAG
        if (sensors(SENSOR_MAG)) {
            if (abs(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 * cfg.P8[PIDMAG] / 30;    // 18 deg
            } else
                magHold = heading;
        }
#endif

#ifdef BARO
        if (sensors(SENSOR_BARO)) {
            if (f.BARO_MODE) {
                if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) {
                    f.BARO_MODE = 0;   // so that a new althold reference is defined
                }
                rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
            }
        }
#endif

        if (sensors(SENSOR_GPS)) {
            // Check that we really need to navigate ?
            if ((!f.GPS_HOME_MODE && !f.GPS_HOLD_MODE) || !f.GPS_FIX_HOME) {
                // If not. Reset nav loops and all nav related parameters
                GPS_reset_nav();
            } else {
                float sin_yaw_y = sinf(heading * 0.0174532925f);
                float cos_yaw_x = cosf(heading * 0.0174532925f);
                if (cfg.nav_slew_rate) {
                    nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -cfg.nav_slew_rate, cfg.nav_slew_rate); // TODO check this on uint8
                    nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]), -cfg.nav_slew_rate, cfg.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;
                }
            }
        }

        // **** PITCH & ROLL & YAW PID ****
        prop = max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])); // range [0;500]
        for (axis = 0; axis < 3; axis++) {
            if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC
                // 50 degrees max inclination
                errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis];
#ifdef LEVEL_PDF
                PTermACC = -(int32_t)angle[axis] * cfg.P8[PIDLEVEL] / 100;
#else
                PTermACC = (int32_t)errorAngle * cfg.P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768   16 bits is ok for result
#endif
                PTermACC = constrain(PTermACC, -cfg.D8[PIDLEVEL] * 5, +cfg.D8[PIDLEVEL] * 5);

                errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp
                ITermACC = ((int32_t)errorAngleI[axis] * cfg.I8[PIDLEVEL]) >> 12;
            }
            if (!f.ANGLE_MODE || axis == 2) { // MODE relying on GYRO or YAW axis
                error = (int32_t)rcCommand[axis] * 10 * 8 / cfg.P8[axis];
                error -= gyroData[axis];

                PTermGYRO = rcCommand[axis];

                errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
                if (abs(gyroData[axis]) > 640)
                    errorGyroI[axis] = 0;
                ITermGYRO = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6;
            }
예제 #5
0
파일: serial.c 프로젝트: jef79m/baseflight
static void evaluateCommand(void)
{
    uint32_t i, tmp, junk;
    uint8_t wp_no;
    int32_t lat = 0, lon = 0, alt = 0;

    switch (cmdMSP) {
    case MSP_SET_RAW_RC:
        for (i = 0; i < 8; i++)
            rcData[i] = read16();
        headSerialReply(0);
        break;
    case MSP_SET_ACC_TRIM:
        cfg.angleTrim[PITCH] = read16();
        cfg.angleTrim[ROLL]  = read16();
        headSerialReply(0);
        break;
    case MSP_SET_RAW_GPS:
        f.GPS_FIX = read8();
        GPS_numSat = read8();
        GPS_coord[LAT] = read32();
        GPS_coord[LON] = read32();
        GPS_altitude = read16();
        GPS_speed = read16();
        GPS_update |= 2;        // New data signalisation to GPS functions
        headSerialReply(0);
        break;
    case MSP_SET_PID:
        for (i = 0; i < PIDITEMS; i++) {
            cfg.P8[i] = read8();
            cfg.I8[i] = read8();
            cfg.D8[i] = read8();
        }
        headSerialReply(0);
        break;
    case MSP_SET_BOX:
        for (i = 0; i < numberBoxItems; i++)
            cfg.activate[availableBoxes[i]] = read16();
        headSerialReply(0);
        break;
    case MSP_SET_RC_TUNING:
        cfg.rcRate8 = read8();
        cfg.rcExpo8 = read8();
        cfg.rollPitchRate = read8();
        cfg.yawRate = read8();
        cfg.dynThrPID = read8();
        cfg.thrMid8 = read8();
        cfg.thrExpo8 = read8();
        headSerialReply(0);
        break;
    case MSP_SET_MISC:
        read16(); // powerfailmeter
        mcfg.minthrottle = read16();
        read32(); // mcfg.maxthrottle, mcfg.mincommand
        cfg.failsafe_throttle = read16();
        read16();
        read32();
        cfg.mag_declination = read16() * 10;
        mcfg.vbatscale = read8();           // actual vbatscale as intended
        mcfg.vbatmincellvoltage = read8();  // vbatlevel_warn1 in MWC2.3 GUI
        mcfg.vbatmaxcellvoltage = read8();  // vbatlevel_warn2 in MWC2.3 GUI
        read8();                            // vbatlevel_crit (unused)
        headSerialReply(0);
        break;
    case MSP_SET_MOTOR:
        for (i = 0; i < 8; i++)
            motor_disarmed[i] = read16();
        break;
    case MSP_SELECT_SETTING:
        if (!f.ARMED) {
            mcfg.current_profile = read8();
            if (mcfg.current_profile > 2)
                mcfg.current_profile = 0;
            // this writes new profile index and re-reads it
            writeEEPROM(0, false);
        }
        headSerialReply(0);
        break;
    case MSP_SET_HEAD:
        magHold = read16();
        headSerialReply(0);
        break;
    case MSP_IDENT:
        headSerialReply(7);
        serialize8(VERSION);                // multiwii version
        serialize8(mcfg.mixerConfiguration); // type of multicopter
        serialize8(MSP_VERSION);            // MultiWii Serial Protocol Version
        serialize32(CAP_PLATFORM_32BIT | CAP_DYNBALANCE | (mcfg.flaps_speed ? CAP_FLAPS : 0));        // "capability"
        break;
    case MSP_STATUS:
        headSerialReply(11);
        serialize16(cycleTime);
        serialize16(i2cGetErrorCounter());
        serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
        // OK, so you waste all the f*****g time to have BOXNAMES and BOXINDEXES etc, and then you go ahead and serialize enabled shit simply by stuffing all
        // the bits in order, instead of setting the enabled bits based on BOXINDEX. WHERE IS THE F*****G LOGIC IN THIS, FUCKWADS.
        // Serialize the boxes in the order we delivered them, until multiwii retards fix their shit
        junk = 0;
        tmp = f.ANGLE_MODE << BOXANGLE | f.HORIZON_MODE << BOXHORIZON |
                    f.BARO_MODE << BOXBARO | f.MAG_MODE << BOXMAG | f.HEADFREE_MODE << BOXHEADFREE | rcOptions[BOXHEADADJ] << BOXHEADADJ |
                    rcOptions[BOXCAMSTAB] << BOXCAMSTAB | rcOptions[BOXCAMTRIG] << BOXCAMTRIG |
                    f.GPS_HOME_MODE << BOXGPSHOME | f.GPS_HOLD_MODE << BOXGPSHOLD |
                    f.PASSTHRU_MODE << BOXPASSTHRU |
                    rcOptions[BOXBEEPERON] << BOXBEEPERON |
                    rcOptions[BOXLEDMAX] << BOXLEDMAX |
                    rcOptions[BOXLLIGHTS] << BOXLLIGHTS |
                    rcOptions[BOXVARIO] << BOXVARIO |
                    rcOptions[BOXCALIB] << BOXCALIB |
                    rcOptions[BOXGOV] << BOXGOV |
                    rcOptions[BOXOSD] << BOXOSD |
                    f.ARMED << BOXARM;
        for (i = 0; i < numberBoxItems; i++) {
            int flag = (tmp & (1 << availableBoxes[i]));
            if (flag)
                junk |= 1 << i;
        }
        serialize32(junk);
        serialize8(mcfg.current_profile);
        break;
    case MSP_RAW_IMU:
        headSerialReply(18);
        // Retarded hack until multiwiidorks start using real units for sensor data
        if (acc_1G > 1024) {
            for (i = 0; i < 3; i++)
                serialize16(accSmooth[i] / 8);
        } else {
            for (i = 0; i < 3; i++)
                serialize16(accSmooth[i]);
        }
        for (i = 0; i < 3; i++)
            serialize16(gyroData[i]);
        for (i = 0; i < 3; i++)
            serialize16(magADC[i]);
        break;
    case MSP_SERVO:
        s_struct((uint8_t *)&servo, 16);
        break;
    case MSP_SERVO_CONF:
        headSerialReply(56);
        for (i = 0; i < MAX_SERVOS; i++) {
            serialize16(cfg.servoConf[i].min);
            serialize16(cfg.servoConf[i].max);
            serialize16(cfg.servoConf[i].middle);
            serialize8(cfg.servoConf[i].rate);
        }
        break;
    case MSP_SET_SERVO_CONF:
        headSerialReply(0);
        for (i = 0; i < MAX_SERVOS; i++) {
            cfg.servoConf[i].min = read16();
            cfg.servoConf[i].max = read16();
            cfg.servoConf[i].middle = read16();
            cfg.servoConf[i].rate = read8();
        }
        break;
    case MSP_MOTOR:
        s_struct((uint8_t *)motor, 16);
        break;
    case MSP_RC:
        headSerialReply(16);
        for (i = 0; i < 8; i++)
            serialize16(rcData[i]);
        break;
    case MSP_RAW_GPS:
        headSerialReply(16);
        serialize8(f.GPS_FIX);
        serialize8(GPS_numSat);
        serialize32(GPS_coord[LAT]);
        serialize32(GPS_coord[LON]);
        serialize16(GPS_altitude);
        serialize16(GPS_speed);
        serialize16(GPS_ground_course);
        break;
    case MSP_COMP_GPS:
        headSerialReply(5);
        serialize16(GPS_distanceToHome);
        serialize16(GPS_directionToHome);
        serialize8(GPS_update & 1);
        break;
    case MSP_ATTITUDE:
        headSerialReply(8);
        for (i = 0; i < 2; i++)
            serialize16(angle[i]);
        serialize16(heading);
        serialize16(headFreeModeHold);
        break;
    case MSP_ALTITUDE:
        headSerialReply(6);
        serialize32(EstAlt);
        serialize16(vario);
        break;
    case MSP_ANALOG:
        headSerialReply(5);
        serialize8(vbat);
        serialize16(0); // power meter trash
        serialize16(rssi);
        break;
    case MSP_RC_TUNING:
        headSerialReply(7);
        serialize8(cfg.rcRate8);
        serialize8(cfg.rcExpo8);
        serialize8(cfg.rollPitchRate);
        serialize8(cfg.yawRate);
        serialize8(cfg.dynThrPID);
        serialize8(cfg.thrMid8);
        serialize8(cfg.thrExpo8);
        break;
    case MSP_PID:
        headSerialReply(3 * PIDITEMS);
        for (i = 0; i < PIDITEMS; i++) {
            serialize8(cfg.P8[i]);
            serialize8(cfg.I8[i]);
            serialize8(cfg.D8[i]);
        }
        break;
    case MSP_PIDNAMES:
        headSerialReply(sizeof(pidnames) - 1);
        serializeNames(pidnames);
        break;
    case MSP_BOX:
        headSerialReply(2 * numberBoxItems);
        for (i = 0; i < numberBoxItems; i++)
            serialize16(cfg.activate[availableBoxes[i]]);
        break;
    case MSP_BOXNAMES:
        // headSerialReply(sizeof(boxnames) - 1);
        serializeBoxNamesReply();
        break;
    case MSP_BOXIDS:
        headSerialReply(numberBoxItems);
        for (i = 0; i < numberBoxItems; i++)
            serialize8(availableBoxes[i]);
        break;
    case MSP_MISC:
        headSerialReply(2 * 6 + 4 + 2 + 4);
        serialize16(0); // intPowerTrigger1 (aka useless trash)
        serialize16(mcfg.minthrottle);
        serialize16(mcfg.maxthrottle);
        serialize16(mcfg.mincommand);
        serialize16(cfg.failsafe_throttle);
        serialize16(0); // plog useless shit
        serialize32(0); // plog useless shit
        serialize16(cfg.mag_declination / 10); // TODO check this shit
        serialize8(mcfg.vbatscale);
        serialize8(mcfg.vbatmincellvoltage);
        serialize8(mcfg.vbatmaxcellvoltage);
        serialize8(0);
        break;
    case MSP_MOTOR_PINS:
        headSerialReply(8);
        for (i = 0; i < 8; i++)
            serialize8(i + 1);
        break;
    case MSP_WP:
        wp_no = read8();    // get the wp number
        headSerialReply(18);
        if (wp_no == 0) {
            lat = GPS_home[LAT];
            lon = GPS_home[LON];
        } else if (wp_no == 16) {
            lat = GPS_hold[LAT];
            lon = GPS_hold[LON];
        }
        serialize8(wp_no);
        serialize32(lat);
        serialize32(lon);
        serialize32(AltHold);           // altitude (cm) will come here -- temporary implementation to test feature with apps
        serialize16(0);                 // heading  will come here (deg)
        serialize16(0);                 // time to stay (ms) will come here
        serialize8(0);                  // nav flag will come here
        break;
    case MSP_SET_WP:
        wp_no = read8();    //get the wp number
        lat = read32();
        lon = read32();
        alt = read32();     // to set altitude (cm)
        read16();           // future: to set heading (deg)
        read16();           // future: to set time to stay (ms)
        read8();            // future: to set nav flag
        if (wp_no == 0) {
            GPS_home[LAT] = lat;
            GPS_home[LON] = lon;
            f.GPS_HOME_MODE = 0;        // with this flag, GPS_set_next_wp will be called in the next loop -- OK with SERIAL GPS / OK with I2C GPS
            f.GPS_FIX_HOME = 1;
            if (alt != 0)
                AltHold = alt;          // temporary implementation to test feature with apps
        } else if (wp_no == 16) {       // OK with SERIAL GPS  --  NOK for I2C GPS / needs more code dev in order to inject GPS coord inside I2C GPS
            GPS_hold[LAT] = lat;
            GPS_hold[LON] = lon;
            if (alt != 0)
                AltHold = alt;          // temporary implementation to test feature with apps
            nav_mode = NAV_MODE_WP;
            GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
        }
        headSerialReply(0);
        break;
    case MSP_RESET_CONF:
        if (!f.ARMED)
            checkFirstTime(true);
        headSerialReply(0);
        break;
    case MSP_ACC_CALIBRATION:
        if (!f.ARMED)
            calibratingA = CALIBRATING_ACC_CYCLES;
        headSerialReply(0);
        break;
    case MSP_MAG_CALIBRATION:
        if (!f.ARMED)
            f.CALIBRATE_MAG = 1;
        headSerialReply(0);
        break;
    case MSP_EEPROM_WRITE:
        writeEEPROM(0, true);
        headSerialReply(0);
        break;
    case MSP_DEBUG:
        headSerialReply(8);
        // make use of this crap, output some useful QA statistics
        debug[3] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000);         // XX0YY [crystal clock : core clock]
        for (i = 0; i < 4; i++)
            serialize16(debug[i]);      // 4 variables are here for general monitoring purpose
        break;

    // Additional commands that are not compatible with MultiWii
    case MSP_ACC_TRIM:
        headSerialReply(4);
        serialize16(cfg.angleTrim[PITCH]);
        serialize16(cfg.angleTrim[ROLL]);
        break;
    case MSP_UID:
        headSerialReply(12);
        serialize32(U_ID_0);
        serialize32(U_ID_1);
        serialize32(U_ID_2);
        break;
    case MSP_GPSSVINFO:
        headSerialReply(1 + (GPS_numCh * 4));
        serialize8(GPS_numCh);
           for (i = 0; i < GPS_numCh; i++){
               serialize8(GPS_svinfo_chn[i]);
               serialize8(GPS_svinfo_svid[i]);
               serialize8(GPS_svinfo_quality[i]);
               serialize8(GPS_svinfo_cno[i]);
            }
        break;
    default:                   // we do not know how to handle the (valid) message, indicate error MSP $M!
        headSerialError(0);
        break;
    }
    tailSerialReply();
}
예제 #6
0
파일: serial.c 프로젝트: fiendie/baseflight
static void evaluateCommand(void)
{
    uint32_t i, j, tmp, junk;
#ifdef GPS
    uint8_t wp_no;
    int32_t lat = 0, lon = 0, alt = 0;
#endif
    const char *build = __DATE__;

    switch (currentPortState->cmdMSP) {
        case MSP_SET_RAW_RC:
            for (i = 0; i < 8; i++)
                rcData[i] = read16();
            headSerialReply(0);
            mspFrameRecieve();
            break;
        case MSP_SET_ACC_TRIM:
            cfg.angleTrim[PITCH] = read16();
            cfg.angleTrim[ROLL]  = read16();
            headSerialReply(0);
            break;
#ifdef GPS
        case MSP_SET_RAW_GPS:
            f.GPS_FIX = read8();
            GPS_numSat = read8();
            GPS_coord[LAT] = read32();
            GPS_coord[LON] = read32();
            GPS_altitude = read16();
            GPS_speed = read16();
            GPS_update |= 2;        // New data signalisation to GPS functions
            headSerialReply(0);
            break;
#endif
        case MSP_SET_PID:
            for (i = 0; i < PIDITEMS; i++) {
                cfg.P8[i] = read8();
                cfg.I8[i] = read8();
                cfg.D8[i] = read8();
            }
            headSerialReply(0);
            break;
        case MSP_SET_BOX:
            for (i = 0; i < numberBoxItems; i++)
                cfg.activate[availableBoxes[i]] = read16();
            headSerialReply(0);
            break;
        case MSP_SET_RC_TUNING:
            cfg.rcRate8 = read8();
            cfg.rcExpo8 = read8();
            cfg.rollPitchRate = read8();
            cfg.yawRate = read8();
            cfg.dynThrPID = read8();
            cfg.thrMid8 = read8();
            cfg.thrExpo8 = read8();
            headSerialReply(0);
            break;
        case MSP_SET_MISC:
            tmp = read16();
            // sanity check
            if (tmp < 1600 && tmp > 1400)
                mcfg.midrc = tmp;
            mcfg.minthrottle = read16();
            mcfg.maxthrottle = read16();
            mcfg.mincommand = read16();
            cfg.failsafe_throttle = read16();
            mcfg.gps_type = read8();
            mcfg.gps_baudrate = read8();
            mcfg.gps_ubx_sbas = read8();
            mcfg.multiwiicurrentoutput = read8();
            mcfg.rssi_aux_channel = read8();
            read8();
            cfg.mag_declination = read16() * 10;
            mcfg.vbatscale = read8();           // actual vbatscale as intended
            mcfg.vbatmincellvoltage = read8();  // vbatlevel_warn1 in MWC2.3 GUI
            mcfg.vbatmaxcellvoltage = read8();  // vbatlevel_warn2 in MWC2.3 GUI
            mcfg.vbatwarningcellvoltage = read8(); // vbatlevel when buzzer starts to alert
            headSerialReply(0);
            break;
        case MSP_SET_MOTOR:
            for (i = 0; i < 8; i++)
                motor_disarmed[i] = read16();
            headSerialReply(0);
            break;
        case MSP_SELECT_SETTING:
            if (!f.ARMED) {
                mcfg.current_profile = read8();
                if (mcfg.current_profile > 2)
                    mcfg.current_profile = 0;
                // this writes new profile index and re-reads it
                writeEEPROM(0, false);
            }
            headSerialReply(0);
            break;
        case MSP_SET_HEAD:
            magHold = read16();
            headSerialReply(0);
            break;
        case MSP_IDENT:
            headSerialReply(7);
            serialize8(VERSION);                    // multiwii version
            serialize8(mcfg.mixerConfiguration);    // type of multicopter
            serialize8(MSP_VERSION);                // MultiWii Serial Protocol Version
            serialize32(CAP_PLATFORM_32BIT | CAP_BASEFLIGHT_CONFIG | CAP_DYNBALANCE | CAP_FW_FLAPS); // "capability"
            break;
        case MSP_STATUS:
            headSerialReply(11);
            serialize16(cycleTime);
            serialize16(i2cGetErrorCounter());
            serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
            // OK, so you waste all the f*****g time to have BOXNAMES and BOXINDEXES etc, and then you go ahead and serialize enabled shit simply by stuffing all
            // the bits in order, instead of setting the enabled bits based on BOXINDEX. WHERE IS THE F*****G LOGIC IN THIS, FUCKWADS.
            // Serialize the boxes in the order we delivered them, until multiwii retards fix their shit
            junk = 0;
            tmp = f.ANGLE_MODE << BOXANGLE | f.HORIZON_MODE << BOXHORIZON |
                  f.BARO_MODE << BOXBARO | f.MAG_MODE << BOXMAG | f.HEADFREE_MODE << BOXHEADFREE | rcOptions[BOXHEADADJ] << BOXHEADADJ |
                  rcOptions[BOXCAMSTAB] << BOXCAMSTAB | rcOptions[BOXCAMTRIG] << BOXCAMTRIG |
                  f.GPS_HOME_MODE << BOXGPSHOME | f.GPS_HOLD_MODE << BOXGPSHOLD |
                  f.PASSTHRU_MODE << BOXPASSTHRU |
                  rcOptions[BOXBEEPERON] << BOXBEEPERON |
                  rcOptions[BOXLEDMAX] << BOXLEDMAX |
                  rcOptions[BOXLLIGHTS] << BOXLLIGHTS |
                  rcOptions[BOXVARIO] << BOXVARIO |
                  rcOptions[BOXCALIB] << BOXCALIB |
                  rcOptions[BOXGOV] << BOXGOV |
                  rcOptions[BOXOSD] << BOXOSD |
                  rcOptions[BOXTELEMETRY] << BOXTELEMETRY |
                  rcOptions[BOXSERVO1] << BOXSERVO1 |
                  rcOptions[BOXSERVO2] << BOXSERVO2 |
                  rcOptions[BOXSERVO3] << BOXSERVO3 |
                  f.ARMED << BOXARM;
            for (i = 0; i < numberBoxItems; i++) {
                int flag = (tmp & (1 << availableBoxes[i]));
                if (flag)
                    junk |= 1 << i;
            }
            serialize32(junk);
            serialize8(mcfg.current_profile);
            break;
        case MSP_RAW_IMU:
            headSerialReply(18);
            // Retarded hack until multiwiidorks start using real units for sensor data
            if (acc_1G > 1024) {
                for (i = 0; i < 3; i++)
                    serialize16(accSmooth[i] / 8);
            } else {
                for (i = 0; i < 3; i++)
                    serialize16(accSmooth[i]);
            }
            for (i = 0; i < 3; i++)
                serialize16(gyroData[i]);
            for (i = 0; i < 3; i++)
                serialize16(magADC[i]);
            break;
        case MSP_SERVO:
            s_struct((uint8_t *)&servo, 16);
            break;
        case MSP_SERVO_CONF:
            headSerialReply(56);
            for (i = 0; i < MAX_SERVOS; i++) {
                serialize16(cfg.servoConf[i].min);
                serialize16(cfg.servoConf[i].max);
                serialize16(cfg.servoConf[i].middle);
                serialize8(getOldServoConfig(i));
            }
            break;
        case MSP_SET_SERVO_CONF:
            headSerialReply(0);
            for (i = 0; i < MAX_SERVOS; i++) {
                cfg.servoConf[i].min = read16();
                cfg.servoConf[i].max = read16();
                cfg.servoConf[i].middle = read16();
                tmp = read8();
                // trash old servo direction
                cfg.servoConf[i].rate = tmp & 0xfc;

                // store old style servo direction depending on current mixer configuration
                switch (mcfg.mixerConfiguration) {
                    case MULTITYPE_BI:
                        storeOldServoConfig(i, tmp, 4, 1, INPUT_PITCH);
                        storeOldServoConfig(i, tmp, 5, 1, INPUT_PITCH);

                        storeOldServoConfig(i, tmp, 4, 2, INPUT_YAW);
                        storeOldServoConfig(i, tmp, 5, 2, INPUT_YAW);

                        break;
                    case MULTITYPE_TRI:
                        storeOldServoConfig(i, tmp, 5, 1, INPUT_YAW);

                        break;
                    case MULTITYPE_FLYING_WING:
                        storeOldServoConfig(i, tmp, 3, 1, INPUT_PITCH);
                        storeOldServoConfig(i, tmp, 4, 1, INPUT_PITCH);

                        storeOldServoConfig(i, tmp, 3, 2, INPUT_ROLL);
                        storeOldServoConfig(i, tmp, 4, 2, INPUT_ROLL);

                        break;
                    case MULTITYPE_DUALCOPTER:
                        storeOldServoConfig(i, tmp, 4, 1, INPUT_PITCH);
                        storeOldServoConfig(i, tmp, 5, 1, INPUT_ROLL);

                        break;
                    case MULTITYPE_SINGLECOPTER:
                        storeOldServoConfig(i, tmp, 3, 1, INPUT_PITCH);
                        storeOldServoConfig(i, tmp, 4, 1, INPUT_PITCH);
                        storeOldServoConfig(i, tmp, 5, 1, INPUT_ROLL);
                        storeOldServoConfig(i, tmp, 6, 1, INPUT_ROLL);

                        storeOldServoConfig(i, tmp, 3, 2, INPUT_YAW);
                        storeOldServoConfig(i, tmp, 4, 2, INPUT_YAW);
                        storeOldServoConfig(i, tmp, 5, 2, INPUT_YAW);
                        storeOldServoConfig(i, tmp, 6, 2, INPUT_YAW);

                        break;
                }
            }
            break;
        case MSP_MOTOR:
            s_struct((uint8_t *)motor, 16);
            break;
        case MSP_RC:
            headSerialReply(16);
            for (i = 0; i < 8; i++)
                serialize16(rcData[i]);
            break;
#ifdef GPS
        case MSP_RAW_GPS:
            headSerialReply(16);
            serialize8(f.GPS_FIX);
            serialize8(GPS_numSat);
            serialize32(GPS_coord[LAT]);
            serialize32(GPS_coord[LON]);
            serialize16(GPS_altitude);
            serialize16(GPS_speed);
            serialize16(GPS_ground_course);
            break;
        case MSP_COMP_GPS:
            headSerialReply(5);
            serialize16(GPS_distanceToHome);
            serialize16(GPS_directionToHome);
            serialize8(GPS_update & 1);
            break;
#endif
        case MSP_ATTITUDE:
            headSerialReply(6);
            for (i = 0; i < 2; i++)
                serialize16(angle[i]);
            serialize16(heading);
            break;
        case MSP_ALTITUDE:
            headSerialReply(6);
            serialize32(EstAlt);
            serialize16(vario);
            break;
        case MSP_ANALOG:
            headSerialReply(7);
            serialize8((uint8_t)constrain(vbat, 0, 255));
            serialize16((uint16_t)constrain(mAhdrawn, 0, 0xFFFF)); // milliamphours drawn from battery
            serialize16(rssi);
            if (mcfg.multiwiicurrentoutput)
                serialize16((uint16_t)constrain((abs(amperage) * 10), 0, 0xFFFF)); // send amperage in 0.001 A steps
            else
                serialize16((uint16_t)constrain(abs(amperage), 0, 0xFFFF)); // send amperage in 0.01 A steps
            break;
        case MSP_RC_TUNING:
            headSerialReply(7);
            serialize8(cfg.rcRate8);
            serialize8(cfg.rcExpo8);
            serialize8(cfg.rollPitchRate);
            serialize8(cfg.yawRate);
            serialize8(cfg.dynThrPID);
            serialize8(cfg.thrMid8);
            serialize8(cfg.thrExpo8);
            break;
        case MSP_PID:
            headSerialReply(3 * PIDITEMS);
            for (i = 0; i < PIDITEMS; i++) {
                serialize8(cfg.P8[i]);
                serialize8(cfg.I8[i]);
                serialize8(cfg.D8[i]);
            }
            break;
        case MSP_PIDNAMES:
            headSerialReply(sizeof(pidnames) - 1);
            serializeNames(pidnames);
            break;
        case MSP_BOX:
            headSerialReply(2 * numberBoxItems);
            for (i = 0; i < numberBoxItems; i++)
                serialize16(cfg.activate[availableBoxes[i]]);
            break;
        case MSP_BOXNAMES:
            // headSerialReply(sizeof(boxnames) - 1);
            serializeBoxNamesReply();
            break;
        case MSP_BOXIDS:
            headSerialReply(numberBoxItems);
            for (i = 0; i < numberBoxItems; i++) {
                for  (j = 0; j < CHECKBOXITEMS; j++) {
                    if (boxes[j].permanentId == availableBoxes[i])
                        serialize8(boxes[j].permanentId);
                }
            }
            break;
        case MSP_MISC:
            headSerialReply(2 * 6 + 4 + 2 + 4);
            serialize16(mcfg.midrc);
            serialize16(mcfg.minthrottle);
            serialize16(mcfg.maxthrottle);
            serialize16(mcfg.mincommand);
            serialize16(cfg.failsafe_throttle);
            serialize8(mcfg.gps_type);
            serialize8(mcfg.gps_baudrate);
            serialize8(mcfg.gps_ubx_sbas);
            serialize8(mcfg.multiwiicurrentoutput);
            serialize8(mcfg.rssi_aux_channel);
            serialize8(0);
            serialize16(cfg.mag_declination / 10); // TODO check this shit
            serialize8(mcfg.vbatscale);
            serialize8(mcfg.vbatmincellvoltage);
            serialize8(mcfg.vbatmaxcellvoltage);
            serialize8(mcfg.vbatwarningcellvoltage);
            break;
        case MSP_MOTOR_PINS:
            headSerialReply(8);
            for (i = 0; i < 8; i++)
                serialize8(i + 1);
            break;
#ifdef GPS
        case MSP_WP:
            wp_no = read8();    // get the wp number
            headSerialReply(18);
            if (wp_no == 0) {
                lat = GPS_home[LAT];
                lon = GPS_home[LON];
            } else if (wp_no == 16) {
                lat = GPS_hold[LAT];
                lon = GPS_hold[LON];
            }
            serialize8(wp_no);
            serialize32(lat);
            serialize32(lon);
            serialize32(AltHold);           // altitude (cm) will come here -- temporary implementation to test feature with apps
            serialize16(0);                 // heading  will come here (deg)
            serialize16(0);                 // time to stay (ms) will come here
            serialize8(0);                  // nav flag will come here
            break;
        case MSP_SET_WP:
            wp_no = read8();    //get the wp number
            lat = read32();
            lon = read32();
            alt = read32();     // to set altitude (cm)
            read16();           // future: to set heading (deg)
            read16();           // future: to set time to stay (ms)
            read8();            // future: to set nav flag
            if (wp_no == 0) {
                GPS_home[LAT] = lat;
                GPS_home[LON] = lon;
                f.GPS_HOME_MODE = 0;        // with this flag, GPS_set_next_wp will be called in the next loop -- OK with SERIAL GPS / OK with I2C GPS
                f.GPS_FIX_HOME = 1;
                if (alt != 0)
                    AltHold = alt;          // temporary implementation to test feature with apps
            } else if (wp_no == 16) {       // OK with SERIAL GPS  --  NOK for I2C GPS / needs more code dev in order to inject GPS coord inside I2C GPS
                GPS_hold[LAT] = lat;
                GPS_hold[LON] = lon;
                if (alt != 0)
                    AltHold = alt;          // temporary implementation to test feature with apps
                nav_mode = NAV_MODE_WP;
                GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
            }
            headSerialReply(0);
            break;
#endif /* GPS */
        case MSP_RESET_CONF:
            if (!f.ARMED)
                checkFirstTime(true);
            headSerialReply(0);
            break;
        case MSP_ACC_CALIBRATION:
            if (!f.ARMED)
                calibratingA = CALIBRATING_ACC_CYCLES;
            headSerialReply(0);
            break;
        case MSP_MAG_CALIBRATION:
            if (!f.ARMED)
                f.CALIBRATE_MAG = 1;
            headSerialReply(0);
            break;
        case MSP_EEPROM_WRITE:
            if (f.ARMED) {
                headSerialError(0);
            } else {
                writeEEPROM(0, true);
                headSerialReply(0);
            }
            break;
        case MSP_DEBUG:
            headSerialReply(8);
            // make use of this crap, output some useful QA statistics
            debug[3] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000);         // XX0YY [crystal clock : core clock]
            for (i = 0; i < 4; i++)
                serialize16(debug[i]);      // 4 variables are here for general monitoring purpose
            break;

        // Additional commands that are not compatible with MultiWii
        case MSP_ACC_TRIM:
            headSerialReply(4);
            serialize16(cfg.angleTrim[PITCH]);
            serialize16(cfg.angleTrim[ROLL]);
            break;
        case MSP_UID:
            headSerialReply(12);
            serialize32(U_ID_0);
            serialize32(U_ID_1);
            serialize32(U_ID_2);
            break;
#ifdef GPS
        case MSP_GPSSVINFO:
            headSerialReply(1 + (GPS_numCh * 4));
            serialize8(GPS_numCh);
            for (i = 0; i < GPS_numCh; i++) {
                serialize8(GPS_svinfo_chn[i]);
                serialize8(GPS_svinfo_svid[i]);
                serialize8(GPS_svinfo_quality[i]);
                serialize8(GPS_svinfo_cno[i]);
            }
            // Poll new SVINFO from GPS
            gpsPollSvinfo();
            break;
        case MSP_GPSDEBUGINFO:
            headSerialReply(16);
            if (sensors(SENSOR_GPS)) {
                serialize32(GPS_update_rate[1] - GPS_update_rate[0]);
                serialize32(GPS_svinfo_rate[1] - GPS_svinfo_rate[0]);
            } else {
                serialize32(0);
                serialize32(0);
            }
            serialize32(GPS_HorizontalAcc);
            serialize32(GPS_VerticalAcc);
            break;
#endif /* GPS */

        case MSP_SET_CONFIG:
            headSerialReply(0);
            mcfg.mixerConfiguration = read8(); // multitype
            featureClearAll();
            featureSet(read32()); // features bitmap
            mcfg.serialrx_type = read8(); // serialrx_type
            mcfg.board_align_roll = read16(); // board_align_roll
            mcfg.board_align_pitch = read16(); // board_align_pitch
            mcfg.board_align_yaw = read16(); // board_align_yaw
            mcfg.currentscale = read16();
            mcfg.currentoffset = read16();
            /// ???
            break;
        case MSP_CONFIG:
            headSerialReply(1 + 4 + 1 + 2 + 2 + 2 + 4);
            serialize8(mcfg.mixerConfiguration);
            serialize32(featureMask());
            serialize8(mcfg.serialrx_type);
            serialize16(mcfg.board_align_roll);
            serialize16(mcfg.board_align_pitch);
            serialize16(mcfg.board_align_yaw);
            serialize16(mcfg.currentscale);
            serialize16(mcfg.currentoffset);
            /// ???
            break;

        case MSP_RCMAP:
            headSerialReply(MAX_INPUTS); // TODO fix this
            for (i = 0; i < MAX_INPUTS; i++)
                serialize8(mcfg.rcmap[i]);
            break;
        case MSP_SET_RCMAP:
            headSerialReply(0);
            for (i = 0; i < MAX_INPUTS; i++)
                mcfg.rcmap[i] = read8();
            break;

        case MSP_REBOOT:
            headSerialReply(0);
            pendReboot = true;
            break;

        case MSP_BUILDINFO:
            headSerialReply(11 + 4 + 4);
            for (i = 0; i < 11; i++)
                serialize8(build[i]); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc
            serialize32(0); // future exp
            serialize32(0); // future exp
            break;

        default:                   // we do not know how to handle the (valid) message, indicate error MSP $M!
            headSerialError(0);
            break;
    }
    tailSerialReply();
}
예제 #7
0
파일: Protocol.cpp 프로젝트: xotab26/rxmwc
void evaluateCommand() {
  uint32_t tmp=0; 

  switch(cmdMSP[CURRENTPORT]) {
    case MSP_PRIVATE:
      //headSerialError();tailSerialReply(); // we don't have any custom msp currently, so tell the gui we do not use that
      break;
    case MSP_SET_RAW_RC:
      s_struct_w((uint8_t*)&rcSerial,16);
      rcSerialCount = 50; // 1s transition 
      break;
    case MSP_SET_PID:
      s_struct_w((uint8_t*)&conf.pid[0].P8,3*PIDITEMS);
      break;
    case MSP_SET_BOX:
      #if EXTAUX
        s_struct_w((uint8_t*)&conf.activate[0],CHECKBOXITEMS*4);
      #else
        s_struct_w((uint8_t*)&conf.activate[0],CHECKBOXITEMS*2);
      #endif
      break;
    case MSP_SET_RC_TUNING:
      s_struct_w((uint8_t*)&conf.rcRate8,7);
      break;
    #if !defined(DISABLE_SETTINGS_TAB)
    case MSP_SET_MISC:
      struct {
        uint16_t a,b,c,d,e,f;
        uint32_t g;
        uint16_t h;
        uint8_t  i,j,k,l;
      } set_misc;
      s_struct_w((uint8_t*)&set_misc,22);
      #if defined(POWERMETER)
        conf.powerTrigger1 = set_misc.a / PLEVELSCALE;
      #endif
      conf.minthrottle = set_misc.b;
      #ifdef FAILSAFE 
        conf.failsafe_throttle = set_misc.e;
      #endif  
      #if MAG
        conf.mag_declination = set_misc.h;
      #endif
      #if defined(VBAT)
        conf.vbatscale        = set_misc.i;
        conf.vbatlevel_warn1  = set_misc.j;
        conf.vbatlevel_warn2  = set_misc.k;
        conf.vbatlevel_crit   = set_misc.l;
      #endif
      break;
    case MSP_MISC:
      struct {
        uint16_t a,b,c,d,e,f;
        uint32_t g;
        uint16_t h;
        uint8_t  i,j,k,l;
      } misc;
      misc.a = intPowerTrigger1;
      misc.b = conf.minthrottle;
      misc.c = MAXTHROTTLE;
      misc.d = MINCOMMAND;
      #ifdef FAILSAFE 
        misc.e = conf.failsafe_throttle;
      #else  
        misc.e = 0;
      #endif
      #ifdef LOG_PERMANENT
        misc.f = plog.arm;
        misc.g = plog.lifetime + (plog.armed_time / 1000000); // <- computation must be moved outside from serial
      #else
        misc.f = 0; misc.g =0;
      #endif
      #if MAG
        misc.h = conf.mag_declination;
      #else
        misc.h = 0;
      #endif
      #ifdef VBAT
        misc.i = conf.vbatscale;
        misc.j = conf.vbatlevel_warn1;
        misc.k = conf.vbatlevel_warn2;
        misc.l = conf.vbatlevel_crit;
      #else
        misc.i = 0;misc.j = 0;misc.k = 0;misc.l = 0;
      #endif
      s_struct((uint8_t*)&misc,22);
      break;
    #endif
    #if defined (DYNBALANCE)
    case MSP_SET_MOTOR:
      s_struct_w((uint8_t*)&motor,16);
    break;
    #endif
    #ifdef MULTIPLE_CONFIGURATION_PROFILES
    case MSP_SELECT_SETTING:
      if(!f.ARMED) {
        global_conf.currentSet = read8();
        if(global_conf.currentSet>2) global_conf.currentSet = 0;
        writeGlobalSet(0);
        readEEPROM();
      }
      //headSerialReply(0);tailSerialReply();
      break;
    #endif
    case MSP_SET_HEAD:
      s_struct_w((uint8_t*)&magHold,2);
      break;
    case MSP_IDENT:
      struct {
        uint8_t v,t,msp_v;
        uint32_t cap;
      } id;
      id.v     = VERSION;
      id.t     = MULTITYPE;
      id.msp_v = MSP_VERSION;
      id.cap   = capability|DYNBAL<<2|FLAP<<3|NAVCAP<<4|EXTAUX<<5|((uint32_t)NAVI_VERSION<<28);			//Navi version is stored in the upper four bits; 
      s_struct((uint8_t*)&id,7);
      break;
    case MSP_STATUS:
      struct {
        uint16_t cycleTime,i2c_errors_count,sensor;
        uint32_t flag;
        uint8_t set;
      } st;
      st.cycleTime        = cycleTime;
      st.i2c_errors_count = i2c_errors_count;
      st.sensor           = ACC|BARO<<1|MAG<<2|GPS<<3|SONAR<<4;
      #if ACC
        if(f.ANGLE_MODE)   tmp |= 1<<BOXANGLE;
        if(f.HORIZON_MODE) tmp |= 1<<BOXHORIZON;
      #endif
      #if BARO && (!defined(SUPPRESS_BARO_ALTHOLD))
        if(f.BARO_MODE) tmp |= 1<<BOXBARO;
      #endif
      if(f.MAG_MODE) tmp |= 1<<BOXMAG;
      #if !defined(FIXEDWING)
        #if defined(HEADFREE)
          if(f.HEADFREE_MODE)       tmp |= 1<<BOXHEADFREE;
          if(rcOptions[BOXHEADADJ]) tmp |= 1<<BOXHEADADJ;
        #endif
      #endif
      #if defined(SERVO_TILT) || defined(GIMBAL)|| defined(SERVO_MIX_TILT)
        if(rcOptions[BOXCAMSTAB]) tmp |= 1<<BOXCAMSTAB;
      #endif
      #if defined(CAMTRIG)
        if(rcOptions[BOXCAMTRIG]) tmp |= 1<<BOXCAMTRIG;
      #endif
      #if GPS
        switch (f.GPS_mode) {
          case GPS_MODE_HOLD: 
            tmp |= 1<<BOXGPSHOLD;
            break;
          case GPS_MODE_RTH:
            tmp |= 1<<BOXGPSHOME;
            break;
          case GPS_MODE_NAV:
            tmp |= 1<<BOXGPSNAV;
            break;
        }
      #endif
      #if defined(FIXEDWING) || defined(HELICOPTER)
        if(f.PASSTHRU_MODE) tmp |= 1<<BOXPASSTHRU;
      #endif
      #if defined(BUZZER)
        if(rcOptions[BOXBEEPERON]) tmp |= 1<<BOXBEEPERON;
      #endif
      #if defined(LED_FLASHER)
        if(rcOptions[BOXLEDMAX]) tmp |= 1<<BOXLEDMAX;
        if(rcOptions[BOXLEDLOW]) tmp |= 1<<BOXLEDLOW;
      #endif
      #if defined(LANDING_LIGHTS_DDR)
        if(rcOptions[BOXLLIGHTS]) tmp |= 1<<BOXLLIGHTS;
      #endif
      #if defined(VARIOMETER)
        if(rcOptions[BOXVARIO]) tmp |= 1<<BOXVARIO;
      #endif
      #if defined(INFLIGHT_ACC_CALIBRATION)
        if(rcOptions[BOXCALIB]) tmp |= 1<<BOXCALIB;
      #endif
      #if defined(GOVERNOR_P)
        if(rcOptions[BOXGOV]) tmp |= 1<<BOXGOV;
      #endif
      #if defined(OSD_SWITCH)
        if(rcOptions[BOXOSD]) tmp |= 1<<BOXOSD;
      #endif
      if(f.ARMED) tmp |= 1<<BOXARM;
      st.flag             = tmp;
      st.set              = global_conf.currentSet;
      s_struct((uint8_t*)&st,11);
      break;
    case MSP_RAW_IMU:
      #if defined(DYNBALANCE)
        for(uint8_t axis=0;axis<3;axis++) {imu.gyroData[axis]=imu.gyroADC[axis];imu.accSmooth[axis]= imu.accADC[axis];} // Send the unfiltered Gyro & Acc values to gui.
      #endif 
      s_struct((uint8_t*)&imu,18);
      break;
    case MSP_SERVO:
      s_struct((uint8_t*)&servo,16);
      break;
    case MSP_SERVO_CONF:
      s_struct((uint8_t*)&conf.servoConf[0].min,56); // struct servo_conf_ is 7 bytes length: min:2 / max:2 / middle:2 / rate:1    ----     8 servo =>  8x7 = 56
      break;
    case MSP_SET_SERVO_CONF:
      s_struct_w((uint8_t*)&conf.servoConf[0].min,56);
      break;
    case MSP_MOTOR:
      s_struct((uint8_t*)&motor,16);
      break;
    case MSP_ACC_TRIM:
      s_struct((uint8_t*)&conf.angleTrim[0],4);
      break;
    case MSP_SET_ACC_TRIM:
      s_struct_w((uint8_t*)&conf.angleTrim[0],4);
      break;
    case MSP_RC:
      s_struct((uint8_t*)&rcData,RC_CHANS*2);
      break;
    #if GPS
    case MSP_SET_RAW_GPS:
      struct {
        uint8_t a,b;
        int32_t c,d;
        int16_t e;
        uint16_t f;
      } set_set_raw_gps;
      s_struct_w((uint8_t*)&set_set_raw_gps,14);
      f.GPS_FIX = set_set_raw_gps.a;
      GPS_numSat = set_set_raw_gps.b;
      GPS_coord[LAT] = set_set_raw_gps.c;
      GPS_coord[LON] = set_set_raw_gps.d;
      GPS_altitude = set_set_raw_gps.e;
      GPS_speed = set_set_raw_gps.f;
      GPS_update |= 2;              // New data signalisation to GPS functions
      break;
    case MSP_RAW_GPS:
      struct {
        uint8_t a,b;
        int32_t c,d;
        int16_t e;
        uint16_t f,g;
      } msp_raw_gps;
      msp_raw_gps.a     = f.GPS_FIX;
      msp_raw_gps.b     = GPS_numSat;
      msp_raw_gps.c     = GPS_coord[LAT];
      msp_raw_gps.d     = GPS_coord[LON];
      msp_raw_gps.e     = GPS_altitude;
      msp_raw_gps.f     = GPS_speed;
      msp_raw_gps.g     = GPS_ground_course;
      s_struct((uint8_t*)&msp_raw_gps,16);
      break;
    case MSP_COMP_GPS:
      struct {
        uint16_t a;
        int16_t b;
        uint8_t c;
      } msp_comp_gps;
      msp_comp_gps.a     = GPS_distanceToHome;
      msp_comp_gps.b     = GPS_directionToHome;
      msp_comp_gps.c     = GPS_update & 1;
      s_struct((uint8_t*)&msp_comp_gps,5);
      break;
    #if defined(USE_MSP_WP)
    case MSP_SET_NAV_CONFIG:
      s_struct_w((uint8_t*)&GPS_conf,sizeof(GPS_conf));
      break;
    case MSP_NAV_CONFIG:
      s_struct((uint8_t*)&GPS_conf,sizeof(GPS_conf));
      break;
    case MSP_NAV_STATUS: // to move to struct transmission
      headSerialReply(7);
      serialize8(f.GPS_mode);
      serialize8(NAV_state);
      serialize8(mission_step.action);
      serialize8(mission_step.number);
      serialize8(NAV_error);
      serialize16( (int16_t)(target_bearing/100));
      //serialize16(magHold);
      tailSerialReply();
      break;
    case MSP_WP: // to move to struct transmission
      {
      uint8_t wp_no;
      uint8_t flag;
      bool    success;

      wp_no = read8(); //get the wp number  
      headSerialReply(21);
      if (wp_no == 0) { //Get HOME coordinates
        serialize8(wp_no);
        serialize8(mission_step.action);
        serialize32(GPS_home[LAT]);
        serialize32(GPS_home[LON]);
        flag = MISSION_FLAG_HOME;
      }
      if (wp_no == 255)	{ //Get poshold coordinates
        serialize8(wp_no);
        serialize8(mission_step.action);
        serialize32(GPS_hold[LAT]);
        serialize32(GPS_hold[LON]);
        flag = MISSION_FLAG_HOLD;
      }
      if ((wp_no>0) && (wp_no<255)) {
        if (NAV_state == NAV_STATE_NONE) {
          success = recallWP(wp_no);
          serialize8(wp_no);
          serialize8(mission_step.action);
          serialize32(mission_step.pos[LAT]);
          serialize32(mission_step.pos[LON]);
          if (success == true) flag = mission_step.flag;
          else flag = MISSION_FLAG_CRC_ERROR;	//CRC error
        } else {
          serialize8(wp_no);
          serialize8(0);
          serialize32(GPS_home[LAT]);
          serialize32(GPS_home[LON]);
          flag = MISSION_FLAG_NAV_IN_PROG;
        }
      }
      serialize32(mission_step.altitude);
      serialize16(mission_step.parameter1);
      serialize16(mission_step.parameter2);
      serialize16(mission_step.parameter3);
      serialize8(flag);
      tailSerialReply();
      }
      break;
    case MSP_SET_WP: // to move to struct transmission
      //TODO: add I2C_gps handling
      {
      uint8_t wp_no = read8(); //Get the step number

      if (NAV_state == NAV_STATE_HOLD_INFINIT && wp_no == 255) { //Special case - during stable poshold we allow change the hold position
        mission_step.number = wp_no;
        mission_step.action = MISSION_HOLD_UNLIM; 
        uint8_t temp = read8();
        mission_step.pos[LAT] =  read32();
        mission_step.pos[LON] =  read32();
        mission_step.altitude =  read32();
        mission_step.parameter1 = read16();
        mission_step.parameter2 = read16();
        mission_step.parameter3 = read16();
        mission_step.flag     =  read8();
        if (mission_step.altitude != 0) set_new_altitude(mission_step.altitude);
        GPS_set_next_wp(&mission_step.pos[LAT], &mission_step.pos[LON], &GPS_coord[LAT], &GPS_coord[LON]);	
        if ((wp_distance/100) >= GPS_conf.safe_wp_distance) NAV_state = NAV_STATE_NONE;
        else NAV_state = NAV_STATE_WP_ENROUTE;			
        break;
      }
      if (NAV_state == NAV_STATE_NONE) { // The Nav state is not zero, so navigation is in progress, silently ignore SET_WP command)
        mission_step.number	   =    wp_no;
        mission_step.action     =  read8();
        mission_step.pos[LAT]   =  read32();
        mission_step.pos[LON]   =  read32();
        mission_step.altitude   =  read32();
        mission_step.parameter1 = read16();
        mission_step.parameter2 = read16();
        mission_step.parameter3 = read16();
        mission_step.flag       =  read8();
        //It's not sure, that we want to do poshold change via mission planner so perhaps the next if is deletable
        /*
        if (mission_step.number == 255) //Set up new hold position via mission planner, It must set the action to MISSION_HOLD_INFINIT 
        {
        if (mission_step.altitude !=0) set_new_altitude(mission_step.altitude); //Set the altitude
        GPS_set_next_wp(&mission_step.pos[LAT], &mission_step.pos[LON], &GPS_coord[LAT], &GPS_coord[LON]);	
        NAV_state = NAV_STATE_WP_ENROUTE; //Go to that position, then it will switch to poshold unlimited when reached
        }
        */
        if (mission_step.number == 0) { //Set new Home position
          GPS_home[LAT] = mission_step.pos[LAT];
          GPS_home[LON] = mission_step.pos[LON];
        }
        if (mission_step.number >0 && mission_step.number<255)			//Not home and not poshold, we are free to store it in the eprom
          if (mission_step.number <= getMaxWPNumber())				    // Do not thrash the EEPROM with invalid wp number
            storeWP();
        //headSerialReply(0);tailSerialReply();
      }
      }
      break;
    #endif //USE_MSP_WP
    #endif //GPS
    case MSP_ATTITUDE:
      s_struct((uint8_t*)&att,6);
      break;
    case MSP_ALTITUDE:
      s_struct((uint8_t*)&alt,6);
      break;
    case MSP_ANALOG:
      s_struct((uint8_t*)&analog,7);
      break;
    case MSP_RC_TUNING:
      s_struct((uint8_t*)&conf.rcRate8,7);
      break;
    case MSP_PID:
      s_struct((uint8_t*)&conf.pid[0].P8,3*PIDITEMS);
      break;
    case MSP_PIDNAMES:
      serializeNames(pidnames);
      break;
    case MSP_BOX:
      #if EXTAUX
        s_struct((uint8_t*)&conf.activate[0],4*CHECKBOXITEMS);
      #else
        s_struct((uint8_t*)&conf.activate[0],2*CHECKBOXITEMS);
      #endif
      break;
    case MSP_BOXNAMES:
      serializeNames(boxnames);
      break;
    case MSP_BOXIDS:
      headSerialReply(CHECKBOXITEMS);
      for(uint8_t i=0;i<CHECKBOXITEMS;i++)
        serialize8(pgm_read_byte(&(boxids[i])));
      tailSerialReply();
      break;
    case MSP_MOTOR_PINS:
      s_struct((uint8_t*)&PWM_PIN,8);
      break;
    case MSP_RESET_CONF:
      if(!f.ARMED) LoadDefaults();
      //headSerialReply(0);tailSerialReply();
      break;
    case MSP_ACC_CALIBRATION:
      if(!f.ARMED) calibratingA=512;
      //headSerialReply(0);tailSerialReply();
      break;
    case MSP_MAG_CALIBRATION:
      if(!f.ARMED) f.CALIBRATE_MAG = 1;
      //headSerialReply(0);tailSerialReply();
      break;
    #if defined(SPEK_BIND)
    case MSP_BIND:
      spekBind();
      //headSerialReply(0);tailSerialReply();
      break;
    #endif
    case MSP_EEPROM_WRITE:
      writeParams(0);
      //headSerialReply(0);tailSerialReply();
      break;
    case MSP_DEBUG:
      s_struct((uint8_t*)&debug,8);
      break;
    #ifdef DEBUGMSG
    case MSP_DEBUGMSG:
      {
      uint8_t size = debugmsg_available();
      if (size > 16) size = 16;
      headSerialReply(size);
      debugmsg_serialize(size);
      tailSerialReply();
      }
      break;
    #endif
    default:  // we do not know how to handle the (valid) message, indicate error MSP $M!
      headSerialError();tailSerialReply();
      break;
  }
}
예제 #8
0
void updateGpsWaypointsAndMode(void)
{
    bool resetNavNow = false;

    if (STATE(GPS_FIX) && GPS_numSat >= 5) {

        //
        // process HOME mode
        //
        // HOME mode takes priority over HOLD mode.

        if (IS_RC_MODE_ACTIVE(BOXGPSHOME)) {
            if (!FLIGHT_MODE(GPS_HOME_MODE)) {

                // Transition to HOME mode
                ENABLE_FLIGHT_MODE(GPS_HOME_MODE);
                DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);
                GPS_set_next_wp(&GPS_home[LAT], &GPS_home[LON]);
                nav_mode = NAV_MODE_WP;
                resetNavNow = true;
            }
        } else {
            if (FLIGHT_MODE(GPS_HOME_MODE)) {

                // Transition from HOME mode
                DISABLE_FLIGHT_MODE(GPS_HOME_MODE);
                nav_mode = NAV_MODE_NONE;
                resetNavNow = true;
            }

            //
            // process HOLD mode
            //

            if (IS_RC_MODE_ACTIVE(BOXGPSHOLD) && areSticksInApModePosition(gpsProfile->ap_mode)) {
                if (!FLIGHT_MODE(GPS_HOLD_MODE)) {

                    // Transition to HOLD mode
                    ENABLE_FLIGHT_MODE(GPS_HOLD_MODE);
                    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;
                    resetNavNow = true;
                }
            } else {
                if (FLIGHT_MODE(GPS_HOLD_MODE)) {

                    // Transition from HOLD mode
                    DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);
                    nav_mode = NAV_MODE_NONE;
                    resetNavNow = true;
                }
            }
        }
    } else {
        if (FLIGHT_MODE(GPS_HOLD_MODE | GPS_HOME_MODE)) {

            // Transition from HOME or HOLD mode
            DISABLE_FLIGHT_MODE(GPS_HOME_MODE);
            DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);
            nav_mode = NAV_MODE_NONE;
            resetNavNow = true;
        }
    }

    if (resetNavNow) {
        GPS_reset_nav();
    }
}
예제 #9
0
void loop(void)
{
    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
    static uint8_t rcSticks;            // this hold sticks position for command combos
    uint8_t stTmp = 0;
    uint8_t axis, i;
    int16_t error, errorAngle;
    int16_t PTerm, ITerm, PTermACC, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm;
    static int16_t errorGyroI[3] = { 0, 0, 0 };
    static int16_t errorAngleI[2] = { 0, 0 };
    int16_t delta;
    static int16_t lastGyro[3] = { 0, 0, 0 };
    static int16_t delta1[3], delta2[3];
    int16_t deltaSum;
    static uint32_t rcTime = 0;
    static int16_t initialThrottleHold;
    static uint32_t loopTime;
    uint16_t auxState = 0;
    int16_t prop;
    static uint8_t GPSNavReset = 1;

    // this will return false if spektrum is disabled. shrug.
    if (spektrumFrameComplete())
        computeRC();

    if ((int32_t)(currentTime - rcTime) >= 0) { // 50Hz
        rcTime = currentTime + 20000;
        // TODO clean this up. computeRC should handle this check
        if (!feature(FEATURE_SPEKTRUM))
            computeRC();
        if (feature(FEATURE_GPS) && mcfg.gps_type == GPS_I2C)
        	GPS_NewData('c');
        // Failsafe routine
        if (feature(FEATURE_FAILSAFE)) {
            if (failsafeCnt > (5 * cfg.failsafe_delay) && f.ARMED) { // Stabilize, and set Throttle to specified level
                for (i = 0; i < 3; i++)
                    rcData[i] = mcfg.midrc;      // after specified guard time after RC signal is lost (in 0.1sec)
                rcData[THROTTLE] = cfg.failsafe_throttle;
                if (failsafeCnt > 5 * (cfg.failsafe_delay + cfg.failsafe_off_delay)) {  // Turn OFF motors after specified Time (in 0.1sec)
                    mwDisarm();             // 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 * cfg.failsafe_delay) && !f.ARMED) {  // Turn off "Ok To arm to prevent the motors from spinning after repowering the RX with low throttle and aux to arm
                mwDisarm();         // 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++;
        }
        // end of failsafe routine - next change is made with RcOptions setting

        // ------------------ STICKS COMMAND HANDLER --------------------
        // checking sticks positions
        for (i = 0; i < 4; i++) {
            stTmp >>= 2;
            if (rcData[i] > mcfg.mincheck)
                stTmp |= 0x80;  // check for MIN
            if (rcData[i] < mcfg.maxcheck)
                stTmp |= 0x40;  // check for MAX
        }
        if (stTmp == rcSticks) {
            if (rcDelayCommand < 250)
                rcDelayCommand++;
        } else
            rcDelayCommand = 0;
        rcSticks = stTmp;

        // perform actions
        if (rcData[THROTTLE] < mcfg.mincheck) {
            errorGyroI[ROLL] = 0;
            errorGyroI[PITCH] = 0;
            errorGyroI[YAW] = 0;
            errorAngleI[ROLL] = 0;
            errorAngleI[PITCH] = 0;
            if (cfg.activate[BOXARM] > 0) { // Arming/Disarming via ARM BOX
                if (rcOptions[BOXARM] && f.OK_TO_ARM)
                    mwArm();
                else if (f.ARMED)
                    mwDisarm();
            }
        }

        if (rcDelayCommand == 20) {
            if (f.ARMED) {      // actions during armed
                // Disarm on throttle down + yaw
                if (cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE))
                    mwDisarm();
                // Disarm on roll (only when retarded_arm is enabled)
                if (mcfg.retarded_arm && cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO))
                    mwDisarm();
            } else {            // actions during not armed
                i = 0;
                // GYRO calibration
                if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
                    calibratingG = 1000;
                    if (feature(FEATURE_GPS))
                        GPS_reset_home_position();
                    if (sensors(SENSOR_BARO))
                        calibratingB = 10; // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
                // Inflight ACC Calibration
                } else if (feature(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) {
                    if (AccInflightCalibrationMeasurementDone) {        // trigger saving into eeprom after landing
                        AccInflightCalibrationMeasurementDone = 0;
                        AccInflightCalibrationSavetoEEProm = 1;
                    } else {
                        AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
                        if (AccInflightCalibrationArmed) {
                            toggleBeep = 2;
                        } else {
                            toggleBeep = 3;
                        }
                    }
                }

                // Multiple configuration profiles
                if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO)          // ROLL left  -> Profile 1
                    i = 1;
                else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE)     // PITCH up   -> Profile 2
                    i = 2;
                else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI)     // ROLL right -> Profile 3
                    i = 3;
                if (i) {
                    mcfg.current_profile = i - 1;
                    writeEEPROM(0, false);
                    blinkLED(2, 40, i);
                    // TODO alarmArray[0] = i;
                }

                // Arm via YAW
                if (cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE))
                    mwArm();
                // Arm via ROLL
                else if (mcfg.retarded_arm && cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI))
                    mwArm();
                // Calibrating Acc
                else if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE)
                    calibratingA = 400;
                // Calibrating Mag
                else if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE)
                    f.CALIBRATE_MAG = 1;
                i = 0;
                // Acc Trim
                if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) {
                    cfg.angleTrim[PITCH] += 2;
                    i = 1;
                } else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {
                    cfg.angleTrim[PITCH] -= 2;
                    i = 1;
                } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {
                    cfg.angleTrim[ROLL] += 2;
                    i = 1;
                } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {
                    cfg.angleTrim[ROLL] -= 2;
                    i = 1;
                }
                if (i) {
                    writeEEPROM(1, false);
                    rcDelayCommand = 0; // allow autorepetition
                }
            }
        }

        if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
            if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > mcfg.mincheck && !rcOptions[BOXARM]) {   // Copter is airborne and you are turning it off via boxarm : start measurement
                InflightcalibratingA = 50;
                AccInflightCalibrationArmed = 0;
            }
            if (rcOptions[BOXCALIB]) {      // Use the Calib Option to activate : Calib = TRUE Meausrement started, Land and Calib = 0 measurement stored
                if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)
                    InflightcalibratingA = 50;
            } else if (AccInflightCalibrationMeasurementDone && !f.ARMED) {
                AccInflightCalibrationMeasurementDone = 0;
                AccInflightCalibrationSavetoEEProm = 1;
            }
        }

        // Check AUX switches
        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 & cfg.activate[i]) > 0;

        // note: if FAILSAFE is disable, failsafeCnt > 5 * FAILSAVE_DELAY is always false
        if ((rcOptions[BOXANGLE] || (failsafeCnt > 5 * cfg.failsafe_delay)) && (sensors(SENSOR_ACC))) {
            // bumpless transfer to Level mode
            if (!f.ANGLE_MODE) {
                errorAngleI[ROLL] = 0;
                errorAngleI[PITCH] = 0;
                f.ANGLE_MODE = 1;
            }
        } else {
            f.ANGLE_MODE = 0;        // failsave support
        }

        if (rcOptions[BOXHORIZON]) {
            f.ANGLE_MODE = 0;
            if (!f.HORIZON_MODE) {
                errorAngleI[ROLL] = 0;
                errorAngleI[PITCH] = 0;
                f.HORIZON_MODE = 1;
            }
        } else {
            f.HORIZON_MODE = 0;
        }

        if ((rcOptions[BOXARM]) == 0)
            f.OK_TO_ARM = 1;
        if (f.ANGLE_MODE || f.HORIZON_MODE) {
            LED1_ON;
        } else {
            LED1_OFF;
        }

#ifdef BARO
        if (sensors(SENSOR_BARO)) {
            // Baro alt hold activate
            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;
            }
            // Vario signalling activate
            if (feature(FEATURE_VARIO)) {
                if (rcOptions[BOXVARIO]) {
                    if (!f.VARIO_MODE) {
                        f.VARIO_MODE = 1;
                    }
                } else {
                    f.VARIO_MODE = 0;
                }
            }
        }
#endif

#ifdef  MAG
        if (sensors(SENSOR_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 (sensors(SENSOR_GPS)) {
            if (f.GPS_FIX && GPS_numSat >= 5) {
                // if both GPS_HOME & GPS_HOLD are checked => GPS_HOME is the priority
                if (rcOptions[BOXGPSHOME]) {
                    if (!f.GPS_HOME_MODE) {
                        f.GPS_HOME_MODE = 1;
                        f.GPS_HOLD_MODE = 0;
                        GPSNavReset = 0;
                        GPS_set_next_wp(&GPS_home[LAT], &GPS_home[LON]);
                        nav_mode = NAV_MODE_WP;
                    }
                } else {
                    f.GPS_HOME_MODE = 0;
                    if (rcOptions[BOXGPSHOLD] && abs(rcCommand[ROLL]) < cfg.ap_mode && abs(rcCommand[PITCH]) < cfg.ap_mode) {
                        if (!f.GPS_HOLD_MODE) {
                            f.GPS_HOLD_MODE = 1;
                            GPSNavReset = 0;
                            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;
                        // both boxes are unselected here, nav is reset if not already done
                        if (GPSNavReset == 0) {
                            GPSNavReset = 1;
                            GPS_reset_nav();
                        }
                    }
                }
            } else {
                f.GPS_HOME_MODE = 0;
                f.GPS_HOLD_MODE = 0;
                nav_mode = NAV_MODE_NONE;
            }
        }

        if (rcOptions[BOXPASSTHRU]) {
            f.PASSTHRU_MODE = 1;
        } else {
            f.PASSTHRU_MODE = 0;
        }

        if (mcfg.mixerConfiguration == MULTITYPE_FLYING_WING || mcfg.mixerConfiguration == MULTITYPE_AIRPLANE) {
            f.HEADFREE_MODE = 0;
        }
    } else {                    // not in rc loop
예제 #10
0
void evaluateCommand(uint8_t c) {
	uint32_t i, tmp = 0, junk;
	uint8_t zczxczxczxc = 0;
	const char *build = __DATE__;

	switch (c) {
		// adding this message as a comment will return an error status for MSP_PRIVATE (end of switch), allowing third party tools to distinguish the implementation of this message
		//case MSP_PRIVATE:
		//  headSerialError();tailSerialReply(); // we don't have any custom msp currently, so tell the gui we do not use that
		//  break;
#if defined(CLEANFLIGHT)
	case MSP_API_VERSION:
		headSerialReply(1 + API_VERSION_LENGTH);
		serialize8(MSP_PROTOCOL_VERSION);
		serialize8(API_VERSION_MAJOR);
		serialize8(API_VERSION_MINOR);
		tailSerialReply();

		USE_CLEANFLIGHT_REPLIES = 1;

		break;
	case MSP_FC_VARIANT:
		headSerialReply(FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);

		for (i = 0; i < FLIGHT_CONTROLLER_IDENTIFIER_LENGTH; i++) {
			serialize8(flightControllerIdentifier[i]);
		}
		tailSerialReply();

		break;
	case MSP_FC_VERSION:
		headSerialReply(FLIGHT_CONTROLLER_VERSION_LENGTH);

		serialize8(FC_VERSION_MAJOR);
		serialize8(FC_VERSION_MINOR);
		serialize8(FC_VERSION_PATCH_LEVEL);
		tailSerialReply();

		break;

	case MSP_BOARD_INFO:
		headSerialReply(
			BOARD_IDENTIFIER_LENGTH +
			BOARD_HARDWARE_REVISION_LENGTH
			);
		for (i = 0; i < BOARD_IDENTIFIER_LENGTH; i++) {
			serialize8(boardIdentifier[i]);
		}

//#ifdef NAZE
//		serialize16(hardwareRevision);
//#else
		serialize16(0); // No other build targets currently have hardware revision detection.
//#endif

		tailSerialReply();

		break;

	case MSP_BF_BUILD_INFO: // BASEFLIGHT
		headSerialReply(11 + 4 + 4);
		for (i = 0; i < 11; i++)
			serialize8(build[i]); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc
		serialize32(0); // future exp
		serialize32(0); // future exp
		tailSerialReply();

	case MSP_BUILD_INFO: // CLEANFLIGHT
		headSerialReply(11 + 4 + 4);
		for (i = 0; i < 11; i++)
			serialize8(build[i]); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc
		serialize32(0); // future exp
		serialize32(0); // future exp

		/*headSerialReply(
			BUILD_DATE_LENGTH +
			BUILD_TIME_LENGTH +
			GIT_SHORT_REVISION_LENGTH
			);

		for (i = 0; i < BUILD_DATE_LENGTH; i++) {
			serialize8(buildDate[i]);
		}
		for (i = 0; i < BUILD_TIME_LENGTH; i++) {
			serialize8(buildTime[i]);
		}

		for (i = 0; i < GIT_SHORT_REVISION_LENGTH; i++) {
			serialize8(shortGitRevision[i]);
		}*/

		tailSerialReply();
		break;
#endif
	case MSP_SUPRESS_DATA_FROM_RX:
		supress_data_from_rx = read8();

		headSerialReply(1);
		serialize8((uint8_t)supress_data_from_rx);
		tailSerialReply();

		break;
	case MSP_SET_RAW_RC:
		s_struct_w((uint8_t*) &rcSerial, 16);
		rcSerialCount = 50; // 1s transition 
		break;
	case MSP_SET_PID:
		mspAck();
		s_struct_w((uint8_t*) &conf.pid[0].P8, 3 * PIDITEMS);
		break;
	case MSP_SET_BOX:
		mspAck();
#if EXTAUX
		s_struct_w((uint8_t*)&conf.activate[0],CHECKBOXITEMS*4);
#else
		s_struct_w((uint8_t*) &conf.activate[0], CHECKBOXITEMS * 2);
#endif
		break;
	case MSP_SET_RC_TUNING:
		mspAck();
		s_struct_w((uint8_t*) &conf.rcRate8, 7);
		break;
#if !defined(DISABLE_SETTINGS_TAB)
	case MSP_SET_MISC:
		struct {
			uint16_t a, b, c, d, e, f;
			uint32_t g;
			uint16_t h;
			uint8_t  i, j, k, l;
		} set_misc;
		mspAck();
		s_struct_w((uint8_t*) &set_misc, 22);
#if defined(POWERMETER)
		conf.powerTrigger1 = set_misc.a / PLEVELSCALE;
#endif
		conf.minthrottle = set_misc.b;
#ifdef FAILSAFE 
		conf.failsafe_throttle = set_misc.e;
#endif  
#if MAG
		conf.mag_declination = set_misc.h;
#endif
#if defined(VBAT)
		conf.vbatscale        = set_misc.i;
		conf.vbatlevel_warn1  = set_misc.j;
		conf.vbatlevel_warn2  = set_misc.k;
		conf.vbatlevel_crit   = set_misc.l;
#endif
		break;
	case MSP_MISC:
		struct {
#if defined(CLEANFLIGHT)
			uint16_t a, b, c, d, e;
			uint8_t f;
			uint8_t g;
			uint8_t h;

			uint8_t i;
			uint8_t j;
			uint8_t k;

			uint16_t l;

			uint8_t  m, n, o, p;
#else
			uint16_t a, b, c, d, e, f;
			uint32_t g;
			uint16_t h;
			uint8_t  i, j, k, l;
#endif
		} misc;
		misc.a = intPowerTrigger1;
		misc.b = conf.minthrottle;
		misc.c = MAXTHROTTLE;
		misc.d = MINCOMMAND;
#ifdef FAILSAFE 
		misc.e = conf.failsafe_throttle;
#else  
		misc.e = 0;
#endif

#if defined(CLEANFLIGHT)
#if GPS
		//serialize8(masterConfig.gpsConfig.provider); // gps_type
		//serialize8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
		//serialize8(masterConfig.gpsConfig.sbasMode); // gps_ubx_sbas
#else
		//f serialize8(0); // gps_type
		//g serialize8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
		//h serialize8(0); // gps_ubx_sbas
		misc.f = 0;
		misc.g = 1;
		misc.h = 0;
#endif
		// i serialize8(masterConfig.batteryConfig.multiwiiCurrentMeterOutput);
		// j serialize8(masterConfig.rxConfig.rssi_channel);
		// k serialize8(0);

		misc.i = 0;
		misc.j = 0;
		misc.k = 0;

	#if MAG
		misc.l = conf.mag_declination;
	#else
		misc.l = 0;
	#endif
	#ifdef VBAT
		misc.m = conf.vbatscale;
		misc.n = conf.vbatlevel_warn1;
		misc.o = conf.vbatlevel_warn2;
		misc.p = conf.vbatlevel_crit;
	#else
		misc.m = 0; misc.n = 0; misc.o = 0; misc.p = 0;
	#endif

		s_struct((uint8_t*) &misc, 32);
#else
	#ifdef LOG_PERMANENT
		misc.f = plog.arm;
		misc.g = plog.lifetime + (plog.armed_time / 1000000); // <- computation must be moved outside from serial
	#else
		misc.f = 0; misc.g = 0;
	#endif

	#if MAG
		misc.h = conf.mag_declination;
	#else
		misc.h = 0;
	#endif
	#ifdef VBAT
		misc.i = conf.vbatscale;
		misc.j = conf.vbatlevel_warn1;
		misc.k = conf.vbatlevel_warn2;
		misc.l = conf.vbatlevel_crit;
	#else
		misc.i = 0; misc.j = 0; misc.k = 0; misc.l = 0;
	#endif

		s_struct((uint8_t*) &misc, 22);
#endif
		
		break;
#endif
//#if defined (DYNBALANCE)
	case MSP_SET_MOTOR:
		mspAck();
		s_struct_w((uint8_t*)&motor_disarmed,16);

		break;
//#endif
#ifdef MULTIPLE_CONFIGURATION_PROFILES
	case MSP_SELECT_SETTING:
		if (!f.ARMED) {
			global_conf.currentSet = read8();
			if (global_conf.currentSet > 2) global_conf.currentSet = 0;
			writeGlobalSet(0);
			readEEPROM();
		}
		mspAck();
		break;
#endif
	case MSP_SET_HEAD:
		mspAck();
		s_struct_w((uint8_t*) &magHold, 2);
		break;
	case MSP_IDENT:
		struct {
			uint8_t v, t, msp_v;
			uint32_t cap;
		} id;
		id.v = VERSION;
		id.t = MULTITYPE;
		id.msp_v = MSP_VERSION;
		id.cap = (0 + BIND_CAPABLE) | DYNBAL << 2 | FLAP << 3 | NAVCAP << 4 | EXTAUX << 5 | ((uint32_t) NAVI_VERSION << 28); //Navi version is stored in the upper four bits; 
		s_struct((uint8_t*) &id, 7);
		break;
	case MSP_STATUS:
		struct {
			uint16_t cycleTime, i2c_errors_count, sensor;
			uint32_t flag;
			uint8_t set;
		} st;

		st.cycleTime = cycleTime;
		st.i2c_errors_count = i2c_errors_count;
		st.sensor = ACC | BARO << 1 | MAG << 2 | GPS << 3 | SONAR << 4;

#if ACC
		if (f.ANGLE_MODE)   tmp |= 1 << BOXANGLE;
		if (f.HORIZON_MODE) tmp |= 1 << BOXHORIZON;
#endif
#if BARO && (!defined(SUPPRESS_BARO_ALTHOLD))
		if (f.BARO_MODE) tmp |= 1 << BOXBARO;
#endif
		if (f.MAG_MODE) tmp |= 1 << BOXMAG;
#if !defined(FIXEDWING)
#if defined(HEADFREE)
		if(f.HEADFREE_MODE)       tmp |= 1<<BOXHEADFREE;
		if(rcOptions[BOXHEADADJ]) tmp |= 1<<BOXHEADADJ;
#endif
#endif
#if defined(SERVO_TILT) || defined(GIMBAL)|| defined(SERVO_MIX_TILT)
		if(rcOptions[BOXCAMSTAB]) tmp |= 1<<BOXCAMSTAB;
#endif
#if defined(CAMTRIG)
		if(rcOptions[BOXCAMTRIG]) tmp |= 1<<BOXCAMTRIG;
#endif
#if GPS
		switch (f.GPS_mode) {
		case GPS_MODE_HOLD:
			tmp |= 1 << BOXGPSHOLD;
			break;
		case GPS_MODE_RTH:
			tmp |= 1 << BOXGPSHOME;
			break;
		case GPS_MODE_NAV:
			tmp |= 1 << BOXGPSNAV;
			break;
		}
#endif
#if defined(FIXEDWING) || defined(HELICOPTER)
		if(f.PASSTHRU_MODE) tmp |= 1<<BOXPASSTHRU;
#endif
#if defined(BUZZER)
		if(rcOptions[BOXBEEPERON]) tmp |= 1<<BOXBEEPERON;
#endif
#if defined(LED_FLASHER)
		if(rcOptions[BOXLEDMAX]) tmp |= 1<<BOXLEDMAX;
		if(rcOptions[BOXLEDLOW]) tmp |= 1<<BOXLEDLOW;
#endif
#if defined(LANDING_LIGHTS_DDR)
		if(rcOptions[BOXLLIGHTS]) tmp |= 1<<BOXLLIGHTS;
#endif
#if defined(VARIOMETER)
		if(rcOptions[BOXVARIO]) tmp |= 1<<BOXVARIO;
#endif
#if defined(INFLIGHT_ACC_CALIBRATION)
		if (rcOptions[BOXCALIB]) tmp |= 1 << BOXCALIB;
#endif
#if defined(GOVERNOR_P)
		if(rcOptions[BOXGOV]) tmp |= 1<<BOXGOV;
#endif
#if defined(OSD_SWITCH)
		if(rcOptions[BOXOSD]) tmp |= 1<<BOXOSD;
#endif
#if defined(INFLIGHT_PID_TUNING)
		if (f.PIDTUNE_MODE) tmp |= 1 << BOXPIDTUNE;
#endif
#if SONAR
		if (f.SONAR_MODE) tmp |= 1 << BOXSONAR;
#endif
		if (f.ARMED) tmp |= 1 << BOXARM;
		st.flag = tmp;
		st.set = global_conf.currentSet;
		s_struct((uint8_t*) &st, 11);
		break;
	case MSP_RAW_IMU:
//#if defined(DYNBALANCE)
		for(uint8_t axis=0;axis<3;axis++) {imu.gyroData[axis]=imu.gyroADC[axis];imu.accSmooth[axis]= imu.accADC[axis];} // Send the unfiltered Gyro & Acc values to gui.
//#endif 
		s_struct((uint8_t*) &imu, 18);
		break;
	case MSP_SERVO:
		s_struct((uint8_t*) &servo, 16);
		break;
	case MSP_SERVO_CONF:
		s_struct((uint8_t*) &conf.servoConf[0].min, 56); // struct servo_conf_ is 7 bytes length: min:2 / max:2 / middle:2 / rate:1    ----     8 servo =>  8x7 = 56
		break;
	case MSP_SET_SERVO_CONF:
		mspAck();
		s_struct_w((uint8_t*) &conf.servoConf[0].min, 56);
		break;
	case MSP_MOTOR:
		s_struct((uint8_t*) &motor, 16);
		break;
	case MSP_ACC_TRIM:
		headSerialReply(4);
		s_struct_partial((uint8_t*) &conf.angleTrim[PITCH], 2);
		s_struct_partial((uint8_t*) &conf.angleTrim[ROLL], 2);
		tailSerialReply();
		break;
	case MSP_SET_ACC_TRIM:
		mspAck();
		s_struct_w((uint8_t*) &conf.angleTrim[PITCH], 2);
		s_struct_w((uint8_t*) &conf.angleTrim[ROLL], 2);
		break;
	case MSP_RC:
		s_struct((uint8_t*) &rcData, RC_CHANS * 2);
		break;
#if GPS
	case MSP_SET_RAW_GPS:
		struct {
			uint8_t a, b;
			int32_t c, d;
			int16_t e;
			uint16_t f;
		} set_set_raw_gps;
		mspAck();
		s_struct_w((uint8_t*) &set_set_raw_gps, 14);
		f.GPS_FIX = set_set_raw_gps.a;
		GPS_numSat = set_set_raw_gps.b;
		GPS_coord[LAT] = set_set_raw_gps.c;
		GPS_coord[LON] = set_set_raw_gps.d;
		GPS_altitude = set_set_raw_gps.e;
		GPS_speed = set_set_raw_gps.f;
		GPS_update |= 2;              // New data signalisation to GPS functions
		break;
	case MSP_RAW_GPS:
		struct {
			uint8_t a, b;
			int32_t c, d;
			int16_t e;
			uint16_t f, g;
		} msp_raw_gps;
		msp_raw_gps.a = f.GPS_FIX;
		msp_raw_gps.b = GPS_numSat;
		msp_raw_gps.c = GPS_coord[LAT];
		msp_raw_gps.d = GPS_coord[LON];
		msp_raw_gps.e = GPS_altitude;
		msp_raw_gps.f = GPS_speed;
		msp_raw_gps.g = GPS_ground_course;

		s_struct((uint8_t*) &msp_raw_gps, 16);
		break;
	case MSP_GPSSVINFO:
		headSerialReply(1 + (GPS_numCh * 4));
		
		serialize8(GPS_numCh);
		for (i = 0; i < GPS_numCh; i++){
			serialize8(GPS_svinfo_chn[i]);
			serialize8(GPS_svinfo_svid[i]);
			serialize8(GPS_svinfo_quality[i]);
			serialize8(GPS_svinfo_cno[i]);
		}
		tailSerialReply();

		break;
	case MSP_COMP_GPS:
		struct {
			uint16_t a;
			int16_t b;
			uint8_t c;
		} msp_comp_gps;
		msp_comp_gps.a = GPS_distanceToHome;
		msp_comp_gps.b = GPS_directionToHome;
		msp_comp_gps.c = GPS_update & 1;
		s_struct((uint8_t*) &msp_comp_gps, 5);
		break;
#if defined(USE_MSP_WP)
	case MSP_SET_NAV_CONFIG:
		mspAck();
		s_struct_w((uint8_t*)&GPS_conf,sizeof(GPS_conf));
		break;
	case MSP_NAV_CONFIG:
		s_struct((uint8_t*)&GPS_conf,sizeof(GPS_conf));
		break;
	case MSP_NAV_STATUS: // to move to struct transmission
		headSerialReply(7);
		serialize8(f.GPS_mode);
		serialize8(NAV_state);
		serialize8(mission_step.action);
		serialize8(mission_step.number);
		serialize8(NAV_error);
		serialize16( (int16_t)(target_bearing/100));
		//serialize16(magHold);
		tailSerialReply();
		break;
	case MSP_WP: // to move to struct transmission
	{
						uint8_t wp_no;
						uint8_t flag;
						bool    success;

						wp_no = read8(); //get the wp number  
						headSerialReply(21);
						if (wp_no == 0) { //Get HOME coordinates
							serialize8(wp_no);
							serialize8(mission_step.action);
							serialize32(GPS_home[LAT]);
							serialize32(GPS_home[LON]);
							flag = MISSION_FLAG_HOME;
						}
						if (wp_no == 255) { //Get poshold coordinates
							serialize8(wp_no);
							serialize8(mission_step.action);
							serialize32(GPS_hold[LAT]);
							serialize32(GPS_hold[LON]);
							flag = MISSION_FLAG_HOLD;
						}
						if ((wp_no>0) && (wp_no<255)) {
							if (NAV_state == NAV_STATE_NONE) {
								success = recallWP(wp_no);
								serialize8(wp_no);
								serialize8(mission_step.action);
								serialize32(mission_step.pos[LAT]);
								serialize32(mission_step.pos[LON]);
								if (success == true) flag = mission_step.flag;
								else flag = MISSION_FLAG_CRC_ERROR; //CRC error
							} else {
								serialize8(wp_no);
								serialize8(0);
								serialize32(GPS_home[LAT]);
								serialize32(GPS_home[LON]);
								flag = MISSION_FLAG_NAV_IN_PROG;
							}
						}
						serialize32(mission_step.altitude);
						serialize16(mission_step.parameter1);
						serialize16(mission_step.parameter2);
						serialize16(mission_step.parameter3);
						serialize8(flag);
						tailSerialReply();
	}
		break;
	case MSP_SET_WP: // to move to struct transmission
	{
							uint8_t wp_no = read8(); //Get the step number

							if (NAV_state == NAV_STATE_HOLD_INFINIT && wp_no == 255) { //Special case - during stable poshold we allow change the hold position
								mission_step.number = wp_no;
								mission_step.action = MISSION_HOLD_UNLIM; 
								uint8_t temp = read8();
								mission_step.pos[LAT] =  read32();
								mission_step.pos[LON] =  read32();
								mission_step.altitude =  read32();
								mission_step.parameter1 = read16();
								mission_step.parameter2 = read16();
								mission_step.parameter3 = read16();
								mission_step.flag     =  read8();
								if (mission_step.altitude != 0) set_new_altitude(mission_step.altitude);
								GPS_set_next_wp(&mission_step.pos[LAT], &mission_step.pos[LON], &GPS_coord[LAT], &GPS_coord[LON]);
								if ((wp_distance/100) >= GPS_conf.safe_wp_distance) NAV_state = NAV_STATE_NONE;
								else NAV_state = NAV_STATE_WP_ENROUTE;
								break;
							}
							if (NAV_state == NAV_STATE_NONE) { // The Nav state is not zero, so navigation is in progress, silently ignore SET_WP command)
								mission_step.number     =  wp_no;
								mission_step.action     =  read8();
								mission_step.pos[LAT]   =  read32();
								mission_step.pos[LON]   =  read32();
								mission_step.altitude   =  read32();
								mission_step.parameter1 = read16();
								mission_step.parameter2 = read16();
								mission_step.parameter3 = read16();
								mission_step.flag       =  read8();
								//It's not sure, that we want to do poshold change via mission planner so perhaps the next if is deletable
								/*
								if (mission_step.number == 255) //Set up new hold position via mission planner, It must set the action to MISSION_HOLD_INFINIT 
								{
								if (mission_step.altitude !=0) set_new_altitude(mission_step.altitude); //Set the altitude
								GPS_set_next_wp(&mission_step.pos[LAT], &mission_step.pos[LON], &GPS_coord[LAT], &GPS_coord[LON]);
								NAV_state = NAV_STATE_WP_ENROUTE; //Go to that position, then it will switch to poshold unlimited when reached
								}
								*/
								if (mission_step.number == 0) { //Set new Home position
									GPS_home[LAT] = mission_step.pos[LAT];
									GPS_home[LON] = mission_step.pos[LON];
								}
								if (mission_step.number >0 && mission_step.number<255)                      //Not home and not poshold, we are free to store it in the eprom
								if (mission_step.number <= getMaxWPNumber())                              // Do not thrash the EEPROM with invalid wp number
									storeWP();
								mspAck();
							}
	}
		break;
#endif //USE_MSP_WP
#endif //GPS
	case MSP_ATTITUDE:
		s_struct((uint8_t*) &att, 6);
		break;
	case MSP_ALTITUDE:
		s_struct((uint8_t*) &alt, 6);
		break;
	case MSP_ANALOG:
		s_struct((uint8_t*) &analog, 7);
		break;
	case MSP_RC_TUNING:
		s_struct((uint8_t*) &conf.rcRate8, 7);

		break;
	case MSP_PID:
		s_struct((uint8_t*) &conf.pid[0].P8, 3 * PIDITEMS);

		break;
	case MSP_PIDNAMES:
		serializeNames(pidnames);

		break;
#if defined(CLEANFLIGHT)
	case MSP_PID_CONTROLLER:
		headSerialReply(1);
		//s_struct((uint8_t *) PID_CONTROLLER, 7);
		serialize8(PID_CONTROLLER - 1);
		tailSerialReply();

		break;
#endif
	case MSP_BOX:
#if EXTAUX
		s_struct((uint8_t*)&conf.activate[0],4*CHECKBOXITEMS);
#else
		s_struct((uint8_t*) &conf.activate[0], 2 * CHECKBOXITEMS);
#endif
		break;
	case MSP_BOXNAMES:
		serializeNames(boxnames);
		break;
	case MSP_BOXIDS:
		headSerialReply(CHECKBOXITEMS);
		for (uint8_t i = 0; i < CHECKBOXITEMS; i++)
			serialize8(pgm_read_byte(&(boxids[i])));
		tailSerialReply();
		break;
	case MSP_MOTOR_PINS:
		s_struct((uint8_t*) &PWM_PIN, 8);
		break;
	case MSP_RESET_CONF:
		if (!f.ARMED) LoadDefaults();
		mspAck();
		break;
	case MSP_ACC_CALIBRATION:
		if (!f.ARMED) calibratingA = 512;
		mspAck();
		break;
#if GYRO
	case MSP_GYRO_CALIBRATION:
		if (!f.ARMED) calibratingG = 512;
		mspAck();
		break;
#endif
#if MAG
	case MSP_MAG_CALIBRATION:
		if (!f.ARMED) f.CALIBRATE_MAG = 1;
		mspAck();
		break;
#endif
#if defined(SPEK_BIND)
	case MSP_BIND:
		spekBind();
		mspAck();
		break;
#endif
	case MSP_EEPROM_WRITE:
		writeParams(0);
		mspAck();
		break;
	case MSP_DEBUG:
		s_struct((uint8_t*) &debug, 8);
		break;
#if defined(CLEANFLIGHT)
	case MSP_BF_CONFIG:
		// baseflight
		//headSerialReply(1 + 4 + 1 + 2 + 2 + 2 + 2 + 2 + 2 + 2);
		headSerialReply(1 + 4 + 1 + 2 + 2 + 2 + 2 + 2 + 2 + 2);
		serialize8((uint8_t) MULTITYPE); // QUADX
		
		// features
		serialize32(1 << 4 | 1 << 9 | 1 << 2); // MOTOR_STOP, FEATURE_SONAR, FEATURE_INFLIGHT_ACC_CAL
		//serialize32((uint32_t)0); // MOTOR_STOP, FEATURE_SONAR

		// rx provider
		serialize8((uint8_t) 0);

		// board alignment
		serialize16((uint16_t) 0);
		serialize16((uint16_t) 0);
		serialize16((uint16_t) 0);

		// battery
		serialize16((uint16_t) 0);
		serialize16((uint16_t) 0);

		// motor_pwn_rate
		serialize16((uint16_t) 0);

		// pitch and roll rate
		// serialize8((uint8_t) s_struct((uint8_t*) &, 7););
		serialize8((uint8_t) conf.rollPitchRate);
		serialize8((uint8_t) conf.rollPitchRate);

		/* ? baseflight
		serialize16(mcfg.currentscale);
		serialize16(mcfg.currentoffset);
		serialize16(mcfg.motor_pwm_rate);
		serialize8(cfg.rollPitchRate[0]);
		serialize8(cfg.rollPitchRate[1]);
		*/

		// battery
		/*serialize16((uint16_t) 0);
		serialize16((uint16_t) 0);*/

		tailSerialReply();

		break;
	case MSP_CF_SERIAL_CONFIG:
		headSerialReply(
			((sizeof(uint8_t) +sizeof(uint16_t) +(sizeof(uint8_t) * 4)) * 4)
			);
		for (i = 0; i < 4; i++) {
			serialize8(0);
			serialize16(0);
			serialize8(0);
			serialize8(0);
			serialize8(0);
			serialize8(0);
		}
		tailSerialReply();
		break;
	case MSP_UID:
		headSerialReply(12);
		serialize32(U_ID_0);
		serialize32(U_ID_1);
		serialize32(U_ID_2);
		tailSerialReply();
		break;
#endif
#ifdef DEBUGMSG
	case MSP_DEBUGMSG:
	{
							uint8_t size = debugmsg_available();
							if (size > 16) size = 16;
							headSerialReply(size);
							debugmsg_serialize(size);
							tailSerialReply();
	}
		break;
#endif
	default:  // we do not know how to handle the (valid) message, indicate error MSP $M!
		headSerialError(); tailSerialReply();
		break;
	}
}
예제 #11
0
/**
  * @brief  手柄姿态控制
  *         
  * @param  接收到的数据指针,在XBEE里
  * @retval None
  */
void HandAttitude_Ctrl(uint8_t *hand_data)
{
	static uint8_t first_in=0,first_in2=0;
	static int32_t GPS_HoldOffset[2];
	static double Point_Distance=0;
	int32_t gps_coord[2];
	static int32_t gps_coord2[2];
	int16_t mag_bear;
	float val;
	Handle_LostCnt=0;//实时清0错误计数

	
	if(f.HANDLE_CTRL_MODE==0)return;//没开启此模式
	
	//普通无GPS下的姿态控制    和    跟随
	if(hand_data[1]==0x30||hand_data[1]==0x31)
	{
		if(!(hand_data[7]&0x01))//没有开启悬停
		{
			HandAttitude_CtrlAngle[ROLL]=constrain((hand_data[2]-35),-35,35);//单位度
			HandAttitude_CtrlAngle[PITCH]=constrain((hand_data[3]-35),-35,35);
			magHold=(hand_data[4]<<8)|hand_data[5];
		}
		else//悬停,无姿态控制
		{
			HandAttitude_CtrlAngle[ROLL]=0;
			HandAttitude_CtrlAngle[PITCH]=0;
		}
		
		if(hand_data[6]==1)//按上升键
		{
			if(HandAttitude_RcDataTh<1450)
			{
				if(f.ARMED)HandAttitude_RcDataTh+=5;
				f.BARO_MODE = 0;
				f.SONAR_MODE = 0;
				AltHold_Baro=50.0f;
				AltHold_Sonar = 50.0f;
			}
			else
			{
				AltHold_Baro+=3.0f;//应该控制在1秒增加半米
				AltHold_Sonar += 3.0f;
			}
		}
		else if(hand_data[6]==2)//按下降键
		{
			if(f.SONAR_MODE)//超声,
			{
				if(AltHold_Sonar>-50.0f)//只有超声才会进行关闭电机判断
				{
					AltHold_Sonar-=3.0f;
				}
				else
				{
					if(EstAlt_Sonar<20.0f)
						if(HandAttitude_RcDataTh>1000)HandAttitude_RcDataTh-=5;
				}
			}
			AltHold_Baro-=3.0f;//气压
		}
		else if(hand_data[6]==4)//行动键双击,则进行上锁或者解锁
		{
			if(f.ARMED)
				f.ARMED = 0;		//上锁
			else if(f.OK_TO_ARM&&!f.ARMED)
			{
				HandAttitude_RcDataTh=1000;
				AltHold_Baro=0;
				AltHold_Sonar=0.0f;
				CalibratingG = 1000;		//校准陀螺仪	1000
				CalibratingB = 10;			//校准气压计,也就是设定气压计原点
				f.ARMED = 1;		//解锁
			}
		}
		else if(hand_data[6]==3)//两个按键长按1秒,校准
		{
			if(!f.ARMED)
			{
				CalibratingA = 400;		//触发加速度计校准
			}
		}
		//飞机跟随手柄运动
		if(hand_data[1]==0x31)//跟随
		{
			gps_coord[LON]=(hand_data[11] << 24) + (hand_data[10] << 16) + (hand_data[9] << 8) + hand_data[8];
			gps_coord[LAT]=(hand_data[15] << 24) + (hand_data[14] << 16) + (hand_data[13] << 8) + hand_data[12];
			if(first_in)
			{
				first_in = 0;
				GPS_HoldOffset[LON]=GPS_coord[LON]-gps_coord[LON];
				GPS_HoldOffset[LAT]=GPS_coord[LAT]-gps_coord[LAT];
			}
			else
			{
				if(abs(GPS_hold[LON]-(gps_coord[LON]+GPS_HoldOffset[LON]))>50||abs(GPS_hold[LAT]-(gps_coord[LAT]+GPS_HoldOffset[LAT]))>50)
				{
					GPS_hold[LON]=gps_coord[LON]+GPS_HoldOffset[LON];//经度
					GPS_hold[LAT]=gps_coord[LAT]+GPS_HoldOffset[LAT];//纬度
					GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
				}
			}
			GPSFollow_Mode = 1;
		}
		else
		{
			first_in = 1;
			if(hand_data[7]&0x01)//悬停
				GPSFollow_Mode = 1;
			else
				GPSFollow_Mode = 0;
		}
		
		if((hand_data[7]&0x04)&&f.SONAR_AVAILABLE  &&f.ARMED )//一键起飞或者一键降落
		{
			if(EstAlt_Sonar<15.0f)//take off
			{
				//magHold = heading-7;
				if (magHold > 180)
					magHold = magHold - 360;
				else if (magHold < -180)
					magHold = magHold + 360;
				HandAttitude_RcDataTh = 1450;
				AltHold_TakeOff = 150.0f;
				f.AUTOTAKEOFF=1;
				f.AUTOLANDING=0;
			}
			else//landing
			{
				f.AUTOLANDING=1;
				f.AUTOTAKEOFF=0;
			}
		}
		
		
		if(hand_data[7]&0x02)OneKey_Roll_Trig=1;//触发一键翻滚
		
		first_in2 = 1;
	}
	else if(hand_data[1]==0x32)//指点飞行
	{
		HandAttitude_CtrlAngle[ROLL]=0;
		HandAttitude_CtrlAngle[PITCH]=0;

		if(first_in2)
		{
			first_in2 = 0;//保存第一次距离
			
			gps_coord2[LON]=(hand_data[11] << 24) + (hand_data[10] << 16) + (hand_data[9] << 8) + hand_data[8];//手柄的GPS坐标
			gps_coord2[LAT]=(hand_data[15] << 24) + (hand_data[14] << 16) + (hand_data[13] << 8) + hand_data[12];
			Point_Distance = sqrt((gps_coord2[LON] - GPS_coord[LON])*(gps_coord2[LON] - GPS_coord[LON])+(gps_coord2[LAT] - GPS_coord[LAT])*(gps_coord2[LAT] - GPS_coord[LAT]));
		}
		else
		{
			mag_bear = (hand_data[4]<<8)|hand_data[5];
			gps_coord[LON] = gps_coord2[LON] + Point_Distance*sinf((float)mag_bear*2.0f*PI/360.0f);//经度
			gps_coord[LAT] = gps_coord2[LAT] + Point_Distance*cosf((float)mag_bear*2.0f*PI/360.0f);//纬度
			if(abs(GPS_hold[LON]-gps_coord[LON])>50||abs(GPS_hold[LAT]-gps_coord[LAT])>50)
			{
				GPS_hold[LON]=gps_coord[LON];//经度
				GPS_hold[LAT]=gps_coord[LAT];//纬度
				GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
			}
		}
		GPSFollow_Mode = 1;
	}
	else
	{
		GPSFollow_Mode = 0;
	}


//  	debug[0]=GPS_hold[LAT];
//  	debug[1]=mag_bear;
// 	debug[2]=Point_Distance;
// 	debug[3]=Point_Distance*cosf((float)mag_bear*2.0f*PI/360.0f);//纬度
}
예제 #12
0
void updateGpsWaypointsAndMode(void)
{
    bool resetNavNow = false;
    static bool gpsReadyBeepDone = false;

    if (STATE(GPS_FIX) && GPS_numSat >= 5) {

        //
        // process HOME mode
        //
        // HOME mode takes priority over HOLD mode.

        if (rcModeIsActive(BOXGPSHOME)) {
            if (!FLIGHT_MODE(GPS_HOME_MODE)) {

                // Transition to HOME mode
                ENABLE_FLIGHT_MODE(GPS_HOME_MODE);
                DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);
                GPS_set_next_wp(&GPS_home[LAT], &GPS_home[LON]);
                nav_mode = NAV_MODE_WP;
                resetNavNow = true;
            }
        } else {
            if (FLIGHT_MODE(GPS_HOME_MODE)) {

                // Transition from HOME mode
                DISABLE_FLIGHT_MODE(GPS_HOME_MODE);
                nav_mode = NAV_MODE_NONE;
                resetNavNow = true;
            }

            //
            // process HOLD mode
            //

            if (rcModeIsActive(BOXGPSHOLD) && areSticksInApModePosition(gpsProfile()->ap_mode)) {
                if (!FLIGHT_MODE(GPS_HOLD_MODE)) {

                    // Transition to HOLD mode
                    ENABLE_FLIGHT_MODE(GPS_HOLD_MODE);
                    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;
                    resetNavNow = true;
                }
            } else {
                if (FLIGHT_MODE(GPS_HOLD_MODE)) {

                    // Transition from HOLD mode
                    DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);
                    nav_mode = NAV_MODE_NONE;
                    resetNavNow = true;
                }
            }
        }
        if (!gpsReadyBeepDone) {            //if 'ready' beep not yet done
            beeper(BEEPER_READY_BEEP);      //do ready beep now
            gpsReadyBeepDone = true;        //only beep once
        }
    } else {
        if (FLIGHT_MODE(GPS_HOLD_MODE | GPS_HOME_MODE)) {

            // Transition from HOME or HOLD mode
            DISABLE_FLIGHT_MODE(GPS_HOME_MODE);
            DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);
            nav_mode = NAV_MODE_NONE;
            resetNavNow = true;
        }
    }

    if (resetNavNow) {
        GPS_reset_nav();
    }
}
예제 #13
0
파일: main.c 프로젝트: jono91/xpressocopter
/*******************************************************************************
**   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