void evaluateCommand() { uint32_t tmp=0; switch(cmdMSP[CURRENTPORT]) { 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: 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); conf.minthrottle = set_misc.b; 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 = 0; misc.b = conf.minthrottle; misc.c = MAXTHROTTLE; misc.d = MINCOMMAND; misc.e = 0; misc.f = 0; misc.g =0; misc.h = 0; misc.i = 0;misc.j = 0;misc.k = 0;misc.l = 0; s_struct((uint8_t*)&misc,22); 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(f.ARMED) tmp |= 1<<BOXARM; st.flag = tmp; st.set = global_conf.currentSet; s_struct((uint8_t*)&st,11); break; case MSP_RAW_IMU: s_struct((uint8_t*)&imu,18); break; case MSP_SERVO: 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; 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,5); 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; 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; case MSP_EEPROM_WRITE: //writeParams(0); headSerialReply(0); break; default: // we do not know how to handle the (valid) message, indicate error MSP $M! headSerialError(0); break; } tailSerialReply(); }
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(); }
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(); }
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(); }
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(); }
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; } }
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(); }
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(); }
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; } }