diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index ba1405722f..ec31d20d51 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -110,6 +110,7 @@ set(msg_files uavcan_parameter_value.msg ulog_stream.msg ulog_stream_ack.msg + vehicle_air_data.msg vehicle_attitude.msg vehicle_attitude_setpoint.msg vehicle_command.msg @@ -120,6 +121,7 @@ set(msg_files vehicle_land_detected.msg vehicle_local_position.msg vehicle_local_position_setpoint.msg + vehicle_magnetometer.msg vehicle_rates_setpoint.msg vehicle_roi.msg vehicle_status.msg diff --git a/msg/sensor_bias.msg b/msg/sensor_bias.msg index 9197dc87bf..2ab6c0bd96 100644 --- a/msg/sensor_bias.msg +++ b/msg/sensor_bias.msg @@ -3,18 +3,10 @@ # scale errors, in-run bias and thermal drift (if thermal compensation is enabled and available). # -float32 gyro_x # Bias corrected angular velocity about X body axis (roll) (in rad/s) -float32 gyro_y # Bias corrected angular velocity about Y body axis (pitch) (in rad/s) -float32 gyro_z # Bias corrected angular velocity about Z body axis (yaw) (in rad/s) - float32 accel_x # Bias corrected acceleration in body X axis (in rad/s) float32 accel_y # Bias corrected acceleration in body Y axis (in rad/s) float32 accel_z # Bias corrected acceleration in body Z axis (in rad/s) -float32 mag_x # Bias corrected magnetometer reading in body X axis (in Gauss) -float32 mag_y # Bias corrected magnetometer reading in body Y axis (in Gauss) -float32 mag_z # Bias corrected magnetometer reading in body Z axis (in Gauss) - # In-run bias estimates (subtract from uncorrected data) float32 gyro_x_bias # X gyroscope in-run bias in body frame (rad/s, x forward) @@ -27,4 +19,4 @@ float32 accel_z_bias # Z accelerometer in-run bias in body frame (m/s^2, z down) float32 mag_x_bias # X magnetometer in-run bias in body frame (Gauss, x forward) float32 mag_y_bias # Y magnetometer in-run bias in body frame (Gauss, y right) -float32 mag_z_bias # Z magnetometer in-run bias in body frame (Gauss, z down) \ No newline at end of file +float32 mag_z_bias # Z magnetometer in-run bias in body frame (Gauss, z down) diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg index 4a18acf3b3..4d1040747b 100644 --- a/msg/sensor_combined.msg +++ b/msg/sensor_combined.msg @@ -7,7 +7,6 @@ int32 RELATIVE_TIMESTAMP_INVALID = 2147483647 # (0x7fffffff) If one of the relative timestamps is set to this value, it means the associated sensor values are invalid - # gyro timstamp is equal to the timestamp of the message float32[3] gyro_rad # average angular rate measured in the XYZ body frame in rad/s over the last gyro sampling period uint32 gyro_integral_dt # gyro measurement sampling period in us @@ -15,11 +14,3 @@ uint32 gyro_integral_dt # gyro measurement sampling period in us int32 accelerometer_timestamp_relative # timestamp + accelerometer_timestamp_relative = Accelerometer timestamp float32[3] accelerometer_m_s2 # average value acceleration measured in the XYZ body frame in m/s/s over the last accelerometer sampling period uint32 accelerometer_integral_dt # accelerometer measurement sampling period in us - -int32 magnetometer_timestamp_relative # timestamp + magnetometer_timestamp_relative = Magnetometer timestamp -float32[3] magnetometer_ga # Magnetic field in NED body frame, in Gauss - -int32 baro_timestamp_relative # timestamp + baro_timestamp_relative = Barometer timestamp -float32 baro_alt_meter # Altitude above MSL calculated from temperature compensated baro sensor data using an ISA corrected for sea level pressure SENS_BARO_QNH. -float32 baro_temp_celcius # Temperature in degrees celsius -float32 baro_pressure_pa # Absolute pressure in pascals diff --git a/msg/vehicle_air_data.msg b/msg/vehicle_air_data.msg new file mode 100644 index 0000000000..ef2246691d --- /dev/null +++ b/msg/vehicle_air_data.msg @@ -0,0 +1,6 @@ + +float32 baro_alt_meter # Altitude above MSL calculated from temperature compensated baro sensor data using an ISA corrected for sea level pressure SENS_BARO_QNH. +float32 baro_temp_celcius # Temperature in degrees celsius +float32 baro_pressure_pa # Absolute pressure in pascals + +float32 rho # air density \ No newline at end of file diff --git a/msg/vehicle_global_position.msg b/msg/vehicle_global_position.msg index d8cefa7a1d..a4cc03f169 100644 --- a/msg/vehicle_global_position.msg +++ b/msg/vehicle_global_position.msg @@ -25,8 +25,6 @@ float32 epv # Standard deviation of vertical position error, (metres) float32 terrain_alt # Terrain altitude WGS84, (metres) bool terrain_alt_valid # Terrain altitude estimate is valid -float32 pressure_alt # Pressure altitude AMSL, (metres) - bool dead_reckoning # True if this position is estimated through dead-reckoning # TOPICS vehicle_global_position vehicle_global_position_groundtruth diff --git a/msg/vehicle_magnetometer.msg b/msg/vehicle_magnetometer.msg new file mode 100644 index 0000000000..c675cb58df --- /dev/null +++ b/msg/vehicle_magnetometer.msg @@ -0,0 +1,2 @@ + +float32[3] magnetometer_ga # Magnetic field in NED body frame, in Gauss diff --git a/src/drivers/barometer/ms5611/ms5611.cpp b/src/drivers/barometer/ms5611/ms5611.cpp index 414ed01527..8dfa27dc86 100644 --- a/src/drivers/barometer/ms5611/ms5611.cpp +++ b/src/drivers/barometer/ms5611/ms5611.cpp @@ -349,7 +349,7 @@ MS5611::init() if (autodetect) { if (_device_type == MS5611_DEVICE) { - if (brp.altitude > 5300.f) { + if (brp.pressure < 520.0f) { /* This is likely not this device, try again */ _device_type = MS5607_DEVICE; _measure_phase = 0; @@ -358,8 +358,8 @@ MS5611::init() } } else if (_device_type == MS5607_DEVICE) { - if (brp.altitude > 5300.f) { - /* Both devices returned very high altitude; + if (brp.pressure < 520.0f) { + /* Both devices returned a very low pressure; * have fun on Everest using MS5611 */ _device_type = MS5611_DEVICE; } diff --git a/src/drivers/telemetry/frsky_telemetry/frsky_data.c b/src/drivers/telemetry/frsky_telemetry/frsky_data.c index 7d8c16bab7..63116c43b7 100644 --- a/src/drivers/telemetry/frsky_telemetry/frsky_data.c +++ b/src/drivers/telemetry/frsky_telemetry/frsky_data.c @@ -53,6 +53,7 @@ #include #include +#include #include #include #include @@ -65,10 +66,12 @@ struct frsky_subscription_data_s { struct battery_status_s battery_status; struct vehicle_global_position_s global_pos; struct sensor_combined_s sensor_combined; + struct vehicle_air_data_s airdata; struct vehicle_gps_position_s vehicle_gps_position; uint8_t current_flight_mode; // == vehicle_status.nav_state int battery_status_sub; + int vehicle_air_data_sub; int vehicle_global_position_sub; int sensor_sub; int vehicle_gps_position_sub; @@ -89,6 +92,7 @@ bool frsky_init() } subscription_data->battery_status_sub = orb_subscribe(ORB_ID(battery_status)); + subscription_data->vehicle_air_data_sub = orb_subscribe(ORB_ID(vehicle_air_data)); subscription_data->vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); subscription_data->sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); subscription_data->vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); @@ -168,6 +172,12 @@ void frsky_update_topics() orb_copy(ORB_ID(sensor_combined), subs->sensor_sub, &subs->sensor_combined); } + orb_check(subs->vehicle_air_data_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_air_data), subs->vehicle_air_data_sub, &subs->airdata); + } + /* get a local copy of the vehicle status */ orb_check(subs->vehicle_status_sub, &updated); @@ -212,10 +222,8 @@ void frsky_send_frame1(int uart) frsky_send_data(uart, FRSKY_ID_ACCEL_Y, roundf(subs->sensor_combined.accelerometer_m_s2[1] * 1000.0f)); frsky_send_data(uart, FRSKY_ID_ACCEL_Z, roundf(subs->sensor_combined.accelerometer_m_s2[2] * 1000.0f)); - frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, - subs->sensor_combined.baro_alt_meter); - frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, - roundf(frac(subs->sensor_combined.baro_alt_meter) * 100.0f)); + frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, subs->airdata.baro_alt_meter); + frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, roundf(frac(subs->airdata.baro_alt_meter) * 100.0f)); frsky_send_data(uart, FRSKY_ID_VFAS, roundf(subs->battery_status.voltage_v * 10.0f)); diff --git a/src/drivers/telemetry/frsky_telemetry/frsky_telemetry.c b/src/drivers/telemetry/frsky_telemetry/frsky_telemetry.c index ba83602537..41d5022aa0 100644 --- a/src/drivers/telemetry/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/telemetry/frsky_telemetry/frsky_telemetry.c @@ -59,7 +59,7 @@ #include #include #include -#include +#include #include // NAN #include "sPort_data.h" @@ -319,11 +319,9 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) PX4_INFO("sending FrSky SmartPort telemetry"); - struct sensor_combined_s sensor_combined = {}; - float filtered_alt = NAN; float last_baro_alt = 0.f; - int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + int airdata_sub = orb_subscribe(ORB_ID(vehicle_air_data)); uint32_t lastBATV_ms = 0; uint32_t lastCUR_ms = 0; @@ -364,17 +362,18 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) /* get a local copy of the current sensor values * in order to apply a lowpass filter to baro pressure. */ - bool sensor_updated; - orb_check(sensor_sub, &sensor_updated); + bool sensor_updated = false; + orb_check(airdata_sub, &sensor_updated); if (sensor_updated) { - orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensor_combined); + struct vehicle_air_data_s airdata; + orb_copy(ORB_ID(vehicle_air_data), airdata_sub, &airdata); if (isnan(filtered_alt)) { - filtered_alt = sensor_combined.baro_alt_meter; + filtered_alt = airdata.baro_alt_meter; } else { - filtered_alt = .05f * sensor_combined.baro_alt_meter + .95f * filtered_alt; + filtered_alt = .05f * airdata.baro_alt_meter + .95f * filtered_alt; } } diff --git a/src/drivers/telemetry/frsky_telemetry/sPort_data.c b/src/drivers/telemetry/frsky_telemetry/sPort_data.c index c68320e0d4..b2910d9206 100644 --- a/src/drivers/telemetry/frsky_telemetry/sPort_data.c +++ b/src/drivers/telemetry/frsky_telemetry/sPort_data.c @@ -52,6 +52,7 @@ #include #include +#include #include #include #include @@ -69,12 +70,14 @@ struct s_port_subscription_data_s { int battery_status_sub; int vehicle_status_sub; int gps_position_sub; + int vehicle_air_data_sub; struct sensor_combined_s sensor_combined; struct vehicle_global_position_s global_pos; struct vehicle_local_position_s local_pos; struct battery_status_s battery_status; struct vehicle_status_s vehicle_status; + struct vehicle_air_data_s vehicle_air_data; struct vehicle_gps_position_s gps_position; }; @@ -92,13 +95,13 @@ bool sPort_init() return false; } - s_port_subscription_data->sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); s_port_subscription_data->global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); s_port_subscription_data->local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); s_port_subscription_data->battery_status_sub = orb_subscribe(ORB_ID(battery_status)); s_port_subscription_data->vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); s_port_subscription_data->gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + s_port_subscription_data->vehicle_air_data_sub = orb_subscribe(ORB_ID(vehicle_air_data)); return true; } @@ -112,6 +115,7 @@ void sPort_deinit() orb_unsubscribe(s_port_subscription_data->battery_status_sub); orb_unsubscribe(s_port_subscription_data->vehicle_status_sub); orb_unsubscribe(s_port_subscription_data->gps_position_sub); + orb_unsubscribe(s_port_subscription_data->vehicle_air_data_sub); free(s_port_subscription_data); s_port_subscription_data = NULL; } @@ -164,6 +168,13 @@ void sPort_update_topics() orb_copy(ORB_ID(vehicle_gps_position), subs->gps_position_sub, &subs->gps_position); } + /* get a local copy of the gps position data */ + orb_check(subs->vehicle_air_data_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_air_data), subs->vehicle_air_data_sub, &subs->vehicle_air_data); + } + } @@ -254,7 +265,7 @@ void sPort_send_CUR(int uart) void sPort_send_ALT(int uart) { /* send data */ - uint32_t alt = (int)(100 * s_port_subscription_data->sensor_combined.baro_alt_meter); + uint32_t alt = (int)(100 * s_port_subscription_data->vehicle_air_data.baro_alt_meter); sPort_send_data(uart, SMARTPORT_ID_ALT, alt); } diff --git a/src/drivers/telemetry/hott/messages.cpp b/src/drivers/telemetry/hott/messages.cpp index 86b14c54ed..3f14dee2ab 100644 --- a/src/drivers/telemetry/hott/messages.cpp +++ b/src/drivers/telemetry/hott/messages.cpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include #include @@ -59,7 +59,7 @@ static int _battery_sub = -1; static int _gps_sub = -1; static int _home_sub = -1; -static int _sensor_sub = -1; +static int _airdata_sub = -1; static int _airspeed_sub = -1; static int _esc_sub = -1; @@ -75,7 +75,7 @@ init_sub_messages(void) _battery_sub = orb_subscribe(ORB_ID(battery_status)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _home_sub = orb_subscribe(ORB_ID(home_position)); - _sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + _airdata_sub = orb_subscribe(ORB_ID(vehicle_air_data)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _esc_sub = orb_subscribe(ORB_ID(esc_status)); } @@ -132,9 +132,8 @@ void build_eam_response(uint8_t *buffer, size_t *size) { /* get a local copy of the current sensor values */ - struct sensor_combined_s raw; - memset(&raw, 0, sizeof(raw)); - orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw); + vehicle_air_data_s airdata = {}; + orb_copy(ORB_ID(vehicle_air_data), _airdata_sub, &airdata); /* get a local copy of the battery data */ struct battery_status_s battery; @@ -149,12 +148,12 @@ build_eam_response(uint8_t *buffer, size_t *size) msg.eam_sensor_id = EAM_SENSOR_ID; msg.sensor_text_id = EAM_SENSOR_TEXT_ID; - msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20); + msg.temperature1 = (uint8_t)(airdata.baro_temp_celcius + 20); msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG; msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10); - uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500); + uint16_t alt = (uint16_t)(airdata.baro_alt_meter + 500); msg.altitude_L = (uint8_t)alt & 0xff; msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; @@ -209,11 +208,6 @@ build_gam_response(uint8_t *buffer, size_t *size) void build_gps_response(uint8_t *buffer, size_t *size) { - /* get a local copy of the current sensor values */ - struct sensor_combined_s raw; - memset(&raw, 0, sizeof(raw)); - orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw); - /* get a local copy of the battery data */ struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index b303596982..da59a63c65 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -55,6 +55,7 @@ #include #include #include +#include extern "C" __EXPORT int attitude_estimator_q_main(int argc, char *argv[]); @@ -106,6 +107,7 @@ private: int _global_pos_sub = -1; int _vision_sub = -1; int _mocap_sub = -1; + int _magnetometer_sub = -1; orb_advert_t _att_pub = nullptr; @@ -260,6 +262,7 @@ void AttitudeEstimatorQ::task_main() _mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _magnetometer_sub = orb_subscribe(ORB_ID(vehicle_magnetometer)); update_parameters(true); @@ -309,10 +312,20 @@ void AttitudeEstimatorQ::task_main() } } - if (sensors.magnetometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) { - _mag(0) = sensors.magnetometer_ga[0]; - _mag(1) = sensors.magnetometer_ga[1]; - _mag(2) = sensors.magnetometer_ga[2]; + _data_good = true; + } + + // Update magnetometer + bool magnetometer_updated = false; + orb_check(_magnetometer_sub, &magnetometer_updated); + + if (magnetometer_updated) { + vehicle_magnetometer_s magnetometer = {}; + + if (orb_copy(ORB_ID(vehicle_magnetometer), _magnetometer_sub, &magnetometer) == PX4_OK) { + _mag(0) = magnetometer.magnetometer_ga[0]; + _mag(1) = magnetometer.magnetometer_ga[1]; + _mag(2) = magnetometer.magnetometer_ga[2]; if (_mag.length() < 0.01f) { PX4_ERR("WARNING: degenerate mag!"); @@ -320,7 +333,6 @@ void AttitudeEstimatorQ::task_main() } } - _data_good = true; } // Update vision and motion capture heading @@ -443,6 +455,7 @@ void AttitudeEstimatorQ::task_main() orb_unsubscribe(_global_pos_sub); orb_unsubscribe(_vision_sub); orb_unsubscribe(_mocap_sub); + orb_unsubscribe(_magnetometer_sub); } void AttitudeEstimatorQ::update_parameters(bool force) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index cd8c934c76..241d9bc7d5 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -68,6 +68,8 @@ #include #include #include +#include +#include using math::constrain; @@ -118,8 +120,6 @@ private: int64_t _last_time_slip_us = 0; ///< Last time slip (uSec) // Initialise time stamps used to send sensor data to the EKF and for logging - uint64_t _timestamp_mag_us = 0; ///< magnetomer data timestamp (uSec) - uint64_t _timestamp_balt_us = 0; ///< pressure altitude data timestamp (uSec) uint8_t _invalid_mag_id_count = 0; ///< number of times an invalid magnetomer device ID has been detected // Used to down sample magnetometer data @@ -520,7 +520,9 @@ void Ekf2::run() int vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); int status_sub = orb_subscribe(ORB_ID(vehicle_status)); int sensor_selection_sub = orb_subscribe(ORB_ID(sensor_selection)); + int airdata_sub = orb_subscribe(ORB_ID(vehicle_air_data)); int landing_target_pose_sub = orb_subscribe(ORB_ID(landing_target_pose)); + int magnetometer_sub = orb_subscribe(ORB_ID(vehicle_magnetometer)); bool imu_bias_reset_request = false; @@ -584,6 +586,7 @@ void Ekf2::run() bool gps_updated = false; bool airspeed_updated = false; + bool airdata_updated = false; bool sensor_selection_updated = false; bool optical_flow_updated = false; bool range_finder_updated = false; @@ -592,6 +595,7 @@ void Ekf2::run() bool vision_attitude_updated = false; bool vehicle_status_updated = false; bool landing_target_pose_updated = false; + bool magnetometer_updated = false; orb_copy(ORB_ID(sensor_combined), sensors_sub, &sensors); // update all other topics if they have new data @@ -615,6 +619,9 @@ void Ekf2::run() orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed); } + orb_check(airdata_sub, &airdata_updated); + orb_check(magnetometer_sub, &magnetometer_updated); + orb_check(sensor_selection_sub, &sensor_selection_updated); // Always update sensor selction first time through if time stamp is non zero @@ -707,14 +714,10 @@ void Ekf2::run() _ekf.setIMUData(now, sensors.gyro_integral_dt, sensors.accelerometer_integral_dt, gyro_integral, accel_integral); // read mag data - if (sensors.magnetometer_timestamp_relative == sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) { - // set a zero timestamp to let the ekf replay program know that this data is not valid - _timestamp_mag_us = 0; - - } else { - if ((sensors.timestamp + sensors.magnetometer_timestamp_relative) != _timestamp_mag_us) { - _timestamp_mag_us = sensors.timestamp + sensors.magnetometer_timestamp_relative; + if (magnetometer_updated) { + vehicle_magnetometer_s magnetometer = {}; + if (orb_copy(ORB_ID(vehicle_magnetometer), magnetometer_sub, &magnetometer) == PX4_OK) { // Reset learned bias parameters if there has been a persistant change in magnetometer ID // Do not reset parmameters when armed to prevent potential time slips casued by parameter set // and notification events @@ -749,11 +752,11 @@ void Ekf2::run() // If the time last used by the EKF is less than specified, then accumulate the // data and push the average when the specified interval is reached. - _mag_time_sum_ms += _timestamp_mag_us / 1000; + _mag_time_sum_ms += magnetometer.timestamp / 1000; _mag_sample_count++; - _mag_data_sum[0] += sensors.magnetometer_ga[0]; - _mag_data_sum[1] += sensors.magnetometer_ga[1]; - _mag_data_sum[2] += sensors.magnetometer_ga[2]; + _mag_data_sum[0] += magnetometer.magnetometer_ga[0]; + _mag_data_sum[1] += magnetometer.magnetometer_ga[1]; + _mag_data_sum[2] += magnetometer.magnetometer_ga[2]; uint32_t mag_time_ms = _mag_time_sum_ms / _mag_sample_count; if (mag_time_ms - _mag_time_ms_last_used > _params->sensor_interval_min_ms) { @@ -777,29 +780,23 @@ void Ekf2::run() } // read baro data - if (sensors.baro_timestamp_relative == sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) { - // set a zero timestamp to let the ekf replay program know that this data is not valid - _timestamp_balt_us = 0; + if (airdata_updated) { + vehicle_air_data_s airdata; - } else { - if ((sensors.timestamp + sensors.baro_timestamp_relative) != _timestamp_balt_us) { - _timestamp_balt_us = sensors.timestamp + sensors.baro_timestamp_relative; + if (orb_copy(ORB_ID(vehicle_air_data), airdata_sub, &airdata) == PX4_OK) { // If the time last used by the EKF is less than specified, then accumulate the // data and push the average when the specified interval is reached. - _balt_time_sum_ms += _timestamp_balt_us / 1000; + _balt_time_sum_ms += airdata.timestamp / 1000; _balt_sample_count++; - _balt_data_sum += sensors.baro_alt_meter; + _balt_data_sum += airdata.baro_alt_meter; uint32_t balt_time_ms = _balt_time_sum_ms / _balt_sample_count; if (balt_time_ms - _balt_time_ms_last_used > (uint32_t)_params->sensor_interval_min_ms) { // take mean across sample period float balt_data_avg = _balt_data_sum / (float)_balt_sample_count; - // estimate air density assuming typical 20degC ambient temperature - const float pressure_to_density = 1.0f / (CONSTANTS_AIR_GAS_CONST * (20.0f - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); - const float rho = pressure_to_density * sensors.baro_pressure_pa; - _ekf.set_air_density(rho); + _ekf.set_air_density(airdata.rho); // calculate static pressure error = Pmeas - Ptruth // model position error sensitivity as a body fixed ellipse with different scale in the positive and negtive X direction @@ -819,11 +816,11 @@ void Ekf2::run() const float y_v2 = fminf(vel_body_wind(1) * vel_body_wind(1), max_airspeed_sq); const float z_v2 = fminf(vel_body_wind(2) * vel_body_wind(2), max_airspeed_sq); - const float pstatic_err = 0.5f * rho * (K_pstatic_coef_x * x_v2) + (_K_pstatic_coef_y.get() * y_v2) + - (_K_pstatic_coef_z.get() * z_v2); + const float pstatic_err = 0.5f * airdata.rho * + (K_pstatic_coef_x * x_v2) + (_K_pstatic_coef_y.get() * y_v2) + (_K_pstatic_coef_z.get() * z_v2); // correct baro measurement using pressure error estimate and assuming sea level gravity - balt_data_avg += pstatic_err / (rho * CONSTANTS_ONE_G); + balt_data_avg += pstatic_err / (airdata.rho * CONSTANTS_ONE_G); // push to estimator _ekf.setBaroData(1000 * (uint64_t)balt_time_ms, balt_data_avg); @@ -1112,8 +1109,6 @@ void Ekf2::run() global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84 } - global_pos.pressure_alt = sensors.baro_alt_meter; // Pressure altitude AMSL (m) - global_pos.dead_reckoning = _ekf.inertial_dead_reckoning(); // True if this position is estimated through dead-reckoning _vehicle_global_position_pub.update(); @@ -1128,18 +1123,10 @@ void Ekf2::run() bias.timestamp = now; - bias.gyro_x = sensors.gyro_rad[0] - gyro_bias[0]; - bias.gyro_y = sensors.gyro_rad[1] - gyro_bias[1]; - bias.gyro_z = sensors.gyro_rad[2] - gyro_bias[2]; - bias.accel_x = sensors.accelerometer_m_s2[0] - accel_bias[0]; bias.accel_y = sensors.accelerometer_m_s2[1] - accel_bias[1]; bias.accel_z = sensors.accelerometer_m_s2[2] - accel_bias[2]; - bias.mag_x = sensors.magnetometer_ga[0] - (_last_valid_mag_cal[0] / 1000.0f); // mGauss -> Gauss - bias.mag_y = sensors.magnetometer_ga[1] - (_last_valid_mag_cal[1] / 1000.0f); // mGauss -> Gauss - bias.mag_z = sensors.magnetometer_ga[2] - (_last_valid_mag_cal[2] / 1000.0f); // mGauss -> Gauss - bias.gyro_x_bias = gyro_bias[0]; bias.gyro_y_bias = gyro_bias[1]; bias.gyro_z_bias = gyro_bias[2]; diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 6758bd6d2b..fc8bd1ea86 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -47,6 +47,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _sub_lidar(nullptr), _sub_sonar(nullptr), _sub_landing_target_pose(ORB_ID(landing_target_pose), 1000 / 40, 0, &getSubscriptions()), + _sub_airdata(ORB_ID(vehicle_air_data), 0, 0, &getSubscriptions()), // publications _pub_lpos(ORB_ID(vehicle_local_position), -1, &getPublications()), @@ -247,17 +248,10 @@ void BlockLocalPositionEstimator::update() bool paramsUpdated = _sub_param_update.updated(); _baroUpdated = false; - if ((_fusion.get() & FUSE_BARO) && _sub_sensor.updated()) { - int32_t baro_timestamp_relative = _sub_sensor.get().baro_timestamp_relative; - - if (baro_timestamp_relative != _sub_sensor.get().RELATIVE_TIMESTAMP_INVALID) { - uint64_t baro_timestamp = _sub_sensor.get().timestamp + \ - _sub_sensor.get().baro_timestamp_relative; - - if (baro_timestamp != _timeStampLastBaro) { - _baroUpdated = true; - _timeStampLastBaro = baro_timestamp; - } + if ((_fusion.get() & FUSE_BARO) && _sub_airdata.updated()) { + if (_sub_airdata.get().timestamp != _timeStampLastBaro) { + _baroUpdated = true; + _timeStampLastBaro = _sub_airdata.get().timestamp; } } @@ -674,7 +668,6 @@ void BlockLocalPositionEstimator::publishGlobalPos() _pub_gpos.get().yaw = _eul(2); _pub_gpos.get().eph = eph; _pub_gpos.get().epv = epv; - _pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter; _pub_gpos.get().terrain_alt = _altOrigin - xLP(X_tz); _pub_gpos.get().terrain_alt_valid = _estimatorInitialized & EST_TZ; _pub_gpos.get().dead_reckoning = !(_estimatorInitialized & EST_XY); diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 91106cca07..3d4f797040 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -23,6 +23,7 @@ #include #include #include +#include // uORB Publications #include @@ -260,6 +261,7 @@ private: uORB::Subscription *_sub_lidar; uORB::Subscription *_sub_sonar; uORB::Subscription _sub_landing_target_pose; + uORB::Subscription _sub_airdata; // publications uORB::Publication _pub_lpos; diff --git a/src/modules/local_position_estimator/sensors/baro.cpp b/src/modules/local_position_estimator/sensors/baro.cpp index 12f8d1f5a6..6dfc9d9948 100644 --- a/src/modules/local_position_estimator/sensors/baro.cpp +++ b/src/modules/local_position_estimator/sensors/baro.cpp @@ -41,9 +41,9 @@ int BlockLocalPositionEstimator::baroMeasure(Vector &y) { //measure y.setZero(); - y(0) = _sub_sensor.get().baro_alt_meter; + y(0) = _sub_airdata.get().baro_alt_meter; _baroStats.update(y); - _time_last_baro = _timeStamp; + _time_last_baro = _sub_airdata.get().timestamp; return OK; } diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index b4ebca9172..7c17974cc2 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -682,8 +682,10 @@ void Logger::add_estimator_replay_topics() add_topic("optical_flow"); add_topic("sensor_combined"); add_topic("sensor_selection"); + add_topic("vehicle_air_data"); add_topic("vehicle_gps_position"); add_topic("vehicle_land_detected"); + add_topic("vehicle_magnetometer"); add_topic("vehicle_status"); add_topic("vehicle_vision_attitude"); add_topic("vehicle_vision_position"); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 8e96145b30..2d31361729 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -104,6 +104,8 @@ #include #include #include +#include +#include #include static uint16_t cm_uint16_from_m_float(float m); @@ -657,6 +659,8 @@ private: MavlinkOrbSubscription *_bias_sub; MavlinkOrbSubscription *_differential_pressure_sub; + MavlinkOrbSubscription *_magnetometer_sub; + MavlinkOrbSubscription *_air_data_sub; uint64_t _accel_timestamp; uint64_t _gyro_timestamp; @@ -673,6 +677,8 @@ protected: _sensor_time(0), _bias_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_bias))), _differential_pressure_sub(_mavlink->add_orb_subscription(ORB_ID(differential_pressure))), + _magnetometer_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_magnetometer))), + _air_data_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_air_data))), _accel_timestamp(0), _gyro_timestamp(0), _mag_timestamp(0), @@ -698,20 +704,27 @@ protected: _gyro_timestamp = sensor.timestamp; } - if (_mag_timestamp != sensor.timestamp + sensor.magnetometer_timestamp_relative) { + vehicle_magnetometer_s magnetometer = {}; + _magnetometer_sub->update(&magnetometer); + + if (_mag_timestamp != magnetometer.timestamp) { /* mark third group dimensions as changed */ fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - _mag_timestamp = sensor.timestamp + sensor.magnetometer_timestamp_relative; + _mag_timestamp = magnetometer.timestamp; } - if (_baro_timestamp != sensor.timestamp + sensor.baro_timestamp_relative) { + vehicle_air_data_s air_data = {}; + _air_data_sub->update(&air_data); + + if (_baro_timestamp != air_data.timestamp) { /* mark last group dimensions as changed */ fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - _baro_timestamp = sensor.timestamp + sensor.baro_timestamp_relative; + _baro_timestamp = air_data.timestamp; } sensor_bias_s bias = {}; _bias_sub->update(&bias); + differential_pressure_s differential_pressure = {}; _differential_pressure_sub->update(&differential_pressure); @@ -724,13 +737,13 @@ protected: msg.xgyro = sensor.gyro_rad[0] - bias.gyro_x_bias; msg.ygyro = sensor.gyro_rad[1] - bias.gyro_y_bias; msg.zgyro = sensor.gyro_rad[2] - bias.gyro_z_bias; - msg.xmag = sensor.magnetometer_ga[0] - bias.mag_x_bias; - msg.ymag = sensor.magnetometer_ga[1] - bias.mag_y_bias; - msg.zmag = sensor.magnetometer_ga[2] - bias.mag_z_bias; - msg.abs_pressure = 0; + msg.xmag = magnetometer.magnetometer_ga[0] - bias.mag_x_bias; + msg.ymag = magnetometer.magnetometer_ga[1] - bias.mag_y_bias; + msg.zmag = magnetometer.magnetometer_ga[2] - bias.mag_z_bias; + msg.abs_pressure = air_data.baro_pressure_pa; msg.diff_pressure = differential_pressure.differential_pressure_raw_pa; - msg.pressure_alt = sensor.baro_alt_meter; - msg.temperature = sensor.baro_temp_celcius; + msg.pressure_alt = air_data.baro_alt_meter; + msg.temperature = air_data.baro_temp_celcius; msg.fields_updated = fields_updated; mavlink_msg_highres_imu_send_struct(_mavlink->get_channel(), &msg); @@ -1025,8 +1038,7 @@ private: MavlinkOrbSubscription *_airspeed_sub; uint64_t _airspeed_time; - MavlinkOrbSubscription *_sensor_sub; - uint64_t _sensor_time; + MavlinkOrbSubscription *_air_data_sub; /* do not allow top copying this class */ MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); @@ -1044,7 +1056,7 @@ protected: _act1_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_1))), _airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))), _airspeed_time(0), - _sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))) + _air_data_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_air_data))) {} bool send(const hrt_abstime t) @@ -1092,12 +1104,12 @@ protected: msg.alt = -pos.z + pos.ref_alt; } else { - sensor_combined_s sensor = {}; - _sensor_sub->update(&sensor); + vehicle_air_data_s air_data = {}; + _air_data_sub->update(&air_data); /* fall back to baro altitude */ - if (sensor.timestamp > 0) { - msg.alt = sensor.baro_alt_meter; + if (air_data.timestamp > 0) { + msg.alt = air_data.baro_alt_meter; } } @@ -1714,7 +1726,7 @@ private: uint64_t _lpos_time; MavlinkOrbSubscription *_home_sub; - MavlinkOrbSubscription *_sensor_sub; + MavlinkOrbSubscription *_air_data_sub; /* do not allow top copying this class */ MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &); @@ -1727,7 +1739,7 @@ protected: _lpos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))), _lpos_time(0), _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))), - _sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))) + _air_data_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_air_data))) {} bool send(const hrt_abstime t) @@ -1746,11 +1758,11 @@ protected: } else { // fall back to baro altitude - sensor_combined_s sensor = {}; - _sensor_sub->update(&sensor); + vehicle_air_data_s air_data = {}; + _air_data_sub->update(&air_data); - if (sensor.timestamp > 0) { - msg.alt = sensor.baro_alt_meter * 1000.0f; + if (air_data.timestamp > 0) { + msg.alt = air_data.baro_alt_meter * 1000.0f; } } @@ -3710,7 +3722,7 @@ public: private: MavlinkOrbSubscription *_local_pos_sub; MavlinkOrbSubscription *_home_sub; - MavlinkOrbSubscription *_sensor_sub; + MavlinkOrbSubscription *_air_data_sub; uint64_t _local_pos_time{0}; @@ -3722,7 +3734,7 @@ protected: explicit MavlinkStreamAltitude(Mavlink *mavlink) : MavlinkStream(mavlink), _local_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))), _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))), - _sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))) + _air_data_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_air_data))) {} bool send(const hrt_abstime t) @@ -3737,14 +3749,14 @@ protected: msg.bottom_clearance = NAN; // always update monotonic altitude - bool sensor_updated = false; - sensor_combined_s sensor = {}; - _sensor_sub->update(&sensor); + bool air_data_updated = false; + vehicle_air_data_s air_data = {}; + _air_data_sub->update(&air_data); - if (sensor.timestamp > 0) { - msg.altitude_monotonic = sensor.baro_alt_meter; + if (air_data.timestamp > 0) { + msg.altitude_monotonic = air_data.baro_alt_meter; - sensor_updated = true; + air_data_updated = true; } bool lpos_updated = false; @@ -3786,7 +3798,7 @@ protected: // avoid publishing only baro altitude_monotonic if possible bool lpos_timeout = (hrt_elapsed_time(&_local_pos_time) > 10000); - if (lpos_updated || (sensor_updated && lpos_timeout)) { + if (lpos_updated || (air_data_updated && lpos_timeout)) { msg.time_usec = hrt_absolute_time(); mavlink_msg_altitude_send_struct(_mavlink->get_channel(), &msg); diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 9153385c70..8d1288d1e8 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -54,7 +54,8 @@ Geofence::Geofence(Navigator *navigator) : ModuleParams(navigator), - _navigator(navigator) + _navigator(navigator), + _sub_airdata(ORB_ID(vehicle_air_data)) { // we assume there's no concurrent fence update on startup _updateFence(); @@ -181,15 +182,13 @@ bool Geofence::checkAll(const struct vehicle_global_position_s &global_position) return checkAll(global_position.lat, global_position.lon, global_position.alt); } -bool Geofence::checkAll(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl) +bool Geofence::checkAll(const struct vehicle_global_position_s &global_position, const float alt) { - return checkAll(global_position.lat, global_position.lon, baro_altitude_amsl); + return checkAll(global_position.lat, global_position.lon, alt); } - -bool Geofence::check(const struct vehicle_global_position_s &global_position, - const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl, - const struct home_position_s home_pos, bool home_position_set) +bool Geofence::check(const vehicle_global_position_s &global_position, const vehicle_gps_position_s &gps_position, + const home_position_s home_pos, bool home_position_set) { if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) { if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { @@ -201,12 +200,15 @@ bool Geofence::check(const struct vehicle_global_position_s &global_position, } } else { + // get baro altitude + _sub_airdata.update(); + const float baro_altitude_amsl = _sub_airdata.get().baro_alt_meter; + if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { return checkAll(global_position, baro_altitude_amsl); } else { - return checkAll((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, - baro_altitude_amsl); + return checkAll((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, baro_altitude_amsl); } } } diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 2052d2534a..daa7507f98 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -46,9 +46,11 @@ #include #include #include -#include +#include +#include #include #include +#include #define GEOFENCE_FILENAME PX4_ROOTFSDIR"/fs/microsd/etc/geofence.txt" @@ -85,9 +87,8 @@ public: * * @return true: system is obeying fence, false: system is violating fence */ - bool check(const struct vehicle_global_position_s &global_position, - const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl, - const struct home_position_s home_pos, bool home_position_set); + bool check(const vehicle_global_position_s &global_position, + const vehicle_gps_position_s &gps_position, const home_position_s home_pos, bool home_position_set); /** * Return whether a mission item obeys the geofence. @@ -165,6 +166,8 @@ private: (ParamFloat) _param_max_ver_distance ) + uORB::Subscription _sub_airdata; + int _outside_counter{0}; uint16_t _update_counter{0}; ///< dataman update counter: if it does not match, we polygon data was updated @@ -193,8 +196,8 @@ private: */ bool checkAll(double lat, double lon, float altitude); - bool checkAll(const struct vehicle_global_position_s &global_position); - bool checkAll(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl); + bool checkAll(const vehicle_global_position_s &global_position); + bool checkAll(const vehicle_global_position_s &global_position, float baro_altitude_amsl); /** * Check if a single point is within a polygon diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 38df462997..662cbe5e50 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -277,7 +277,6 @@ private: int _local_pos_sub{-1}; /**< local position subscription */ int _offboard_mission_sub{-1}; /**< offboard mission subscription */ int _param_update_sub{-1}; /**< param update subscription */ - int _sensor_combined_sub{-1}; /**< sensor combined subscription */ int _traffic_sub{-1}; /**< traffic subscription */ int _vehicle_command_sub{-1}; /**< vehicle commands (onboard and offboard) */ int _vstatus_sub{-1}; /**< vehicle status subscription */ @@ -294,7 +293,6 @@ private: fw_pos_ctrl_status_s _fw_pos_ctrl_status{}; /**< fixed wing navigation capabilities */ home_position_s _home_pos{}; /**< home position for RTL */ mission_result_s _mission_result{}; - sensor_combined_s _sensor_combined{}; /**< sensor values */ vehicle_global_position_s _global_pos{}; /**< global vehicle position */ vehicle_gps_position_s _gps_pos{}; /**< gps position */ vehicle_land_detected_s _land_detected{}; /**< vehicle land_detected */ @@ -366,7 +364,6 @@ private: void home_position_update(bool force = false); void local_position_update(); void params_update(); - void sensor_combined_update(); void vehicle_land_detected_update(); void vehicle_status_update(); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 1d528ed00e..20706e2cef 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -132,12 +132,6 @@ Navigator::gps_position_update() orb_copy(ORB_ID(vehicle_gps_position), _gps_pos_sub, &_gps_pos); } -void -Navigator::sensor_combined_update() -{ - orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); -} - void Navigator::home_position_update(bool force) { @@ -201,7 +195,6 @@ Navigator::run() _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); @@ -217,7 +210,6 @@ Navigator::run() global_position_update(); local_position_update(); gps_position_update(); - sensor_combined_update(); home_position_update(true); fw_pos_ctrl_status_update(true); params_update(); @@ -281,13 +273,6 @@ Navigator::run() } } - /* sensors combined updated */ - orb_check(_sensor_combined_sub, &updated); - - if (updated) { - sensor_combined_update(); - } - /* parameters updated */ orb_check(_param_update_sub, &updated); @@ -538,7 +523,7 @@ Navigator::run() (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && (hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) { - bool inside = _geofence.check(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, + bool inside = _geofence.check(_global_pos, _gps_pos, _home_pos, home_position_valid()); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; @@ -711,7 +696,6 @@ Navigator::run() orb_unsubscribe(_global_pos_sub); orb_unsubscribe(_local_pos_sub); orb_unsubscribe(_gps_pos_sub); - orb_unsubscribe(_sensor_combined_sub); orb_unsubscribe(_fw_pos_ctrl_status_sub); orb_unsubscribe(_vstatus_sub); orb_unsubscribe(_land_detected_sub); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 59821bbb90..d5228dcd10 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -63,6 +63,7 @@ #include #include #include +#include #include #include #include @@ -368,6 +369,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(&lidar, 0, sizeof(lidar)); struct vehicle_rates_setpoint_s rates_setpoint; memset(&rates_setpoint, 0, sizeof(rates_setpoint)); + struct vehicle_air_data_s airdata; + memset(&airdata, 0, sizeof(vehicle_air_data_s)); /* subscribe */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -380,6 +383,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int vision_position_sub = orb_subscribe(ORB_ID(vehicle_vision_position)); int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); int vehicle_rate_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + int vehicle_air_data_sub = orb_subscribe(ORB_ID(vehicle_air_data)); // because we can have several distance sensor instances with different orientations int distance_sensor_subs[ORB_MULTI_MAX_INSTANCES]; @@ -430,26 +434,34 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds_init[0].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - bool baro_updated = (sensor.baro_timestamp_relative != sensor.RELATIVE_TIMESTAMP_INVALID); + bool baro_updated = false; + orb_check(vehicle_air_data_sub, &baro_updated); - if (wait_baro && sensor.timestamp + sensor.baro_timestamp_relative != baro_timestamp && baro_updated) { - baro_timestamp = sensor.timestamp + sensor.baro_timestamp_relative; - baro_wait_for_sample_time = hrt_absolute_time(); + if (baro_updated) { + orb_copy(ORB_ID(vehicle_air_data), vehicle_air_data_sub, &airdata); - /* mean calculation over several measurements */ - if (baro_init_cnt < baro_init_num) { - if (PX4_ISFINITE(sensor.baro_alt_meter)) { - baro_offset += sensor.baro_alt_meter; - baro_init_cnt++; + if (wait_baro && airdata.timestamp != baro_timestamp) { + + baro_timestamp = airdata.timestamp; + baro_wait_for_sample_time = hrt_absolute_time(); + + /* mean calculation over several measurements */ + if (baro_init_cnt < baro_init_num) { + if (PX4_ISFINITE(airdata.baro_alt_meter)) { + baro_offset += airdata.baro_alt_meter; + baro_init_cnt++; + } + + } else { + wait_baro = false; + baro_offset /= (float) baro_init_cnt; + local_pos.z_valid = true; + local_pos.v_z_valid = true; } - - } else { - wait_baro = false; - baro_offset /= (float) baro_init_cnt; - local_pos.z_valid = true; - local_pos.v_z_valid = true; } } + + } } else { @@ -532,9 +544,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) accel_updates++; } - if (sensor.timestamp + sensor.baro_timestamp_relative != baro_timestamp) { - corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0]; - baro_timestamp = sensor.timestamp + sensor.baro_timestamp_relative; + if (airdata.timestamp != baro_timestamp) { + corr_baro = baro_offset - airdata.baro_alt_meter - z_est[0]; + baro_timestamp = airdata.timestamp; baro_updates++; } } @@ -1405,8 +1417,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.terrain_alt_valid = false; } - global_pos.pressure_alt = sensor.baro_alt_meter; - if (vehicle_global_position_pub == nullptr) { vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 8a0037ecc7..61bc67def5 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1357,8 +1357,8 @@ int sdlog2_thread_main(int argc, char *argv[]) /* track changes in sensor_combined topic */ hrt_abstime gyro_timestamp = 0; hrt_abstime accelerometer_timestamp = 0; - hrt_abstime magnetometer_timestamp = 0; - hrt_abstime barometer_timestamp = 0; + //hrt_abstime magnetometer_timestamp = 0; + //hrt_abstime barometer_timestamp = 0; /* initialize calculated mean SNR */ float snr_mean = 0.0f; @@ -1526,15 +1526,15 @@ int sdlog2_thread_main(int argc, char *argv[]) write_IMU = true; } - if (buf.sensor.timestamp + buf.sensor.magnetometer_timestamp_relative != magnetometer_timestamp) { - magnetometer_timestamp = buf.sensor.timestamp + buf.sensor.magnetometer_timestamp_relative; - write_IMU = true; - } +// if (buf.sensor.timestamp + buf.sensor.magnetometer_timestamp_relative != magnetometer_timestamp) { +// magnetometer_timestamp = buf.sensor.timestamp + buf.sensor.magnetometer_timestamp_relative; +// write_IMU = true; +// } - if (buf.sensor.timestamp + buf.sensor.baro_timestamp_relative != barometer_timestamp) { - barometer_timestamp = buf.sensor.timestamp + buf.sensor.baro_timestamp_relative; - write_SENS = true; - } +// if (buf.sensor.timestamp + buf.sensor.baro_timestamp_relative != barometer_timestamp) { +// barometer_timestamp = buf.sensor.timestamp + buf.sensor.baro_timestamp_relative; +// write_SENS = true; +// } if (write_IMU) { log_msg.msg_type = LOG_IMU_MSG; @@ -1545,9 +1545,9 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[0]; log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[1]; log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[2]; - log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0]; - log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1]; - log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2]; +// log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0]; +// log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1]; +// log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2]; log_msg.body.log_IMU.temp_gyro = 0; log_msg.body.log_IMU.temp_acc = 0; log_msg.body.log_IMU.temp_mag = 0; @@ -1555,12 +1555,12 @@ int sdlog2_thread_main(int argc, char *argv[]) } if (write_SENS) { - log_msg.msg_type = LOG_SENS_MSG; - - log_msg.body.log_SENS.baro_pres = 0; - log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter; - log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius; - LOGBUFFER_WRITE_AND_COUNT(SENS); +// log_msg.msg_type = LOG_SENS_MSG; +// +// log_msg.body.log_SENS.baro_pres = 0; +// log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter; +// log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius; +// LOGBUFFER_WRITE_AND_COUNT(SENS); } } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index e743ded83f..bdaf5a7cf5 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -85,6 +85,8 @@ #include #include #include +#include +#include #include @@ -169,6 +171,8 @@ private: int _params_sub{-1}; /**< notification of parameter updates */ orb_advert_t _sensor_pub{nullptr}; /**< combined sensor data topic */ + orb_advert_t _airdata_pub{nullptr}; /**< combined sensor data topic */ + orb_advert_t _magnetometer_pub{nullptr}; /**< combined sensor data topic */ orb_advert_t _battery_pub[BOARD_NUMBER_BRICKS] {}; /**< battery status */ #if BOARD_NUMBER_BRICKS > 1 @@ -212,7 +216,7 @@ private: * @param raw Combined sensor data structure into which * data should be returned. */ - void diff_pres_poll(struct sensor_combined_s &raw); + void diff_pres_poll(const vehicle_air_data_s &airdata); /** * Check for changes in vehicle control mode. @@ -285,7 +289,7 @@ Sensors::adc_init() } void -Sensors::diff_pres_poll(struct sensor_combined_s &raw) +Sensors::diff_pres_poll(const vehicle_air_data_s &raw) { bool updated; orb_check(_diff_pres_sub, &updated); @@ -569,7 +573,9 @@ Sensors::run() #endif } - struct sensor_combined_s raw = {}; + sensor_combined_s raw = {}; + vehicle_air_data_s airdata = {}; + vehicle_magnetometer_s magnetometer = {}; struct sensor_preflight_s preflt = {}; @@ -592,14 +598,16 @@ Sensors::run() _actuator_ctrl_0_sub = orb_subscribe(ORB_ID(actuator_controls_0)); /* get a set of initial values */ - _voted_sensors_update.sensors_poll(raw); + _voted_sensors_update.sensors_poll(raw, airdata, magnetometer); - diff_pres_poll(raw); + diff_pres_poll(airdata); _rc_update.rc_parameter_map_poll(_parameter_handles, true /* forced */); /* advertise the sensor_combined topic and make the initial publication */ _sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); + _airdata_pub = orb_advertise(ORB_ID(vehicle_air_data), &airdata); + _magnetometer_pub = orb_advertise(ORB_ID(vehicle_magnetometer), &magnetometer); /* advertise the sensor_preflight topic and make the initial publication */ preflt.accel_inconsistency_m_s_s = 0.0f; @@ -649,12 +657,15 @@ Sensors::run() /* the timestamp of the raw struct is updated by the gyro_poll() method (this makes the gyro * a mandatory sensor) */ - _voted_sensors_update.sensors_poll(raw); + const uint64_t airdata_prev_timestamp = airdata.timestamp; + const uint64_t magnetometer_prev_timestamp = magnetometer.timestamp; + + _voted_sensors_update.sensors_poll(raw, airdata, magnetometer); /* check battery voltage */ adc_poll(); - diff_pres_poll(raw); + diff_pres_poll(airdata); if (raw.timestamp > 0) { @@ -662,6 +673,14 @@ Sensors::run() orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); + if (airdata.timestamp != airdata_prev_timestamp) { + orb_publish(ORB_ID(vehicle_air_data), _airdata_pub, &airdata); + } + + if (magnetometer.timestamp != magnetometer_prev_timestamp) { + orb_publish(ORB_ID(vehicle_magnetometer), _magnetometer_pub, &magnetometer); + } + _voted_sensors_update.check_failover(); /* If the the vehicle is disarmed calculate the length of the maximum difference between @@ -673,7 +692,6 @@ Sensors::run() _voted_sensors_update.calc_gyro_inconsistency(preflt); _voted_sensors_update.calc_mag_inconsistency(preflt); orb_publish(ORB_ID(sensor_preflight), _sensor_preflight, &preflt); - } } @@ -704,6 +722,8 @@ Sensors::run() orb_unsubscribe(_params_sub); orb_unsubscribe(_actuator_ctrl_0_sub); orb_unadvertise(_sensor_pub); + orb_unadvertise(_airdata_pub); + orb_unadvertise(_magnetometer_pub); _rc_update.deinit(); _voted_sensors_update.deinit(); diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 1d60e521f8..ba2fc8d333 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -42,7 +42,7 @@ #include #include -#include +#include #define MAG_ROT_VAL_INTERNAL -1 #define CAL_ERROR_APPLY_CAL_MSG "FAILED APPLYING %s CAL #%u" @@ -55,8 +55,6 @@ VotedSensorsUpdate::VotedSensorsUpdate(const Parameters ¶meters, bool hil_en { memset(&_last_sensor_data, 0, sizeof(_last_sensor_data)); memset(&_last_accel_timestamp, 0, sizeof(_last_accel_timestamp)); - memset(&_last_mag_timestamp, 0, sizeof(_last_mag_timestamp)); - memset(&_last_baro_timestamp, 0, sizeof(_last_baro_timestamp)); memset(&_accel_diff, 0, sizeof(_accel_diff)); memset(&_gyro_diff, 0, sizeof(_gyro_diff)); memset(&_mag_diff, 0, sizeof(_mag_diff)); @@ -90,8 +88,6 @@ VotedSensorsUpdate::VotedSensorsUpdate(const Parameters ¶meters, bool hil_en int VotedSensorsUpdate::init(sensor_combined_s &raw) { raw.accelerometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID; - raw.magnetometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID; - raw.baro_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID; raw.timestamp = 0; initialize_sensors(); @@ -747,7 +743,7 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw) } } -void VotedSensorsUpdate::mag_poll(struct sensor_combined_s &raw) +void VotedSensorsUpdate::mag_poll(vehicle_magnetometer_s &magnetometer) { for (unsigned uorb_index = 0; uorb_index < _mag.subscription_count; uorb_index++) { bool mag_updated; @@ -772,11 +768,11 @@ void VotedSensorsUpdate::mag_poll(struct sensor_combined_s &raw) matrix::Vector3f vect(mag_report.x, mag_report.y, mag_report.z); vect = _mag_rotation[uorb_index] * vect; - _last_sensor_data[uorb_index].magnetometer_ga[0] = vect(0); - _last_sensor_data[uorb_index].magnetometer_ga[1] = vect(1); - _last_sensor_data[uorb_index].magnetometer_ga[2] = vect(2); + _last_magnetometer[uorb_index].timestamp = mag_report.timestamp; + _last_magnetometer[uorb_index].magnetometer_ga[0] = vect(0); + _last_magnetometer[uorb_index].magnetometer_ga[1] = vect(1); + _last_magnetometer[uorb_index].magnetometer_ga[2] = vect(2); - _last_mag_timestamp[uorb_index] = mag_report.timestamp; _mag.voter.put(uorb_index, mag_report.timestamp, vect.data(), mag_report.error_count, _mag.priority[uorb_index]); } } @@ -785,7 +781,7 @@ void VotedSensorsUpdate::mag_poll(struct sensor_combined_s &raw) _mag.voter.get_best(hrt_absolute_time(), &best_index); if (best_index >= 0) { - memcpy(&raw.magnetometer_ga, &_last_sensor_data[best_index].magnetometer_ga, sizeof(raw.magnetometer_ga)); + magnetometer = _last_magnetometer[best_index]; _mag.last_best_vote = (uint8_t)best_index; if (_selection.mag_device_id != _mag_device_id[best_index]) { @@ -795,7 +791,7 @@ void VotedSensorsUpdate::mag_poll(struct sensor_combined_s &raw) } } -void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw) +void VotedSensorsUpdate::baro_poll(vehicle_air_data_s &airdata) { bool got_update = false; float *offsets[] = {&_corrections.baro_offset_0, &_corrections.baro_offset_1, &_corrections.baro_offset_2 }; @@ -837,11 +833,11 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw) got_update = true; matrix::Vector3f vect(baro_report.pressure, 0.f, 0.f); - _last_sensor_data[uorb_index].baro_temp_celcius = baro_report.temperature; - _last_sensor_data[uorb_index].baro_pressure_pa = corrected_pressure; + _last_airdata[uorb_index].timestamp = baro_report.timestamp; + _last_airdata[uorb_index].baro_temp_celcius = baro_report.temperature; + _last_airdata[uorb_index].baro_pressure_pa = corrected_pressure; - _last_baro_timestamp[uorb_index] = baro_report.timestamp; - _baro.voter.put(uorb_index, baro_report.timestamp, vect.data, baro_report.error_count, _baro.priority[uorb_index]); + _baro.voter.put(uorb_index, baro_report.timestamp, vect.data(), baro_report.error_count, _baro.priority[uorb_index]); } } @@ -850,8 +846,7 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw) _baro.voter.get_best(hrt_absolute_time(), &best_index); if (best_index >= 0) { - raw.baro_temp_celcius = _last_sensor_data[best_index].baro_temp_celcius; - raw.baro_pressure_pa = _last_sensor_data[best_index].baro_pressure_pa; + airdata = _last_airdata[best_index]; if (_baro.last_best_vote != best_index) { _baro.last_best_vote = (uint8_t)best_index; @@ -873,7 +868,7 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw) const float p1 = _parameters.baro_qnh * 0.1f; /* measured pressure in kPa */ - const float p = raw.baro_pressure_pa * 0.001f; + const float p = airdata.baro_pressure_pa * 0.001f; /* * Solve: @@ -884,8 +879,15 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw) * h = ------------------------------- + h1 * a */ - raw.baro_alt_meter = (((powf((p / p1), (-(a * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * T1) - T1) / a; + airdata.baro_alt_meter = (((powf((p / p1), (-(a * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * T1) - T1) / a; + + // calculate air density + // estimate air density assuming typical 20degC ambient temperature + // TODO: use air temperature if available (differential pressure sensors) + static constexpr float pressure_to_density = 1.0f / (CONSTANTS_AIR_GAS_CONST * (20.0f - + CONSTANTS_ABSOLUTE_NULL_CELSIUS)); + airdata.rho = pressure_to_density * airdata.baro_pressure_pa; } } } @@ -1015,12 +1017,13 @@ VotedSensorsUpdate::apply_mag_calibration(DevHandle &h, const struct mag_calibra #endif } -void VotedSensorsUpdate::sensors_poll(sensor_combined_s &raw) +void VotedSensorsUpdate::sensors_poll(sensor_combined_s &raw, vehicle_air_data_s &airdata, + vehicle_magnetometer_s &magnetometer) { accel_poll(raw); gyro_poll(raw); - mag_poll(raw); - baro_poll(raw); + mag_poll(magnetometer); + baro_poll(airdata); // publish sensor corrections if necessary if (!_hil_enabled && _corrections_changed) { @@ -1065,15 +1068,6 @@ void VotedSensorsUpdate::set_relative_timestamps(sensor_combined_s &raw) raw.accelerometer_timestamp_relative = (int32_t)((int64_t)_last_accel_timestamp[_accel.last_best_vote] - (int64_t)raw.timestamp); } - - if (_last_mag_timestamp[_mag.last_best_vote]) { - raw.magnetometer_timestamp_relative = (int32_t)((int64_t)_last_mag_timestamp[_mag.last_best_vote] - - (int64_t)raw.timestamp); - } - - if (_last_baro_timestamp[_baro.last_best_vote]) { - raw.baro_timestamp_relative = (int32_t)((int64_t)_last_baro_timestamp[_baro.last_best_vote] - (int64_t)raw.timestamp); - } } void @@ -1191,8 +1185,9 @@ void VotedSensorsUpdate::calc_mag_inconsistency(sensor_preflight_s &preflt) // calculate mag_diff_sum_sq for the specified sensor against the primary for (unsigned axis_index = 0; axis_index < 3; axis_index++) { _mag_diff[axis_index][check_index] = 0.95f * _mag_diff[axis_index][check_index] + 0.05f * - (_last_sensor_data[_mag.last_best_vote].magnetometer_ga[axis_index] - - _last_sensor_data[sensor_index].magnetometer_ga[axis_index]); + (_last_magnetometer[_mag.last_best_vote].magnetometer_ga[axis_index] - + _last_magnetometer[sensor_index].magnetometer_ga[axis_index]); + mag_diff_sum_sq += _mag_diff[axis_index][check_index] * _mag_diff[axis_index][check_index]; } diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 259178402b..590118deca 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -56,6 +56,8 @@ #include #include #include +#include +#include #include @@ -106,7 +108,7 @@ public: /** * read new sensor data */ - void sensors_poll(sensor_combined_s &raw); + void sensors_poll(sensor_combined_s &raw, vehicle_air_data_s &airdata, vehicle_magnetometer_s &magnetometer); /** * set the relative timestamps of each sensor timestamp, based on the last sensors_poll, @@ -189,7 +191,7 @@ private: * @param raw Combined sensor data structure into which * data should be returned. */ - void mag_poll(struct sensor_combined_s &raw); + void mag_poll(vehicle_magnetometer_s &magnetometer); /** * Poll the barometer for updated data. @@ -197,7 +199,7 @@ private: * @param raw Combined sensor data structure into which * data should be returned. */ - void baro_poll(struct sensor_combined_s &raw); + void baro_poll(vehicle_air_data_s &airdata); /** * Check & handle failover of a sensor @@ -244,10 +246,10 @@ private: orb_advert_t _mavlink_log_pub = nullptr; sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX]; /**< latest sensor data from all sensors instances */ + vehicle_air_data_s _last_airdata[SENSOR_COUNT_MAX]; /**< latest sensor data from all sensors instances */ + vehicle_magnetometer_s _last_magnetometer[SENSOR_COUNT_MAX]; /**< latest sensor data from all sensors instances */ uint64_t _last_accel_timestamp[ACCEL_COUNT_MAX]; /**< latest full timestamp */ - uint64_t _last_mag_timestamp[MAG_COUNT_MAX]; /**< latest full timestamp */ - uint64_t _last_baro_timestamp[BARO_COUNT_MAX]; /**< latest full timestamp */ matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ matrix::Dcmf _mag_rotation[MAG_COUNT_MAX]; /**< rotation matrix for the orientation that the external mag0 is mounted */ diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index f471ace5c5..689e24697c 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -99,7 +99,6 @@ struct RawMPUData { #pragma pack(push, 1) struct RawBaroData { float pressure; - float altitude; float temperature; }; #pragma pack(pop) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 2d2ebcc252..30b2f84ba7 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -249,7 +249,6 @@ void Simulator::update_sensors(mavlink_hil_sensor_t *imu) // Get air pressure and pressure altitude // valid for troposphere (below 11km AMSL) baro.pressure = imu->abs_pressure; - baro.altitude = imu->pressure_alt; baro.temperature = imu->temperature; write_baro_data(&baro); @@ -652,7 +651,6 @@ void Simulator::initializeSensorData() RawBaroData baro = {}; // calculate air pressure from altitude (valid for low altitude) baro.pressure = 120000.0f; - baro.altitude = 0.0f; baro.temperature = 25.0f; write_baro_data(&baro); diff --git a/src/platforms/posix/drivers/barosim/CMakeLists.txt b/src/platforms/posix/drivers/barosim/CMakeLists.txt index 73f07279de..c40350c556 100644 --- a/src/platforms/posix/drivers/barosim/CMakeLists.txt +++ b/src/platforms/posix/drivers/barosim/CMakeLists.txt @@ -36,7 +36,6 @@ include_directories(${PX4_SOURCE_DIR}/mavlink/include/mavlink) px4_add_module( MODULE platforms__posix__drivers__barosim MAIN barosim - COMPILE_FLAGS SRCS baro.cpp DEPENDS diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index 4cacb5f948..158475fe22 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -139,7 +139,6 @@ BAROSIM::BAROSIM(const char *path) : _measure_perf(perf_alloc(PC_ELAPSED, "barosim_measure")), _comms_errors(perf_alloc(PC_COUNT, "barosim_comms_errors")) { - } BAROSIM::~BAROSIM() @@ -191,6 +190,9 @@ BAROSIM::init() return -ENODEV; } + /* fill report structures */ + start(); + out: return ret; }