From c25cef299f7bd2e09062e38cebd94298b331e6e7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 7 Sep 2012 16:56:47 +0200 Subject: [PATCH] Fixed to mag measurement and filter --- apps/drivers/hmc5883/hmc5883.cpp | 6 +++--- apps/mavlink/mavlink.c | 21 ++----------------- .../attitude_estimator_bm.c | 2 +- 3 files changed, 6 insertions(+), 23 deletions(-) diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index 1f05a33434..da3b837771 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -612,8 +612,8 @@ HMC5883::collect() #pragma pack(push, 1) struct { /* status register and data as read back from the device */ uint8_t x[2]; - uint8_t y[2]; uint8_t z[2]; + uint8_t y[2]; } hmc_report; #pragma pack(pop) struct { @@ -833,7 +833,7 @@ test() err(1, "immediate read failed"); warnx("single read"); - warnx("measurement: %.6f %.6f %.6f", report.x, report.y, report.z); + warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); warnx("time: %lld", report.timestamp); /* set the queue depth to 10 */ @@ -863,7 +863,7 @@ test() err(1, "periodic read failed"); warnx("periodic read %u", i); - warnx("measurement: %.6f %.6f %.6f", report.x, report.y, report.z); + warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); warnx("time: %lld", report.timestamp); } diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 7289bd7a8e..57c4594308 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -943,32 +943,15 @@ static void *uorb_receiveloop(void *arg) /* Only send in HIL mode */ if (mavlink_hil_enabled) { - /* hacked HIL implementation in order for the APM Planner to work - * (correct cmd: mavlink_msg_hil_controls_send()) - */ - - mavlink_msg_rc_channels_scaled_send(chan, - hrt_absolute_time(), - 0, // port 0 - buf.att_sp.roll_body, - buf.att_sp.pitch_body, - buf.att_sp.thrust, - buf.att_sp.yaw_body, - 0, - 0, - 0, - 0, - 1 /*rssi=1*/); - /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; uint8_t mavlink_mode = 0; get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode); - /* correct HIL message as per MAVLink spec */ + /* HIL message as per MAVLink spec */ mavlink_msg_hil_controls_send(chan, hrt_absolute_time(), - buf.att_sp.roll_body, + buf.att_sp.roll_body, /* this may be replaced by ctrl0 later */ buf.att_sp.pitch_body, buf.att_sp.yaw_body, buf.att_sp.thrust, diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c index 0f6b6044fa..45267f3157 100644 --- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c +++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c @@ -226,7 +226,7 @@ int attitude_estimator_bm_main(int argc, char *argv[]) att.timestamp = sensor_combined_s_local.timestamp; att.roll = euler.x; att.pitch = euler.y; - att.yaw = euler.z - M_PI_F; + att.yaw = euler.z; if (att.yaw > M_PI_F) { att.yaw -= 2.0f * M_PI_F;