|
|
|
@ -576,6 +576,44 @@ void AP_MSP_Telem_Backend::msp_handle_airspeed(const MSP::msp_airspeed_data_mess
@@ -576,6 +576,44 @@ void AP_MSP_Telem_Backend::msp_handle_airspeed(const MSP::msp_airspeed_data_mess
|
|
|
|
|
#endif |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_api_version(sbuf_t *dst) |
|
|
|
|
{ |
|
|
|
|
const struct { |
|
|
|
|
uint8_t proto; |
|
|
|
|
uint8_t major; |
|
|
|
|
uint8_t minor; |
|
|
|
|
} api_version { |
|
|
|
|
proto : MSP_PROTOCOL_VERSION, |
|
|
|
|
major : API_VERSION_MAJOR, |
|
|
|
|
minor : API_VERSION_MINOR |
|
|
|
|
}; |
|
|
|
|
|
|
|
|
|
sbuf_write_data(dst, &api_version, sizeof(api_version)); |
|
|
|
|
return MSP_RESULT_ACK; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_fc_version(sbuf_t *dst) |
|
|
|
|
{ |
|
|
|
|
const struct { |
|
|
|
|
uint8_t major; |
|
|
|
|
uint8_t minor; |
|
|
|
|
uint8_t patch; |
|
|
|
|
} fc_version { |
|
|
|
|
major : FC_VERSION_MAJOR, |
|
|
|
|
minor : FC_VERSION_MINOR, |
|
|
|
|
patch : FC_VERSION_PATCH_LEVEL |
|
|
|
|
}; |
|
|
|
|
|
|
|
|
|
sbuf_write_data(dst, &fc_version, sizeof(fc_version)); |
|
|
|
|
return MSP_RESULT_ACK; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_fc_variant(sbuf_t *dst) |
|
|
|
|
{ |
|
|
|
|
sbuf_write_data(dst, "ARDU", FLIGHT_CONTROLLER_IDENTIFIER_LENGTH); |
|
|
|
|
return MSP_RESULT_ACK; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_raw_gps(sbuf_t *dst) |
|
|
|
|
{ |
|
|
|
|
#if OSD_ENABLED |
|
|
|
|