예제 #1
0
size_t CNetworkCommandSync::Serialize(unsigned char *buf) const
{
	unsigned char *p = buf;
	p += serialize32(p, this->syncSeed);
	p += serialize32(p, this->syncHash);
	return p - buf;
}
예제 #2
0
const unsigned char *CInitMessage_Hello::Serialize() const
{
	unsigned char *buf = new unsigned char[Size()];
	unsigned char *p = buf;

	p += header.Serialize(p);
	p += serialize(p, this->PlyName);
	p += serialize32(p, this->Stratagus);
	p += serialize32(p, this->Version);
	return buf;
}
예제 #3
0
const unsigned char *CInitMessage_Welcome::Serialize() const
{
	unsigned char *buf = new unsigned char[Size()];
	unsigned char *p = buf;

	p += header.Serialize(p);
	for (int i = 0; i < PlayerMax; ++i) {
		p += this->hosts[i].Serialize(p);
	}
	p += serialize32(p, this->Lag);
	p += serialize32(p, this->gameCyclesPerUpdate);
	return buf;
}
예제 #4
0
const unsigned char *CInitMessage_ProtocolMismatch::Serialize() const
{
	unsigned char *buf = new unsigned char[Size()];
	unsigned char *p = buf;

	p += header.Serialize(p);
	p += serialize32(p, this->Version);
	return buf;
}
예제 #5
0
const unsigned char *CInitMessage_EngineMismatch::Serialize() const
{
	unsigned char *buf = new unsigned char[Size()];
	unsigned char *p = buf;

	p += header.Serialize(p);
	p += serialize32(p, this->Stratagus);
	return buf;
}
예제 #6
0
const unsigned char *CInitMessage_Map::Serialize() const
{
	unsigned char *buf = new unsigned char[Size()];
	unsigned char *p = buf;

	p += header.Serialize(p);
	p += serialize(p, MapPath);
	p += serialize32(p, this->MapUID);
	return buf;
}
예제 #7
0
size_t CNetworkHost::Serialize(unsigned char *buf) const
{
	unsigned char *p = buf;

	p += serialize32(p, this->Host);
	p += serialize16(p, this->Port);
	p += serialize16(p, this->PlyNr);
	p += serialize(p, this->PlyName);

	return p - buf;
}
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();
}
예제 #9
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();
}
예제 #10
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();
}
예제 #11
0
파일: serial.c 프로젝트: di9it/therminator
static void evaluateCommand(void)
{
    uint32_t i;
    uint8_t wp_no;

    switch (cmdMSP) {
    case MSP_SET_RAW_RC:
        for (i = 0; i < 8; i++)
            rcData[i] = 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 < CHECKBOXITEMS; i++)
            cfg.activate[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:
        headSerialReply(0);
        break;
    case MSP_IDENT:
        headSerialReply(7);
        serialize8(VERSION);                // multiwii version
        serialize8(cfg.mixerConfiguration); // type of multicopter
        serialize8(MSP_VERSION);            // MultiWii Serial Protocol Version
        serialize32(PLATFORM_32BIT);        // "capability"
        break;
    case MSP_STATUS:
        headSerialReply(10);
        serialize16(cycleTime);
        serialize16(i2cGetErrorCounter());
        serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3);
        serialize32(f.ANGLE_MODE << BOXANGLE | f.HORIZON_MODE << BOXHORIZON | f.BARO_MODE << BOXBARO | f.MAG_MODE << BOXMAG | f.ARMED << BOXARM |
                    rcOptions[BOXCAMSTAB] << BOXCAMSTAB | rcOptions[BOXCAMTRIG] << BOXCAMTRIG |
                    f.GPS_HOME_MODE << BOXGPSHOME | f.GPS_HOLD_MODE << BOXGPSHOLD | f.HEADFREE_MODE << BOXHEADFREE | f.PASSTHRU_MODE << BOXPASSTHRU |
                    rcOptions[BOXBEEPERON] << BOXBEEPERON | rcOptions[BOXLEDMAX] << BOXLEDMAX | rcOptions[BOXLLIGHTS] << BOXLLIGHTS | rcOptions[BOXHEADADJ] << BOXHEADADJ);
        break;
    case MSP_RAW_IMU:
        headSerialReply(18);
        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:
        headSerialReply(16);
        for (i = 0; i < 8; i++)
            serialize16(servo[i]);
        break;
    case MSP_MOTOR:
        headSerialReply(16);
        for (i = 0; i < 8; i++)
            serialize16(motor[i]);
        break;
    case MSP_RC:
        headSerialReply(16);
        for (i = 0; i < 8; i++)
            serialize16(rcData[i]);
        break;
    case MSP_RAW_GPS:
        headSerialReply(14);
        serialize8(f.GPS_FIX);
        serialize8(GPS_numSat);
        serialize32(GPS_coord[LAT]);
        serialize32(GPS_coord[LON]);
        serialize16(GPS_altitude);
        serialize16(GPS_speed);
        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(4);
        serialize32(EstAlt);
        break;
    case MSP_BAT:
        headSerialReply(3);
        serialize8(vbat);
        serialize16(0); // power meter trash
        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_BOX:
        headSerialReply(2 * CHECKBOXITEMS);
        for (i = 0; i < CHECKBOXITEMS; i++)
            serialize16(cfg.activate[i]);
        break;
    case MSP_BOXNAMES:
        headSerialReply(sizeof(boxnames) - 1);
        serializeNames(boxnames);
        break;
    case MSP_PIDNAMES:
        headSerialReply(sizeof(pidnames) - 1);
        serializeNames(pidnames);
        break;
    case MSP_MISC:
        headSerialReply(2);
        serialize16(0); // intPowerTrigger1
        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(12);
        if (wp_no == 0) {
            serialize8(0);                   // wp0
            serialize32(GPS_home[LAT]);
            serialize32(GPS_home[LON]);
            serialize16(0);                  // altitude will come here
            serialize8(0);                   // nav flag will come here
        } else if (wp_no == 16) {
            serialize8(16);                  // wp16
            serialize32(GPS_hold[LAT]);
            serialize32(GPS_hold[LON]);
            serialize16(0);                  // altitude will come here
            serialize8(0);                   // nav flag will come here
        }
        break;
    case MSP_RESET_CONF:
        checkFirstTime(true);
        headSerialReply(0);
        break;
    case MSP_ACC_CALIBRATION:
        calibratingA = 400;
        headSerialReply(0);
        break;
    case MSP_MAG_CALIBRATION:
        f.CALIBRATE_MAG = 1;
        headSerialReply(0);
        break;
    case MSP_EEPROM_WRITE:
        writeParams(0);
        headSerialReply(0);
        break;
    case MSP_DEBUG:
        headSerialReply(8);
        for (i = 0; i < 4; i++)
            serialize16(debug[i]);      // 4 variables are here for general monitoring purpose
        break;
    default:                   // we do not know how to handle the (valid) message, indicate error MSP $M!
        headSerialError(0);
        break;
    }
    tailSerialReply();
}
예제 #12
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;
  }
}
예제 #13
0
파일: serial.c 프로젝트: Reini60/harakiri
static void evaluateCommand(void)
{
    uint32_t i;
    uint8_t wp_no;

    switch (cmdMSP)
    {
    case MSP_SET_RAW_RC:
        for (i = 0; i < 8; i++)
            rcDataSAVE[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 < CHECKBOXITEMS; i++)
        {
            if (cfg.auxChannels > 4) cfg.activate[i] = read32();
            else cfg.activate[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:
        headSerialReply(0);
        break;
    case MSP_IDENT:
        headSerialReply(7);
        serialize8(VERSION);                // multiwii version
        serialize8(cfg.mixerConfiguration); // type of multicopter
        serialize8(MSP_VERSION);            // MultiWii Serial Protocol Version
        serialize32(PLATFORM_32BIT);        // "capability"
        break;
    case MSP_STATUS:
        headSerialReply(10);
        serialize16(cycleTime);
        serialize16(i2cGetErrorCounter());
        serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
        serialize32(f.ANGLE_MODE << BOXANGLE | f.HORIZON_MODE << BOXHORIZON | f.BARO_MODE << BOXBARO | f.MAG_MODE << BOXMAG | f.ARMED << BOXARM |
                    rcOptions[BOXCAMSTAB] << BOXCAMSTAB | rcOptions[BOXCAMTRIG] << BOXCAMTRIG |
                    f.GPS_HOME_MODE << BOXGPSHOME | f.GPS_HOLD_MODE << BOXGPSHOLD | f.HEADFREE_MODE << BOXHEADFREE | f.PASSTHRU_MODE << BOXPASSTHRU |
                    rcOptions[BOXBEEPERON] << BOXBEEPERON | rcOptions[BOXLEDMAX] << BOXLEDMAX | rcOptions[BOXLLIGHTS] << BOXLLIGHTS | rcOptions[BOXHEADADJ] << BOXHEADADJ);
        break;
    case MSP_RAW_IMU:
        headSerialReply(18);
        if (!feature(FEATURE_PASS))                               // Just Do the normal stuff
        {
            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((int16_t)magADCfloat[i]);
        }
        else                                                      // Just serialize unfiltered AccZ for Balancing
        {
            for (i = 0; i < 2; i++) serialize16(0);
            serialize16(accADC[YAW] - acc_1G);                    // Put accz into the middle
            for (i = 0; i < 3; i++) serialize16(0);
            for (i = 0; i < 3; i++) serialize16(0);
        }
        break;
    case MSP_SERVO:
        headSerialReply(16);
        for (i = 0; i < 8; i++)
            serialize16(servo[i]);
        break;
    case MSP_MOTOR:
        headSerialReply(16);
        for (i = 0; i < 8; i++)
            serialize16(motor[i]);
        break;
    case MSP_RC:
        headSerialReply((cfg.auxChannels + 4) * 2); // headSerialReply(16);
        for (i = 0; i < cfg.auxChannels + 4; i++)   // for (i = 0; i < 8; i++)
            serialize16(rcDataSAVE[i]);
        break;
    case MSP_RAW_GPS:
        headSerialReply(14);
        serialize8(f.GPS_FIX);
        serialize8(GPS_numSat);
        if (cfg.gps_debug == 1)
        {
            serialize32(Real_GPS_coord[LAT]);
            serialize32(Real_GPS_coord[LON]);
        }
        else
        {
            serialize32(GPS_coord[LAT]);
            serialize32(GPS_coord[LON]);
        }
        serialize16(GPS_altitude);
        serialize16(GPS_speed);
        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((int16_t)heading);
        serialize16((int16_t)headFreeModeHold);
        break;
    case MSP_ALTITUDE:
        headSerialReply(4);
        if (feature(FEATURE_PASS))
            serialize32(0);
        else
            serialize32(EstAlt);
        break;
    case MSP_BAT:
        headSerialReply(3);
        serialize8(vbat);
        serialize16(0); // power meter trash
        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_BOX:
        if (cfg.auxChannels > 4) headSerialReply(4 * CHECKBOXITEMS);
        else headSerialReply(2 * CHECKBOXITEMS);
        for (i = 0; i < CHECKBOXITEMS; i++)
        {
            if (cfg.auxChannels > 4) serialize32(cfg.activate[i]);
            else serialize16(cfg.activate[i]);
        }
        break;
    case MSP_BOXNAMES:
        headSerialReply(sizeof(boxnames) - 1);
        serializeNames(boxnames);
        break;
    case MSP_PIDNAMES:
        headSerialReply(sizeof(pidnames) - 1);
        serializeNames(pidnames);
        break;
    case MSP_MISC:
        headSerialReply(2);
        serialize16(0); // intPowerTrigger1
        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(12);
        if (wp_no == 0)
        {
            serialize8(0);                   // wp0
            serialize32(GPS_home[LAT]);
            serialize32(GPS_home[LON]);
            serialize16(0);                  // altitude will come here
            serialize8(0);                   // nav flag will come here
        }
        else if (wp_no == 16)
        {
            serialize8(16);                  // wp16
            serialize32(GPS_WP[LAT]);
            serialize32(GPS_WP[LON]);
            serialize16(0);                  // altitude will come here
            serialize8(0);                   // nav flag will come here
        }
        break;
    case MSP_RESET_CONF:
        checkFirstTime(true);
        headSerialReply(0);
        break;
    case MSP_ACC_CALIBRATION:
        calibratingA = 400;
        headSerialReply(0);
        break;
    case MSP_MAG_CALIBRATION:
        f.CALIBRATE_MAG = 1;
        headSerialReply(0);
        break;
    case MSP_EEPROM_WRITE:
        writeParams(0);
        headSerialReply(0);
        break;
    case MSP_ACC_TRIM:
        headSerialReply(4);
        serialize16(cfg.angleTrim[PITCH]);
        serialize16(cfg.angleTrim[ROLL]);
        break;
    case MSP_DEBUG:
        headSerialReply(8);
        for (i = 0; i < 4; i++) serialize16(debug[i]);      // 4 variables are here for general monitoring purpose
        break;

        // new @Johannes based of MultiWii V2.21
    case MSP_SERVO_CONF:  // @Johannes:
        //s_struct((uint8_t*)&cfg.servoConf[0].min,56); // struct servo_conf_ is 7 bytes length: min:2 / max:2 / middle:2 / rate:1    ----     8 servo =>  8x7 = 56
        headSerialReply(56);
        for(i=0;i<8;i++) { // new servostructure Johannes: 
            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: // @Johannes:
        //s_struct_w((uint8_t*)&cfg.servoConf[0].min,56);	 doesn't work Johannes
        headSerialReply(0);
        for(i=0;i<8;i++) { // new servostructure Johannes: 
            cfg.servoConf[i].min =  read16();
            cfg.servoConf[i].max =  read16();
            cfg.servoConf[i].middle =  read16();
            cfg.servoConf[i].rate =  read8();
        }
        break;

		// Additional commands that are not compatible with MultiWii
	case MSP_UID:	                                          
        headSerialReply(12);
        serialize32(U_ID_0);
        serialize32(U_ID_1);
        serialize32(U_ID_2);
        break;
    case MSP_TEMPERATURE:
        headSerialReply(2);
        serialize16(telemTemperature1);
        break;
    case MSP_SONAR:
        headSerialReply(2);
        serialize16(sonarAlt);
        break;
    case MSP_FIRMWARE:
        headSerialReply(sizeof(FIRMWARE) - 1);
        serializeNames(FIRMWARE);
        break;
    default:                                                // we do not know how to handle the (valid) message, indicate error MSP $M!
        headSerialError(0);
        break;
    }
    tailSerialReply();
}
예제 #14
0
static void evaluateCommand(void)
{
    uint32_t i, tmpu32 = 0;
    uint8_t wp_no;

    switch (cmdMSP)
    {
    case MSP_SET_RAW_RC:
        for (i = 0; i < 8; i++)
            rcDataSAVE[i] = 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 < CHECKBOXITEMS; i++)
        {
            tmpu32 = 0;
            if (cfg.rc_auxch > 4) tmpu32 = read32();
            else tmpu32 = read16();
            cfg.activate[i] = tmpu32;
        }
        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:
        headSerialReply(0);
        break;
    case MSP_IDENT:
        headSerialReply(7);
        serialize8(VERSION);                // multiwii version
        serialize8(cfg.mixerConfiguration); // type of multicopter
        serialize8(MSP_VERSION);            // MultiWii Serial Protocol Version
        serialize32(PLATFORM_32BIT);        // "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);

        serialize32(f.ANGLE_MODE           << BOXANGLE    |
                    f.HORIZON_MODE         << BOXHORIZON  |
                    f.BARO_MODE            << BOXBARO     |
                    f.MAG_MODE             << BOXMAG      |
                    f.ARMED                << BOXARM      |
                    rcOptions[BOXCAMSTAB]  << BOXCAMSTAB  |
                    f.GPS_HOME_MODE        << BOXGPSHOME  |
                    f.GPS_HOLD_MODE        << BOXGPSHOLD  |
                    f.GPS_LOG_MODE         << BOXGPSLOG   |
                    f.HEADFREE_MODE        << BOXHEADFREE |
                    f.PASSTHRU_MODE        << BOXPASSTHRU |
                    rcOptions[BOXBEEPERON] << BOXBEEPERON |
                    rcOptions[BOXHEADADJ]  << BOXHEADADJ  |
                    rcOptions[BOXOSD]      << BOXOSD);
        serialize8(0);
        break;
    case MSP_RAW_IMU:
        headSerialReply(18);
        if (!feature(FEATURE_PASS))                               // Just Do the normal stuff
        {
            for (i = 0; i < 3; i++) serialize16((int16_t)accSmooth[i]);
            for (i = 0; i < 3; i++) serialize16((int16_t)gyroData[i]);
            for (i = 0; i < 3; i++) serialize16((int16_t)magADCfloat[i]);
        }
        else                                                      // Just serialize unfiltered AccZ for Balancing
        {
            for (i = 0; i < 2; i++) serialize16(0);
            serialize16((int16_t)accADC[YAW] - (uint16_t)acc_1G); // Put accz into the middle
            for (i = 0; i < 3; i++) serialize16(0);
            for (i = 0; i < 3; i++) serialize16(0);
        }
        break;
    case MSP_SERVO:
        headSerialReply(16);
        for (i = 0; i < 8; i++) serialize16(servo[i]);
        break;
    case MSP_MOTOR:
        headSerialReply(16);
        for (i = 0; i < 8; i++) serialize16(motor[i]);
        break;
    case MSP_RC:
        headSerialReply((cfg.rc_auxch + 4) * 2); // headSerialReply(16);
        for (i = 0; i < cfg.rc_auxch + 4; i++)   // for (i = 0; i < 8; i++)
            serialize16(rcDataSAVE[i]);
        break;
    case MSP_RAW_GPS:
        headSerialReply(14);
        serialize8(f.GPS_FIX);
        serialize8(GPS_numSat);
        serialize32(GPS_coord[LAT]);
        serialize32(GPS_coord[LON]);
        serialize16(GPS_altitude);
        serialize16(GPS_speed);
        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((int16_t)angle[i]);
        serialize16((int16_t)heading);
        serialize16((int16_t)headFreeModeHold);
        break;
    case MSP_ALTITUDE:
        headSerialReply(6);
        if (feature(FEATURE_PASS))
        {
            serialize32(0);
            serialize16(0);
        }
        else
        {
            serialize32((int32_t)EstAlt);
            serialize16((int16_t)vario);
        }
        break;
    case MSP_BAT:
        headSerialReply(5);
        serialize8(vbat);
        serialize16(0);                               // power meter stuff
        serialize16((uint16_t)rssi << 2);             // Upscale 255 to 1020, so 100% (1023) is hard to achieve, who cares?
        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_BOX:
        if (cfg.rc_auxch > 4) headSerialReply(4 * CHECKBOXITEMS);
        else headSerialReply(2 * CHECKBOXITEMS);
        for (i = 0; i < CHECKBOXITEMS; i++)
        {
            tmpu32 = cfg.activate[i];
            if (cfg.rc_auxch > 4) serialize32(tmpu32);
            else serialize16((int16_t)tmpu32);
        }
        break;
    case MSP_BOXNAMES:
        headSerialReply(sizeof(boxnames) - 1);
        serializeNames(boxnames);
        break;
    case MSP_PIDNAMES:
        headSerialReply(sizeof(pidnames) - 1);
        serializeNames(pidnames);
        break;
    case MSP_MISC:
        headSerialReply(2);
        serialize16(0); // intPowerTrigger1
        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(12);
        if (wp_no == 0)
        {
            serialize8(0);                   // wp0
            serialize32(GPS_home[LAT]);
            serialize32(GPS_home[LON]);
            serialize16(0);                  // altitude will come here
            serialize8(0);                   // nav flag will come here
        }
        else if (wp_no == 16)
        {
            serialize8(16);                  // wp16
            serialize32(GPS_WP[LAT]);
            serialize32(GPS_WP[LON]);
            serialize16(0);                  // altitude will come here
            serialize8(0);                   // nav flag will come here
        }
        break;
    case MSP_RESET_CONF:
        checkFirstTime(true);
        headSerialReply(0);
        break;
    case MSP_ACC_CALIBRATION:
        calibratingA = true;
        headSerialReply(0);
        break;
    case MSP_MAG_CALIBRATION:
        f.CALIBRATE_MAG = 1;
        headSerialReply(0);
        break;
    case MSP_EEPROM_WRITE:
        writeParams(0);
        headSerialReply(0);
        break;
    case MSP_DEBUG:
        headSerialReply(8);
        for (i = 0; i < 4; i++) serialize16(debug[i]);      // 4 variables are here for general monitoring purpose
        break;
    default:                                                // we do not know how to handle the (valid) message, indicate error MSP $M!
        headSerialError(0);
        break;
    }
    tailSerialReply();
}
예제 #15
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;
	}
}