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 25 Packages Projects Releases Wiki Activity

[GUIDANCE] abs(c_route) and abs(s_route) instead of abs(route) in north and east route components calculation

Browse Source
This commit is contained in:
Loic Drumettaz
2013-04-04 11:01:31 +02:00
parent bf9e273974
commit 580cd4d8aa
1 changed files with 4 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c
+4 -3
View File
@@ -123,10 +123,11 @@ void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp) {
/* Compute route reference before saturation */ /* Compute route reference before saturation */
// use metric precision or values are too large // use metric precision or values are too large
INT32_ATAN2(route_ref, -pos_err.y, -pos_err.x); INT32_ATAN2(route_ref, -pos_err.y, -pos_err.x);
route_ref = abs(route_ref);
/* Compute North and East route components */ /* Compute North and East route components */
PPRZ_ITRIG_SIN(s_route_ref, route_ref); PPRZ_ITRIG_SIN(s_route_ref, route_ref);
PPRZ_ITRIG_COS(c_route_ref, route_ref); PPRZ_ITRIG_COS(c_route_ref, route_ref);
c_route_ref = abs(c_route_ref);
s_route_ref = abs(s_route_ref);
/* Compute maximum acceleration*/ /* Compute maximum acceleration*/
gh_max_accel_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, c_route_ref, INT32_TRIG_FRAC); gh_max_accel_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, c_route_ref, INT32_TRIG_FRAC);
gh_max_accel_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, s_route_ref, INT32_TRIG_FRAC); gh_max_accel_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, s_route_ref, INT32_TRIG_FRAC);
@@ -191,11 +192,11 @@ void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) {
/* Compute route reference before saturation */ /* Compute route reference before saturation */
// use metric precision or values are too large // use metric precision or values are too large
INT32_ATAN2(route_ref, -speed_sp.y, -speed_sp.x); INT32_ATAN2(route_ref, -speed_sp.y, -speed_sp.x);
route_ref = abs(route_ref);
/* Compute North and East route components */ /* Compute North and East route components */
PPRZ_ITRIG_SIN(s_route_ref, route_ref); PPRZ_ITRIG_SIN(s_route_ref, route_ref);
PPRZ_ITRIG_COS(c_route_ref, route_ref); PPRZ_ITRIG_COS(c_route_ref, route_ref);
c_route_ref = abs(c_route_ref);
s_route_ref = abs(s_route_ref);
/* Compute maximum acceleration*/ /* Compute maximum acceleration*/
gh_max_accel_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, c_route_ref, INT32_TRIG_FRAC); gh_max_accel_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, c_route_ref, INT32_TRIG_FRAC);
gh_max_accel_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, s_route_ref, INT32_TRIG_FRAC); gh_max_accel_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, s_route_ref, INT32_TRIG_FRAC);