dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Various HIL-related fixes

This commit is contained in:
Anton Babushkin 2013-11-27 23:04:49 +04:00
parent f1fece2bb6
commit 3c027a8e4d
2 changed files with 9 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -382,16 +382,15 @@ handle_message(mavlink_message_t *msg)
/* hil gyro */ /* hil gyro */
static const float mrad2rad = 1.0e-3f; 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[0] = imu.xgyro / mrad2rad;
hil_sensors.gyro_raw[1] = imu.ygyro / mrad2rad; hil_sensors.gyro_raw[1] = imu.ygyro / mrad2rad;
hil_sensors.gyro_raw[2] = imu.zgyro / mrad2rad; hil_sensors.gyro_raw[2] = imu.zgyro / mrad2rad;
hil_sensors.gyro_rad_s[0] = imu.xgyro; hil_sensors.gyro_rad_s[0] = imu.xgyro;
hil_sensors.gyro_rad_s[1] = imu.ygyro; hil_sensors.gyro_rad_s[1] = imu.ygyro;
hil_sensors.gyro_rad_s[2] = imu.zgyro; hil_sensors.gyro_rad_s[2] = imu.zgyro;
hil_sensors.gyro_counter = hil_counter;
/* accelerometer */ /* accelerometer */
hil_sensors.accelerometer_counter = hil_counter;
static const float mg2ms2 = 9.8f / 1000.0f; static const float mg2ms2 = 9.8f / 1000.0f;
hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
hil_sensors.accelerometer_raw[1] = imu.yacc / 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_m_s2[2] = imu.zacc;
hil_sensors.accelerometer_mode = 0; // TODO what is this? hil_sensors.accelerometer_mode = 0; // TODO what is this?
hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
hil_sensors.accelerometer_counter = hil_counter;
/* adc */ /* adc */
hil_sensors.adc_voltage_v[0] = 0.0f; hil_sensors.adc_voltage_v[0] = 0.0f;
@ -409,7 +409,6 @@ handle_message(mavlink_message_t *msg)
/* magnetometer */ /* magnetometer */
float mga2ga = 1.0e-3f; float mga2ga = 1.0e-3f;
hil_sensors.magnetometer_counter = hil_counter;
hil_sensors.magnetometer_raw[0] = imu.xmag / mga2ga; hil_sensors.magnetometer_raw[0] = imu.xmag / mga2ga;
hil_sensors.magnetometer_raw[1] = imu.ymag / mga2ga; hil_sensors.magnetometer_raw[1] = imu.ymag / mga2ga;
hil_sensors.magnetometer_raw[2] = imu.zmag / 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_range_ga = 32.7f; // int16
hil_sensors.magnetometer_mode = 0; // TODO what is this hil_sensors.magnetometer_mode = 0; // TODO what is this
hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
hil_sensors.magnetometer_counter = hil_counter;
/* baro */ /* baro */
hil_sensors.baro_pres_mbar = imu.abs_pressure; hil_sensors.baro_pres_mbar = imu.abs_pressure;
hil_sensors.baro_alt_meter = imu.pressure_alt; hil_sensors.baro_alt_meter = imu.pressure_alt;
hil_sensors.baro_temp_celcius = imu.temperature; hil_sensors.baro_temp_celcius = imu.temperature;
hil_sensors.baro_counter = hil_counter;
hil_sensors.gyro_counter = hil_counter;
hil_sensors.magnetometer_counter = hil_counter;
hil_sensors.accelerometer_counter = hil_counter;
/* differential pressure */ /* differential pressure */
hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa

11
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -236,13 +236,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (ret < 0) { if (ret < 0) {
/* poll error */ /* poll error */
errx(1, "subscriptions poll error on init."); mavlink_log_info(mavlink_fd, "[inav] poll error on init");
} else if (ret > 0) { } else if (ret > 0) {
if (fds_init[0].revents & POLLIN) { if (fds_init[0].revents & POLLIN) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); 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; baro_counter = sensor.baro_counter;
/* mean calculation over several measurements */ /* mean calculation over several measurements */
@ -320,8 +320,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (ret < 0) { if (ret < 0) {
/* poll error */ /* poll error */
warnx("subscriptions poll error."); mavlink_log_info(mavlink_fd, "[inav] poll error on init");
thread_should_exit = true;
continue; continue;
} else if (ret > 0) { } else if (ret > 0) {
@ -355,7 +354,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (fds[4].revents & POLLIN) { if (fds[4].revents & POLLIN) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); 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) { if (att.R_valid) {
/* correct accel bias, now only for Z */ /* correct accel bias, now only for Z */
sensor.accelerometer_m_s2[2] -= accel_bias[2]; sensor.accelerometer_m_s2[2] -= accel_bias[2];
@ -381,7 +380,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
accel_updates++; accel_updates++;
} }
if (sensor.baro_counter > baro_counter) { if (sensor.baro_counter != baro_counter) {
baro_corr = - sensor.baro_alt_meter - z_est[0]; baro_corr = - sensor.baro_alt_meter - z_est[0];
baro_counter = sensor.baro_counter; baro_counter = sensor.baro_counter;
baro_updates++; baro_updates++;