AP_ToshibaCAN: consume current sent by ESC

libraries/AP_ToshibaCAN/AP_ToshibaCAN.cpp

@@ -313,9 +313,18 @@ void AP_ToshibaCAN::loop()
                     if (esc_id < TOSHIBACAN_MAX_NUM_ESCS) {
                         WITH_SEMAPHORE(_telem_sem);
                         _telemetry[esc_id].rpm = be16toh(reply_data.rpm);
-                        _telemetry[esc_id].voltage_mv = be16toh(reply_data.voltage_mv);
+                        _telemetry[esc_id].current_ca = MAX((int16_t)be16toh(reply_data.current_ma), 0) * (4.0f * 0.1f);    // milli-amps to centi-amps
+                        _telemetry[esc_id].voltage_cv = be16toh(reply_data.voltage_mv) * 0.1f;  // millivolts to centi-volts
                         _telemetry[esc_id].count++;
                         _telemetry[esc_id].new_data = true;
+                        // update total current
+                        const uint32_t now_ms = AP_HAL::millis();
+                        const uint32_t diff_ms = now_ms - _telemetry[esc_id].last_update_ms;
+                        if (diff_ms <= 1000) {
+                            // convert centi-amps miiliseconds to mAh
+                            _telemetry[esc_id].current_tot_mah += _telemetry[esc_id].current_ca * diff_ms * centiamp_ms_to_mah;
+                        }
+                        _telemetry[esc_id].last_update_ms = now_ms;
                         _esc_present_bitmask |= ((uint32_t)1 << esc_id);
                     }
                 }
@@ -431,10 +440,10 @@ void AP_ToshibaCAN::update()
             if (_telemetry[i].new_data) {
                 logger->Write_ESC(i, time_us,
                               _telemetry[i].rpm * 100U,
-                              _telemetry[i].voltage_mv * 0.1f,
-                              0,
+                              _telemetry[i].voltage_cv,
+                              _telemetry[i].current_ca,
                               _telemetry[i].temperature * 100.0f,
-                              0);
+                              constrain_float(_telemetry[i].current_tot_mah, 0, UINT16_MAX));
                 _telemetry[i].new_data = false;
             }
         }
@@ -473,15 +482,18 @@ void AP_ToshibaCAN::send_esc_telemetry_mavlink(uint8_t mav_chan)
             // arrays to hold output
             uint8_t temperature[4] {};
             uint16_t voltage[4] {};
+            uint16_t current[4] {};
+            uint16_t current_tot[4] {};
             uint16_t rpm[4] {};
             uint16_t count[4] {};
-            uint16_t nosup[4] {};   // single empty array for unsupported current and current_tot
 
             // fill in output arrays
             for (uint8_t j = 0; j < 4; j++) {
                 uint8_t esc_id = i * 4 + j;
                 temperature[j] = _telemetry[esc_id].temperature;
-                voltage[j] = _telemetry[esc_id].voltage_mv * 0.1f;
+                voltage[j] = _telemetry[esc_id].voltage_cv;
+                current[j] = _telemetry[esc_id].current_ca;
+                current_tot[j] = constrain_float(_telemetry[esc_id].current_tot_mah, 0, UINT16_MAX);
                 rpm[j] = _telemetry[esc_id].rpm;
                 count[j] = _telemetry[esc_id].count;
             }
@@ -489,13 +501,13 @@ void AP_ToshibaCAN::send_esc_telemetry_mavlink(uint8_t mav_chan)
             // send messages
             switch (i) {
                 case 0:
-                    mavlink_msg_esc_telemetry_1_to_4_send((mavlink_channel_t)mav_chan, temperature, voltage, nosup, nosup, rpm, count);
+                    mavlink_msg_esc_telemetry_1_to_4_send((mavlink_channel_t)mav_chan, temperature, voltage, current, current_tot, rpm, count);
                     break;
                 case 1:
-                    mavlink_msg_esc_telemetry_5_to_8_send((mavlink_channel_t)mav_chan, temperature, voltage, nosup, nosup, rpm, count);
+                    mavlink_msg_esc_telemetry_5_to_8_send((mavlink_channel_t)mav_chan, temperature, voltage, current, current_tot, rpm, count);
                     break;
                 case 2:
-                    mavlink_msg_esc_telemetry_9_to_12_send((mavlink_channel_t)mav_chan, temperature, voltage, nosup, nosup, rpm, count);
+                    mavlink_msg_esc_telemetry_9_to_12_send((mavlink_channel_t)mav_chan, temperature, voltage, current, current_tot, rpm, count);
                     break;
                 default:
                     break;

libraries/AP_ToshibaCAN/AP_ToshibaCAN.h

@@ -70,13 +70,17 @@ private:
     HAL_Semaphore _telem_sem;
     struct telemetry_info_t {
         uint16_t rpm;               // rpm
-        uint16_t voltage_mv;        // voltage in millivolts
+        uint16_t voltage_cv;        // voltage in centi-volts
+        uint16_t current_ca;        // current in centi-amps
         uint16_t temperature;       // temperature in degrees
         uint16_t count;             // total number of packets sent
-        bool new_data;
+        uint32_t last_update_ms;    // system time telemetry was last update (used to calc total current)
+        float current_tot_mah;      // total current in mAh
+        bool new_data;              // true if new telemetry data has been filled in but not logged yet
     } _telemetry[TOSHIBACAN_MAX_NUM_ESCS];
     uint32_t _telemetry_req_ms;     // system time (in milliseconds) to request data from escs (updated at 10hz)
     uint8_t _telemetry_temp_req_counter;    // counter used to trigger temp data requests from ESCs (10x slower than other telem data)
+    const float centiamp_ms_to_mah = 1.0f / 360000.0f;  // for converting centi-amps milliseconds to mAh
 
     // bitmask of which escs seem to be present
     uint16_t _esc_present_bitmask;
@@ -136,7 +140,7 @@ private:
             uint8_t rxng:1;
             uint8_t state:7;
             uint16_t rpm;
-            uint16_t reserved;
+            uint16_t current_ma;    // current in milliamps
             uint16_t voltage_mv;    // voltage in millivolts
             uint8_t position_est_error;
         };