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

[rotorcraft guidance] get rid of guidance_h_psi_sp

Browse Source 
* directly set final guidance_h_command_body.psi
  * in HOVER directly from RC psi setpoint
  * in NAV directly from nav_heading
* This should also fix a bug with changing psi setpoint
  when switching between HOVER and NAV mode.
This commit is contained in:
Felix Ruess
2012-05-22 18:32:12 +02:00
parent 9782d0a12b
commit 448e91c306
2 changed files with 14 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+14 -17
View File
@@ -39,7 +39,6 @@
uint8_t guidance_h_mode;
struct Int32Vect2 guidance_h_pos_sp;
int32_t guidance_h_psi_sp;
struct Int32Vect2 guidance_h_pos_ref;
struct Int32Vect2 guidance_h_speed_ref;
struct Int32Vect2 guidance_h_accel_ref;
@@ -95,7 +94,6 @@ static inline void guidance_h_nav_enter(void);
void guidance_h_init(void) {
guidance_h_mode = GUIDANCE_H_MODE_KILL;
guidance_h_psi_sp = 0;
INT_VECT2_ZERO(guidance_h_pos_sp);
INT_VECT2_ZERO(guidance_h_pos_err_sum);
INT_EULERS_ZERO(guidance_h_rc_sp);
@@ -165,7 +163,6 @@ void guidance_h_read_rc(bool_t in_flight) {
case GUIDANCE_H_MODE_NAV:
if (radio_control.status == RC_OK) {
stabilization_attitude_read_rc_setpoint_eulers(&guidance_h_rc_sp, in_flight);
guidance_h_rc_sp.psi = 0;
}
else {
INT_EULERS_ZERO(guidance_h_rc_sp);
@@ -195,7 +192,12 @@ void guidance_h_run(bool_t in_flight) {
case GUIDANCE_H_MODE_HOVER:
guidance_h_update_reference(FALSE);
/* set psi command */
guidance_h_command_body.psi = guidance_h_rc_sp.psi;
/* compute roll and pitch commands and set final attitude setpoint */
guidance_h_traj_run(in_flight);
stabilization_attitude_run(in_flight);
break;
@@ -206,6 +208,8 @@ void guidance_h_run(bool_t in_flight) {
if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) {
stab_att_sp_euler.phi = nav_roll;
stab_att_sp_euler.theta = nav_pitch;
/* FIXME: heading can't be set via attitude block yet, use current heading for now */
stab_att_sp_euler.psi = ahrs.ltp_to_body_euler.psi;
#ifdef STABILISATION_ATTITUDE_TYPE_QUAT
INT32_QUAT_OF_EULERS(stab_att_sp_quat, stab_att_sp_euler);
INT32_QUAT_WRAP_SHORTEST(stab_att_sp_quat);
@@ -219,8 +223,9 @@ void guidance_h_run(bool_t in_flight) {
#else
guidance_h_update_reference(FALSE);
#endif
guidance_h_psi_sp = nav_heading;
/* set psi command */
guidance_h_command_body.psi = nav_heading;
/* compute roll and pitch commands and set final attitude setpoint */
guidance_h_traj_run(in_flight);
}
stabilization_attitude_run(in_flight);
@@ -304,12 +309,10 @@ static inline void guidance_h_traj_run(bool_t in_flight) {
guidance_h_command_body.theta =
- ( c_psi * guidance_h_command_earth.x + s_psi * guidance_h_command_earth.y) >> INT32_TRIG_FRAC;
guidance_h_command_body.psi = guidance_h_psi_sp;
/* Add RC setpoint */
EULERS_ADD(guidance_h_command_body, guidance_h_rc_sp);
INT32_ANGLE_NORMALIZE(guidance_h_command_body.psi);
/* Add RC roll and pitch setpoints for emergency corrections */
guidance_h_command_body.phi += guidance_h_rc_sp.phi;
guidance_h_command_body.theta += guidance_h_rc_sp.theta;
/* Set attitude setpoint in eulers and as quaternion */
EULERS_COPY(stab_att_sp_euler, guidance_h_command_body);
@@ -343,13 +346,7 @@ static inline void guidance_h_nav_enter(void) {
/* reset psi reference, set psi setpoint to current psi */
reset_psi_ref_from_body();
guidance_h_rc_sp.psi = ahrs.ltp_to_body_euler.psi;
guidance_h_psi_sp = ahrs.ltp_to_body_euler.psi;
nav_heading = guidance_h_psi_sp;
/* set RC heading setpoint to zero,
* since that is added to guidance_h_psi_sp later */
guidance_h_rc_sp.psi = 0;
nav_heading = ahrs.ltp_to_body_euler.psi;
INT_VECT2_ZERO(guidance_h_pos_err_sum);
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
-1
View File
@@ -48,7 +48,6 @@ extern uint8_t guidance_h_mode;
*/
extern struct Int32Vect2 guidance_h_pos_sp;
extern int32_t guidance_h_psi_sp; ///< with #INT32_ANGLE_FRAC
extern struct Int32Vect2 guidance_h_pos_ref;
extern struct Int32Vect2 guidance_h_speed_ref;
extern struct Int32Vect2 guidance_h_accel_ref;