From 39d66b0fdc250430663845647ac4a3e85629e850 Mon Sep 17 00:00:00 2001
From: Paul Guenette <pfguenette@gmail.com>
Date: Mon, 10 Oct 2022 22:41:08 -0700
Subject: [PATCH] Add more cyclic messages

---
 Firmware/MotorControl/axis.hpp            | 16 +++++++
 Firmware/communication/can/can_simple.cpp | 51 ++++++++++++++---------
 Firmware/odrive-interface.yaml            |  8 ++++
 3 files changed, 56 insertions(+), 19 deletions(-)

diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp
index 14f05a63..1bfd58d9 100644
--- a/Firmware/MotorControl/axis.hpp
+++ b/Firmware/MotorControl/axis.hpp
@@ -56,6 +56,14 @@ public:
         bool is_extended = false;
         uint32_t heartbeat_rate_ms = 100;
         uint32_t encoder_rate_ms = 10;
+        uint32_t motor_error_rate_ms = 0;
+        uint32_t encoder_error_rate_ms = 0;
+        uint32_t controller_error_rate_ms = 0;
+        uint32_t sensorless_error_rate_ms = 0;
+        uint32_t encoder_count_rate_ms = 0;
+        uint32_t iq_rate_ms = 0;
+        uint32_t sensorless_rate_ms = 0;
+        uint32_t bus_vi_rate_ms = 0;
     };
 
     struct Config_t {
@@ -101,6 +109,14 @@ public:
     struct CAN_t {
         uint32_t last_heartbeat = 0;
         uint32_t last_encoder = 0;
+        uint32_t last_motor_error = 0;
+        uint32_t last_encoder_error = 0;
+        uint32_t last_controller_error = 0;
+        uint32_t last_sensorless_error = 0;
+        uint32_t last_encoder_count = 0;
+        uint32_t last_iq = 0;
+        uint32_t last_sensorless = 0;
+        uint32_t last_bus_vi = 0;
     };
 
     Axis(int axis_num,
diff --git a/Firmware/communication/can/can_simple.cpp b/Firmware/communication/can/can_simple.cpp
index c7143700..988c3c47 100644
--- a/Firmware/communication/can/can_simple.cpp
+++ b/Firmware/communication/can/can_simple.cpp
@@ -2,6 +2,7 @@
 #include "can_simple.hpp"
 
 #include <odrive_main.h>
+#include <functional>
 
 bool CANSimple::init() {
     for (size_t i = 0; i < AXIS_COUNT; ++i) {
@@ -398,26 +399,38 @@ uint32_t CANSimple::service_stack() {
         }
     }
 
-    for (auto& a : axes) {
-        MEASURE_TIME(a.task_times_.can_heartbeat) {
-            if (a.config_.can.heartbeat_rate_ms > 0) {
-                if ((now - a.can_.last_heartbeat) >= a.config_.can.heartbeat_rate_ms) {
-                    if (send_heartbeat(a))
-                        a.can_.last_heartbeat = now;
+    struct periodic {
+        const uint32_t& rate;
+        uint32_t& last_time;
+        bool (CANSimple::* callback)(const Axis& axis);
+    };
+
+    for (auto& axis : axes) {
+        std::array<periodic, 10> periodics = {{
+            {axis.config_.can.heartbeat_rate_ms, axis.can_.last_heartbeat, &CANSimple::send_heartbeat},
+            {axis.config_.can.encoder_rate_ms, axis.can_.last_encoder, &CANSimple::get_encoder_estimates_callback},
+            {axis.config_.can.motor_error_rate_ms, axis.can_.last_motor_error, &CANSimple::get_motor_error_callback},
+            {axis.config_.can.encoder_error_rate_ms, axis.can_.last_encoder_error, &CANSimple::get_encoder_error_callback},
+            {axis.config_.can.controller_error_rate_ms, axis.can_.last_controller_error, &CANSimple::get_controller_error_callback},
+            {axis.config_.can.sensorless_error_rate_ms, axis.can_.last_sensorless_error, &CANSimple::get_sensorless_error_callback},
+            {axis.config_.can.encoder_count_rate_ms, axis.can_.last_encoder_count, &CANSimple::get_encoder_count_callback},
+            {axis.config_.can.iq_rate_ms, axis.can_.last_iq, &CANSimple::get_iq_callback},
+            {axis.config_.can.sensorless_rate_ms, axis.can_.last_sensorless, &CANSimple::get_sensorless_estimates_callback},
+            {axis.config_.can.bus_vi_rate_ms, axis.can_.last_bus_vi, &CANSimple::get_bus_voltage_current_callback},
+        }};
+
+        MEASURE_TIME(axis.task_times_.can_heartbeat) {
+            for (auto& msg : periodics) {
+                if (msg.rate > 0) {
+                    if ((now - msg.last_time) >= msg.rate) {
+                        if (std::invoke(msg.callback, this, axis)) {
+                            msg.last_time = now;
+                        }
+                    }
+
+                    int nextAxisService = msg.last_time + msg.rate - now;
+                    nextServiceTime = std::min(nextServiceTime, static_cast<uint32_t>(std::max(0, nextAxisService)));
                 }
-
-                int nextAxisService = a.can_.last_heartbeat + a.config_.can.heartbeat_rate_ms - now;
-                nextServiceTime = std::min(nextServiceTime, static_cast<uint32_t>(std::max(0, nextAxisService)));
-            }
-
-            if (a.config_.can.encoder_rate_ms > 0) {
-                if ((now - a.can_.last_encoder) >= a.config_.can.encoder_rate_ms) {
-                    if (get_encoder_estimates_callback(a))
-                        a.can_.last_encoder = now;
-                }
-
-                int nextAxisService = a.can_.last_encoder + a.config_.can.encoder_rate_ms - now;
-                nextServiceTime = std::min(nextServiceTime, static_cast<uint32_t>(std::max(0, nextAxisService)));
             }
         }
     }
diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml
index e38c7187..5ece8756 100644
--- a/Firmware/odrive-interface.yaml
+++ b/Firmware/odrive-interface.yaml
@@ -600,6 +600,14 @@ interfaces:
       is_extended: bool
       heartbeat_rate_ms: uint32
       encoder_rate_ms: uint32
+      motor_error_rate_ms: uint32
+      encoder_error_rate_ms: uint32
+      controller_error_rate_ms: uint32
+      sensorless_error_rate_ms: uint32
+      encoder_count_rate_ms: uint32
+      iq_rate_ms: uint32
+      sensorless_rate_ms: uint32
+      bus_vi_rate_ms: uint32
 
   ODrive.ThermistorCurrentLimiter:
     c_is_class: False