diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index 53021236f5..bcb58719bd 100644 --- a/src/main/interface/msp.c +++ b/src/main/interface/msp.c @@ -1030,6 +1030,29 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) sbufWriteU8(dst, GPS_svinfo_cno[i]); } break; + + case MSP_GPS_RESCUE: + sbufWriteU16(dst, gpsRescue()->angle); + sbufWriteU16(dst, gpsRescue()->initialAltitude); + sbufWriteU16(dst, gpsRescue()->descentDistance); + sbufWriteU16(dst, gpsRescue()->rescueGroundspeed); + sbufWriteU16(dst, gpsRescue()->throttleMin); + sbufWriteU16(dst, gpsRescue()->throttleMax); + sbufWriteU16(dst, gpsRescue()->throttleHover); + sbufWriteU16(dst, gpsRescue()->throttleMax); + sbufWriteU8(dst, gpsRescue()->sanityChecks); + sbufWriteU8(dst, gpsRescue()->minSats); + break; + + case MSP_GPS_RESCUE_PIDS: + sbufWriteU16(dst, gpsRescue()->throttleP); + sbufWriteU16(dst, gpsRescue()->throttleI); + sbufWriteU16(dst, gpsRescue()->throttleD); + sbufWriteU16(dst, gpsRescue()->velP); + sbufWriteU16(dst, gpsRescue()->velI); + sbufWriteU16(dst, gpsRescue()->velD); + sbufWriteU16(dst, gpsRescue()->yawP); + break; #endif case MSP_ACC_TRIM: @@ -1553,6 +1576,28 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) gpsConfigMutable()->autoConfig = sbufReadU8(src); gpsConfigMutable()->autoBaud = sbufReadU8(src); break; + + case MSP_SET_GPS_RESCUE: + gpsRescueMutable()->angle = sbufReadU16(src); + gpsRescueMutable()->initialAltitude = sbufReadU16(src); + gpsRescueMutable()->descentDistance = sbufReadU16(src); + gpsRescueMutable()->rescueGroundspeed = sbufReadU16(src); + gpsRescueMutable()->throttleMin = sbufReadU16(src); + gpsRescueMutable()->throttleMax = sbufReadU16(src); + gpsRescueMutable()->throttleHover = sbufReadU16(src); + gpsRescueMutable()->sanityChecks = sbufReadU8(src); + gpsRescueMutable()->minSats = sbufReadU8(src); + break; + + case MSP_SET_GPS_RESCUE_PIDS: + gpsRescueMutable()->throttleP = sbufReadU16(src); + gpsRescueMutable()->throttleI = sbufReadU16(src); + gpsRescueMutable()->throttleD = sbufReadU16(src); + gpsRescueMutable()->velP = sbufReadU16(src); + gpsRescueMutable()->velI = sbufReadU16(src); + gpsRescueMutable()->velD = sbufReadU16(src); + gpsRescueMutable()->yawP = sbufReadU16(src); + break; #endif #ifdef USE_MAG diff --git a/src/main/interface/msp_protocol.h b/src/main/interface/msp_protocol.h index c04a5de57a..ecca30036b 100644 --- a/src/main/interface/msp_protocol.h +++ b/src/main/interface/msp_protocol.h @@ -280,6 +280,8 @@ #define MSP_GPS_CONFIG 132 //out message GPS configuration #define MSP_COMPASS_CONFIG 133 //out message Compass configuration #define MSP_ESC_SENSOR_DATA 134 //out message Extra ESC data from 32-Bit ESCs (Temperature, RPM) +#define MSP_GPS_RESCUE 135 //out message GPS Rescues's angle, initialAltitude, descentDistance, rescueGroundSpeed, sanityChecks and minSats +#define MSP_GPS_RESCUE_PIDS 136 //out message GPS Rescues's throttleP and velocity PIDS + yaw P #define MSP_SET_RAW_RC 200 //in message 8 rc chan #define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed @@ -306,6 +308,8 @@ #define MSP_SET_MOTOR_CONFIG 222 //out message Motor configuration (min/max throttle, etc) #define MSP_SET_GPS_CONFIG 223 //out message GPS configuration #define MSP_SET_COMPASS_CONFIG 224 //out message Compass configuration +#define MSP_SET_GPS_RESCUE 225 //in message GPS Rescues's angle, initialAltitude, descentDistance, rescueGroundSpeed, sanityChecks and minSats +#define MSP_SET_GPS_RESCUE_PIDS 226 //in message GPS Rescues's throttleP and velocity PIDS + yaw P // #define MSP_BIND 240 //in message no param // #define MSP_ALARMS 242