Example #1
0
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;
  }
}
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();
}
Example #3
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;
	}
}