mirror of https://github.com/ArduPilot/ardupilot
AP_Torqeedo: add healthy and pre-arm checks
This commit is contained in:
parent
719b1e1267
commit
7bcd43eb0d
|
@ -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
|
||||
|
|
|
@ -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)
|
||||
|
|
Loading…
Reference in New Issue