AP_Torqeedo: health reporting based on sending and receiving

libraries/AP_Torqeedo/AP_Torqeedo.cpp

@@ -199,8 +199,10 @@ bool AP_Torqeedo::healthy()
         return false;
     }
     {
+        // healthy if both receive and send have occurred in the last 3 seconds
         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;
             }
             _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
             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();
     _send_delay_us = calc_send_delay_us(buff_size);
 
-    // consider driver healthy
     {
+        // record time of send for health reporting
         WITH_SEMAPHORE(_last_healthy_sem);
-        _last_healthy_ms = AP_HAL::millis();
+        _last_send_motor_ms = AP_HAL::millis();
     }
 
 }

libraries/AP_Torqeedo/AP_Torqeedo.h

@@ -121,12 +121,12 @@ private:
     bool _initialised;              // true once driver has been initialised
     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)
-    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
 
     // 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
+    HAL_Semaphore _last_healthy_sem;// semaphore protecting reading and updating of _last_send_motor_ms and _last_received_ms
     uint32_t _last_debug_ms;        // system time (in millis) that debug was last output
 
     // message parsing members
@@ -135,6 +135,7 @@ private:
     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_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;
 };