AP_Radio: implement OTA update for cc2500

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;
+    uint8_t data = pkt->data;
-    tx_pps = pkt->telem_pps;
+    /*
+      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
 

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);
 };
 
 

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 {

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 wifi_chan; // wifi channel number on Sonix
-    uint8_t tx_max;
+    uint8_t tx_max;  // max TX power
-    uint8_t note_adjust;
+    uint8_t note_adjust; // buzzer tone adjustment
 };
 
-// play a tune
+// write to new firmware. This is also used to play a tune
-struct PACKED telem_play {
-    uint8_t seq;
-    uint8_t tune_index;
-};
-
-// write to new firmware
 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
+  packet type - controls data field. We have 4 bits, giving 16 possible
-  into channels 8, 9 and 10 (using 32 bits of a possible 33)
+  data field types
  */
-struct PACKED telem_tx_status {
+enum packet_type {
-    uint8_t crc;
+    PKTYPE_VOLTAGE    = 0,
-    enum tx_telem_type type;
+    PKTYPE_YEAR       = 1,
-    uint16_t data;
+    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 {
+struct PACKED srt_packet {
-    uint8_t length;
+    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];