size_t CNetworkHost::Serialize(unsigned char *buf) const
{
unsigned char *p = buf;
p += serialize32(p, this->Host);
p += serialize16(p, this->Port);
p += serialize16(p, this->PlyNr);
p += serialize(p, this->PlyName);
return p - buf;
}
size_t CNetworkSelection::Serialize(unsigned char *buf) const
{
unsigned char *p = buf;
p += serialize16(p, this->player);
p += serialize16(p, uint16_t(this->Units.size()));
for (size_t i = 0; i != this->Units.size(); ++i) {
p += serialize16(p, Units[i]);
}
return p - buf;
}
static void sendTime(void)
{
uint32_t seconds = millis() / 1000;
uint8_t minutes = (seconds / 60) % 60;
// if we fly for more than an hour, something's wrong anyway
sendDataHead(ID_HOUR_MINUTE);
serialize16(minutes << 8);
sendDataHead(ID_SECOND);
serialize16(seconds % 60);
}
static void sendFuelLevel(void)
{
sendDataHead(ID_FUEL_LEVEL);
if (batteryConfig()->batteryCapacity > 0) {
serialize16((uint16_t)batteryCapacityRemainingPercentage());
} else {
amperageMeter_t *state = getAmperageMeter(batteryConfig()->amperageMeterSource);
serialize16((uint16_t)constrain(state->mAhDrawn, 0, 0xFFFF));
}
}
static void sendGpsAltitude(void)
{
uint16_t altitude = GPS_altitude;
//Send real GPS altitude only if it's reliable (there's a GPS fix)
if (!STATE(GPS_FIX)) {
altitude = 0;
}
sendDataHead(ID_GPS_ALTIDUTE_BP);
serialize16(altitude);
sendDataHead(ID_GPS_ALTIDUTE_AP);
serialize16(0);
}
static void sendSpeed(void)
{
if (!STATE(GPS_FIX)) {
return;
}
//Speed should be sent in knots (GPS speed is in cm/s)
sendDataHead(ID_GPS_SPEED_BP);
//convert to knots: 1cm/s = 0.0194384449 knots
serialize16(GPS_speed * 1944 / 100000);
sendDataHead(ID_GPS_SPEED_AP);
serialize16((GPS_speed * 1944 / 100) % 100);
}
static void sendThrottleOrBatterySizeAsRpm(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
{
uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER;
sendDataHead(ID_RPM);
if (ARMING_FLAG(ARMED)) {
throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle);
if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP))
throttleForRPM = 0;
serialize16(throttleForRPM);
} else {
serialize16((batteryConfig->batteryCapacity / BLADE_NUMBER_DIVIDER));
}
}
size_t serialize(unsigned char *buf, const std::vector<unsigned char> &data)
{
if (buf) {
if (data.empty()) {
return serialize16(buf, uint16_t(data.size()));
}
buf += serialize16(buf, uint16_t(data.size()));
memcpy(buf, &data[0], data.size());
buf += data.size();
if ((data.size() & 0x03) != 0) {
memset(buf, 0, data.size() & 0x03);
}
}
return 2 + ((data.size() + 3) & ~0x03); // round up to multiple of 4 for alignment.
}
static void sendAccel(void)
{
for (int i = 0; i < 3; i++) {
sendDataHead(ID_ACC_X + i);
serialize16(1000 * (int32_t)accSmooth[i] / acc.acc_1G);
}
}
/*
* Send voltage via ID_VOLT
*
* NOTE: This sends voltage divided by batteryCellCount. To get the real
* battery voltage, you need to multiply the value by batteryCellCount.
*/
static void sendVoltage(void)
{
static uint16_t currentCell = 0;
uint32_t cellVoltage;
uint16_t payload;
/*
* Format for Voltage Data for single cells is like this:
*
* llll llll cccc hhhh
* l: Low voltage bits
* h: High voltage bits
* c: Cell number (starting at 0)
*
* The actual value sent for cell voltage has resolution of 0.002 volts
* Since vbat has resolution of 0.1 volts it has to be multiplied by 50
*/
cellVoltage = ((uint32_t)vbat * 100 + batteryCellCount) / (batteryCellCount * 2);
// Cell number is at bit 9-12
payload = (currentCell << 4);
// Lower voltage bits are at bit 0-8
payload |= ((cellVoltage & 0x0ff) << 8);
// Higher voltage bits are at bits 13-15
payload |= ((cellVoltage & 0xf00) >> 8);
sendDataHead(ID_VOLT);
serialize16(payload);
currentCell++;
currentCell %= batteryCellCount;
}
static void sendAmperage(void)
{
amperageMeter_t *state = getAmperageMeter(batteryConfig()->amperageMeterSource);
sendDataHead(ID_CURRENT);
serialize16((uint16_t)(state->amperage / 10));
}
/*
* Send voltage with ID_VOLTAGE_AMP
*/
static void sendVoltageAmp(void)
{
if (telemetryConfig->frsky_vfas_precision == FRSKY_VFAS_PRECISION_HIGH) {
/*
* Use new ID 0x39 to send voltage directly in 0.1 volts resolution
*/
sendDataHead(ID_VOLTAGE_AMP);
serialize16(vbat);
} else {
uint16_t voltage = (vbat * 110) / 21;
sendDataHead(ID_VOLTAGE_AMP_BP);
serialize16(voltage / 100);
sendDataHead(ID_VOLTAGE_AMP_AP);
serialize16(((voltage % 100) + 5) / 10);
}
}
static void sendSatalliteSignalQualityAsTemperature2(void)
{
uint16_t satellite = GPS_numSat;
if (GPS_hdop > GPS_BAD_QUALITY && ( (cycleNum % 16 ) < 8)) {//Every 1s
satellite = constrain(GPS_hdop, 0, GPS_MAX_HDOP_VAL);
}
sendDataHead(ID_TEMPRATURE2);
if (telemetryConfig->frsky_unit == FRSKY_UNIT_METRICS) {
serialize16(satellite);
} else {
float tmp = (satellite - 32) / 1.8f;
//Round the value
tmp += (tmp < 0) ? -0.5f : 0.5f;
serialize16(tmp);
}
}
static void sendAccel(void)
{
uint8_t i;
for (i = 0; i < 3; i++) {
sendDataHead(0x24 + i);
serialize16(((float)accSmooth[i] / acc_1G) * 1000);
}
}
static void sendAccel(void)
{
int i;
for (i = 0; i < 3; i++) {
sendDataHead(ID_ACC_X + i);
serialize16(((float)accSmooth[i] / acc.acc_1G) * 1000);
}
}
size_t serialize(unsigned char *buf, const std::string &s)
{
if (buf) {
buf += serialize16(buf, uint16_t(s.size()));
memcpy(buf, s.c_str(), s.size());
buf += s.size();
if ((s.size() & 0x03) != 0) {
memset(buf, 0, s.size() & 0x03);
}
}
return 2 + ((s.size() + 3) & ~0x03); // round up to multiple of 4 for alignment.
}
static void sendGPS(void)
{
int32_t localGPS_coord[2] = {0,0};
// Don't set dummy GPS data, if we already had a GPS fix
// it can be usefull to keep last valid coordinates
static uint8_t gpsFixOccured = 0;
//Dummy data if no 3D fix, this way we can display heading in Taranis
if (STATE(GPS_FIX) || gpsFixOccured == 1) {
localGPS_coord[LAT] = GPS_coord[LAT];
localGPS_coord[LON] = GPS_coord[LON];
gpsFixOccured = 1;
} else {
// Send dummy GPS Data in order to display compass value
localGPS_coord[LAT] = (telemetryConfig->gpsNoFixLatitude * GPS_DEGREES_DIVIDER);
localGPS_coord[LON] = (telemetryConfig->gpsNoFixLongitude * GPS_DEGREES_DIVIDER);
}
gpsCoordinateDDDMMmmmm_t coordinate;
GPStoDDDMM_MMMM(localGPS_coord[LAT], &coordinate);
sendDataHead(ID_LATITUDE_BP);
serialize16(coordinate.dddmm);
sendDataHead(ID_LATITUDE_AP);
serialize16(coordinate.mmmm);
sendDataHead(ID_N_S);
serialize16(localGPS_coord[LAT] < 0 ? 'S' : 'N');
GPStoDDDMM_MMMM(localGPS_coord[LON], &coordinate);
sendDataHead(ID_LONGITUDE_BP);
serialize16(coordinate.dddmm);
sendDataHead(ID_LONGITUDE_AP);
serialize16(coordinate.mmmm);
sendDataHead(ID_E_W);
serialize16(localGPS_coord[LON] < 0 ? 'W' : 'E');
}
size_t serialize(unsigned char *buf, const std::string &s)
{
if (buf) {
buf += serialize16(buf, uint16_t(s.size()));
memcpy(buf, s.c_str(), s.size());
buf += s.size();
//Wyrmgus start
//Andrettin: fix to the multiplayer OOS issue - the fix works, but it would be better if someone reviewed the code! I am leaving the Wyrmgus tags on these changes until the code has been properly reviewed
/*
if ((s.size() & 0x03) != 0) {
memset(buf, 0, s.size() & 0x03);
}
*/
//Wyrmgus end
}
//Wyrmgus start
// return 2 + ((s.size() + 3) & ~0x03); // round up to multiple of 4 for alignment.
return 2 + (s.size() + 3);
//Wyrmgus end
}
static void sendGPS(void)
{
sendDataHead(0x13);
serialize16(abs(GPS_coord[LAT]) / 100000);
sendDataHead(0x13 + 8);
serialize16((abs(GPS_coord[LAT]) / 100000) % 10000);
sendDataHead(0x1B + 8);
serialize16(GPS_coord[LAT] < 0 ? 'S' : 'N');
sendDataHead(0x12);
serialize16(abs(GPS_coord[LON]) / 100000);
sendDataHead(0x12 + 8);
serialize16((abs(GPS_coord[LON]) / 100000) % 10000);
sendDataHead(0x1A + 8);
serialize16(GPS_coord[LON] < 0 ? 'W' : 'E');
}
static void sendGPS(void)
{
sendDataHead(ID_LATITUDE_BP);
serialize16(abs(GPS_coord[LAT]) / 100000);
sendDataHead(ID_LATITUDE_AP);
serialize16((abs(GPS_coord[LAT]) / 10) % 10000);
sendDataHead(ID_N_S);
serialize16(GPS_coord[LAT] < 0 ? 'S' : 'N');
sendDataHead(ID_LONGITUDE_BP);
serialize16(abs(GPS_coord[LON]) / 100000);
sendDataHead(ID_LONGITUDE_AP);
serialize16((abs(GPS_coord[LON]) / 10) % 10000);
sendDataHead(ID_E_W);
serialize16(GPS_coord[LON] < 0 ? 'W' : 'E');
}
/*
* Send voltage via ID_VOLT
*
* NOTE: This sends voltage divided by batteryCellCount. To get the real
* battery voltage, you need to multiply the value by batteryCellCount.
*/
static void sendVoltage(void)
{
static uint16_t currentCell = 0;
uint16_t cellNumber;
uint32_t cellVoltage;
uint16_t payload;
/*
* Note: F**k the pdf. Format for Voltage Data for single cells is like this:
*
* llll llll cccc hhhh
* l: Low voltage bits
* h: High voltage bits
* c: Cell number (starting at 0)
*/
cellVoltage = vbat / batteryCellCount;
// Map to 12 bit range
cellVoltage = (cellVoltage * 2100) / 42;
cellNumber = currentCell % batteryCellCount;
// Cell number is at bit 9-12
payload = (cellNumber << 4);
// Lower voltage bits are at bit 0-8
payload |= ((cellVoltage & 0x0ff) << 8);
// Higher voltage bits are at bits 13-15
payload |= ((cellVoltage & 0xf00) >> 8);
sendDataHead(ID_VOLT);
serialize16(payload);
currentCell++;
currentCell %= batteryCellCount;
}
static void sendLatLong(int32_t coord[2])
{
gpsCoordinateDDDMMmmmm_t coordinate;
GPStoDDDMM_MMMM(coord[LAT], &coordinate);
sendDataHead(ID_LATITUDE_BP);
serialize16(coordinate.dddmm);
sendDataHead(ID_LATITUDE_AP);
serialize16(coordinate.mmmm);
sendDataHead(ID_N_S);
serialize16(coord[LAT] < 0 ? 'S' : 'N');
GPStoDDDMM_MMMM(coord[LON], &coordinate);
sendDataHead(ID_LONGITUDE_BP);
serialize16(coordinate.dddmm);
sendDataHead(ID_LONGITUDE_AP);
serialize16(coordinate.mmmm);
sendDataHead(ID_E_W);
serialize16(coord[LON] < 0 ? 'W' : 'E');
}
static void sendAmperage(void)
{
sendDataHead(ID_CURRENT);
serialize16((uint16_t)(amperage / 10));
}
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();
}
/*
* Send vertical speed for opentx. ID_VERT_SPEED
* Unit is cm/s
*/
static void sendVario(void)
{
sendDataHead(ID_VERT_SPEED);
serialize16(vario);
}
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 sendTemperature1(void)
{
sendDataHead(ID_TEMPRATURE1);
serialize16((int16_t)telemTemperature1 / 10);
}
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;
}
}
}
static void sendTemperature1(void)
{
sendDataHead(0x02);
serialize16(telemTemperature1 / 10);
}
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();
}
