From 0f41d8b5077856cf8d00f30acf9f4bc52f6e41a6 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Wed, 18 Jun 2014 13:43:03 +0800 Subject: [PATCH 1/7] Added pitch, roll, and yaw offsets to compensate for imperfect fmu placement. These were removed in 61a3177979838649af2a6e8e50bea7d15e2765f4 --- .../attitude_estimator_ekf_main.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 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 66a949af15..95685b4077 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -512,10 +512,11 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* send out */ att.timestamp = raw.timestamp; - - att.roll = euler[0]; - att.pitch = euler[1]; - att.yaw = euler[2] + ekf_params.mag_decl; + + /* Compensate for fmu placement offsets and magnetic declination */ + att.roll = euler[0] - ekf_params.roll_off; + att.pitch = euler[1] - ekf_params.pitch_off; + att.yaw = euler[2] + ekf_params.mag_decl - ekf_params.yaw_off; att.rollspeed = x_aposteriori[0]; att.pitchspeed = x_aposteriori[1]; From 90a89ff2cdbb5268f4b8bee64b8da24db508f5a7 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Wed, 18 Jun 2014 20:27:35 +0800 Subject: [PATCH 2/7] WIP: Support in-flight fine tuning of board alignment. Implemented by applying a user supplied rotation matrix via new SENS_BOARD_(PITCH, ROLL, YAW)_OFF params in the sensors app. Currently only tested in QGC. --- src/modules/sensors/sensor_params.c | 24 ++++++++++++++++++++++++ src/modules/sensors/sensors.cpp | 26 +++++++++++++++++++++++--- 2 files changed, 47 insertions(+), 3 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 999cf8bb3a..c906338227 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -242,6 +242,30 @@ PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); */ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); +/** + * Board rotation pitch offset + * + * This parameter defines a pitch offset from the board rotation. It allows the user + * to fine tune the board offset in the event of misalignment. + */ + PARAM_DEFINE_FLOAT(SENS_BOARD_PITCH_OFF, 0); + +/** + * Board rotation roll offset + * + * This parameter defines a pitch offset from the board rotation. It allows the user + * to fine tune the board offset in the event of misalignment. + */ +PARAM_DEFINE_FLOAT(SENS_BOARD_ROLL_OFF, 0); + +/** + * Board rotation YAW offset + * + * This parameter defines a pitch offset from the board rotation. It allows the user + * to fine tune the board offset in the event of misalignment. + */ +PARAM_DEFINE_FLOAT(SENS_BOARD_YAW_OFF, 0); + /** * External magnetometer rotation * diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b268b1b36b..1a6202d7d8 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -230,6 +230,8 @@ private: math::Matrix<3, 3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ bool _mag_is_external; /**< true if the active mag is on an external board */ + math::Matrix<3, 3> _board_rotation_offset; /**< rotation matrix for fine tuning board offset **/ + uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */ @@ -252,6 +254,8 @@ private: int board_rotation; int external_mag_rotation; + + float board_offset[3]; int rc_map_roll; int rc_map_pitch; @@ -341,6 +345,8 @@ private: param_t board_rotation; param_t external_mag_rotation; + + param_t board_offset[3]; } _parameter_handles; /**< handles for interesting parameters */ @@ -587,6 +593,11 @@ Sensors::Sensors() : /* rotations */ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT"); + + /* rotation offsets */ + _parameter_handles.board_offset[0] = param_find("SENS_BOARD_ROLL_OFF"); + _parameter_handles.board_offset[1] = param_find("SENS_BOARD_PITCH_OFF"); + _parameter_handles.board_offset[2] = param_find("SENS_BOARD_YAW_OFF"); /* fetch initial parameter values */ parameters_update(); @@ -791,6 +802,15 @@ Sensors::parameters_update() get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation); + + param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0])); + param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1])); + param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2])); + + _board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0], + M_DEG_TO_RAD_F * _parameters.board_offset[1], + M_DEG_TO_RAD_F * _parameters.board_offset[2]); + return OK; } @@ -970,7 +990,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); - vect = _board_rotation * vect; + vect = _board_rotation * _board_rotation_offset * vect; raw.accelerometer_m_s2[0] = vect(0); raw.accelerometer_m_s2[1] = vect(1); @@ -996,7 +1016,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); - vect = _board_rotation * vect; + vect = _board_rotation * _board_rotation_offset * vect; raw.gyro_rad_s[0] = vect(0); raw.gyro_rad_s[1] = vect(1); @@ -1027,7 +1047,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) vect = _external_mag_rotation * vect; } else { - vect = _board_rotation * vect; + vect = _board_rotation * _board_rotation_offset * vect; } raw.magnetometer_ga[0] = vect(0); From 95003c6e1061b61dce6ed86e6c68d662e39a222d Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Wed, 18 Jun 2014 20:49:48 +0800 Subject: [PATCH 3/7] Removed unused ATT_XXX_OFF3 paramters. Offset tuning now accomplished via the sensors app using new SENS_BOARD_XXX_OFF parameters. --- .../attitude_estimator_ekf_main.cpp | 9 ++++----- .../attitude_estimator_ekf_params.c | 13 ------------- .../attitude_estimator_ekf_params.h | 1 - 3 files changed, 4 insertions(+), 19 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 95685b4077..66a949af15 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -512,11 +512,10 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* send out */ att.timestamp = raw.timestamp; - - /* Compensate for fmu placement offsets and magnetic declination */ - att.roll = euler[0] - ekf_params.roll_off; - att.pitch = euler[1] - ekf_params.pitch_off; - att.yaw = euler[2] + ekf_params.mag_decl - ekf_params.yaw_off; + + att.roll = euler[0]; + att.pitch = euler[1]; + att.yaw = euler[2] + ekf_params.mag_decl; att.rollspeed = x_aposteriori[0]; att.pitchspeed = x_aposteriori[1]; 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 3ff3d99229..49a8926099 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -61,11 +61,6 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f); /* offset estimation - UNUSED */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f); -/* offsets in roll, pitch and yaw of sensor plane and body */ -PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f); -PARAM_DEFINE_FLOAT(ATT_PITCH_OFF3, 0.0f); -PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f); - /* magnetic declination, in degrees */ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); @@ -85,10 +80,6 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h) h->r2 = param_find("EKF_ATT_V4_R2"); h->r3 = param_find("EKF_ATT_V4_R3"); - h->roll_off = param_find("ATT_ROLL_OFF3"); - h->pitch_off = param_find("ATT_PITCH_OFF3"); - h->yaw_off = param_find("ATT_YAW_OFF3"); - h->mag_decl = param_find("ATT_MAG_DECL"); h->acc_comp = param_find("ATT_ACC_COMP"); @@ -109,10 +100,6 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru param_get(h->r2, &(p->r[2])); param_get(h->r3, &(p->r[3])); - param_get(h->roll_off, &(p->roll_off)); - param_get(h->pitch_off, &(p->pitch_off)); - param_get(h->yaw_off, &(p->yaw_off)); - param_get(h->mag_decl, &(p->mag_decl)); p->mag_decl *= M_PI / 180.0f; 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 74a1416091..5985541cac 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h @@ -54,7 +54,6 @@ struct attitude_estimator_ekf_params { struct attitude_estimator_ekf_param_handles { param_t r0, r1, r2, r3; param_t q0, q1, q2, q3, q4; - param_t roll_off, pitch_off, yaw_off; param_t mag_decl; param_t acc_comp; }; From 5ed1cf7e8d6201f1f2e0115f4941ac551f61d628 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Wed, 18 Jun 2014 21:34:38 +0800 Subject: [PATCH 4/7] Fixed too-long param names. --- src/modules/sensors/sensor_params.c | 10 +++++----- src/modules/sensors/sensors.cpp | 6 +++--- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index c906338227..687efc49b3 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -248,23 +248,23 @@ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); * This parameter defines a pitch offset from the board rotation. It allows the user * to fine tune the board offset in the event of misalignment. */ - PARAM_DEFINE_FLOAT(SENS_BOARD_PITCH_OFF, 0); + PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0); /** * Board rotation roll offset * - * This parameter defines a pitch offset from the board rotation. It allows the user + * This parameter defines a roll offset from the board rotation. It allows the user * to fine tune the board offset in the event of misalignment. */ -PARAM_DEFINE_FLOAT(SENS_BOARD_ROLL_OFF, 0); +PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0); /** * Board rotation YAW offset * - * This parameter defines a pitch offset from the board rotation. It allows the user + * This parameter defines a yaw offset from the board rotation. It allows the user * to fine tune the board offset in the event of misalignment. */ -PARAM_DEFINE_FLOAT(SENS_BOARD_YAW_OFF, 0); +PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0); /** * External magnetometer rotation diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 1a6202d7d8..fe741894be 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -595,9 +595,9 @@ Sensors::Sensors() : _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT"); /* rotation offsets */ - _parameter_handles.board_offset[0] = param_find("SENS_BOARD_ROLL_OFF"); - _parameter_handles.board_offset[1] = param_find("SENS_BOARD_PITCH_OFF"); - _parameter_handles.board_offset[2] = param_find("SENS_BOARD_YAW_OFF"); + _parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF"); + _parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF"); + _parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF"); /* fetch initial parameter values */ parameters_update(); From 2475efe13d8f8d9d801276b73b98759676a64db6 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Thu, 19 Jun 2014 13:22:45 +0800 Subject: [PATCH 5/7] Pre-compute board orientation offsets on param update. --- src/modules/sensors/sensors.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index fe741894be..16fcb4c26d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -229,8 +229,6 @@ private: math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ math::Matrix<3, 3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ bool _mag_is_external; /**< true if the active mag is on an external board */ - - math::Matrix<3, 3> _board_rotation_offset; /**< rotation matrix for fine tuning board offset **/ uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */ @@ -807,10 +805,13 @@ Sensors::parameters_update() param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1])); param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2])); - _board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0], + /** fine tune board offset on parameter update **/ + math::Matrix<3, 3> board_rotation_offset; + board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0], M_DEG_TO_RAD_F * _parameters.board_offset[1], M_DEG_TO_RAD_F * _parameters.board_offset[2]); - + + _board_rotation = _board_rotation * board_rotation_offset; return OK; } @@ -990,7 +991,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); - vect = _board_rotation * _board_rotation_offset * vect; + vect = _board_rotation * vect; raw.accelerometer_m_s2[0] = vect(0); raw.accelerometer_m_s2[1] = vect(1); @@ -1016,7 +1017,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); - vect = _board_rotation * _board_rotation_offset * vect; + vect = _board_rotation * vect; raw.gyro_rad_s[0] = vect(0); raw.gyro_rad_s[1] = vect(1); @@ -1047,7 +1048,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) vect = _external_mag_rotation * vect; } else { - vect = _board_rotation * _board_rotation_offset * vect; + vect = _board_rotation * vect; } raw.magnetometer_ga[0] = vect(0); From 45d5e2600911682a9117b26603e3213c6b918fa4 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Fri, 20 Jun 2014 14:37:44 +0800 Subject: [PATCH 6/7] Define float params properly: 0.0f instead of just 0 --- src/modules/sensors/sensor_params.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 687efc49b3..850e5f4e22 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -248,7 +248,7 @@ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); * This parameter defines a pitch offset from the board rotation. It allows the user * to fine tune the board offset in the event of misalignment. */ - PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0); + PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f); /** * Board rotation roll offset @@ -256,7 +256,7 @@ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); * This parameter defines a roll offset from the board rotation. It allows the user * to fine tune the board offset in the event of misalignment. */ -PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0); +PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f); /** * Board rotation YAW offset @@ -264,7 +264,7 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0); * This parameter defines a yaw offset from the board rotation. It allows the user * to fine tune the board offset in the event of misalignment. */ -PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0); +PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f); /** * External magnetometer rotation From ebd68f4ffa2b6f3b6fdb1a19ef22b1aab54b4251 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Sun, 22 Jun 2014 16:43:05 +0800 Subject: [PATCH 7/7] Clarified parameter comments and added @group attributed for auto-generation. --- src/modules/sensors/sensor_params.c | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 850e5f4e22..c8a3ec8f03 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -243,26 +243,32 @@ PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); /** - * Board rotation pitch offset + * Board rotation Y (Pitch) offset * - * This parameter defines a pitch offset from the board rotation. It allows the user + * This parameter defines a rotational offset in degrees around the Y (Pitch) axis. It allows the user * to fine tune the board offset in the event of misalignment. + * + * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f); /** - * Board rotation roll offset + * Board rotation X (Roll) offset * - * This parameter defines a roll offset from the board rotation. It allows the user + * This parameter defines a rotational offset in degrees around the X (Roll) axis It allows the user * to fine tune the board offset in the event of misalignment. + * + * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f); /** - * Board rotation YAW offset + * Board rotation Z (YAW) offset * - * This parameter defines a yaw offset from the board rotation. It allows the user + * This parameter defines a rotational offset in degrees around the Z (Yaw) axis. It allows the user * to fine tune the board offset in the event of misalignment. + * + * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f);