diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-01-06 14:33:58 +0100 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-01-06 14:33:58 +0100 |
commit | 63815909979d8b01738a18bd694afcf2bc11c3a3 (patch) | |
tree | 5bd91e8e14b3f0c64e9f0cb00a9b654328d70ef1 | |
parent | c094a1a33da61e00bbb2eca2dc8e19b18b1c603a (diff) | |
download | px4-firmware-63815909979d8b01738a18bd694afcf2bc11c3a3.tar.gz px4-firmware-63815909979d8b01738a18bd694afcf2bc11c3a3.tar.bz2 px4-firmware-63815909979d8b01738a18bd694afcf2bc11c3a3.zip |
attitude_estimator_ekf: acc compensation improvements
3 files changed, 79 insertions, 39 deletions
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 36e82db43..7cf100014 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -59,6 +59,7 @@ #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/parameter_update.h> #include <drivers/drv_hrt.h> @@ -219,6 +220,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds memset(&raw, 0, sizeof(raw)); struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_control_mode_s control_mode; @@ -226,9 +229,20 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds uint64_t last_data = 0; uint64_t last_measurement = 0; - uint64_t last_gps = 0; - - float vel_prev[3] = { 0.0f, 0.0f, 0.0f }; + uint64_t last_vel_t = 0; + + /* current velocity */ + math::Vector<3> vel; + vel.zero(); + /* previous velocity */ + math::Vector<3> vel_prev; + vel_prev.zero(); + /* actual acceleration (by GPS velocity) in body frame */ + math::Vector<3> acc; + acc.zero(); + /* rotation matrix */ + math::Matrix<3, 3> R; + R.identity(); /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); @@ -238,6 +252,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* subscribe to GPS */ int sub_gps = orb_subscribe(ORB_ID(vehicle_gps_position)); + /* subscribe to GPS */ + int sub_global_pos = orb_subscribe(ORB_ID(vehicle_global_position)); + /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); @@ -276,9 +293,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; unsigned offset_count = 0; - /* actual acceleration (by GPS velocity) in body frame */ - float acc[3] = { 0.0f, 0.0f, 0.0f }; - /* rotation matrix for magnetic declination */ math::Matrix<3, 3> R_decl; R_decl.identity(); @@ -327,7 +341,18 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* get latest measurements */ orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); - orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps); + + bool gps_updated; + orb_check(sub_gps, &gps_updated); + if (gps_updated) { + orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps); + } + + bool global_pos_updated; + orb_check(sub_global_pos, &global_pos_updated); + if (global_pos_updated) { + orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos); + } if (!initialized) { // XXX disabling init for now @@ -374,43 +399,50 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds sensor_last_timestamp[1] = raw.timestamp; } - if (gps.fix_type >= 3 && gps.eph_m < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) { - if (last_gps != 0 && gps.timestamp_velocity != last_gps) { - float gps_dt = (gps.timestamp_velocity - last_gps) / 1000000.0f; - /* calculate acceleration in NED frame */ - float acc_NED[3]; - acc_NED[0] = (gps.vel_n_m_s - vel_prev[0]) / gps_dt; - acc_NED[1] = (gps.vel_e_m_s - vel_prev[1]) / gps_dt; - acc_NED[2] = (gps.vel_d_m_s - vel_prev[2]) / gps_dt; - - /* project acceleration to body frame */ - for (int i = 0; i < 3; i++) { - acc[i] = 0.0f; - for (int j = 0; j < 3; j++) { - acc[i] += att.R[j][i] * acc_NED[j]; - } - } + hrt_abstime vel_t = 0; + bool vel_valid = false; + if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph_m < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) { + vel_valid = true; + if (gps_updated) { + vel_t = gps.timestamp_velocity; + vel(0) = gps.vel_n_m_s; + vel(1) = gps.vel_e_m_s; + vel(2) = gps.vel_d_m_s; + } - vel_prev[0] = gps.vel_n_m_s; - vel_prev[1] = gps.vel_e_m_s; - vel_prev[2] = gps.vel_d_m_s; + } else if (ekf_params.acc_comp == 2 && global_pos_updated && global_pos.valid && hrt_absolute_time() < global_pos.timestamp + 500000) { + vel_valid = true; + if (global_pos_updated) { + vel_t = global_pos.timestamp; + vel(0) = global_pos.vx; + vel(1) = global_pos.vy; + vel(2) = global_pos.vz; + } + } + + if (vel_valid) { + /* velocity is valid */ + if (vel_t != 0) { + /* velocity updated */ + if (last_vel_t != 0 && vel_t != last_vel_t) { + float vel_dt = (vel_t - last_vel_t) / 1000000.0f; + /* calculate acceleration in body frame */ + acc = R.transposed() * ((vel - vel_prev) / vel_dt); + } + last_vel_t = vel_t; + vel_prev = vel; } - last_gps = gps.timestamp_velocity; } else { - acc[0] = 0.0f; - acc[1] = 0.0f; - acc[2] = 0.0f; - - vel_prev[0] = 0.0f; - vel_prev[1] = 0.0f; - vel_prev[2] = 0.0f; - last_gps = 0; + /* velocity is valid, reset acceleration */ + acc.zero(); + vel_prev.zero(); + last_vel_t = 0; } - z_k[3] = raw.accelerometer_m_s2[0] - acc[0]; - z_k[4] = raw.accelerometer_m_s2[1] - acc[1]; - z_k[5] = raw.accelerometer_m_s2[2] - acc[2]; + z_k[3] = raw.accelerometer_m_s2[0] - acc(0); + z_k[4] = raw.accelerometer_m_s2[1] - acc(1); + z_k[5] = raw.accelerometer_m_s2[2] - acc(2); /* update magnetometer measurements */ if (sensor_last_count[2] != raw.magnetometer_counter) { @@ -506,7 +538,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* magnetic declination */ math::Matrix<3, 3> R_body = (&Rot_matrix[0]); - math::Matrix<3, 3> R = R_decl * R_body; + R = R_decl * R_body; /* copy rotation matrix */ memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R)); diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 999446a47..44f47b47c 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -68,6 +68,8 @@ PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f); /* magnetic declination, in degrees */ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); +PARAM_DEFINE_INT32(ATT_ACC_COMP, 0); + int parameters_init(struct attitude_estimator_ekf_param_handles *h) { /* PID parameters */ @@ -88,6 +90,8 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h) h->mag_decl = param_find("ATT_MAG_DECL"); + h->acc_comp = param_find("ATT_ACC_COMP"); + return OK; } @@ -110,5 +114,7 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru param_get(h->mag_decl, &(p->mag_decl)); + param_get(h->acc_comp, &(p->acc_comp)); + return OK; } diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h index b4b3ca50d..74a141609 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h @@ -48,6 +48,7 @@ struct attitude_estimator_ekf_params { float pitch_off; float yaw_off; float mag_decl; + int acc_comp; }; struct attitude_estimator_ekf_param_handles { @@ -55,6 +56,7 @@ struct attitude_estimator_ekf_param_handles { param_t q0, q1, q2, q3, q4; param_t roll_off, pitch_off, yaw_off; param_t mag_decl; + param_t acc_comp; }; /** |