From 030c3b8ca06c99c91e7cadfdaaef5781c6ab2f21 Mon Sep 17 00:00:00 2001
From: Phil Tokumaru <tokumaru@avinc.com>
Date: Tue, 27 Aug 2024 13:20:08 -0700
Subject: [PATCH] Add board init status messages along with full parameters
 listing

---
 include/board.h               | 13 +++++++------
 include/comm_manager.h        |  3 ++-
 include/interface/comm_link.h |  1 -
 src/comm_manager.cpp          | 31 ++++++++++++++++++++++++++++---
 4 files changed, 37 insertions(+), 11 deletions(-)

diff --git a/include/board.h b/include/board.h
index 8f081542..0151003a 100644
--- a/include/board.h
+++ b/include/board.h
@@ -54,6 +54,13 @@ class Board
   virtual void init_board() = 0;
   virtual void board_reset(bool bootloader) = 0;
 
+  virtual void sensors_init(void) = 0;
+  virtual uint16_t sensors_errors_count() = 0;
+
+  virtual uint16_t sensors_init_message_count() =  0;
+  virtual bool sensors_init_message_good(uint16_t i) = 0;
+  virtual uint16_t sensors_init_message(char *message, uint16_t size, uint16_t i) = 0;
+
   // clock
   virtual uint32_t clock_millis() = 0;
   virtual uint64_t clock_micros() = 0;
@@ -67,12 +74,6 @@ class Board
   virtual uint8_t serial_read() = 0;
   virtual void serial_flush() = 0;
 
-  // sensors
-  virtual void sensors_init(void) = 0;
-  virtual uint16_t sensors_errors_count() = 0;
-  virtual uint16_t sensors_init_message_count() =  0;
-  virtual uint16_t sensors_init_message(char &message, uint32_t i) = 0;
-
   // IMU
   virtual bool imu_present() = 0;
   virtual bool imu_has_new_data() = 0;
diff --git a/include/comm_manager.h b/include/comm_manager.h
index 1125019e..b647b72f 100644
--- a/include/comm_manager.h
+++ b/include/comm_manager.h
@@ -180,8 +180,9 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis
   void send_param_value(uint16_t param_id);
   void update_status();
   void log(CommLinkInterface::LogSeverity severity, const char * fmt, ...);
+  void log_message(CommLinkInterface::LogSeverity severity, char * text);
 
-  void send_parameter_list();
+//  void send_parameter_list();
   void send_named_value_float(const char * const name, float value);
 
   void send_backup_data(const StateManager::BackupData & backup_data);
diff --git a/include/interface/comm_link.h b/include/interface/comm_link.h
index c5a156e0..1bb3a816 100644
--- a/include/interface/comm_link.h
+++ b/include/interface/comm_link.h
@@ -32,7 +32,6 @@
 #ifndef ROSFLIGHT_FIRMWARE_COMM_LINK_H
 #define ROSFLIGHT_FIRMWARE_COMM_LINK_H
 
-#include "board.h"
 #include "param.h"
 #include "sensors.h"
 #include "state_manager.h"
diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp
index 7d4b9ed5..eaefaca7 100644
--- a/src/comm_manager.cpp
+++ b/src/comm_manager.cpp
@@ -129,7 +129,10 @@ void CommManager::param_request_list_callback(uint8_t target_system)
   if (target_system == sysid_) { send_params_index_ = 0; }
 }
 
-void CommManager::send_parameter_list() { send_params_index_ = 0; }
+//void CommManager::send_parameter_list()
+//{
+//  send_params_index_ = 0;
+//}
 
 void CommManager::param_request_read_callback(uint8_t target_system, const char * const param_name,
                                               int16_t param_index)
@@ -329,10 +332,15 @@ void CommManager::log(CommLinkInterface::LogSeverity severity, const char * fmt,
   // Convert the format string to a raw char array
   va_list args;
   va_start(args, fmt);
-  char text[LOG_MSG_SIZE];
-  vsnprintf(text, LOG_MSG_SIZE, fmt, args);
+  char message[LOG_MSG_SIZE];
+  vsnprintf(message, LOG_MSG_SIZE, fmt, args);
   va_end(args);
 
+  log_message(severity, message);
+}
+
+void CommManager::log_message(CommLinkInterface::LogSeverity severity, char * text)
+{
   if (initialized_ && connected_) {
     comm_link_.send_log_message(sysid_, severity, text);
   } else {
@@ -526,6 +534,23 @@ void CommManager::send_named_value_float(const char * const name, float value)
 
 void CommManager::send_next_param(void)
 {
+  //PTT Hack
+  //TODO figure out a better place to do this
+  if (send_params_index_==0)
+  {
+
+    //   Board initialization status
+      for(uint16_t i=0; i<RF_.board_.sensors_init_message_count(); i++)
+      {
+        char message[LOG_MSG_SIZE]; // No point in makeing a message larger than this.
+        if(RF_.board_.sensors_init_message(message,LOG_MSG_SIZE,i) !=0)
+        {
+          CommLinkInterface::LogSeverity severity =  CommLinkInterface::LogSeverity::LOG_INFO;
+          if(!RF_.board_.sensors_init_message_good(i)) { severity = CommLinkInterface::LogSeverity::LOG_ERROR; }
+          log_message(severity, message);
+        }
+      }
+  }
   if (send_params_index_ < PARAMS_COUNT) {
     send_param_value(static_cast<uint16_t>(send_params_index_));
     send_params_index_++;