AP_Torqeedo: add healthy and pre-arm checks

This commit is contained in:
Randy Mackay 2021-07-13 21:14:07 +09:00
parent 719b1e1267
commit 7bcd43eb0d
2 changed files with 45 additions and 0 deletions

View File

@ -151,6 +151,33 @@ void AP_Torqeedo::thread_main()
}
}
// returns true if communicating with the motor
bool AP_Torqeedo::healthy()
{
if (!_initialised) {
return false;
}
{
WITH_SEMAPHORE(_last_healthy_sem);
return ((AP_HAL::millis() - _last_healthy_ms) < 3000);
}
}
// run pre-arm check. returns false on failure and fills in failure_msg
bool AP_Torqeedo::pre_arm_checks(char *failure_msg, uint8_t failure_msg_len)
{
const char *failure_prefix = "Torqeedo:";
if (!_initialised) {
hal.util->snprintf(failure_msg, failure_msg_len, "%s not initialised", failure_prefix);
return false;
}
if (!healthy()) {
hal.util->snprintf(failure_msg, failure_msg_len, "%s not healthy", failure_prefix);
return false;
}
return true;
}
// process a single byte received on serial port
// return true if a this driver should send a set-motor-speed message
bool AP_Torqeedo::parse_byte(uint8_t b)
@ -294,6 +321,13 @@ void AP_Torqeedo::send_motor_speed_cmd()
_last_send_motor_us = AP_HAL::micros();
_send_delay_us = calc_send_delay_us(buff_size);
// consider driver healthy
{
WITH_SEMAPHORE(_last_healthy_sem);
_last_healthy_ms = AP_HAL::millis();
}
}
// get the AP_Torqeedo singleton

View File

@ -24,6 +24,7 @@
#if HAL_TORQEEDO_ENABLED
#include <AP_Param/AP_Param.h>
#include <AP_HAL/Semaphores.h>
#define TORQEEDO_MESSAGE_LEN_MAX 30 // messages are no more than 30 bytes
@ -42,6 +43,12 @@ public:
// runs in background thread
void thread_main();
// returns true if communicating with the motor
bool healthy();
// run pre-arm check. returns false on failure and fills in failure_msg
bool pre_arm_checks(char *failure_msg, uint8_t failure_msg_len);
static const struct AP_Param::GroupInfo var_info[];
private:
@ -93,6 +100,10 @@ private:
uint32_t _last_send_motor_us; // system time (in micros) last motor speed command was sent
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
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_healthy_ms
// message parsing members
ParseState _parse_state; // current state of parsing
uint32_t _parse_error_count; // total number of parsing errors (for reporting)