AP_Torqeedo: add healthy and pre-arm checks

libraries/AP_Torqeedo/AP_Torqeedo.cpp

@@ -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

libraries/AP_Torqeedo/AP_Torqeedo.h

@@ -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)