Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_ToshibaCAN: add get_present_mask for use in pre-arm checks

Browse Source
This commit is contained in:
Randy Mackay 2019-10-30 16:02:23 +09:00
parent ad55b961a4
commit 45ce8e8d79
2 changed files with 32 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_ToshibaCAN/AP_ToshibaCAN.cpp
View File

@ -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].current_tot_mah += _telemetry[esc_id].current_ca * diff_ms * centiamp_ms_to_mah;
} }
_telemetry[esc_id].last_update_ms = now_ms; _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) { if (esc_id < TOSHIBACAN_MAX_NUM_ESCS) {
WITH_SEMAPHORE(_telem_sem); WITH_SEMAPHORE(_telem_sem);
_telemetry[esc_id].temperature = temp_max < 20 ? 0 : temp_max / 5 - 20; _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! // 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); 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 // called from SRV_Channels
void AP_ToshibaCAN::update() void AP_ToshibaCAN::update()
{ {

12
libraries/AP_ToshibaCAN/AP_ToshibaCAN.h
View File

@ -41,6 +41,9 @@ public:
// send ESC telemetry messages over MAVLink // send ESC telemetry messages over MAVLink
void send_esc_telemetry_mavlink(uint8_t mav_chan); 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: private:
// loop to send output to ESCs in background thread // loop to send output to ESCs in background thread
@ -52,6 +55,9 @@ private:
// read frame on CAN bus, returns true on success // read frame on CAN bus, returns true on success
bool read_frame(uavcan::CanFrame &recv_frame, uavcan::MonotonicTime timeout); bool read_frame(uavcan::CanFrame &recv_frame, uavcan::MonotonicTime timeout);
// update esc_present_bitmask
void update_esc_present_bitmask();
bool _initialized; bool _initialized;
char _thread_name[9]; char _thread_name[9];
uint8_t _driver_index; 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) 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 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 // variables for updating bitmask of responsive escs
uint16_t _esc_present_bitmask; 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 // structure for sending motor lock command to ESC
union motor_lock_cmd_t { union motor_lock_cmd_t {