diff --git a/libraries/AP_ToshibaCAN/AP_ToshibaCAN.cpp b/libraries/AP_ToshibaCAN/AP_ToshibaCAN.cpp index c7fdac0678..1bbe35deb8 100644 --- a/libraries/AP_ToshibaCAN/AP_ToshibaCAN.cpp +++ b/libraries/AP_ToshibaCAN/AP_ToshibaCAN.cpp @@ -305,19 +305,24 @@ void AP_ToshibaCAN::loop() // store response in telemetry array const uint8_t esc_id = recv_frame.id - MOTOR_DATA1; if (esc_id < TOSHIBACAN_MAX_NUM_ESCS) { - WITH_SEMAPHORE(_telem_sem); - _telemetry[esc_id].rpm = (int16_t)be16toh(reply_data.rpm); - _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_rpm(esc_id, (int16_t)be16toh(reply_data.rpm)); + // update total current const uint32_t now_ms = AP_HAL::native_millis(); const uint32_t diff_ms = now_ms - _telemetry[esc_id].last_update_ms; + TelemetryData t {}; + t.voltage = float(be16toh(reply_data.voltage_mv)) * 0.001f; // millivolts to volts + t.current = MAX((int16_t)be16toh(reply_data.current_ma), 0) * (4.0f * 0.001f); // milli-amps to amps 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; + // convert centi-amps miliseconds to mAh + _telemetry[esc_id].current_tot_mah += t.current * diff_ms * amp_ms_to_mah; } + t.consumption_mah = _telemetry[esc_id].current_tot_mah; + update_telem_data(esc_id, t, + AP_ESC_Telem_Backend::TelemetryType::CURRENT + | AP_ESC_Telem_Backend::TelemetryType::VOLTAGE + | AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION); + _telemetry[esc_id].last_update_ms = now_ms; _esc_present_bitmask_recent |= ((uint32_t)1 << esc_id); } @@ -340,10 +345,16 @@ void AP_ToshibaCAN::loop() // store response in telemetry array uint8_t esc_id = recv_frame.id - MOTOR_DATA2; if (esc_id < TOSHIBACAN_MAX_NUM_ESCS) { - WITH_SEMAPHORE(_telem_sem); - _telemetry[esc_id].esc_temp = temp_max < 100 ? 0 : temp_max / 5 - 20; - _telemetry[esc_id].motor_temp = motor_temp < 100 ? 0 : motor_temp / 5 - 20; + const int16_t esc_temp_deg = temp_max < 100 ? 0 : temp_max / 5 - 20; + const int16_t motor_temp_deg = motor_temp < 100 ? 0 : motor_temp / 5 - 20; _esc_present_bitmask_recent |= ((uint32_t)1 << esc_id); + + TelemetryData t { + .temperature_cdeg = int16_t(esc_temp_deg * 100) + }; + t.motor_temp_cdeg = int16_t(motor_temp_deg * 100); + update_telem_data(esc_id, t, AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE | + AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE); } } @@ -358,9 +369,11 @@ void AP_ToshibaCAN::loop() // store response in telemetry array uint8_t esc_id = recv_frame.id - MOTOR_DATA3; if (esc_id < TOSHIBACAN_MAX_NUM_ESCS) { - WITH_SEMAPHORE(_telem_sem); - _telemetry[esc_id].usage_sec = usage_sec; _esc_present_bitmask_recent |= ((uint32_t)1 << esc_id); + + TelemetryData t {}; + t.usage_s = usage_sec; + update_telem_data(esc_id, t, AP_ESC_Telem_Backend::TelemetryType::USAGE); } } } @@ -456,102 +469,6 @@ void AP_ToshibaCAN::update() } update_count++; } - - // log ESCs telemetry info - AP_Logger *logger = AP_Logger::get_singleton(); - if (logger && logger->logging_enabled()) { - WITH_SEMAPHORE(_telem_sem); - - // log if any new data received. Logging only supports up to 8 ESCs - const uint64_t time_us = AP_HAL::native_micros64(); - for (uint8_t i = 0; i < MIN(TOSHIBACAN_MAX_NUM_ESCS, 8); i++) { - if (_telemetry[i].new_data) { - logger->Write_ESC(i, time_us, - _telemetry[i].rpm * 100U, - _telemetry[i].voltage_cv, - _telemetry[i].current_ca, - _telemetry[i].esc_temp * 100U, - constrain_float(_telemetry[i].current_tot_mah, 0, UINT16_MAX), - _telemetry[i].motor_temp * 100U); - _telemetry[i].new_data = false; - } - } - } -} - -// send ESC telemetry messages over MAVLink -void AP_ToshibaCAN::send_esc_telemetry_mavlink(uint8_t mav_chan) -{ - // compile time check this method handles the correct number of motors - static_assert(TOSHIBACAN_MAX_NUM_ESCS == 12, "update AP_ToshibaCAN::send_esc_telemetry_mavlink only handles 12 motors"); - - // return immediately if no ESCs have been found - if (_esc_present_bitmask == 0) { - return; - } - - // output telemetry messages - { - // take semaphore to access telemetry data - WITH_SEMAPHORE(_telem_sem); - - // loop through 3 groups of 4 ESCs - for (uint8_t i = 0; i < 3; i++) { - - // return if no space in output buffer to send mavlink messages - if (!HAVE_PAYLOAD_SPACE((mavlink_channel_t)mav_chan, ESC_TELEMETRY_1_TO_4)) { - return; - } - - // skip this group of ESCs if no data to send - if ((_esc_present_bitmask & ((uint32_t)0x0F << i*4)) == 0) { - continue; - } - - // 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] {}; - - // fill in output arrays - for (uint8_t j = 0; j < 4; j++) { - uint8_t esc_id = i * 4 + j; - temperature[j] = _telemetry[esc_id].esc_temp; - 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] = abs(_telemetry[esc_id].rpm); // mavlink message only accepts positive rpm values - count[j] = _telemetry[esc_id].count; - } - - // send messages - switch (i) { - case 0: - 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, current, current_tot, rpm, count); - break; - case 2: - mavlink_msg_esc_telemetry_9_to_12_send((mavlink_channel_t)mav_chan, temperature, voltage, current, current_tot, rpm, count); - break; - default: - break; - } - } - } -} - -// return total usage time in seconds -uint32_t AP_ToshibaCAN::get_usage_seconds(uint8_t esc_id) const -{ - if (esc_id >= TOSHIBACAN_MAX_NUM_ESCS) { - return 0; - } - return _telemetry[esc_id].usage_sec; } // helper function to create motor_request_data_cmd_t diff --git a/libraries/AP_ToshibaCAN/AP_ToshibaCAN.h b/libraries/AP_ToshibaCAN/AP_ToshibaCAN.h index baec28bb8c..c638631c20 100644 --- a/libraries/AP_ToshibaCAN/AP_ToshibaCAN.h +++ b/libraries/AP_ToshibaCAN/AP_ToshibaCAN.h @@ -15,6 +15,7 @@ #pragma once +#include #include #include @@ -22,7 +23,7 @@ class CANTester; -class AP_ToshibaCAN : public AP_CANDriver { +class AP_ToshibaCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { friend class CANTester; public: AP_ToshibaCAN(); @@ -42,15 +43,9 @@ public: // called from SRV_Channels void update(); - // send ESC telemetry messages over MAVLink - void send_esc_telemetry_mavlink(uint8_t mav_chan); - // return a bitmask of escs that are "present" which means they are responding to requests. Bitmask matches RC outputs uint16_t get_present_mask() const { return _esc_present_bitmask; } - // return total usage time in seconds - uint32_t get_usage_seconds(uint8_t esc_id) const; - private: // data format for messages from flight controller @@ -101,24 +96,14 @@ private: uint16_t update_count_sent; // counter of outputs successfully sent uint8_t send_stage; // stage of sending algorithm (each stage sends one frame to ESCs) - // telemetry data (rpm, voltage) - HAL_Semaphore _telem_sem; struct telemetry_info_t { - int16_t rpm; // rpm - uint16_t voltage_cv; // voltage in centi-volts - uint16_t current_ca; // current in centi-amps - uint16_t esc_temp; // esc temperature in degrees - uint16_t motor_temp; // motor temperature in degrees - uint16_t count; // total number of packets sent - uint32_t usage_sec; // motor's total usage in seconds 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) uint8_t _telemetry_usage_req_counter; // counter used to trigger usage data requests from ESCs (100x slower than other telem data) - const float centiamp_ms_to_mah = 1.0f / 360000.0f; // for converting centi-amps milliseconds to mAh + const float amp_ms_to_mah = 1.0f / 3600.0f; // for converting amp milliseconds to mAh // variables for updating bitmask of responsive escs uint16_t _esc_present_bitmask; // bitmask of which escs seem to be present