From 9b7effd1f574d232a1a39ec9f72e7645bde22b75 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Wed, 15 Nov 2023 14:27:26 -0700 Subject: [PATCH] Rewored sensor and RC acquisition Previously, sensors and RC hardware would collect and store data at a rate that was independent of how often the core rosflight code checked for sensor data. This commit changes that, making the core rosflight code be dependent on the hardware's aquisition rate, syncronizing the two. --- include/board.h | 43 ++++---- include/comm_manager.h | 12 +-- include/param.h | 15 --- include/sensors.h | 23 ++++- src/comm_manager.cpp | 102 +++++++------------ src/param.cpp | 15 --- src/rc.cpp | 13 +-- src/rosflight.cpp | 18 ++-- src/sensors.cpp | 223 ++++++++++++++++++++--------------------- 9 files changed, 202 insertions(+), 262 deletions(-) diff --git a/include/board.h b/include/board.h index 64e0c455..91d28f58 100644 --- a/include/board.h +++ b/include/board.h @@ -70,45 +70,48 @@ class Board virtual void sensors_init() = 0; virtual uint16_t num_sensor_errors() = 0; - virtual bool new_imu_data() = 0; + // IMU + virtual uint8_t imu_has_new_data() = 0; virtual bool imu_read(float accel[3], float *temperature, float gyro[3], uint64_t *time) = 0; virtual void imu_not_responding_error() = 0; + // Mag virtual bool mag_present() = 0; - virtual void mag_update() = 0; - virtual void mag_read(float mag[3]) = 0; + virtual uint8_t mag_has_new_data() = 0; + virtual bool mag_read(float mag[3]) = 0; + // Baro virtual bool baro_present() = 0; - virtual void baro_update() = 0; - virtual void baro_read(float *pressure, float *temperature) = 0; + virtual uint8_t baro_has_new_data() = 0; + virtual bool baro_read(float *pressure, float *temperature) = 0; + // Pitot virtual bool diff_pressure_present() = 0; - virtual void diff_pressure_update() = 0; - virtual void diff_pressure_read(float *diff_pressure, float *temperature) = 0; + virtual uint8_t diff_pressure_has_new_data() = 0; + virtual bool diff_pressure_read(float *diff_pressure, float *temperature) = 0; + // Sonar virtual bool sonar_present() = 0; - virtual void sonar_update() = 0; - virtual float sonar_read() = 0; + virtual uint8_t sonar_has_new_data() = 0; + virtual bool sonar_read(float *range) =0; + // GPS virtual bool gnss_present() = 0; - virtual void gnss_update() = 0; + virtual uint8_t gnss_has_new_data() = 0; + virtual bool gnss_read( GNSSData *gnss, GNSSFull *gnss_full) = 0; - virtual GNSSData gnss_read() = 0; - virtual bool gnss_has_new_data() = 0; - virtual GNSSFull gnss_full_read() = 0; - - virtual bool battery_voltage_present() const = 0; - virtual float battery_voltage_read() const = 0; + // Battery + virtual bool battery_present() = 0; + virtual uint8_t battery_has_new_data() = 0; + virtual bool battery_read(float *voltage, float* current) = 0; virtual void battery_voltage_set_multiplier(double multiplier) = 0; - - virtual bool battery_current_present() const = 0; - virtual float battery_current_read() const = 0; virtual void battery_current_set_multiplier(double multiplier) = 0; // RC virtual void rc_init(rc_type_t rc_type) = 0; virtual bool rc_lost() = 0; - virtual float rc_read(uint8_t channel) = 0; + virtual uint8_t rc_has_new_data() = 0; + virtual float rc_read(uint8_t chan) = 0; // PWM virtual void pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) = 0; diff --git a/include/comm_manager.h b/include/comm_manager.h index c84ffcf0..d820c2f2 100644 --- a/include/comm_manager.h +++ b/include/comm_manager.h @@ -163,15 +163,6 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis void send_next_param(void); - Stream streams_[STREAM_COUNT] = { - Stream(0, [this] { this->send_heartbeat(); }), Stream(0, [this] { this->send_status(); }), - Stream(0, [this] { this->send_attitude(); }), Stream(0, [this] { this->send_imu(); }), - Stream(0, [this] { this->send_diff_pressure(); }), Stream(0, [this] { this->send_baro(); }), - Stream(0, [this] { this->send_sonar(); }), Stream(0, [this] { this->send_mag(); }), - Stream(0, [this] { this->send_battery_status(); }), Stream(0, [this] { this->send_output_raw(); }), - Stream(0, [this] { this->send_gnss(); }), Stream(0, [this] { this->send_gnss_full(); }), - Stream(0, [this] { this->send_rc_raw(); }), Stream(20000, [this] { this->send_low_priority(); })}; - // the time of week stamp for the last sent GNSS message, to prevent re-sending uint32_t last_sent_gnss_tow_ = 0; uint32_t last_sent_gnss_full_tow_ = 0; @@ -182,9 +173,8 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis void init(); void param_change_callback(uint16_t param_id) override; void receive(void); - void stream(); + void stream(got_flags got); void send_param_value(uint16_t param_id); - void set_streaming_rate(uint8_t stream_id, int16_t param_id); void update_status(); void log(CommLinkInterface::LogSeverity severity, const char* fmt, ...); diff --git a/include/param.h b/include/param.h index 9233d363..0ea929a7 100644 --- a/include/param.h +++ b/include/param.h @@ -51,21 +51,6 @@ enum : uint16_t /*** MAVLINK CONFIGURATION ***/ /*****************************/ PARAM_SYSTEM_ID, - PARAM_STREAM_HEARTBEAT_RATE, - PARAM_STREAM_STATUS_RATE, - - PARAM_STREAM_ATTITUDE_RATE, - PARAM_STREAM_IMU_RATE, - PARAM_STREAM_MAG_RATE, - PARAM_STREAM_BARO_RATE, - PARAM_STREAM_AIRSPEED_RATE, - PARAM_STREAM_SONAR_RATE, - PARAM_STREAM_GNSS_RATE, - PARAM_STREAM_GNSS_FULL_RATE, - PARAM_STREAM_BATTERY_STATUS_RATE, - - PARAM_STREAM_OUTPUT_RAW_RATE, - PARAM_STREAM_RC_RAW_RATE, /********************************/ /*** CONTROLLER CONFIGURATION ***/ diff --git a/include/sensors.h b/include/sensors.h index bdd0f6f8..fcedca51 100644 --- a/include/sensors.h +++ b/include/sensors.h @@ -43,6 +43,20 @@ namespace rosflight_firmware { + +typedef struct +{ + uint8_t imu; + uint8_t gnss; + uint8_t gnss_full; + uint8_t baro; + uint8_t mag; + uint8_t diff_pressure; + uint8_t sonar; + uint8_t battery; + uint8_t rc; +} got_flags; + enum GNSSFixType { GNSS_FIX_TYPE_NO_FIX, @@ -145,11 +159,10 @@ class Sensors : public ParamListenerInterface bool sonar_range_valid = false; GNSSData gnss_data; - bool gnss_new_data = false; + GNSSFull gnss_full; float gps_CNO = 0; // What is this? bool gnss_present = false; - GNSSFull gnss_full; - + turbomath::Vector mag = {0, 0, 0}; bool baro_present = false; @@ -169,7 +182,7 @@ class Sensors : public ParamListenerInterface // function declarations void init(); - bool run(); + got_flags run(); void param_change_callback(uint16_t param_id) override; // Calibration Functions @@ -188,6 +201,8 @@ class Sensors : public ParamListenerInterface return true; } + got_flags got; + private: static const float BARO_MAX_CHANGE_RATE; static const float BARO_SAMPLE_RATE; diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp index fa365144..172373c5 100644 --- a/src/comm_manager.cpp +++ b/src/comm_manager.cpp @@ -83,19 +83,6 @@ void CommManager::init() send_params_index_ = PARAMS_COUNT; update_system_id(PARAM_SYSTEM_ID); - set_streaming_rate(STREAM_ID_HEARTBEAT, PARAM_STREAM_HEARTBEAT_RATE); - set_streaming_rate(STREAM_ID_STATUS, PARAM_STREAM_STATUS_RATE); - set_streaming_rate(STREAM_ID_IMU, PARAM_STREAM_IMU_RATE); - set_streaming_rate(STREAM_ID_ATTITUDE, PARAM_STREAM_ATTITUDE_RATE); - set_streaming_rate(STREAM_ID_DIFF_PRESSURE, PARAM_STREAM_AIRSPEED_RATE); - set_streaming_rate(STREAM_ID_BARO, PARAM_STREAM_BARO_RATE); - set_streaming_rate(STREAM_ID_SONAR, PARAM_STREAM_SONAR_RATE); - set_streaming_rate(STREAM_ID_GNSS, PARAM_STREAM_GNSS_RATE); - set_streaming_rate(STREAM_ID_GNSS_FULL, PARAM_STREAM_GNSS_FULL_RATE); - set_streaming_rate(STREAM_ID_MAG, PARAM_STREAM_MAG_RATE); - set_streaming_rate(STREAM_ID_BATTERY_STATUS, PARAM_STREAM_BATTERY_STATUS_RATE); - set_streaming_rate(STREAM_ID_SERVO_OUTPUT_RAW, PARAM_STREAM_OUTPUT_RAW_RATE); - set_streaming_rate(STREAM_ID_RC_RAW, PARAM_STREAM_RC_RAW_RATE); initialized_ = true; } @@ -107,45 +94,6 @@ void CommManager::param_change_callback(uint16_t param_id) case PARAM_SYSTEM_ID: update_system_id(param_id); break; - case PARAM_STREAM_HEARTBEAT_RATE: - set_streaming_rate(STREAM_ID_HEARTBEAT, param_id); - break; - case PARAM_STREAM_STATUS_RATE: - set_streaming_rate(STREAM_ID_STATUS, param_id); - break; - case PARAM_STREAM_IMU_RATE: - set_streaming_rate(STREAM_ID_IMU, param_id); - break; - case PARAM_STREAM_ATTITUDE_RATE: - set_streaming_rate(STREAM_ID_ATTITUDE, param_id); - break; - case PARAM_STREAM_AIRSPEED_RATE: - set_streaming_rate(STREAM_ID_DIFF_PRESSURE, param_id); - break; - case PARAM_STREAM_BARO_RATE: - set_streaming_rate(STREAM_ID_BARO, param_id); - break; - case PARAM_STREAM_SONAR_RATE: - set_streaming_rate(STREAM_ID_SONAR, param_id); - break; - case PARAM_STREAM_GNSS_RATE: - set_streaming_rate(STREAM_ID_GNSS, param_id); - break; - case PARAM_STREAM_GNSS_FULL_RATE: - set_streaming_rate(STREAM_ID_GNSS_FULL, param_id); - break; - case PARAM_STREAM_MAG_RATE: - set_streaming_rate(STREAM_ID_MAG, param_id); - break; - case PARAM_STREAM_OUTPUT_RAW_RATE: - set_streaming_rate(STREAM_ID_SERVO_OUTPUT_RAW, param_id); - break; - case PARAM_STREAM_RC_RAW_RATE: - set_streaming_rate(STREAM_ID_RC_RAW, param_id); - break; - case PARAM_STREAM_BATTERY_STATUS_RATE: - set_streaming_rate(STREAM_ID_BATTERY_STATUS, param_id); - break; default: // do nothing break; @@ -390,10 +338,6 @@ void CommManager::heartbeat_callback(void) comm_link_.send_error_data(sysid_, backup_data_buffer_); have_backup_data_ = false; } - - /// JSJ: I don't think we need this - // respond to heartbeats with a heartbeat - this->send_heartbeat(); } // function definitions @@ -572,19 +516,49 @@ void CommManager::send_low_priority(void) } // function definitions -void CommManager::stream() +void CommManager::stream(got_flags got) { uint64_t time_us = RF_.board_.clock_micros(); - for (int i = 0; i < STREAM_COUNT; i++) + + // Send out data + + if(got.imu) // Nominally 400Hz + { + send_imu(); + send_attitude(); + static uint64_t ro_count=0; + if(!((ro_count++)%8)) send_output_raw(); // Raw output at 400Hz/8 = 50Hz + } + + if(got.diff_pressure) send_diff_pressure(); // Pitot sensor + if(got.baro) send_baro(); // Baro altimeter + if(got.mag) send_mag(); // Manetometer + if(got.sonar) send_sonar(); // Height above ground sensor (not enabled) + if(got.battery) send_battery_status(); // Battery V & I + if(got.gnss) send_gnss(); // GPS data (GNSS Packed) + if(got.gnss_full) send_gnss_full(); // GPS full data (not needed) + if(got.rc) // report at half the S.Bus rate. { - streams_[i].stream(time_us); + static uint64_t rc_count=0; + if(!((rc_count++)%2)) send_rc_raw(); // RC (S.Bus) inputs, scaled 1000-2000 } - RF_.board_.serial_flush(); -} -void CommManager::set_streaming_rate(uint8_t stream_id, int16_t param_id) -{ - streams_[stream_id].set_rate(RF_.params_.get_param_int(param_id)); + { + static uint64_t next_heartbeat = 0, next_status =0; + + if((time_us)/1000000 >= next_heartbeat ) // 1 Hz + { + send_heartbeat(); + next_heartbeat = time_us/1000000+1; + } + if((time_us)/100000 >= next_status ) // 10 Hz + { + send_status(); + next_status = time_us/100000+1; + } + } + + send_low_priority(); // parameter values and logging messages } void CommManager::send_named_value_int(const char* const name, int32_t value) diff --git a/src/param.cpp b/src/param.cpp index 36a5f15b..2287e6d3 100644 --- a/src/param.cpp +++ b/src/param.cpp @@ -121,21 +121,6 @@ void Params::set_defaults(void) /*** MAVLINK CONFIGURATION ***/ /*****************************/ init_param_int(PARAM_SYSTEM_ID, "SYS_ID", 1); // Mavlink System ID | 1 | 255 - init_param_int(PARAM_STREAM_HEARTBEAT_RATE, "STRM_HRTBT", 1); // Rate of heartbeat stream (Hz) | 0 | 1000 - init_param_int(PARAM_STREAM_STATUS_RATE, "STRM_STATUS", 10); // Rate of status stream (Hz) | 0 | 1000 - - init_param_int(PARAM_STREAM_ATTITUDE_RATE, "STRM_ATTITUDE", 200); // Rate of attitude stream (Hz) | 0 | 1000 - init_param_int(PARAM_STREAM_IMU_RATE, "STRM_IMU", 250); // Rate of IMU stream (Hz) | 0 | 1000 - init_param_int(PARAM_STREAM_MAG_RATE, "STRM_MAG", 50); // Rate of magnetometer stream (Hz) | 0 | 75 - init_param_int(PARAM_STREAM_BARO_RATE, "STRM_BARO", 50); // Rate of barometer stream (Hz) | 0 | 100 - init_param_int(PARAM_STREAM_AIRSPEED_RATE, "STRM_AIRSPEED", 50); // Rate of airspeed stream (Hz) | 0 | 50 - init_param_int(PARAM_STREAM_SONAR_RATE, "STRM_SONAR", 40); // Rate of sonar stream (Hz) | 0 | 40 - init_param_int(PARAM_STREAM_GNSS_RATE, "STRM_GNSS", 1000); // Maximum rate of GNSS stream (Hz) | 0 | 10 - init_param_int(PARAM_STREAM_GNSS_FULL_RATE, "STRM_GNSS_FULL", 10); //Rate of GNSS full stream (Hz) | 0 | 10 - init_param_int(PARAM_STREAM_BATTERY_STATUS_RATE, "STRM_BATTERY", 10); //Rate of battery status stream (Hz) | 0 | 10 - - init_param_int(PARAM_STREAM_OUTPUT_RAW_RATE, "STRM_SERVO", 50); // Rate of raw output stream | 0 | 490 - init_param_int(PARAM_STREAM_RC_RAW_RATE, "STRM_RC", 50); // Rate of raw RC input stream | 0 | 50 /********************************/ /*** CONTROLLER CONFIGURATION ***/ diff --git a/src/rc.cpp b/src/rc.cpp index df58f4e1..390819ca 100644 --- a/src/rc.cpp +++ b/src/rc.cpp @@ -265,28 +265,19 @@ bool RC::run() { uint32_t now = RF_.board_.clock_millis(); - // if it has been more than 20ms then look for new RC values and parse them - if (now - last_rc_receive_time < 20) - { - return false; - } - last_rc_receive_time = now; - // Check for rc lost if (check_rc_lost()) return false; - // read and normalize stick values for (uint8_t channel = 0; channel < static_cast(STICKS_COUNT); channel++) { - float pwm = RF_.board_.rc_read(sticks[channel].channel); if (sticks[channel].one_sided) // generally only F is one_sided { - stick_values[channel] = pwm; + stick_values[channel] = RF_.board_.rc_read(sticks[channel].channel); } else { - stick_values[channel] = 2.0 * (pwm - 0.5); + stick_values[channel] = 2.0 * (RF_.board_.rc_read(sticks[channel].channel) - 0.5); } } diff --git a/src/rosflight.cpp b/src/rosflight.cpp index 09647da8..8f95b9a9 100644 --- a/src/rosflight.cpp +++ b/src/rosflight.cpp @@ -96,16 +96,22 @@ void ROSflight::init() state_manager_.check_backup_memory(); } -// Main loop +/** + * @fn void run() + * @brief Main Loop + * + */ void ROSflight::run() { /*********************/ /*** Control Loop ***/ /*********************/ uint64_t start = board_.clock_micros(); - if (sensors_.run()) + + got_flags got = sensors_.run(); // IMU, GNSS, Baro, Mag, Pitot, SONAR, Battery + + if(got.imu) { - // If I have new IMU data, then perform control estimator_.run(); controller_.run(); mixer_.mix_output(); @@ -116,7 +122,7 @@ void ROSflight::run() /*** Post-Process ***/ /*********************/ // internal timers figure out what and when to send - comm_manager_.stream(); + comm_manager_.stream(got); // receive mavlink messages comm_manager_.receive(); @@ -124,8 +130,8 @@ void ROSflight::run() // update the state machine, an internal timer runs this at a fixed rate state_manager_.run(); - // get RC, an internal timer runs this every 20 ms (50 Hz) - rc_.run(); + // get RC, synchronous with rc data acquisition + if(got.rc) rc_.run(); // update commands (internal logic tells whether or not we should do anything or not) command_manager_.run(); diff --git a/src/sensors.cpp b/src/sensors.cpp index 9b733c0c..8936c17a 100644 --- a/src/sensors.cpp +++ b/src/sensors.cpp @@ -44,10 +44,12 @@ namespace rosflight_firmware { +// TODO: These values don't change actual rates, is there a way to just reference actual rates +// as defined in hardware board implementation? const float Sensors::BARO_MAX_CHANGE_RATE = 200.0f; // approx 200 m/s -const float Sensors::BARO_SAMPLE_RATE = 50.0f; +const float Sensors::BARO_SAMPLE_RATE = 60.0f; const float Sensors::DIFF_MAX_CHANGE_RATE = 225.0f; // approx 15 m/s^2 -const float Sensors::DIFF_SAMPLE_RATE = 50.0f; +const float Sensors::DIFF_SAMPLE_RATE = 100.0f; const float Sensors::SONAR_MAX_CHANGE_RATE = 100.0f; // 100 m/s const float Sensors::SONAR_SAMPLE_RATE = 50.0f; @@ -124,41 +126,65 @@ void Sensors::param_change_callback(uint16_t param_id) } } -bool Sensors::run(void) +got_flags Sensors::run() { - // First, check for new IMU data - bool got_imu = update_imu(); + memset(&got,0,sizeof(got)); - // Look for sensors that may not have been recognized at start because they weren't attached - // to the 5V rail (only if disarmed) - if (!rf_.state_manager_.state().armed) - look_for_disabled_sensors(); + // IMU: + if ((got.imu = rf_.board_.imu_has_new_data())>0) + { + rf_.state_manager_.clear_error(StateManager::ERROR_IMU_NOT_RESPONDING); + last_imu_update_ms_ = rf_.board_.clock_millis(); - // Update other sensors - update_other_sensors(); - return got_imu; -} + if (!rf_.board_.imu_read(accel_, &data_.imu_temperature, gyro_, &data_.imu_time)) + got.imu = 0; -void Sensors::update_other_sensors() -{ - switch (next_sensor_to_update_) + // Move data into local copy + data_.accel.x = accel_[0]; + data_.accel.y = accel_[1]; + data_.accel.z = accel_[2]; + + data_.accel = data_.fcu_orientation * data_.accel; + + data_.gyro.x = gyro_[0]; + data_.gyro.y = gyro_[1]; + data_.gyro.z = gyro_[2]; + + data_.gyro = data_.fcu_orientation * data_.gyro; + + if (calibrating_acc_flag_) + calibrate_accel(); + if (calibrating_gyro_flag_) + calibrate_gyro(); + + // Apply bias correction + correct_imu(); + + // Integrate for filtered IMU + float dt = (data_.imu_time - prev_imu_read_time_us_) * 1e-6; + accel_int_ += dt * data_.accel; + gyro_int_ += dt * data_.gyro; + prev_imu_read_time_us_ = data_.imu_time; + + } + + // GNSS: + if (rf_.board_.gnss_present()) { - case GNSS: - if (rf_.board_.gnss_present() && rf_.board_.gnss_has_new_data()) + data_.gnss_present = true; + if((got.gnss = rf_.board_.gnss_has_new_data())>0) { - data_.gnss_present = true; - data_.gnss_new_data = true; - rf_.board_.gnss_update(); - this->data_.gnss_data = rf_.board_.gnss_read(); - this->data_.gnss_full = rf_.board_.gnss_full_read(); + rf_.board_.gnss_read(&this->data_.gnss_data, &this->data_.gnss_full); } - break; + got.gnss_full = got.gnss; // bot come with the pvt GPS data + } - case BAROMETER: - if (rf_.board_.baro_present()) - { - data_.baro_present = true; - rf_.board_.baro_update(); + // BAROMETER: + if (rf_.board_.baro_present()) + { + data_.baro_present = true; + if((got.baro = rf_.board_.baro_has_new_data())>0) + { float raw_pressure; float raw_temp; rf_.board_.baro_read(&raw_pressure, &raw_temp); @@ -169,87 +195,73 @@ void Sensors::update_other_sensors() correct_baro(); } } - break; + } - case MAGNETOMETER: - if (rf_.board_.mag_present()) + // MAGNETOMETER: + if (rf_.board_.mag_present()) + { + data_.mag_present = true; + float mag[3]; + if((got.mag=rf_.board_.mag_has_new_data())>0) { - data_.mag_present = true; - float mag[3]; - rf_.board_.mag_update(); rf_.board_.mag_read(mag); data_.mag.x = mag[0]; data_.mag.y = mag[1]; data_.mag.z = mag[2]; correct_mag(); } - break; + } - case DIFF_PRESSURE: - if (rf_.board_.diff_pressure_present() || data_.diff_pressure_present) + // DIFF_PRESSURE: + if (rf_.board_.diff_pressure_present()) + { + data_.diff_pressure_present = true; + if((got.diff_pressure = rf_.board_.diff_pressure_has_new_data())>0) { - // if diff_pressure is currently present OR if it has historically been - // present (diff_pressure_present default is false) - rf_.board_.diff_pressure_update(); // update assists in recovering sensor if it temporarily - // disappears - - if (rf_.board_.diff_pressure_present()) + float raw_pressure; + float raw_temp; + rf_.board_.diff_pressure_read(&raw_pressure, &raw_temp); + data_.diff_pressure_valid = diff_outlier_filt_.update(raw_pressure, &data_.diff_pressure); + if (data_.diff_pressure_valid) { - data_.diff_pressure_present = true; - float raw_pressure; - float raw_temp; - rf_.board_.diff_pressure_read(&raw_pressure, &raw_temp); - data_.diff_pressure_valid = diff_outlier_filt_.update(raw_pressure, &data_.diff_pressure); - if (data_.diff_pressure_valid) - { - data_.diff_pressure_temp = raw_temp; - correct_diff_pressure(); - } + data_.diff_pressure_temp = raw_temp; + correct_diff_pressure(); } } - break; + } - case SONAR: - rf_.board_.sonar_update(); - if (rf_.board_.sonar_present()) + // SONAR: + if (rf_.board_.sonar_present()) + { + data_.sonar_present = true; + if((got.sonar = rf_.board_.sonar_has_new_data())>0) { - data_.sonar_present = true; float raw_distance; - rf_.board_.sonar_update(); - raw_distance = rf_.board_.sonar_read(); + rf_.board_.sonar_read( &raw_distance); data_.sonar_range_valid = sonar_outlier_filt_.update(raw_distance, &data_.sonar_range); } - break; - case BATTERY_MONITOR: - if (rf_.board_.clock_millis() - last_battery_monitor_update_ms_ > BATTERY_MONITOR_UPDATE_PERIOD_MS) - { - last_battery_monitor_update_ms_ = rf_.board_.clock_millis(); - update_battery_monitor(); - } - break; - default: - break; } - next_sensor_to_update_ = (next_sensor_to_update_ + 1) % NUM_LOW_PRIORITY_SENSORS; -} + // BATTERY_MONITOR: + if((got.battery = rf_.board_.battery_has_new_data())>0) + { + last_battery_monitor_update_ms_ = rf_.board_.clock_millis(); + update_battery_monitor(); + } -void Sensors::look_for_disabled_sensors() -{ - // Look for disabled sensors while disarmed (poll every second) - // These sensors need power to respond, so they might not have been - // detected on startup, but will be detected whenever power is applied - // to the 5V rail. - if (rf_.board_.clock_millis() > last_time_look_for_disarmed_sensors_ + 1000) + // RC + if((got.rc = rf_.board_.rc_has_new_data())>0) { - last_time_look_for_disarmed_sensors_ = rf_.board_.clock_millis(); - rf_.board_.baro_update(); - rf_.board_.mag_update(); - rf_.board_.diff_pressure_update(); - rf_.board_.sonar_update(); + rf_.board_.rc_read(0); } + + return got; } +void Sensors::update_other_sensors(){} + +void Sensors::look_for_disabled_sensors(){} + bool Sensors::start_imu_calibration(void) { start_gyro_calibration(); @@ -299,7 +311,8 @@ bool Sensors::gyro_calibration_complete(void) // local function definitions bool Sensors::update_imu(void) { - if (rf_.board_.new_imu_data()) + bool new_data; + if ((new_data = rf_.board_.imu_has_new_data())>0) { rf_.state_manager_.clear_error(StateManager::ERROR_IMU_NOT_RESPONDING); last_imu_update_ms_ = rf_.board_.clock_millis(); @@ -333,27 +346,8 @@ bool Sensors::update_imu(void) gyro_int_ += dt * data_.gyro; prev_imu_read_time_us_ = data_.imu_time; - return true; - } - else - { - // if we have lost 10 IMU messages then something is wrong - // However, because we look for disabled sensors while disarmed, - // we get IMU timeouts, which last for at least 10 ms. Therefore - // we have an adjustable imu_timeout. - int imu_timeout = rf_.state_manager_.state().armed ? 10 : 1000; - if (rf_.board_.clock_millis() > last_imu_update_ms_ + imu_timeout) - { - // Tell the board to fix it - last_imu_update_ms_ = rf_.board_.clock_millis(); - if (!rf_.state_manager_.state().armed) - rf_.board_.imu_not_responding_error(); - - // Indicate an IMU error - rf_.state_manager_.set_error(StateManager::ERROR_IMU_NOT_RESPONDING); - } - return false; - } + } + return new_data; } void Sensors::get_filtered_IMU(turbomath::Vector &accel, turbomath::Vector &gyro, uint64_t &stamp_us) @@ -369,17 +363,13 @@ void Sensors::get_filtered_IMU(turbomath::Vector &accel, turbomath::Vector &gyro void Sensors::update_battery_monitor() { - if (rf_.board_.battery_voltage_present()) + if (rf_.board_.battery_present()) { + float battery_voltage,battery_current; + rf_.board_.battery_read(&battery_voltage,&battery_current); data_.battery_monitor_present = true; - data_.battery_voltage = data_.battery_voltage * battery_voltage_alpha_ - + rf_.board_.battery_voltage_read() * (1 - battery_voltage_alpha_); - } - if (rf_.board_.battery_current_present()) - { - data_.battery_monitor_present = true; - data_.battery_current = data_.battery_current * battery_current_alpha_ - + rf_.board_.battery_current_read() * (1 - battery_current_alpha_); + data_.battery_voltage = battery_voltage; + data_.battery_current = battery_current; } } @@ -629,6 +619,7 @@ void Sensors::correct_diff_pressure() if (!diff_pressure_calibrated_) calibrate_diff_pressure(); data_.diff_pressure -= rf_.params_.get_param_float(PARAM_DIFF_PRESS_BIAS); + float atm = 101325.0f; if (data_.baro_present) atm = data_.baro_pressure;