Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 29 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)
{
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,
&(stateGetPositionEnu_i()->x),
&(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.x,
&carrot_up,
&guidance_h.sp.heading,
&carrot_heading,
&stabilization_cmd[COMMAND_THRUST],
&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_trim_att_integrator);
INT_EULERS_ZERO(guidance_h.rc_sp);
guidance_h.sp.heading = 0;
guidance_h.sp.heading_rate = 0;
FLOAT_EULERS_ZERO(guidance_h.rc_sp);
guidance_h.sp.heading = 0.0;
guidance_h.sp.heading_rate = 0.0;
guidance_h.gains.p = GUIDANCE_H_PGAIN;
guidance_h.gains.i = GUIDANCE_H_IGAIN;
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);
break;
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
read_rc_setpoint_speed_i(&guidance_h.sp.speed, in_flight);
/* enable x,y velocity setpoints */
@@ -334,9 +334,9 @@ void guidance_h_read_rc(bool 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, FALSE, FALSE);
stabilization_attitude_read_rc_setpoint_eulers_f(&guidance_h.rc_sp, in_flight, FALSE, FALSE);
} else {
INT_EULERS_ZERO(guidance_h.rc_sp);
FLOAT_EULERS_ZERO(guidance_h.rc_sp);
}
break;
case GUIDANCE_H_MODE_FLIP:
@@ -434,8 +434,8 @@ static void guidance_h_update_reference(void)
/* update heading setpoint from rate */
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;
INT32_ANGLE_NORMALIZE(guidance_h.sp.heading);
guidance_h.sp.heading += guidance_h.sp.heading_rate / PERIODIC_FREQUENCY;
FLOAT_ANGLE_NORMALIZE(guidance_h.sp.heading);
}
}
@@ -540,7 +540,7 @@ void guidance_h_hover_enter(void)
reset_guidance_reference_from_current_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;
}
@@ -555,6 +555,7 @@ void guidance_h_nav_enter(void)
reset_guidance_reference_from_current_position();
nav_heading = stateGetNedToBodyEulers_i()->psi;
guidance_h.sp.heading = stateGetNedToBodyEulers_f()->psi;
}
void guidance_h_from_nav(bool in_flight)
@@ -590,8 +591,8 @@ void guidance_h_from_nav(bool in_flight)
guidance_h_update_reference();
/* set psi command */
guidance_h.sp.heading = nav_heading;
INT32_ANGLE_NORMALIZE(guidance_h.sp.heading);
guidance_h.sp.heading = ANGLE_FLOAT_OF_BFP(nav_heading);
FLOAT_ANGLE_NORMALIZE(guidance_h.sp.heading);
#if GUIDANCE_INDI
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 */
guidance_h_traj_run(in_flight);
/* 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,
guidance_h.sp.heading);
heading_sp_i);
#endif
#endif
@@ -678,7 +680,8 @@ void guidance_h_guided_run(bool in_flight)
/* compute x,y earth commands */
guidance_h_traj_run(in_flight);
/* 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
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) {
ClearBit(guidance_h.sp.mask, 7);
guidance_h.sp.heading = ANGLE_BFP_OF_REAL(heading);
INT32_ANGLE_NORMALIZE(guidance_h.sp.heading);
guidance_h.sp.heading = heading;
FLOAT_ANGLE_NORMALIZE(guidance_h.sp.heading);
return true;
}
return false;
@@ -728,7 +731,7 @@ bool guidance_h_set_guided_heading_rate(float rate)
{
if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED) {
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 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_float.h"
#include "firmwares/rotorcraft/guidance/guidance_h_ref.h"
#include "generated/airframe.h"
@@ -69,8 +70,8 @@ struct HorizontalGuidanceSetpoint {
*/
struct Int32Vect2 pos;
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
int32_t heading_rate; ///< with #INT32_RATE_FRAC
float heading;
float heading_rate;
uint8_t mask; ///< bit 5: vx & vy, bit 6: vz, bit 7: vyaw
};
@@ -99,7 +100,7 @@ struct HorizontalGuidance {
struct HorizontalGuidanceSetpoint sp; ///< 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;
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 heading the desired heading [rad]
* @param heading_sp the desired heading [rad]
*
* 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
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);
//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
@@ -278,7 +278,7 @@ void guidance_indi_calcG(struct FloatMat33 *Gmat) {
*
* 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;
//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) {
/* get current heading setpoint */
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 */
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.h
+2 -2
View File
@@ -35,8 +35,8 @@
#include "math/pprz_algebra_float.h"
extern void guidance_indi_enter(void);
extern void guidance_indi_run(bool in_flight, int32_t heading);
extern void stabilization_attitude_set_setpoint_rp_quat_f(struct FloatEulers* indi_rp_cmd, 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, float heading);
extern float guidance_indi_thrust_specific_force_gain;
extern struct FloatVect3 euler_cmd;