GCS_MAVLink: added DATA96 hook for AP_Radio

This commit is contained in:
Andrew Tridgell 2018-01-18 17:32:22 +11:00
parent 779de02acb
commit 8f1e8e787a
2 changed files with 37 additions and 1 deletions

View File

@ -311,7 +311,7 @@ protected:
bool try_send_camera_message(enum ap_message id);
bool try_send_gps_message(enum ap_message id);
void send_hwstatus();
void handle_data_packet(mavlink_message_t *msg);
private:
float adjust_rate_for_stream_trigger(enum streams stream_num);

View File

@ -30,6 +30,11 @@
#include <fcntl.h>
#endif
#ifdef HAL_RCINPUT_WITH_AP_RADIO
#include <AP_Radio/AP_Radio.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#endif
extern const AP_HAL::HAL& hal;
uint32_t GCS_MAVLINK::last_radio_status_remrssi_ms;
@ -1815,6 +1820,33 @@ void GCS_MAVLINK::handle_set_gps_global_origin(const mavlink_message_t *msg)
set_ekf_origin(ekf_origin);
}
/*
handle a DATA96 message
*/
void GCS_MAVLINK::handle_data_packet(mavlink_message_t *msg)
{
#ifdef HAL_RCINPUT_WITH_AP_RADIO
mavlink_data96_t m;
mavlink_msg_data96_decode(msg, &m);
switch (m.type) {
case 42:
case 43: {
// pass to AP_Radio (for firmware upload and playing test tunes)
AP_Radio *radio = AP_Radio::instance();
if (radio != nullptr) {
radio->handle_data_packet(chan, m);
}
break;
}
default:
// unknown
break;
}
#endif
}
/*
handle messages which don't require vehicle specific data
*/
@ -1929,6 +1961,10 @@ void GCS_MAVLINK::handle_common_message(mavlink_message_t *msg)
case MAVLINK_MSG_ID_RALLY_FETCH_POINT:
handle_common_rally_message(msg);
break;
case MAVLINK_MSG_ID_DATA96:
handle_data_packet(msg);
break;
}
}