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

guidance_h heading setpoint in float (#2051)

Browse Source
This commit is contained in:
Ewoud Smeur
2017-05-06 22:25:35 +02:00
committed by Michal Podhradsky
parent 0a65e14903
commit 86baffe98a
5 changed files with 32 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/autopilot_firmware.c
+2 -1
View File
@@ -121,6 +121,7 @@ static void send_energy(struct transport_tx *trans, struct link_device *dev)
static void send_fp(struct transport_tx *trans, struct link_device *dev) static void send_fp(struct transport_tx *trans, struct link_device *dev)
{ {
int32_t carrot_up = -guidance_v_z_sp; int32_t carrot_up = -guidance_v_z_sp;
int32_t carrot_heading = ANGLE_BFP_OF_REAL(guidance_h.sp.heading);
pprz_msg_send_ROTORCRAFT_FP(trans, dev, AC_ID, pprz_msg_send_ROTORCRAFT_FP(trans, dev, AC_ID,
&(stateGetPositionEnu_i()->x), &(stateGetPositionEnu_i()->x),
&(stateGetPositionEnu_i()->y), &(stateGetPositionEnu_i()->y),
@@ -134,7 +135,7 @@ static void send_fp(struct transport_tx *trans, struct link_device *dev)
&guidance_h.sp.pos.y, &guidance_h.sp.pos.y,
&guidance_h.sp.pos.x, &guidance_h.sp.pos.x,
&carrot_up, &carrot_up,
&guidance_h.sp.heading, &carrot_heading,
&stabilization_cmd[COMMAND_THRUST], &stabilization_cmd[COMMAND_THRUST],
&autopilot.flight_time); &autopilot.flight_time);
} }
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+19 -16
View File
@@ -173,9 +173,9 @@ void guidance_h_init(void)
INT_VECT2_ZERO(guidance_h.sp.pos); INT_VECT2_ZERO(guidance_h.sp.pos);
INT_VECT2_ZERO(guidance_h_trim_att_integrator); INT_VECT2_ZERO(guidance_h_trim_att_integrator);
INT_EULERS_ZERO(guidance_h.rc_sp); FLOAT_EULERS_ZERO(guidance_h.rc_sp);
guidance_h.sp.heading = 0; guidance_h.sp.heading = 0.0;
guidance_h.sp.heading_rate = 0; guidance_h.sp.heading_rate = 0.0;
guidance_h.gains.p = GUIDANCE_H_PGAIN; guidance_h.gains.p = GUIDANCE_H_PGAIN;
guidance_h.gains.i = GUIDANCE_H_IGAIN; guidance_h.gains.i = GUIDANCE_H_IGAIN;
guidance_h.gains.d = GUIDANCE_H_DGAIN; guidance_h.gains.d = GUIDANCE_H_DGAIN;
@@ -318,7 +318,7 @@ void guidance_h_read_rc(bool in_flight)
stabilization_attitude_read_rc(in_flight, FALSE, FALSE); stabilization_attitude_read_rc(in_flight, FALSE, FALSE);
break; break;
case GUIDANCE_H_MODE_HOVER: case GUIDANCE_H_MODE_HOVER:
stabilization_attitude_read_rc_setpoint_eulers(&guidance_h.rc_sp, in_flight, FALSE, FALSE); stabilization_attitude_read_rc_setpoint_eulers_f(&guidance_h.rc_sp, in_flight, FALSE, FALSE );
#if GUIDANCE_H_USE_SPEED_REF #if GUIDANCE_H_USE_SPEED_REF
read_rc_setpoint_speed_i(&guidance_h.sp.speed, in_flight); read_rc_setpoint_speed_i(&guidance_h.sp.speed, in_flight);
/* enable x,y velocity setpoints */ /* enable x,y velocity setpoints */
@@ -334,9 +334,9 @@ void guidance_h_read_rc(bool in_flight)
case GUIDANCE_H_MODE_NAV: case GUIDANCE_H_MODE_NAV:
if (radio_control.status == RC_OK) { if (radio_control.status == RC_OK) {
stabilization_attitude_read_rc_setpoint_eulers(&guidance_h.rc_sp, in_flight, FALSE, FALSE); stabilization_attitude_read_rc_setpoint_eulers_f(&guidance_h.rc_sp, in_flight, FALSE, FALSE);
} else { } else {
INT_EULERS_ZERO(guidance_h.rc_sp); FLOAT_EULERS_ZERO(guidance_h.rc_sp);
} }
break; break;
case GUIDANCE_H_MODE_FLIP: case GUIDANCE_H_MODE_FLIP:
@@ -434,8 +434,8 @@ static void guidance_h_update_reference(void)
/* update heading setpoint from rate */ /* update heading setpoint from rate */
if (bit_is_set(guidance_h.sp.mask, 7)) { if (bit_is_set(guidance_h.sp.mask, 7)) {
guidance_h.sp.heading += (guidance_h.sp.heading_rate >> (INT32_ANGLE_FRAC - INT32_RATE_FRAC)) / PERIODIC_FREQUENCY; guidance_h.sp.heading += guidance_h.sp.heading_rate / PERIODIC_FREQUENCY;
INT32_ANGLE_NORMALIZE(guidance_h.sp.heading); FLOAT_ANGLE_NORMALIZE(guidance_h.sp.heading);
} }
} }
@@ -540,7 +540,7 @@ void guidance_h_hover_enter(void)
reset_guidance_reference_from_current_position(); reset_guidance_reference_from_current_position();
/* set guidance to current heading and position */ /* set guidance to current heading and position */
guidance_h.rc_sp.psi = stateGetNedToBodyEulers_i()->psi; guidance_h.rc_sp.psi = stateGetNedToBodyEulers_f()->psi;
guidance_h.sp.heading = guidance_h.rc_sp.psi; guidance_h.sp.heading = guidance_h.rc_sp.psi;
} }
@@ -555,6 +555,7 @@ void guidance_h_nav_enter(void)
reset_guidance_reference_from_current_position(); reset_guidance_reference_from_current_position();
nav_heading = stateGetNedToBodyEulers_i()->psi; nav_heading = stateGetNedToBodyEulers_i()->psi;
guidance_h.sp.heading = stateGetNedToBodyEulers_f()->psi;
} }
void guidance_h_from_nav(bool in_flight) void guidance_h_from_nav(bool in_flight)
@@ -590,8 +591,8 @@ void guidance_h_from_nav(bool in_flight)
guidance_h_update_reference(); guidance_h_update_reference();
/* set psi command */ /* set psi command */
guidance_h.sp.heading = nav_heading; guidance_h.sp.heading = ANGLE_FLOAT_OF_BFP(nav_heading);
INT32_ANGLE_NORMALIZE(guidance_h.sp.heading); FLOAT_ANGLE_NORMALIZE(guidance_h.sp.heading);
#if GUIDANCE_INDI #if GUIDANCE_INDI
guidance_indi_run(in_flight, guidance_h.sp.heading); guidance_indi_run(in_flight, guidance_h.sp.heading);
@@ -599,8 +600,9 @@ void guidance_h_from_nav(bool in_flight)
/* compute x,y earth commands */ /* compute x,y earth commands */
guidance_h_traj_run(in_flight); guidance_h_traj_run(in_flight);
/* set final attitude setpoint */ /* set final attitude setpoint */
int32_t heading_sp_i = ANGLE_BFP_OF_REAL(guidance_h.sp.heading);
stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth, stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth,
guidance_h.sp.heading); heading_sp_i);
#endif #endif
#endif #endif
@@ -678,7 +680,8 @@ void guidance_h_guided_run(bool in_flight)
/* compute x,y earth commands */ /* compute x,y earth commands */
guidance_h_traj_run(in_flight); guidance_h_traj_run(in_flight);
/* set final attitude setpoint */ /* set final attitude setpoint */
stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth, guidance_h.sp.heading); int32_t heading_sp_i = ANGLE_BFP_OF_REAL(guidance_h.sp.heading);
stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth, heading_sp_i);
#endif #endif
stabilization_attitude_run(in_flight); stabilization_attitude_run(in_flight);
} }
@@ -698,8 +701,8 @@ bool guidance_h_set_guided_heading(float heading)
{ {
if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED) { if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED) {
ClearBit(guidance_h.sp.mask, 7); ClearBit(guidance_h.sp.mask, 7);
guidance_h.sp.heading = ANGLE_BFP_OF_REAL(heading); guidance_h.sp.heading = heading;
INT32_ANGLE_NORMALIZE(guidance_h.sp.heading); FLOAT_ANGLE_NORMALIZE(guidance_h.sp.heading);
return true; return true;
} }
return false; return false;
@@ -728,7 +731,7 @@ bool guidance_h_set_guided_heading_rate(float rate)
{ {
if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED) { if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED) {
SetBit(guidance_h.sp.mask, 7); SetBit(guidance_h.sp.mask, 7);
guidance_h.sp.heading_rate = RATE_BFP_OF_REAL(rate); guidance_h.sp.heading_rate = rate;
return true; return true;
} }
return false; return false;
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
+4 -3
View File
@@ -29,6 +29,7 @@
#include "math/pprz_algebra_int.h" #include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h"
#include "firmwares/rotorcraft/guidance/guidance_h_ref.h" #include "firmwares/rotorcraft/guidance/guidance_h_ref.h"
#include "generated/airframe.h" #include "generated/airframe.h"
@@ -69,8 +70,8 @@ struct HorizontalGuidanceSetpoint {
*/ */
struct Int32Vect2 pos; struct Int32Vect2 pos;
struct Int32Vect2 speed; ///< only used in HOVER mode if GUIDANCE_H_USE_SPEED_REF or in GUIDED mode struct Int32Vect2 speed; ///< only used in HOVER mode if GUIDANCE_H_USE_SPEED_REF or in GUIDED mode
int32_t heading; ///< with #INT32_ANGLE_FRAC float heading;
int32_t heading_rate; ///< with #INT32_RATE_FRAC float heading_rate;
uint8_t mask; ///< bit 5: vx & vy, bit 6: vz, bit 7: vyaw uint8_t mask; ///< bit 5: vx & vy, bit 6: vz, bit 7: vyaw
}; };
@@ -99,7 +100,7 @@ struct HorizontalGuidance {
struct HorizontalGuidanceSetpoint sp; ///< setpoints struct HorizontalGuidanceSetpoint sp; ///< setpoints
struct HorizontalGuidanceReference ref; ///< reference calculated from setpoints struct HorizontalGuidanceReference ref; ///< reference calculated from setpoints
struct Int32Eulers rc_sp; ///< with #INT32_ANGLE_FRAC struct FloatEulers rc_sp;
}; };
extern struct HorizontalGuidance guidance_h; extern struct HorizontalGuidance guidance_h;
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c
+5 -5
View File
@@ -122,11 +122,11 @@ void guidance_indi_enter(void) {
/** /**
* @param in_flight in flight boolean * @param in_flight in flight boolean
* @param heading the desired heading [rad] * @param heading_sp the desired heading [rad]
* *
* main indi guidance function * main indi guidance function
*/ */
void guidance_indi_run(bool in_flight, int32_t heading) { void guidance_indi_run(bool in_flight, float heading_sp) {
//filter accel to get rid of noise and filter attitude to synchronize with accel //filter accel to get rid of noise and filter attitude to synchronize with accel
guidance_indi_propagate_filters(); guidance_indi_propagate_filters();
@@ -209,7 +209,7 @@ void guidance_indi_run(bool in_flight, int32_t heading) {
Bound(guidance_euler_cmd.theta, -GUIDANCE_H_MAX_BANK, GUIDANCE_H_MAX_BANK); Bound(guidance_euler_cmd.theta, -GUIDANCE_H_MAX_BANK, GUIDANCE_H_MAX_BANK);
//set the quat setpoint with the calculated roll and pitch //set the quat setpoint with the calculated roll and pitch
stabilization_attitude_set_setpoint_rp_quat_f(&guidance_euler_cmd, in_flight, heading); stabilization_attitude_set_setpoint_rp_quat_f(&guidance_euler_cmd, in_flight, heading_sp);
} }
#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
@@ -278,7 +278,7 @@ void guidance_indi_calcG(struct FloatMat33 *Gmat) {
* *
* function that creates a quaternion from a roll, pitch and yaw setpoint * function that creates a quaternion from a roll, pitch and yaw setpoint
*/ */
void stabilization_attitude_set_setpoint_rp_quat_f(struct FloatEulers* indi_rp_cmd, bool in_flight, int32_t heading) void stabilization_attitude_set_setpoint_rp_quat_f(struct FloatEulers* indi_rp_cmd, bool in_flight, float heading)
{ {
struct FloatQuat q_rp_cmd; struct FloatQuat q_rp_cmd;
//this is a quaternion without yaw! add the desired yaw before you use it! //this is a quaternion without yaw! add the desired yaw before you use it!
@@ -300,7 +300,7 @@ void stabilization_attitude_set_setpoint_rp_quat_f(struct FloatEulers* indi_rp_c
if (in_flight) { if (in_flight) {
/* get current heading setpoint */ /* get current heading setpoint */
struct FloatQuat q_yaw_sp; struct FloatQuat q_yaw_sp;
float_quat_of_axis_angle(&q_yaw_sp, &zaxis, ANGLE_FLOAT_OF_BFP(heading)); float_quat_of_axis_angle(&q_yaw_sp, &zaxis, heading);
/* rotation between current yaw and yaw setpoint */ /* rotation between current yaw and yaw setpoint */
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.h
+2 -2
View File
@@ -35,8 +35,8 @@
#include "math/pprz_algebra_float.h" #include "math/pprz_algebra_float.h"
extern void guidance_indi_enter(void); extern void guidance_indi_enter(void);
extern void guidance_indi_run(bool in_flight, int32_t heading); extern void guidance_indi_run(bool in_flight, float heading_sp);
extern void stabilization_attitude_set_setpoint_rp_quat_f(struct FloatEulers* indi_rp_cmd, bool in_flight, int32_t heading); extern void stabilization_attitude_set_setpoint_rp_quat_f(struct FloatEulers* indi_rp_cmd, bool in_flight, float heading);
extern float guidance_indi_thrust_specific_force_gain; extern float guidance_indi_thrust_specific_force_gain;
extern struct FloatVect3 euler_cmd; extern struct FloatVect3 euler_cmd;