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

[math] algebra: remove some more type specific macros

Browse Source
This commit is contained in:
Felix Ruess
2014-09-11 13:59:04 +02:00
parent aa10dbe2a8
commit 1a8be652fc
13 changed files with 57 additions and 112 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/navigation.c
+5 -5
View File
@@ -155,8 +155,8 @@ void nav_init(void) {
nav_altitude = POS_BFP_OF_REAL(SECURITY_HEIGHT);
nav_flight_altitude = nav_altitude;
flight_altitude = SECURITY_ALT;
INT32_VECT3_COPY(navigation_target, waypoints[WP_HOME]);
INT32_VECT3_COPY(navigation_carrot, waypoints[WP_HOME]);
VECT3_COPY(navigation_target, waypoints[WP_HOME]);
VECT3_COPY(navigation_carrot, waypoints[WP_HOME]);
horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
vertical_mode = VERTICAL_MODE_ALT;
@@ -386,7 +386,7 @@ unit_t nav_reset_alt( void ) {
}
void nav_init_stage( void ) {
INT32_VECT3_COPY(nav_last_point, *stateGetPositionEnu_i());
VECT3_COPY(nav_last_point, *stateGetPositionEnu_i());
stage_time = 0;
nav_circle_radians = 0;
horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
@@ -419,7 +419,7 @@ void nav_move_waypoint_lla(uint8_t wp_id, struct LlaCoor_i* new_lla_pos) {
void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos) {
if (wp_id < nb_waypoint) {
INT32_VECT3_COPY(waypoints[wp_id],(*new_pos));
VECT3_COPY(waypoints[wp_id],(*new_pos));
DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &(new_pos->x),
&(new_pos->y), &(new_pos->z));
}
@@ -458,7 +458,7 @@ bool_t nav_is_in_flight(void) {
/** Home mode navigation */
void nav_home(void) {
horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
INT32_VECT3_COPY(navigation_target, waypoints[WP_HOME]);
VECT3_COPY(navigation_target, waypoints[WP_HOME]);
vertical_mode = VERTICAL_MODE_ALT;
nav_altitude = waypoints[WP_HOME].z;
sw/airborne/firmwares/rotorcraft/navigation.h
+1 -1
View File
@@ -124,7 +124,7 @@ extern bool_t nav_set_heading_current(void);
/*********** Navigation to waypoint *************************************/
#define NavGotoWaypoint(_wp) { \
horizontal_mode = HORIZONTAL_MODE_WAYPOINT; \
INT32_VECT3_COPY( navigation_target, waypoints[_wp]); \
VECT3_COPY(navigation_target, waypoints[_wp]); \
dist2_to_wp = get_dist2_to_waypoint(_wp); \
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
+1 -1
View File
@@ -315,7 +315,7 @@ void stabilization_attitude_run(bool_t enable_integrator) {
scaled_att_err.qz = att_err.qz / IERROR_SCALE;
FLOAT_QUAT_COMP(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err);
FLOAT_QUAT_NORMALIZE(new_sum_err);
FLOAT_QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err);
QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err);
FLOAT_EULERS_OF_QUAT(stabilization_att_sum_err, stabilization_att_sum_err_quat);
} else {
/* reset accumulator */
sw/airborne/math/pprz_algebra.h
+5 -1
View File
@@ -397,7 +397,11 @@ extern "C" {
if ((_v).r > (_v_max).r) (_v).r = (_v_max).r; else if ((_v).r < (_v_min).r) (_v).r = (_v_min).r; \
}
#define RATES_ADD_SCALED_VECT(_ro, _v, _s) { \
(_ro).p += (_v).x * (_s); \
(_ro).q += (_v).y * (_s); \
(_ro).r += (_v).z * (_s); \
}
//
//
sw/airborne/math/pprz_algebra_float.h
+2 -26
View File
@@ -142,13 +142,6 @@ struct FloatRates {
#define FLOAT_RATES_NORM(_v) (sqrtf((_v).p*(_v).p + (_v).q*(_v).q + (_v).r*(_v).r))
#define FLOAT_RATES_ADD_SCALED_VECT(_ro, _v, _s) { \
_ro.p += _v.x * _s; \
_ro.q += _v.y * _s; \
_ro.r += _v.z * _s; \
}
#define FLOAT_RATES_LIN_CMB(_ro, _r1, _s1, _r2, _s2) { \
_ro.p = _s1 * _r1.p + _s2 * _r2.p; \
_ro.q = _s1 * _r1.q + _s2 * _r2.q; \
@@ -285,23 +278,6 @@ extern float float_rmat_reorthogonalize(struct FloatRMat* rm);
#define FLOAT_QUAT_ZERO(_q) QUAT_ASSIGN(_q, 1., 0., 0., 0.)
#define FLOAT_QUAT_ASSIGN(_qi, _i, _x, _y, _z) QUAT_ASSIGN(_qi, _i, _x, _y, _z)
/* _q += _qa */
#define FLOAT_QUAT_ADD(_qo, _qi) QUAT_ADD(_qo, _qi)
/* _qo = _qi * _s */
#define FLOAT_QUAT_SMUL(_qo, _qi, _s) QUAT_SMUL(_qo, _qi, _s)
/* _qo = _qo / _s */
#define FLOAT_QUAT_SDIV( _qo, _qi, _s) QUAT_SDIV(_qo, _qi, _s)
/* */
#define FLOAT_QUAT_EXPLEMENTARY(b,a) QUAT_EXPLEMENTARY(b,a)
/* */
#define FLOAT_QUAT_COPY(_qo, _qi) QUAT_COPY(_qo, _qi)
#define FLOAT_QUAT_NORM(_q) float_quat_norm(&(_q))
#define FLOAT_QUAT_NORMALIZE(_q) float_quat_normalize(&(_q))
@@ -407,7 +383,7 @@ static inline void float_quat_wrap_shortest(struct FloatQuat* q)
VECT3_CROSS_PRODUCT(v1, _vi, quat3); \
VECT3_SMUL(v2, _vi, (_qi.qi)); \
VECT3_ADD(v2, v1); \
FLOAT_QUAT_ASSIGN(_mright, qi, v2.x, v2.y, v2.z);\
QUAT_ASSIGN(_mright, qi, v2.x, v2.y, v2.z);\
}
@@ -426,7 +402,7 @@ static inline void float_quat_wrap_shortest(struct FloatQuat* q)
VECT3_CROSS_PRODUCT(v1, quat3, _vi); \
VECT3_SMUL(v2, _vi, (_qi.qi)); \
VECT3_ADD(v2, v1); \
FLOAT_QUAT_ASSIGN(_mleft, qi, v2.x, v2.y, v2.z);\
QUAT_ASSIGN(_mleft, qi, v2.x, v2.y, v2.z);\
}
sw/airborne/math/pprz_algebra_int.h
+8 -43
View File
@@ -231,8 +231,6 @@ extern int32_t int32_sqrt(int32_t in);
#define INT_VECT2_ZERO(_v) VECT2_ASSIGN(_v, 0, 0)
#define INT_VECT2_ASSIGN(_a, _x, _y) VECT2_ASSIGN(_a, _x, _y)
#define INT32_VECT2_NORM(n, v) { \
int32_t n2 = (v).x*(v).x + (v).y*(v).y; \
n = int32_sqrt(n2); \
@@ -267,26 +265,12 @@ extern int32_t int32_sqrt(int32_t in);
#define INT_VECT3_ZERO(_v) VECT3_ASSIGN(_v, 0, 0, 0)
#define INT32_VECT3_ZERO(_v) VECT3_ASSIGN(_v, 0, 0, 0)
#define INT_VECT3_ASSIGN(_a, _x, _y, _z) VECT3_ASSIGN(_a, _x, _y, _z)
#define INT32_VECT3_ASSIGN(_a, _x, _y, _z) VECT3_ASSIGN(_a, _x, _y, _z)
#define INT32_VECT3_COPY(_o, _i) VECT3_COPY(_o, _i)
#define INT32_VECT3_SUM(_c, _a, _b) VECT3_SUM(_c, _a, _b)
#define INT32_VECT3_DIFF(_c, _a, _b) VECT3_DIFF(_c, _a, _b)
#define INT32_VECT3_ADD(_a, _b) VECT3_ADD(_a, _b)
#define INT32_VECT3_SCALE_2(_a, _b, _num, _den) { \
(_a).x = ((_b).x * (_num)) / (_den); \
(_a).y = ((_b).y * (_num)) / (_den); \
(_a).z = ((_b).z * (_num)) / (_den); \
}
#define INT32_VECT3_SDIV(_a, _b, _s) VECT3_SDIV(_a, _b, _s)
#define INT32_VECT3_NORM(n, v) { \
int32_t n2 = (v).x*(v).x + (v).y*(v).y + (v).z*(v).z; \
n = int32_sqrt(n2); \
@@ -304,12 +288,6 @@ extern int32_t int32_sqrt(int32_t in);
(_o).z = ((_i).z << (_l)); \
}
#define INT32_VECT3_CROSS_PRODUCT(_vo, _v1, _v2) { \
(_vo).x = (_v1).y*(_v2).z - (_v1).z*(_v2).y; \
(_vo).y = (_v1).z*(_v2).x - (_v1).x*(_v2).z; \
(_vo).z = (_v1).x*(_v2).y - (_v1).y*(_v2).x; \
}
/*
@@ -415,15 +393,13 @@ extern void int32_rmat_of_eulers_312(struct Int32RMat* rm, struct Int32Eulers* e
*
*/
#define INT32_QUAT_ZERO(_q) { \
(_q).qi = QUAT1_BFP_OF_REAL(1); \
(_q).qx = 0; \
(_q).qy = 0; \
(_q).qz = 0; \
}
#define INT32_QUAT_INVERT(_qo, _qi) QUAT_INVERT(_qo, _qi)
static inline void int32_quat_zero(struct Int32Quat* q)
{
q->qi = QUAT1_BFP_OF_REAL(1);
q->qx = 0;
q->qy = 0;
q->qz = 0;
}
static inline int32_t int32_quat_norm(struct Int32Quat* q)
{
@@ -482,6 +458,7 @@ extern void int32_quat_of_axis_angle(struct Int32Quat* q, struct Int32Vect3* uv,
extern void int32_quat_of_rmat(struct Int32Quat* q, struct Int32RMat* r);
/* defines for backwards compatibility */
#define INT32_QUAT_ZERO(_q) int32_quat_zero(&(_q))
#define INT32_QUAT_NORM(n, q) { n = int32_quat_norm(&(q)); }
#define INT32_QUAT_WRAP_SHORTEST(q) int32_quat_wrap_shortest(&(q))
#define INT32_QUAT_NORMALIZE(q) int32_quat_normalize(&(q))
@@ -532,18 +509,6 @@ extern void int32_eulers_of_quat(struct Int32Eulers* e, struct Int32Quat* q);
#define INT_RATES_ZERO(_e) RATES_ASSIGN(_e, 0, 0, 0)
#define INT_RATES_ADD_SCALED_VECT(_ro, _v, _s) { \
_ro.p += _v.x * _s; \
_ro.q += _v.y * _s; \
_ro.r += _v.z * _s; \
}
#define INT_RATES_SDIV(_ro, _s, _ri) { \
_ro.p = _ri.p / _s; \
_ro.q = _ri.q / _s; \
_ro.r = _ri.r / _s; \
}
#define INT_RATES_RSHIFT(_o, _i, _r) { \
(_o).p = ((_i).p >> (_r)); \
(_o).q = ((_i).q >> (_r)); \
sw/airborne/math/pprz_geodetic_int.c
+2 -2
View File
@@ -238,7 +238,7 @@ void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct Ne
void ecef_of_enu_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu)
{
ecef_of_enu_vect_i(ecef, def, enu);
INT32_VECT3_ADD(*ecef, def->ecef);
VECT3_ADD(*ecef, def->ecef);
}
@@ -271,7 +271,7 @@ void ecef_of_enu_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct Enu
VECT3_SMUL(enu_cm, *enu, 25);
INT32_VECT3_RSHIFT(enu_cm, enu_cm, INT32_POS_FRAC - 2);
ecef_of_enu_vect_i(ecef, def, &enu_cm);
INT32_VECT3_ADD(*ecef, def->ecef);
VECT3_ADD(*ecef, def->ecef);
}
sw/airborne/modules/mission/mission_rotorcraft_nav.c
+1 -1
View File
@@ -122,7 +122,7 @@ static inline bool_t mission_nav_wp(struct _mission_element * el) {
}
//Go to Mission Waypoint
horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
INT32_VECT3_COPY(navigation_target, *target_wp);
VECT3_COPY(navigation_target, *target_wp);
NavVerticalAutoThrottleMode(RadOfDeg(0.000000));
NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(target_wp->z), 0.);
sw/airborne/modules/multi/follow.c
+1 -1
View File
@@ -72,5 +72,5 @@ void follow_change_wp( unsigned char* buffer ) {
// TODO: Remove the angle to the north
// Move the waypoint
INT32_VECT3_COPY(waypoints[FOLLOW_WAYPOINT_ID], enu);
VECT3_COPY(waypoints[FOLLOW_WAYPOINT_ID], enu);
}
sw/airborne/state.h
+14 -14
View File
@@ -499,21 +499,21 @@ static inline bool_t stateIsGlobalCoordinateValid(void) {
/// Set position from ECEF coordinates (int).
static inline void stateSetPositionEcef_i(struct EcefCoor_i* ecef_pos) {
INT32_VECT3_COPY(state.ecef_pos_i, *ecef_pos);
VECT3_COPY(state.ecef_pos_i, *ecef_pos);
/* clear bits for all position representations and only set the new one */
state.pos_status = (1 << POS_ECEF_I);
}
/// Set position from local NED coordinates (int).
static inline void stateSetPositionNed_i(struct NedCoor_i* ned_pos) {
INT32_VECT3_COPY(state.ned_pos_i, *ned_pos);
VECT3_COPY(state.ned_pos_i, *ned_pos);
/* clear bits for all position representations and only set the new one */
state.pos_status = (1 << POS_NED_I);
}
/// Set position from local ENU coordinates (int).
static inline void stateSetPositionEnu_i(struct EnuCoor_i* enu_pos) {
INT32_VECT3_COPY(state.enu_pos_i, *enu_pos);
VECT3_COPY(state.enu_pos_i, *enu_pos);
/* clear bits for all position representations and only set the new one */
state.pos_status = (1 << POS_ENU_I);
}
@@ -534,15 +534,15 @@ static inline void stateSetPosition_i(
/* clear all status bit */
state.pos_status = 0;
if (ecef_pos != NULL) {
INT32_VECT3_COPY(state.ecef_pos_i, *ecef_pos);
VECT3_COPY(state.ecef_pos_i, *ecef_pos);
state.pos_status |= (1 << POS_ECEF_I);
}
if (ned_pos != NULL) {
INT32_VECT3_COPY(state.ned_pos_i, *ned_pos);
VECT3_COPY(state.ned_pos_i, *ned_pos);
state.pos_status |= (1 << POS_NED_I);
}
if (enu_pos != NULL) {
INT32_VECT3_COPY(state.enu_pos_i, *enu_pos);
VECT3_COPY(state.enu_pos_i, *enu_pos);
state.pos_status |= (1 << POS_ENU_I);
}
if (lla_pos != NULL) {
@@ -710,21 +710,21 @@ extern void stateCalcHorizontalSpeedDir_f(void);
/// Set ground speed in local NED coordinates (int).
static inline void stateSetSpeedNed_i(struct NedCoor_i* ned_speed) {
INT32_VECT3_COPY(state.ned_speed_i, *ned_speed);
VECT3_COPY(state.ned_speed_i, *ned_speed);
/* clear bits for all speed representations and only set the new one */
state.speed_status = (1 << SPEED_NED_I);
}
/// Set ground speed in local ENU coordinates (int).
static inline void stateSetSpeedEnu_i(struct EnuCoor_i* enu_speed) {
INT32_VECT3_COPY(state.enu_speed_i, *enu_speed);
VECT3_COPY(state.enu_speed_i, *enu_speed);
/* clear bits for all speed representations and only set the new one */
state.speed_status = (1 << SPEED_ENU_I);
}
/// Set ground speed in ECEF coordinates (int).
static inline void stateSetSpeedEcef_i(struct EcefCoor_i* ecef_speed) {
INT32_VECT3_COPY(state.ecef_speed_i, *ecef_speed);
VECT3_COPY(state.ecef_speed_i, *ecef_speed);
/* clear bits for all speed representations and only set the new one */
state.speed_status = (1 << SPEED_ECEF_I);
}
@@ -737,15 +737,15 @@ static inline void stateSetSpeed_i(
/* clear all status bit */
state.speed_status = 0;
if (ecef_speed != NULL) {
INT32_VECT3_COPY(state.ecef_speed_i, *ecef_speed);
VECT3_COPY(state.ecef_speed_i, *ecef_speed);
state.speed_status |= (1 << SPEED_ECEF_I);
}
if (ned_speed != NULL) {
INT32_VECT3_COPY(state.ned_speed_i, *ned_speed);
VECT3_COPY(state.ned_speed_i, *ned_speed);
state.speed_status |= (1 << SPEED_NED_I);
}
if (enu_speed != NULL) {
INT32_VECT3_COPY(state.enu_speed_i, *enu_speed);
VECT3_COPY(state.enu_speed_i, *enu_speed);
state.speed_status |= (1 << SPEED_ENU_I);
}
}
@@ -892,14 +892,14 @@ static inline bool_t stateIsAccelValid(void) {
/// Set acceleration in NED coordinates (int).
static inline void stateSetAccelNed_i(struct NedCoor_i* ned_accel) {
INT32_VECT3_COPY(state.ned_accel_i, *ned_accel);
VECT3_COPY(state.ned_accel_i, *ned_accel);
/* clear bits for all accel representations and only set the new one */
state.accel_status = (1 << ACCEL_NED_I);
}
/// Set acceleration in ECEF coordinates (int).
static inline void stateSetAccelEcef_i(struct EcefCoor_i* ecef_accel) {
INT32_VECT3_COPY(state.ecef_accel_i, *ecef_accel);
VECT3_COPY(state.ecef_accel_i, *ecef_accel);
/* clear bits for all accel representations and only set the new one */
state.accel_status = (1 << ACCEL_ECEF_I);
}
sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c
+10 -10
View File
@@ -330,7 +330,7 @@ void ahrs_update_accel(void) {
*/
const float gravity_rate_update_gain = -2 * ahrs_impl.accel_zeta * ahrs_impl.accel_omega *
ahrs_impl.weight * AHRS_PROPAGATE_FREQUENCY / (AHRS_CORRECT_FREQUENCY * 9.81);
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, gravity_rate_update_gain);
RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, gravity_rate_update_gain);
/* Complementary filter integral gain
* Correct the gyro bias.
@@ -338,7 +338,7 @@ void ahrs_update_accel(void) {
*/
const float gravity_bias_update_gain = ahrs_impl.accel_omega * ahrs_impl.accel_omega *
ahrs_impl.weight * ahrs_impl.weight / (AHRS_CORRECT_FREQUENCY * 9.81);
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, gravity_bias_update_gain);
RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, gravity_bias_update_gain);
/* FIXME: saturate bias */
@@ -373,7 +373,7 @@ void ahrs_update_mag_full(void) {
const float mag_rate_update_gain = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega *
AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain);
RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain);
/* Complementary filter integral gain
* Correct the gyro bias.
@@ -381,7 +381,7 @@ void ahrs_update_mag_full(void) {
*/
const float mag_bias_update_gain = -(ahrs_impl.mag_omega * ahrs_impl.mag_omega) /
AHRS_MAG_CORRECT_FREQUENCY;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain);
RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain);
}
@@ -418,7 +418,7 @@ void ahrs_update_mag_2d(void) {
*/
const float mag_rate_update_gain = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega *
AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain);
RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain);
/* Complementary filter integral gain
* Correct the gyro bias.
@@ -426,7 +426,7 @@ void ahrs_update_mag_2d(void) {
*/
const float mag_bias_update_gain = -(ahrs_impl.mag_omega * ahrs_impl.mag_omega) /
AHRS_MAG_CORRECT_FREQUENCY;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain);
RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain);
}
@@ -450,9 +450,9 @@ void ahrs_update_mag_2d_dumb(void) {
RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,1),
RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,2)};
const float mag_rate_update_gain = 2.5;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, r2, (mag_rate_update_gain*res_norm));
RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, r2, (mag_rate_update_gain*res_norm));
const float mag_bias_update_gain = -2.5e-4;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, r2, (mag_bias_update_gain*res_norm));
RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, r2, (mag_bias_update_gain*res_norm));
}
@@ -509,7 +509,7 @@ void ahrs_update_heading(float heading) {
FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_impl.ltp_to_imu_rmat, residual_ltp);
const float heading_rate_update_gain = 2.5;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, heading_rate_update_gain);
RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, heading_rate_update_gain);
float heading_bias_update_gain;
/* crude attempt to only update bias if deviation is small
@@ -522,7 +522,7 @@ void ahrs_update_heading(float heading) {
heading_bias_update_gain = -2.5e-4;
else
heading_bias_update_gain = 0.0;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, heading_bias_update_gain);
RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, heading_bias_update_gain);
}
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
+3 -3
View File
@@ -349,13 +349,13 @@ void ahrs_update_accel(void) {
/* and subtract it from imu measurement to get a corrected measurement
* of the gravity vector */
INT32_VECT3_DIFF(pseudo_gravity_measurement, imu.accel, acc_c_imu);
VECT3_DIFF(pseudo_gravity_measurement, imu.accel, acc_c_imu);
} else {
VECT3_COPY(pseudo_gravity_measurement, imu.accel);
}
/* compute the residual of the pseudo gravity vector in imu frame */
INT32_VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2);
VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2);
/* FIR filtered pseudo_gravity_measurement */
@@ -460,7 +460,7 @@ static inline void ahrs_update_mag_full(void) {
INT32_RMAT_VMULT(expected_imu, ltp_to_imu_rmat, ahrs_impl.mag_h);
struct Int32Vect3 residual;
INT32_VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu);
VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu);
/* Complementary filter proportionnal gain.
* Kp = 2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY
sw/airborne/subsystems/ins/ins_float_invariant.c
+4 -4
View File
@@ -593,15 +593,15 @@ static inline void invariant_model(float * o, const float * x, const int n, cons
RATES_DIFF(rates, c->rates, s->bias);
VECT3_ASSIGN(tmp_vect, rates.p, rates.q, rates.r);
FLOAT_QUAT_VMUL_LEFT(s_dot.quat, s->quat, tmp_vect);
FLOAT_QUAT_SMUL(s_dot.quat, s_dot.quat, 0.5);
QUAT_SMUL(s_dot.quat, s_dot.quat, 0.5);
FLOAT_QUAT_VMUL_RIGHT(tmp_quat, s->quat, ins_impl.corr.LE);
FLOAT_QUAT_ADD(s_dot.quat, tmp_quat);
QUAT_ADD(s_dot.quat, tmp_quat);
norm = FLOAT_QUAT_NORM(s->quat);
norm = 1. - (norm*norm);
FLOAT_QUAT_SMUL(tmp_quat, s->quat, norm);
FLOAT_QUAT_ADD(s_dot.quat, tmp_quat);
QUAT_SMUL(tmp_quat, s->quat, norm);
QUAT_ADD(s_dot.quat, tmp_quat);
/* dot_V = A + (1/as) * (q * am * q-1) + ME */
FLOAT_QUAT_RMAT_B2N(s_dot.speed, s->quat, c->accel);