diff options
| -rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 11 | ||||
| -rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_main.c | 11 |
2 files changed, 9 insertions, 13 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index bfccd5fb0..7b6fad658 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -382,16 +382,15 @@ handle_message(mavlink_message_t *msg) /* hil gyro */ static const float mrad2rad = 1.0e-3f; - hil_sensors.gyro_counter = hil_counter; hil_sensors.gyro_raw[0] = imu.xgyro / mrad2rad; hil_sensors.gyro_raw[1] = imu.ygyro / mrad2rad; hil_sensors.gyro_raw[2] = imu.zgyro / mrad2rad; hil_sensors.gyro_rad_s[0] = imu.xgyro; hil_sensors.gyro_rad_s[1] = imu.ygyro; hil_sensors.gyro_rad_s[2] = imu.zgyro; + hil_sensors.gyro_counter = hil_counter; /* accelerometer */ - hil_sensors.accelerometer_counter = hil_counter; static const float mg2ms2 = 9.8f / 1000.0f; hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2; @@ -401,6 +400,7 @@ handle_message(mavlink_message_t *msg) hil_sensors.accelerometer_m_s2[2] = imu.zacc; hil_sensors.accelerometer_mode = 0; // TODO what is this? hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 + hil_sensors.accelerometer_counter = hil_counter; /* adc */ hil_sensors.adc_voltage_v[0] = 0.0f; @@ -409,7 +409,6 @@ handle_message(mavlink_message_t *msg) /* magnetometer */ float mga2ga = 1.0e-3f; - hil_sensors.magnetometer_counter = hil_counter; hil_sensors.magnetometer_raw[0] = imu.xmag / mga2ga; hil_sensors.magnetometer_raw[1] = imu.ymag / mga2ga; hil_sensors.magnetometer_raw[2] = imu.zmag / mga2ga; @@ -419,15 +418,13 @@ handle_message(mavlink_message_t *msg) hil_sensors.magnetometer_range_ga = 32.7f; // int16 hil_sensors.magnetometer_mode = 0; // TODO what is this hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; + hil_sensors.magnetometer_counter = hil_counter; /* baro */ hil_sensors.baro_pres_mbar = imu.abs_pressure; hil_sensors.baro_alt_meter = imu.pressure_alt; hil_sensors.baro_temp_celcius = imu.temperature; - - hil_sensors.gyro_counter = hil_counter; - hil_sensors.magnetometer_counter = hil_counter; - hil_sensors.accelerometer_counter = hil_counter; + hil_sensors.baro_counter = hil_counter; /* differential pressure */ hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 10007bf96..3084b6d92 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -236,13 +236,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (ret < 0) { /* poll error */ - errx(1, "subscriptions poll error on init."); + mavlink_log_info(mavlink_fd, "[inav] poll error on init"); } else if (ret > 0) { if (fds_init[0].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (wait_baro && sensor.baro_counter > baro_counter) { + if (wait_baro && sensor.baro_counter != baro_counter) { baro_counter = sensor.baro_counter; /* mean calculation over several measurements */ @@ -320,8 +320,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (ret < 0) { /* poll error */ - warnx("subscriptions poll error."); - thread_should_exit = true; + mavlink_log_info(mavlink_fd, "[inav] poll error on init"); continue; } else if (ret > 0) { @@ -355,7 +354,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds[4].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (sensor.accelerometer_counter > accel_counter) { + if (sensor.accelerometer_counter != accel_counter) { if (att.R_valid) { /* correct accel bias, now only for Z */ sensor.accelerometer_m_s2[2] -= accel_bias[2]; @@ -381,7 +380,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) accel_updates++; } - if (sensor.baro_counter > baro_counter) { + if (sensor.baro_counter != baro_counter) { baro_corr = - sensor.baro_alt_meter - z_est[0]; baro_counter = sensor.baro_counter; baro_updates++; |