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

[rotorcraft] horizontal guidance fixedpoint fixes

Browse Source 
Significantly reduce the quantization error of the horizontal guidance cmds.
This should result in nicer position hold and better trajectory tracking.
Verified in NPS sim.
This commit is contained in:
Felix Ruess
2013-03-25 13:38:13 +01:00
parent d1a78516d1
commit 68d07e6128
4 changed files with 15 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+8 -8
View File
@@ -288,15 +288,15 @@ static inline void guidance_h_traj_run(bool_t in_flight) {
/* run PID */ /* run PID */
guidance_h_command_earth.x = guidance_h_command_earth.x =
guidance_h_pgain * (guidance_h_pos_err.x >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + ((guidance_h_pgain * guidance_h_pos_err.x) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) +
guidance_h_dgain * (guidance_h_speed_err.x >> (INT32_SPEED_FRAC - GH_GAIN_SCALE)) + ((guidance_h_dgain * (guidance_h_speed_err.x >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2)) +
guidance_h_igain * (guidance_h_pos_err_sum.x >> (12 + INT32_POS_FRAC - GH_GAIN_SCALE)) + ((guidance_h_igain * (guidance_h_pos_err_sum.x >> 12)) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) +
guidance_h_again * (guidance_h_accel_ref.x >> 8); /* feedforward gain */ ((guidance_h_again * guidance_h_accel_ref.x) >> 8); /* feedforward gain */
guidance_h_command_earth.y = guidance_h_command_earth.y =
guidance_h_pgain * (guidance_h_pos_err.y >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + ((guidance_h_pgain * guidance_h_pos_err.y) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) +
guidance_h_dgain * (guidance_h_speed_err.y >> (INT32_SPEED_FRAC - GH_GAIN_SCALE)) + ((guidance_h_dgain * (guidance_h_speed_err.y >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2)) +
guidance_h_igain * (guidance_h_pos_err_sum.y >> (12 + INT32_POS_FRAC - GH_GAIN_SCALE)) + ((guidance_h_igain * (guidance_h_pos_err_sum.y >> 12)) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) +
guidance_h_again * (guidance_h_accel_ref.y >> 8); /* feedforward gain */ ((guidance_h_again * guidance_h_accel_ref.y) >> 8); /* feedforward gain */
VECT2_STRIM(guidance_h_command_earth, -TRAJ_MAX_BANK, TRAJ_MAX_BANK); VECT2_STRIM(guidance_h_command_earth, -TRAJ_MAX_BANK, TRAJ_MAX_BANK);
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
+3 -3
View File
@@ -49,9 +49,9 @@ extern uint8_t guidance_h_mode;
*/ */
extern struct Int32Vect2 guidance_h_pos_sp; extern struct Int32Vect2 guidance_h_pos_sp;
extern struct Int32Vect2 guidance_h_pos_ref; extern struct Int32Vect2 guidance_h_pos_ref; ///< with #INT32_POS_FRAC
extern struct Int32Vect2 guidance_h_speed_ref; extern struct Int32Vect2 guidance_h_speed_ref; ///< with #INT32_SPEED_FRAC
extern struct Int32Vect2 guidance_h_accel_ref; extern struct Int32Vect2 guidance_h_accel_ref; ///< with #INT32_ACCEL_FRAC
extern struct Int32Vect2 guidance_h_pos_err; extern struct Int32Vect2 guidance_h_pos_err;
extern struct Int32Vect2 guidance_h_speed_err; extern struct Int32Vect2 guidance_h_speed_err;
sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h
+2 -1
View File
@@ -54,7 +54,8 @@ extern struct Int64Vect2 b2_gh_pos_ref;
/* Saturations definition */ /* Saturations definition */
#ifndef GUIDANCE_H_REF_MAX_ACCEL #ifndef GUIDANCE_H_REF_MAX_ACCEL
#define GUIDANCE_H_REF_MAX_ACCEL ( tanf(RadOfDeg(30.))*9.81 ) /* tanf(RadOfDeg(30.))*9.81 = 5.66 */
#define GUIDANCE_H_REF_MAX_ACCEL 5.66
#endif #endif
#define B2_GH_MAX_ACCEL BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, B2_GH_ACCEL_REF_FRAC) #define B2_GH_MAX_ACCEL BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, B2_GH_ACCEL_REF_FRAC)
sw/airborne/firmwares/rotorcraft/navigation.c
+2
View File
@@ -123,6 +123,7 @@ void nav_run(void) {
VECT2_STRIM(path_to_waypoint, -(1<<15), (1<<15)); VECT2_STRIM(path_to_waypoint, -(1<<15), (1<<15));
#if !GUIDANCE_H_USE_REF #if !GUIDANCE_H_USE_REF
PRINT_CONFIG_MSG("NOT using horizontal guidance reference :-(")
int32_t dist_to_waypoint; int32_t dist_to_waypoint;
INT32_VECT2_NORM(dist_to_waypoint, path_to_waypoint); INT32_VECT2_NORM(dist_to_waypoint, path_to_waypoint);
@@ -136,6 +137,7 @@ void nav_run(void) {
VECT2_SUM(navigation_carrot, path_to_carrot, *stateGetPositionEnu_i()); VECT2_SUM(navigation_carrot, path_to_carrot, *stateGetPositionEnu_i());
} }
#else #else
PRINT_CONFIG_MSG("Using horizontal guidance reference :-)")
// if H_REF is used, CARROT_DIST is not used // if H_REF is used, CARROT_DIST is not used
VECT2_COPY(navigation_carrot, navigation_target); VECT2_COPY(navigation_carrot, navigation_target);
#endif #endif