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_Torqeedo: health reporting based on sending and receiving

Browse Source
This commit is contained in:
Randy Mackay 2021-08-04 13:56:54 +09:00
parent 9ffd22fad0
commit 2890c1c651
2 changed files with 14 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_Torqeedo/AP_Torqeedo.cpp
View File

@ -199,8 +199,10 @@ bool AP_Torqeedo::healthy()
return false; return false;
} }
{ {
// healthy if both receive and send have occurred in the last 3 seconds
WITH_SEMAPHORE(_last_healthy_sem); WITH_SEMAPHORE(_last_healthy_sem);
return ((AP_HAL::millis() - _last_healthy_ms) < 3000); uint32_t now_ms = AP_HAL::millis();
return ((now_ms - _last_received_ms < 3000) && (now_ms - _last_send_motor_ms < 3000));
} }
} }
@ -254,6 +256,11 @@ bool AP_Torqeedo::parse_byte(uint8_t b)
break; break;
} }
_parse_success_count++; _parse_success_count++;
{
// record time of successful receive for health reporting
WITH_SEMAPHORE(_last_healthy_sem);
_last_received_ms = AP_HAL::millis();
}
// check message id // check message id
MsgId msg_id = (MsgId)_received_buff[0]; MsgId msg_id = (MsgId)_received_buff[0];
@ -377,10 +384,10 @@ void AP_Torqeedo::send_motor_speed_cmd()
_last_send_motor_us = AP_HAL::micros(); _last_send_motor_us = AP_HAL::micros();
_send_delay_us = calc_send_delay_us(buff_size); _send_delay_us = calc_send_delay_us(buff_size);
// consider driver healthy
{ {
// record time of send for health reporting
WITH_SEMAPHORE(_last_healthy_sem); WITH_SEMAPHORE(_last_healthy_sem);
_last_healthy_ms = AP_HAL::millis(); _last_send_motor_ms = AP_HAL::millis();
} }
} }

7
libraries/AP_Torqeedo/AP_Torqeedo.h
View File

@ -121,12 +121,12 @@ private:
bool _initialised; // true once driver has been initialised bool _initialised; // true once driver has been initialised
bool _send_motor_speed; // true if motor speed should be sent at next opportunity bool _send_motor_speed; // true if motor speed should be sent at next opportunity
int16_t _motor_speed; // desired motor speed (set from within update method) int16_t _motor_speed; // desired motor speed (set from within update method)
uint32_t _last_send_motor_us; // system time (in micros) last motor speed command was sent uint32_t _last_send_motor_ms; // system time (in millis) last motor speed command was sent (used for health reporting)
uint32_t _last_send_motor_us; // system time (in micros) last motor speed command was sent (used for timing to unset DE pin)
uint32_t _send_delay_us; // delay (in micros) to allow bytes to be sent after which pin can be unset. 0 if not delaying uint32_t _send_delay_us; // delay (in micros) to allow bytes to be sent after which pin can be unset. 0 if not delaying
// health reporting // health reporting
uint32_t _last_healthy_ms; // system time (in millis) that driver was last considered healthy HAL_Semaphore _last_healthy_sem;// semaphore protecting reading and updating of _last_send_motor_ms and _last_received_ms
HAL_Semaphore _last_healthy_sem;// semaphore protecting reading and updating of _last_healthy_ms
uint32_t _last_debug_ms; // system time (in millis) that debug was last output uint32_t _last_debug_ms; // system time (in millis) that debug was last output
// message parsing members // message parsing members
@ -135,6 +135,7 @@ private:
uint32_t _parse_success_count; // number of messages successfully parsed (for reporting) uint32_t _parse_success_count; // number of messages successfully parsed (for reporting)
uint8_t _received_buff[TORQEEDO_MESSAGE_LEN_MAX]; // characters received uint8_t _received_buff[TORQEEDO_MESSAGE_LEN_MAX]; // characters received
uint8_t _received_buff_len; // number of characters received uint8_t _received_buff_len; // number of characters received
uint32_t _last_received_ms; // system time (in millis) that a message was successfully parsed (for health reporting)
static AP_Torqeedo *_singleton; static AP_Torqeedo *_singleton;
}; };