size_t CInitMessage_Header::Serialize(unsigned char *p) const
{
unsigned char *buf = p;
p += serialize8(p, type);
p += serialize8(p, subtype);
return p - buf;
}
void headSerialResponse(unsigned char s){ // Data package header { $ M > dataNum cmd}
serialize8('$');
serialize8('M');
serialize8('>');
checkSum = 0; // start a new check sum
serialize8(s);
serialize8(cmd);
}
void headSerialResponse(uint8_t err, uint8_t s) {
serialize8('$');
serialize8('M');
serialize8(err ? '!' : '>');
checksum[CURRENTPORT] = 0; // start calculating a new checksum
serialize8(s);
serialize8(cmdMSP[CURRENTPORT]);
}
void headSerialResponse(uint8_t err, uint8_t s)
{
serialize8('$');
serialize8('M');
serialize8(err ? '!' : '>');
currentPortState->checksum = 0; // start calculating a new checksum
serialize8(s);
serialize8(currentPortState->cmdMSP);
}
void MSP::headSerialResponse(uint8_t err, uint8_t s)
{
serialize8('$');
serialize8('M');
serialize8(err ? '!' : '>');
portState.checksum = 0; // start calculating a new checksum
serialize8(s);
serialize8(portState.cmdMSP);
}
size_t CNetworkExtendedCommand::Serialize(unsigned char *buf) const
{
unsigned char *p = buf;
p += serialize8(p, this->ExtendedType);
p += serialize8(p, this->Arg1);
p += serialize16(p, this->Arg2);
p += serialize16(p, this->Arg3);
p += serialize16(p, this->Arg4);
return p - buf;
}
size_t CNetworkPacketHeader::Serialize(unsigned char *p) const
{
if (p != NULL) {
for (int i = 0; i != MaxNetworkCommands; ++i) {
p += serialize8(p, this->Type[i]);
}
p += serialize8(p, this->Cycle);
p += serialize8(p, this->OrigPlayer);
}
return MaxNetworkCommands + 1 + 1;
}
static void debugmsg_serialize(uint8_t l) {
for (uint8_t i=0; i<l; i++) {
if (head_debug != tail_debug) {
serialize8(debug_buf[tail_debug++]);
if (tail_debug == DEBUG_MSG_BUFFER_SIZE) {
tail_debug = 0;
}
} else {
serialize8('\0');
}
}
}
const unsigned char *CInitMessage_Config::Serialize() const
{
unsigned char *buf = new unsigned char[Size()];
unsigned char *p = buf;
p += header.Serialize(p);
p += serialize8(p, this->clientIndex);
p += serialize8(p, this->hostsCount);
for (int i = 0; i != PlayerMax; ++i) {
p += this->hosts[i].Serialize(p);
}
return buf;
}
void MSP::serialize32(uint32_t a)
{
serialize8(a & 0xFF);
serialize8((a >> 8) & 0xFF);
serialize8((a >> 16) & 0xFF);
serialize8((a >> 24) & 0xFF);
}
// 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 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();
// }
}
void serializeBoxNamesReply(void)
{
int i, idx, j, flag = 1, count = 0, len;
reset:
// in first run of the loop, we grab total size of junk to be sent
// then come back and actually send it
for (i = 0; i < numberBoxItems; i++) {
idx = availableBoxes[i];
len = strlen(boxes[idx].boxName);
if (flag) {
count += len;
} else {
for (j = 0; j < len; j++)
serialize8(boxes[idx].boxName[j]);
}
}
if (flag) {
headSerialReply(count);
flag = 0;
goto reset;
}
}
void s_struct(uint8_t *cb,uint8_t siz) {
headSerialReply(siz);
while(siz--) serialize8(*cb++);
}
void serializeNames(PGM_P s) {
headSerialReply(strlen_P(s));
for (PGM_P c = s; pgm_read_byte(c); c++) {
serialize8(pgm_read_byte(c));
}
}
void tailSerialReply() {
serialize8(checksum[CURRENTPORT]);UartSendData(CURRENTPORT);
}
void serialize16(int a){ // Low first, high later
serialize8((a ) & 0xFF);
serialize8((a>>8) & 0xFF);
}
void serialize32(uint32_t a) {
serialize8((a ) & 0xFF);
serialize8((a>> 8) & 0xFF);
serialize8((a>>16) & 0xFF);
serialize8((a>>24) & 0xFF);
}
void serialize16(int16_t a) {
serialize8((a ) & 0xFF);
serialize8((a>>8) & 0xFF);
}
void tailSerialReply(void)
{
serialize8(checksum);
}
//void headSerialReply(unsigned char s){
// headSerialResponse(s);
//}
void tailSerialReply(void){ // Add check sum , then start to send
serialize8(checkSum);
UartSendData();
}
void s_struct(unsigned char *cb, unsigned char siz){
headSerialResponse(siz);
while(siz--) serialize8(*cb++);
}//OUTPUT: $ M > charCnt cmd data (+checksum)
void MSP::serialize16(int16_t a)
{
serialize8(a & 0xFF);
serialize8((a >> 8) & 0xFF);
}
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();
}
void tailSerialReply(void)
{
serialize8(currentPortState->checksum);
}
void serializeNames(const char *s)
{
const char *c;
for (c = s; *c; c++)
serialize8(*c);
}
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();
}
void MSP::tailSerialReply(void)
{
serialize8(portState.checksum);
}
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();
}
