void evaluateCommand(){
static unsigned char sendThis = 0;
switch( sendThis ){
case 0:
sendThis = 1;
cmd = MSP_RAW_IMU;
// imu.gyroData[0] = temp;
s_struct((unsigned char*)&imu, 18); // accADC[]; gyroData[2](instead of gyroADC[]); magADC[]
break;
case 1:
sendThis = 2;
cmd = MSP_ATTITUDE;
s_struct((unsigned char*)&att, 6); // roll; pitch; yaw(heading)
break;
case 2:
sendThis = 0;
cmd = MSP_RC;
s_struct((unsigned char*)&rcData, 8); // rcRoll; rcPitch; rcYaw; rcThrottle
break;
// case 3:
// sendThis = 0;
// cmd =
}
tailSerialReply();
}
void serialBuzzerRefresh()
{
// for (unsigned char nSendRepeat = 0; nSendRepeat < 3; nSendRepeat++)
// {
serialize8('$');
serialize8('M');
serialize8('!');
checksum[CURRENTPORT] = 0; // start calculating a new checksum
serialize8(1);
serialize8(0x0D);
serialize8(cStateBuzzer);
tailSerialReply();
// }
}
// Skypup 2015.07.26
void MSPEncode()
{
CURRENTPORT = 0;
// if (serialHeadTX[CURRENTPORT] != serialTailTX[CURRENTPORT])
// return;
// Head
serialize8('$');
serialize8('M');
serialize8('>');
checksum[CURRENTPORT] = 0; // start calculating a new checksum
serialize8(RC_CHANS*2);
serialize8(MSP_ENCODE);
// Payload
s_struct_partial((uint8_t*)&rcData,RC_CHANS*2);
// Tail
tailSerialReply();
}
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();
}
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, 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 mspAck() {
headSerialReply(0);
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;
}
}
static void serializeNames(PGM_P s) {
headSerialReply(strlen_P(s));
for (PGM_P c = s; pgm_read_byte(c); c++)
serialize8(pgm_read_byte(c));
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 s_struct(uint8_t *cb,uint8_t siz) {
headSerialReply(siz);
while(siz--) serialize8(*cb++);
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++)
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 MSP::update(bool armed)
{
static bool pendReboot;
// pendReboot will be set for flashing
this->_board->checkReboot(pendReboot);
while (this->_board->serialAvailableBytes()) {
uint8_t c = this->_board->serialReadByte();
if (portState.c_state == IDLE) {
portState.c_state = (c == '$') ? HEADER_START : IDLE;
if (portState.c_state == IDLE && !armed) {
if (c == '#')
;
else if (c == CONFIG_REBOOT_CHARACTER)
this->_board->reboot();
}
} else if (portState.c_state == HEADER_START) {
portState.c_state = (c == 'M') ? HEADER_M : IDLE;
} else if (portState.c_state == HEADER_M) {
portState.c_state = (c == '<') ? HEADER_ARROW : IDLE;
} else if (portState.c_state == HEADER_ARROW) {
if (c > INBUF_SIZE) { // now we are expecting the payload size
portState.c_state = IDLE;
continue;
}
portState.dataSize = c;
portState.offset = 0;
portState.checksum = 0;
portState.indRX = 0;
portState.checksum ^= c;
portState.c_state = HEADER_SIZE; // the command is to follow
} else if (portState.c_state == HEADER_SIZE) {
portState.cmdMSP = c;
portState.checksum ^= c;
portState.c_state = HEADER_CMD;
} else if (portState.c_state == HEADER_CMD &&
portState.offset < portState.dataSize) {
portState.checksum ^= c;
portState.inBuf[portState.offset++] = c;
} else if (portState.c_state == HEADER_CMD && portState.offset >= portState.dataSize) {
if (portState.checksum == c) { // compare calculated and transferred checksum
switch (portState.cmdMSP) {
case MSP_SET_RAW_RC:
for (uint8_t i = 0; i < 8; i++)
this->_rc->data[i] = read16();
headSerialReply(0);
break;
case MSP_SET_MOTOR:
for (uint8_t i = 0; i < 4; i++)
this->_mixer->motorsDisarmed[i] = read16();
headSerialReply(0);
break;
case MSP_RC:
headSerialReply(16);
for (uint8_t i = 0; i < 8; i++)
serialize16(this->_rc->data[i]);
break;
case MSP_ATTITUDE:
headSerialReply(6);
for (uint8_t i = 0; i < 3; i++)
serialize16(this->_imu->angle[i]);
break;
case MSP_BARO_SONAR_RAW:
//headSerialReply(8);
//serialize32(baroPressure);
//serialize32(sonarDistance);
break;
case MSP_ALTITUDE:
//headSerialReply(6);
//serialize32(estAlt);
//serialize16(vario);
break;
case MSP_REBOOT:
headSerialReply(0);
pendReboot = true;
break;
// don't know how to handle the (valid) message, indicate error MSP $M!
default:
headSerialError(0);
break;
}
tailSerialReply();
}
portState.c_state = IDLE;
}
}
}
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 s_struct(uint8_t *cb,uint8_t siz) {
headSerialReply(siz);
s_struct_partial(cb,siz);
tailSerialReply();
}