AP_Radio: support DATA96 packets for fw update

this allows for update of remote radio firmware via MAVLink DATA96
packets
This commit is contained in:
Andrew Tridgell 2018-01-05 20:51:00 +11:00
parent 6ef5f61faa
commit b1ccf575f7
5 changed files with 49 additions and 1 deletions

View File

@ -213,6 +213,14 @@ uint32_t AP_Radio::last_recv_us(void)
return driver->last_recv_us();
}
// handle a data96 mavlink packet for fw upload
void AP_Radio::handle_data_packet(mavlink_channel_t chan, const mavlink_data96_t &m)
{
if (driver) {
driver->handle_data_packet(chan, m);
}
}
// update status, should be called from main thread
void AP_Radio::update(void)
{

View File

@ -88,6 +88,9 @@ public:
return _instance;
}
// handle a data96 mavlink packet for fw upload
void handle_data_packet(mavlink_channel_t chan, const mavlink_data96_t &m);
// set the 2.4GHz wifi channel used by companion computer, so it can be avoided
void set_wifi_channel(uint8_t channel);

View File

@ -48,6 +48,9 @@ public:
// return current PWM of a channel
virtual uint16_t read(uint8_t chan) = 0;
// handle a data96 mavlink packet for fw upload
virtual void handle_data_packet(mavlink_channel_t chan, const mavlink_data96_t &m) {}
// update status
virtual void update(void) = 0;

View File

@ -1479,7 +1479,7 @@ void AP_Radio_cypress::dsm_choose_channel(void)
*/
void AP_Radio_cypress::start_recv_bind(void)
{
if (!dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
if (!dev->get_semaphore()->take(0)) {
// shouldn't be possible
return;
}
@ -1709,9 +1709,40 @@ void AP_Radio_cypress::send_FCC_test_packet(void)
transmit16(pkt);
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
hrt_call_after(&wait_call, 10000, (hrt_callout)irq_timeout_trampoline, nullptr);
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
//Note: please review Frequency in chconf.h to ensure the range of wait
chVTSet(&timeout_vt, MS2ST(10), trigger_timeout_event, nullptr);
#endif
#endif
}
// handle a data96 mavlink packet for fw upload
void AP_Radio_cypress::handle_data_packet(mavlink_channel_t chan, const mavlink_data96_t &m)
{
uint32_t ofs=0;
memcpy(&ofs, &m.data[0], 4);
Debug(4, "got data96 of len %u from chan %u at offset %u\n", m.len, chan, ofs);
if (sem->take_nonblocking()) {
fwupload.chan = chan;
fwupload.need_ack = false;
fwupload.offset = ofs;
fwupload.length = MIN(m.len-4, 92);
fwupload.acked = 0;
fwupload.sequence++;
if (m.type == 43) {
// sending a tune to play - for development testing
fwupload.fw_type = TELEM_PLAY;
fwupload.length = MIN(m.len, 90);
fwupload.offset = 0;
memcpy(&fwupload.pending_data[0], &m.data[0], fwupload.length);
} else {
// sending a chunk of firmware OTA upload
fwupload.fw_type = TELEM_FW;
memcpy(&fwupload.pending_data[0], &m.data[4], fwupload.length);
}
sem->give();
}
}
#endif // HAL_RCINPUT_WITH_AP_RADIO

View File

@ -59,6 +59,9 @@ public:
// return current PWM of a channel
uint16_t read(uint8_t chan) override;
// handle a data96 mavlink packet for fw upload
void handle_data_packet(mavlink_channel_t chan, const mavlink_data96_t &m) override;
// update status
void update(void) override;