From 45ce8e8d79c0b67b97eba38dcc2d12c4dc05e2f8 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 30 Oct 2019 16:02:23 +0900 Subject: [PATCH] AP_ToshibaCAN: add get_present_mask for use in pre-arm checks --- libraries/AP_ToshibaCAN/AP_ToshibaCAN.cpp | 24 +++++++++++++++++++++-- libraries/AP_ToshibaCAN/AP_ToshibaCAN.h | 12 ++++++++++-- 2 files changed, 32 insertions(+), 4 deletions(-) diff --git a/libraries/AP_ToshibaCAN/AP_ToshibaCAN.cpp b/libraries/AP_ToshibaCAN/AP_ToshibaCAN.cpp index b34b37fc39..bd717c9295 100644 --- a/libraries/AP_ToshibaCAN/AP_ToshibaCAN.cpp +++ b/libraries/AP_ToshibaCAN/AP_ToshibaCAN.cpp @@ -325,7 +325,7 @@ void AP_ToshibaCAN::loop() _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); + _esc_present_bitmask_recent |= ((uint32_t)1 << esc_id); } } @@ -347,10 +347,13 @@ void AP_ToshibaCAN::loop() if (esc_id < TOSHIBACAN_MAX_NUM_ESCS) { WITH_SEMAPHORE(_telem_sem); _telemetry[esc_id].temperature = temp_max < 20 ? 0 : temp_max / 5 - 20; - _esc_present_bitmask |= ((uint32_t)1 << esc_id); + _esc_present_bitmask_recent |= ((uint32_t)1 << esc_id); } } } + + // update bitmask of escs that replied + update_esc_present_bitmask(); } // success! @@ -404,6 +407,23 @@ bool AP_ToshibaCAN::read_frame(uavcan::CanFrame &recv_frame, uavcan::MonotonicTi return (_can_driver->getIface(CAN_IFACE_INDEX)->receive(recv_frame, time, utc_time, flags) == 1); } +// update esc_present_bitmask +void AP_ToshibaCAN::update_esc_present_bitmask() +{ + // recently detected escs are immediately considered present + _esc_present_bitmask |= _esc_present_bitmask_recent; + + // escs that don't respond disappear in 1 to 2 seconds + // set the _esc_present_bitmask to the "recent" bitmask and + // clear the "recent" bitmask every second + uint32_t now_ms = AP_HAL::millis(); + if (now_ms - _esc_present_update_ms > 1000) { + _esc_present_bitmask = _esc_present_bitmask_recent; + _esc_present_bitmask_recent = 0; + _esc_present_update_ms = now_ms; + } +} + // called from SRV_Channels void AP_ToshibaCAN::update() { diff --git a/libraries/AP_ToshibaCAN/AP_ToshibaCAN.h b/libraries/AP_ToshibaCAN/AP_ToshibaCAN.h index 962d906159..709c268b88 100644 --- a/libraries/AP_ToshibaCAN/AP_ToshibaCAN.h +++ b/libraries/AP_ToshibaCAN/AP_ToshibaCAN.h @@ -41,6 +41,9 @@ public: // 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; } + private: // loop to send output to ESCs in background thread @@ -52,6 +55,9 @@ private: // read frame on CAN bus, returns true on success bool read_frame(uavcan::CanFrame &recv_frame, uavcan::MonotonicTime timeout); + // update esc_present_bitmask + void update_esc_present_bitmask(); + bool _initialized; char _thread_name[9]; uint8_t _driver_index; @@ -82,8 +88,10 @@ private: 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; + // variables for updating bitmask of responsive escs + uint16_t _esc_present_bitmask; // bitmask of which escs seem to be present + uint16_t _esc_present_bitmask_recent; // bitmask of escs that have responded in the last second + uint32_t _esc_present_update_ms; // system time _esc_present_bitmask was last updated // structure for sending motor lock command to ESC union motor_lock_cmd_t {