diff --git a/common/Varmint.cpp b/common/Varmint.cpp index c76c7b1..1dad499 100644 --- 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;istatus() != 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 index b02f5a8..fc69c38 100644 --- 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;