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

[rotorcraft] guidance_h: use shift defines instead of hardcoded numbers

Browse Source 
and fixed some typos

closes part of #1634
This commit is contained in:
kirkscheper
2016-04-24 14:47:26 +02:00
committed by Felix Ruess
parent 731adc38b9
commit 0f53c6f270
3 changed files with 9 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+7 -7
View File
@@ -515,11 +515,11 @@ static void guidance_h_traj_run(bool in_flight)
((guidance_h.gains.p * guidance_h_pos_err.y) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) +
((guidance_h.gains.d * (guidance_h_speed_err.y >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2));
guidance_h_cmd_earth.x = pd_x +
((guidance_h.gains.v * guidance_h.ref.speed.x) >> 17) + /* speed feedforward gain */
((guidance_h.gains.a * guidance_h.ref.accel.x) >> 8); /* acceleration feedforward gain */
((guidance_h.gains.v * guidance_h.ref.speed.x) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE)) + /* speed feedforward gain */
((guidance_h.gains.a * guidance_h.ref.accel.x) >> (INT32_ACCEL_FRAC - GH_GAIN_SCALE)); /* acceleration feedforward gain */
guidance_h_cmd_earth.y = pd_y +
((guidance_h.gains.v * guidance_h.ref.speed.y) >> 17) + /* speed feedforward gain */
((guidance_h.gains.a * guidance_h.ref.accel.y) >> 8); /* acceleration feedforward gain */
((guidance_h.gains.v * guidance_h.ref.speed.y) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE)) + /* speed feedforward gain */
((guidance_h.gains.a * guidance_h.ref.accel.y) >> (INT32_ACCEL_FRAC - GH_GAIN_SCALE)); /* acceleration feedforward gain */
/* trim max bank angle from PD */
VECT2_STRIM(guidance_h_cmd_earth, -traj_max_bank, traj_max_bank);
@@ -533,10 +533,10 @@ static void guidance_h_traj_run(bool in_flight)
guidance_h_trim_att_integrator.x += (guidance_h.gains.i * pd_x);
guidance_h_trim_att_integrator.y += (guidance_h.gains.i * pd_y);
/* saturate it */
VECT2_STRIM(guidance_h_trim_att_integrator, -(traj_max_bank << 16), (traj_max_bank << 16));
VECT2_STRIM(guidance_h_trim_att_integrator, -(traj_max_bank << (INT32_ANGLE_FRAC + GH_GAIN_SCALE*2)), (traj_max_bank << (INT32_ANGLE_FRAC + GH_GAIN_SCALE*2)));
/* add it to the command */
guidance_h_cmd_earth.x += (guidance_h_trim_att_integrator.x >> 16);
guidance_h_cmd_earth.y += (guidance_h_trim_att_integrator.y >> 16);
guidance_h_cmd_earth.x += (guidance_h_trim_att_integrator.x >> (INT32_ANGLE_FRAC + GH_GAIN_SCALE*2));
guidance_h_cmd_earth.y += (guidance_h_trim_att_integrator.y >> (INT32_ANGLE_FRAC + GH_GAIN_SCALE*2));
} else {
INT_VECT2_ZERO(guidance_h_trim_att_integrator);
}
sw/airborne/firmwares/rotorcraft/navigation.c
+1 -1
View File
@@ -504,7 +504,7 @@ void compute_dist2_to_home(void)
too_far_from_home = dist2_to_home > max_dist2_from_home;
}
/** Set nav_heading in degrees. */
/** Set nav_heading in radians. */
bool nav_set_heading_rad(float rad)
{
nav_heading = ANGLE_BFP_OF_REAL(rad);
sw/airborne/subsystems/ins.h
+1 -1
View File
@@ -59,7 +59,7 @@ extern void ins_reset_local_origin(void);
extern void ins_reset_altitude_ref(void);
/** INS utm zone reset.
* Reset UTM zone according te the actual position.
* Reset UTM zone according the the actual position.
* Only used with fixedwing firmware.
* Can be overwritte by specifc INS implementation.
* @param utm initial utm zone, returns the corrected utm position