Changes to bsp for logging

rosflight · Aug 27, 2024 · 5348fb6 · 5348fb6
1 parent 31032f2
commit 5348fb6
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 7 deletions.
diff --git a/common/Varmint.cpp b/common/Varmint.cpp
@@ -117,22 +117,35 @@ void Varmint::serial_flush(void)
 //
 //////////////////////////////////////////////////////////////////////////////////////////////////
 
-// Report any initialization errors
 void Varmint::sensors_init()
 {
   sensor_errors_ = 0;
+  for (uint32_t i = 0; i < varmint.status_len(); i++) {
+    if (varmint.status(i)->status() != DRIVER_OK) sensor_errors_++;
+  }
+}
+uint16_t Varmint::sensors_errors_count() { return sensor_errors_; }
+
+uint16_t Varmint::sensors_init_message_count() { return varmint.status_len(); }
+
+bool Varmint::sensors_init_message_good(uint16_t i) { return varmint.status(i)->initGood(); }
 
-  for(uint32_t i=0;i<varmint.status_len();i++ )
+uint16_t Varmint::sensors_init_message(char * message, uint16_t size, uint16_t i)
   {
-    if(varmint.status(i)->status() != DRIVER_OK)sensor_errors_++;
+  if (i > varmint.status_len()) return 0;
+
+  uint32_t status = varmint.status(i)->status();
+  if (status == DRIVER_OK) {
+    snprintf(message, size, "%s: INIT OK", varmint.status(i)->name());
+  } else { //PTT TODO: we can add better messages later
+    snprintf(message, size, "%s: INIT ERROR 0x%08lX", varmint.status(i)->name(), status);
   }
+  return 1;
 }
 
-uint16_t Varmint::num_sensor_errors() { return sensor_errors_; }
-
 ///////////////////////////////////////////////////////////////////////////////////////////////
 // IMU
-
+bool Varmint::imu_present() { return imu0_.initGood(); }
 bool Varmint::imu_has_new_data() { return imu0_.rxFifoCount() > 0; }
 bool Varmint::imu_read(float accel[3], float * temperature, float gyro[3], uint64_t * time_us)
 {

diff --git a/common/Varmint.h b/common/Varmint.h
@@ -105,8 +105,12 @@ class Varmint : public rosflight_firmware::Board
 
   // sensors
   void sensors_init() override;
-  uint16_t num_sensor_errors() override;
+  uint16_t sensors_errors_count() override;
+  uint16_t sensors_init_message_count() override;
+  uint16_t sensors_init_message(char * message, uint16_t size, uint16_t i) override;
+  bool sensors_init_message_good(uint16_t i) override;
 
+  bool imu_present() override;
   bool imu_has_new_data() override;
   bool imu_read(float accel[3], float * temperature, float gyro[3], uint64_t * time_us) override;
   void imu_not_responding_error() override;