Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-23 04:45:37 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

[ahrs] int_cmpl_quat refactor

Browse Source 
- rename ahrs_impl -> ahrs_icq
- ahrs_x functions are ahrs_icq_x
- pass imu data as args
- register function, might be removed again...
This commit is contained in:
Felix Ruess
2014-10-13 18:02:44 +02:00
parent 5f00d73ff2
commit 4693db1ec9
5 changed files with 194 additions and 170 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/settings/estimation/ahrs_int_cmpl_quat.xml
+5 -5
View File
@@ -4,11 +4,11 @@
<dl_settings>
<dl_settings NAME="AHRS">
<dl_setting var="ahrs_impl.gravity_heuristic_factor" min="0" step="1" max="50" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="g_heuristic" param="AHRS_GRAVITY_HEURISTIC_FACTOR"/>
<dl_setting var="ahrs_impl.accel_omega" min="0.02" step="0.02" max="0.2" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="acc_omega" param="AHRS_ACCEL_OMEGA" unit="rad/s" handler="SetAccelOmega"/>
<dl_setting var="ahrs_impl.accel_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="acc_zeta" param="AHRS_ACCEL_ZETA" handler="SetAccelZeta"/>
<dl_setting var="ahrs_impl.mag_omega" min="0.02" step="0.01" max="0.1" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="mag_omega" param="AHRS_MAG_OMEGA" unit="rad/s" handler="SetMagOmega"/>
<dl_setting var="ahrs_impl.mag_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="mag_zeta" param="AHRS_MAG_ZETA" handler="SetMagZeta"/>
<dl_setting var="ahrs_icq.gravity_heuristic_factor" min="0" step="1" max="50" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="g_heuristic" param="AHRS_GRAVITY_HEURISTIC_FACTOR"/>
<dl_setting var="ahrs_icq.accel_omega" min="0.02" step="0.02" max="0.2" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="acc_omega" param="AHRS_ACCEL_OMEGA" unit="rad/s" handler="SetAccelOmega"/>
<dl_setting var="ahrs_icq.accel_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="acc_zeta" param="AHRS_ACCEL_ZETA" handler="SetAccelZeta"/>
<dl_setting var="ahrs_icq.mag_omega" min="0.02" step="0.01" max="0.1" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="mag_omega" param="AHRS_MAG_OMEGA" unit="rad/s" handler="SetMagOmega"/>
<dl_setting var="ahrs_icq.mag_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="mag_zeta" param="AHRS_MAG_ZETA" handler="SetMagZeta"/>
</dl_settings>
</dl_settings>
sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c
+1 -1
View File
@@ -108,7 +108,7 @@ void ahrs_mlkf_init(struct OrientationReps* body_to_imu) {
}
bool_t ahrs_mlkf_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
struct Int32Vect3* lp_mag)
struct Int32Vect3* lp_mag)
{
/* Compute an initial orientation from accel and mag directly as quaternion */
sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h
+1 -1
View File
@@ -61,7 +61,7 @@ extern struct AhrsMlkf ahrs_mlkf;
extern void ahrs_mlkf_register(void);
extern void ahrs_mlkf_init(struct OrientationReps* body_to_imu);
extern bool_t ahrs_mlkf_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
struct Int32Vect3* lp_mag);
struct Int32Vect3* lp_mag);
extern void ahrs_mlkf_propagate(struct Int32Rates* gyro, float dt);
extern void ahrs_mlkf_update_accel(struct Int32Vect3* accel, float dt);
extern void ahrs_mlkf_update_mag(struct Int32Vect3* mag, float dt);
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
+161 -149
View File
File diff suppressed because it is too large Load Diff
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h
+26 -14
View File
@@ -90,49 +90,61 @@ struct AhrsIntCmplQuat {
/* internal counters for the gains */
uint16_t accel_cnt; ///< number of propagations since last accel update
uint16_t mag_cnt; ///< number of propagations since last mag update
struct OrientationReps* body_to_imu;
};
extern struct AhrsIntCmplQuat ahrs_impl;
extern struct AhrsIntCmplQuat ahrs_icq;
#define DefaultAhrsImpl ahrs_icq
extern void ahrs_icq_register(void);
extern void ahrs_icq_init(struct OrientationReps* body_to_imu);
extern bool_t ahrs_icq_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
struct Int32Vect3* lp_mag);
extern void ahrs_icq_propagate(struct Int32Rates* gyro, float dt);
extern void ahrs_icq_update_accel(struct Int32Vect3* accel, float dt);
extern void ahrs_icq_update_mag(struct Int32Vect3* mag, float dt);
extern void ahrs_icq_update_gps(void);
/** Update yaw based on a heading measurement.
* e.g. from GPS course
* @param heading Heading in body frame, radians (CW/north) with #INT32_ANGLE_FRAC
*/
void ahrs_update_heading(int32_t heading);
void ahrs_icq_update_heading(int32_t heading);
/** Hard reset yaw to a heading.
* Doesn't affect the bias.
* Sets ahrs_impl.heading_aligned to TRUE.
* Sets ahrs_icq.heading_aligned to TRUE.
* @param heading Heading in body frame, radians (CW/north) with #INT32_ANGLE_FRAC
*/
void ahrs_realign_heading(int32_t heading);
void ahrs_icq_realign_heading(int32_t heading);
/// update pre-computed inv_kp and inv_ki gains from acc_omega and acc_zeta
extern void ahrs_set_accel_gains(void);
extern void ahrs_icq_set_accel_gains(void);
static inline void ahrs_int_cmpl_quat_SetAccelOmega(float omega) {
ahrs_impl.accel_omega = omega;
ahrs_set_accel_gains();
ahrs_icq.accel_omega = omega;
ahrs_icq_set_accel_gains();
}
static inline void ahrs_int_cmpl_quat_SetAccelZeta(float zeta) {
ahrs_impl.accel_zeta = zeta;
ahrs_set_accel_gains();
ahrs_icq.accel_zeta = zeta;
ahrs_icq_set_accel_gains();
}
/// update pre-computed kp and ki gains from mag_omega and mag_zeta
extern void ahrs_set_mag_gains(void);
extern void ahrs_icq_set_mag_gains(void);
static inline void ahrs_int_cmpl_quat_SetMagOmega(float omega) {
ahrs_impl.mag_omega = omega;
ahrs_set_mag_gains();
ahrs_icq.mag_omega = omega;
ahrs_icq_set_mag_gains();
}
static inline void ahrs_int_cmpl_quat_SetMagZeta(float zeta) {
ahrs_impl.mag_zeta = zeta;
ahrs_set_mag_gains();
ahrs_icq.mag_zeta = zeta;
ahrs_icq_set_mag_gains();
}