From 8d43c6c3dc029df607f0ddf6792e4450afce9f08 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 2 Feb 2018 17:21:48 +1100 Subject: [PATCH] AP_Radio: implement OTA update for cc2500 --- libraries/AP_Radio/AP_Radio_cc2500.cpp | 111 +++++++++++++++++++++++-- libraries/AP_Radio/AP_Radio_cc2500.h | 25 +++++- libraries/AP_Radio/AP_Radio_cypress.h | 2 +- libraries/AP_Radio/telem_structure.h | 53 +++++------- 4 files changed, 154 insertions(+), 37 deletions(-) diff --git a/libraries/AP_Radio/AP_Radio_cc2500.cpp b/libraries/AP_Radio/AP_Radio_cc2500.cpp index 4986932822..ef77124842 100644 --- a/libraries/AP_Radio/AP_Radio_cc2500.cpp +++ b/libraries/AP_Radio/AP_Radio_cc2500.cpp @@ -114,6 +114,7 @@ uint16_t AP_Radio_cc2500::read(uint8_t chan) */ void AP_Radio_cc2500::update(void) { + check_fw_ack(); } @@ -338,6 +339,29 @@ void AP_Radio_cc2500::start_recv_bind(void) // handle a data96 mavlink packet for fw upload void AP_Radio_cc2500::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(); + } } /* @@ -408,10 +432,50 @@ bool AP_Radio_cc2500::handle_SRT_packet(const uint8_t *packet) // we map the buttons onto two PWM channels for ease of integration with ArduPilot pwm_channels[4] = 1000 + (pkt->buttons & 0x7) * 100; pwm_channels[5] = 1000 + (pkt->buttons >> 3) * 100; - pwm_channels[6] = pkt->tx_voltage * 4; - tx_rssi = pkt->telem_rssi; - tx_pps = pkt->telem_pps; + uint8_t data = pkt->data; + /* + decode special data field + */ + switch (pkt->pkt_type) { + case PKTYPE_VOLTAGE: + // voltage from TX is in 0.025 volt units. Convert to 0.01 volt units for easier display + pwm_channels[6] = data * 4; + break; + case PKTYPE_YEAR: + tx_date.firmware_year = data; + break; + case PKTYPE_MONTH: + tx_date.firmware_month = data; + break; + case PKTYPE_DAY: + tx_date.firmware_day = data; + break; + case PKTYPE_TELEM_RSSI: + tx_rssi = data; + break; + case PKTYPE_TELEM_PPS: + tx_pps = data; + break; + case PKTYPE_BL_VERSION: + // unused so far for cc2500 + break; + case PKTYPE_FW_ACK: { + // got an fw upload ack + Debug(4, "ack %u seq=%u acked=%u length=%u len=%u\n", + data, fwupload.sequence, fwupload.acked, fwupload.length, fwupload.len); + if (fwupload.sequence == data && sem->take_nonblocking()) { + fwupload.sequence++; + fwupload.acked += fwupload.len; + if (fwupload.acked == fwupload.length) { + // trigger send of DATA16 ack to client + fwupload.need_ack = true; + } + sem->give(); + } + break; + } + } if (chan_count < 7) { chan_count = 7; @@ -964,10 +1028,30 @@ void AP_Radio_cc2500::send_SRT_telemetry(void) t_status.tx_max = get_tx_max_power(); t_status.note_adjust = get_tx_buzzer_adjust(); - pkt.type = TELEM_STATUS; + // send fw update packet for 7/8 of packets if any data pending + if (fwupload.length != 0 && + fwupload.length > fwupload.acked && + ((fwupload.counter++ & 0x07) != 0) && + sem->take_nonblocking()) { + pkt.type = fwupload.fw_type; + pkt.payload.fw.seq = fwupload.sequence; + uint32_t len = fwupload.length>fwupload.acked?fwupload.length - fwupload.acked:0; + pkt.payload.fw.len = len<=8?len:8; + pkt.payload.fw.offset = fwupload.offset+fwupload.acked; + memcpy(&pkt.payload.fw.data[0], &fwupload.pending_data[fwupload.acked], pkt.payload.fw.len); + fwupload.len = pkt.payload.fw.len; + Debug(4, "sent fw seq=%u offset=%u len=%u type=%u\n", + pkt.payload.fw.seq, + pkt.payload.fw.offset, + pkt.payload.fw.len, + pkt.type); + sem->give(); + } else { + pkt.type = TELEM_STATUS; + pkt.payload.status = t_status; + } pkt.txid[0] = bindTxId[0]; pkt.txid[1] = bindTxId[1]; - pkt.payload.status = t_status; uint16_t lcrc = calc_crc((const uint8_t *)&pkt, sizeof(pkt)-2); pkt.crc[0] = lcrc>>8; @@ -983,5 +1067,22 @@ void AP_Radio_cc2500::send_SRT_telemetry(void) cc2500.Strobe(CC2500_STX); } +/* + send a fwupload ack if needed + */ +void AP_Radio_cc2500::check_fw_ack(void) +{ + if (fwupload.need_ack && sem->take_nonblocking()) { + // ack the send of a DATA96 fw packet to TX + fwupload.need_ack = false; + uint8_t data16[16] {}; + uint32_t ack_to = fwupload.offset + fwupload.acked; + memcpy(&data16[0], &ack_to, 4); + mavlink_msg_data16_send(fwupload.chan, 42, 4, data16); + Debug(4,"sent ack DATA16\n"); + sem->give(); + } +} + #endif // HAL_RCINPUT_WITH_AP_RADIO diff --git a/libraries/AP_Radio/AP_Radio_cc2500.h b/libraries/AP_Radio/AP_Radio_cc2500.h index b8beb639b3..c46f6009c0 100644 --- a/libraries/AP_Radio/AP_Radio_cc2500.h +++ b/libraries/AP_Radio/AP_Radio_cc2500.h @@ -67,7 +67,8 @@ public: // get TX fw version uint32_t get_tx_version(void) override { - return 0; + // pack date into 16 bits for vendor_id in AUTOPILOT_VERSION + return (uint16_t(tx_date.firmware_year)<<12) + (uint16_t(tx_date.firmware_month)<<8) + tx_date.firmware_day; } // get radio statistics structure @@ -177,6 +178,25 @@ private: }; static const config radio_config[]; + struct { + mavlink_channel_t chan; + bool need_ack; + uint8_t counter; + uint8_t sequence; + uint32_t offset; + uint32_t length; + uint32_t acked; + uint8_t len; + enum telem_type fw_type; + uint8_t pending_data[92]; + } fwupload; + + struct { + uint8_t firmware_year; + uint8_t firmware_month; + uint8_t firmware_day; + } tx_date; + struct telem_status t_status; uint32_t last_pps_ms; uint8_t tx_rssi; @@ -184,6 +204,9 @@ private: bool handle_D16_packet(const uint8_t *packet); bool handle_SRT_packet(const uint8_t *packet); + + // check sending of fw upload ack + void check_fw_ack(void); }; diff --git a/libraries/AP_Radio/AP_Radio_cypress.h b/libraries/AP_Radio/AP_Radio_cypress.h index 7c222c5588..c207c5f039 100644 --- a/libraries/AP_Radio/AP_Radio_cypress.h +++ b/libraries/AP_Radio/AP_Radio_cypress.h @@ -227,7 +227,7 @@ private: enum telem_type fw_type; uint8_t pending_data[92]; } fwupload; - + // bind structure saved to storage static const uint16_t bind_magic = 0x43F6; struct PACKED bind_info { diff --git a/libraries/AP_Radio/telem_structure.h b/libraries/AP_Radio/telem_structure.h index 8cf6ce7601..08139d8b57 100644 --- a/libraries/AP_Radio/telem_structure.h +++ b/libraries/AP_Radio/telem_structure.h @@ -11,6 +11,7 @@ enum telem_type { TELEM_FW = 2, // update new firmware }; +// flags in telem_status structure #define TELEM_FLAG_GPS_OK (1U<<0) #define TELEM_FLAG_ARM_OK (1U<<1) #define TELEM_FLAG_BATT_OK (1U<<2) @@ -24,18 +25,12 @@ struct PACKED telem_status { uint8_t rssi; // lowpass rssi uint8_t flags; // TELEM_FLAG_* uint8_t flight_mode; // flight mode - uint8_t wifi_chan; - uint8_t tx_max; - uint8_t note_adjust; + uint8_t wifi_chan; // wifi channel number on Sonix + uint8_t tx_max; // max TX power + uint8_t note_adjust; // buzzer tone adjustment }; -// play a tune -struct PACKED telem_play { - uint8_t seq; - uint8_t tune_index; -}; - -// write to new firmware +// write to new firmware. This is also used to play a tune struct PACKED telem_firmware { uint8_t seq; uint8_t len; @@ -50,7 +45,7 @@ struct PACKED telem_packet_cypress { uint8_t crc; // simple CRC enum telem_type type; union { - uint8_t pkt[14]; + uint8_t pkt[14]; struct telem_status status; struct telem_firmware fw; } payload; @@ -71,39 +66,37 @@ struct PACKED telem_packet_cc2500 { uint8_t crc[2]; }; - -enum tx_telem_type { - TXTELEM_RSSI = 0, - TXTELEM_CRC1 = 1, - TXTELEM_CRC2 = 2, -}; - /* - tx_status structure sent one byte at a time to RX. This is packed - into channels 8, 9 and 10 (using 32 bits of a possible 33) + packet type - controls data field. We have 4 bits, giving 16 possible + data field types */ -struct PACKED telem_tx_status { - uint8_t crc; - enum tx_telem_type type; - uint16_t data; +enum packet_type { + PKTYPE_VOLTAGE = 0, + PKTYPE_YEAR = 1, + PKTYPE_MONTH = 2, + PKTYPE_DAY = 3, + PKTYPE_TELEM_RSSI = 4, + PKTYPE_TELEM_PPS = 5, + PKTYPE_BL_VERSION = 6, + PKTYPE_FW_ACK = 7, + PKTYPE_NUM_TYPES = 8 // used for modulus }; /* skyrocket specific packet for cc2500 */ -struct srt_packet { - uint8_t length; +struct PACKED srt_packet { + uint8_t length; // required for cc2500 FIFO uint8_t txid[2]; - uint8_t version; // protocol version + uint8_t version:4; // protocol version + uint8_t pkt_type:4; // packet type uint8_t chan1; uint8_t chan2; uint8_t chan3; uint8_t chan4; uint8_t chan_high; - uint8_t tx_voltage; // ADC value / 4 + uint8_t data; // data according to pkt_type uint8_t buttons; // see channels.h - uint8_t telem_pps; - uint8_t telem_rssi; uint8_t channr; uint8_t chanskip; uint8_t crc[2];