Skip to content

Commit

Permalink
[guidance] Expansion of the guided protocol (#1694)
Browse files Browse the repository at this point in the history 
Updates the GUIDED_SETPOINT_NED message to set the reference frame of each input separately and adds a heading_rate setpoint.

Frame can be specified with the bits 0-3
Velocity of position setpoint can be specified with the bits 5-7
Flags field definition:
- bit 0: x,y as offset coordinates
- bit 1: x,y in body coordinates
- bit 2: z as offset coordinates
- bit 3: yaw as offset coordinates
- bit 4: free
- bit 5: x,y as vel
- bit 6: z as vel
- bit 7: yaw as rate
  • Loading branch information
@kirkscheper @flixr
kirkscheper authored and flixr committed Jun 10, 2016
1 parent 2a7ab46 commit 6686c9a
Show file tree
Hide file tree
Showing 8 changed files with 189 additions and 67 deletions.
85 changes: 76 additions & 9 deletions sw/airborne/firmwares/rotorcraft/autopilot.c
View file
Expand Up @@ -526,7 +526,6 @@ void autopilot_set_mode(uint8_t new_autopilot_mode)
//if switching to rate mode but rate mode is not defined, the function returned
autopilot_mode = new_autopilot_mode;
}

}

bool autopilot_guided_goto_ned(float x, float y, float z, float heading)
Expand Down Expand Up @@ -576,6 +575,77 @@ bool autopilot_guided_move_ned(float vx, float vy, float vz, float heading)
return false;
}

/* Set guided mode setpoint
* Note: Offset position command in NED frame or body frame will only be implemented if
* local reference frame has been initialised.
* Flag definition:
bit 0: x,y as offset coordinates
bit 1: x,y in body coordinates
bit 2: z as offset coordinates
bit 3: yaw as offset coordinates
bit 4: free
bit 5: x,y as vel
bit 6: z as vel
bit 7: yaw as rate
*/
void autopilot_guided_update(uint8_t flags, float x, float y, float z, float yaw)
{
/* only update setpoints when in guided mode */
if (autopilot_mode != AP_MODE_GUIDED) {
return;
}

// handle x,y
if (bit_is_set(flags, 5)) { // velocity setpoint
if (bit_is_set(flags, 1)) { // set velocity in body frame
float psi = stateGetNedToBodyEulers_f()->psi;
x = cosf(-psi) * x + sinf(-psi) * y;
y = -sinf(-psi) * x + cosf(-psi) * y;
}
guidance_h_set_guided_vel(x, y);
} else { // position setpoint
if (!bit_is_set(flags, 0) && !bit_is_set(flags, 1)) { // set absolute position setpoint
guidance_h_set_guided_pos(x, y);
} else {
if (stateIsLocalCoordinateValid()) {
if (bit_is_set(flags, 1)) { // set position as offset in body frame
float psi = stateGetNedToBodyEulers_f()->psi;
x = stateGetPositionNed_f()->x + cosf(-psi) * x + sinf(-psi) * y;
y = stateGetPositionNed_f()->y - sinf(-psi) * x + cosf(-psi) * y;
} else { // set position as offset in NED frame
x += stateGetPositionNed_f()->x;
y += stateGetPositionNed_f()->y;
}
guidance_h_set_guided_pos(x, y);
}
}
}

//handle z
if (bit_is_set(flags, 6)) { // speed set-point
guidance_v_set_guided_vz(z);
} else { // position set-point
if (bit_is_set(flags, 2)) { // set position as offset in NED frame
if (stateIsLocalCoordinateValid()) {
z += stateGetPositionNed_f()->z;
guidance_v_set_guided_z(z);
}
} else {
guidance_v_set_guided_z(z);
}
}

//handle yaw
if (bit_is_set(flags, 7)) { // speed set-point
guidance_h_set_guided_heading_rate(z);
} else { // position set-point
if (bit_is_set(flags, 3)) { // set yaw as offset
yaw += stateGetNedToBodyEulers_f()->psi; // will be wrapped to [-pi,pi] later
}
guidance_h_set_guided_heading(yaw);
}
}

void autopilot_check_in_flight(bool motors_on)
{
if (autopilot_in_flight) {
Expand Down Expand Up @@ -631,11 +701,9 @@ static uint8_t ap_mode_of_3way_switch(void)
{
if (radio_control.values[RADIO_MODE] > THRESHOLD_2_PPRZ) {
return autopilot_mode_auto2;
}
else if (radio_control.values[RADIO_MODE] > THRESHOLD_1_PPRZ) {
} else if (radio_control.values[RADIO_MODE] > THRESHOLD_1_PPRZ) {
return MODE_AUTO1;
}
else {
} else {
return MODE_MANUAL;
}
}
Expand All @@ -654,16 +722,15 @@ static uint8_t ap_mode_of_two_switches(void)
if (radio_control.values[RADIO_MODE] < THRESHOLD_1_PPRZ) {
/* RADIO_MODE in MANUAL position */
return MODE_MANUAL;
}
else {
} else {
/* RADIO_MODE not in MANUAL position.
* Select AUTO mode bassed on RADIO_AUTO_MODE channel
*/
if (radio_control.values[RADIO_AUTO_MODE] > THRESHOLD_2_PPRZ) {
return autopilot_mode_auto2;
}
else
} else {
return MODE_AUTO1;
}
}
}
#endif
Expand Down
21 changes: 21 additions & 0 deletions sw/airborne/firmwares/rotorcraft/autopilot.h
View file
Expand Up @@ -219,4 +219,25 @@ extern bool autopilot_guided_goto_body_relative(float dx, float dy, float dz, fl
*/
extern bool autopilot_guided_move_ned(float vx, float vy, float vz, float heading);

/** Set guided setpoints using flag mask in GUIDED mode.
* @param flags Bits 0-3 are used to determine the axis system to be used.
* If bits 0 and 1 are clear then the coordinates are set in absolute NE coordinates.
* If bit 1 is set bit 0 is ignored.
* Bits 5-7 define whether the setpoints should be used as position or velocity.
* Bit flags are defined as follows:
bit 0: x,y as offset coordinates
bit 1: x,y in body coordinates
bit 2: z as offset coordinates
bit 3: yaw as offset coordinates
bit 4: free
bit 5: x,y as vel
bit 6: z as vel
bit 7: yaw as rate
* @param x North position/velocity in meters or meters/sec.
* @param y East position/velocity in meters or meters/sec.
* @param z Down position/velocity in meters or meters/sec.
* @param yaw Heading or heading rate setpoint in radians or radians/sec.
*/
extern void autopilot_guided_update(uint8_t flags, float x, float y, float z, float yaw);

#endif /* AUTOPILOT_H */
55 changes: 38 additions & 17 deletions sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
View file
Expand Up @@ -176,6 +176,7 @@ void guidance_h_init(void)
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;
guidance_h.gains.p = GUIDANCE_H_PGAIN;
guidance_h.gains.i = GUIDANCE_H_IGAIN;
guidance_h.gains.d = GUIDANCE_H_DGAIN;
Expand Down Expand Up @@ -248,6 +249,7 @@ void guidance_h_mode_changed(uint8_t new_mode)
stabilization_attitude_enter();
break;

case GUIDANCE_H_MODE_GUIDED:
case GUIDANCE_H_MODE_HOVER:
#if GUIDANCE_INDI
guidance_indi_enter();
Expand Down Expand Up @@ -325,7 +327,6 @@ void guidance_h_read_rc(bool in_flight)
#if GUIDANCE_H_USE_SPEED_REF
read_rc_setpoint_speed_i(&guidance_h.sp.speed, in_flight);
/* enable x,y velocity setpoints */
SetBit(guidance_h.sp.mask, 4);
SetBit(guidance_h.sp.mask, 5);
#endif
break;
Expand Down Expand Up @@ -404,12 +405,11 @@ void guidance_h_run(bool in_flight)
guidance_h_nav_enter();
}

if(horizontal_mode == HORIZONTAL_MODE_MANUAL) {
if (horizontal_mode == HORIZONTAL_MODE_MANUAL) {
stabilization_cmd[COMMAND_ROLL] = nav_roll;
stabilization_cmd[COMMAND_PITCH] = nav_pitch;
stabilization_cmd[COMMAND_YAW] = nav_heading;
}
else if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) {
} else if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) {
struct Int32Eulers sp_cmd_i;
sp_cmd_i.phi = nav_roll;
sp_cmd_i.theta = nav_pitch;
Expand Down Expand Up @@ -457,7 +457,7 @@ static void guidance_h_update_reference(void)
{
/* compute reference even if usage temporarily disabled via guidance_h_use_ref */
#if GUIDANCE_H_USE_REF
if (bit_is_set(guidance_h.sp.mask, 4) && bit_is_set(guidance_h.sp.mask, 5)) {
if (bit_is_set(guidance_h.sp.mask, 5)) {
gh_update_ref_from_speed_sp(guidance_h.sp.speed);
} else {
gh_update_ref_from_pos_sp(guidance_h.sp.pos);
Expand All @@ -481,8 +481,13 @@ static void guidance_h_update_reference(void)
VECT2_COPY(guidance_h.sp.pos, guidance_h.ref.pos); // for display only
}
#endif
}

/* 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);
}
}

#define MAX_POS_ERR POS_BFP_OF_REAL(16.)
#define MAX_SPEED_ERR SPEED_BFP_OF_REAL(16.)
Expand Down Expand Up @@ -521,11 +526,13 @@ 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) >> (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.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) >> (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 */
((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);
Expand All @@ -539,10 +546,11 @@ 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 << (INT32_ANGLE_FRAC + GH_GAIN_SCALE*2)), (traj_max_bank << (INT32_ANGLE_FRAC + GH_GAIN_SCALE*2)));
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 >> (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));
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);
}
Expand All @@ -568,22 +576,27 @@ static void guidance_h_hover_enter(void)
/* disable horizontal velocity setpoints,
* might still be activated in guidance_h_read_rc if GUIDANCE_H_USE_SPEED_REF
*/
ClearBit(guidance_h.sp.mask, 4);
ClearBit(guidance_h.sp.mask, 5);
ClearBit(guidance_h.sp.mask, 7);

/* set horizontal setpoint to current position */
VECT2_COPY(guidance_h.sp.pos, *stateGetPositionNed_i());

/* reset guidance reference */
reset_guidance_reference_from_current_position();

/* set guidance to current heading and position */
guidance_h.rc_sp.psi = stateGetNedToBodyEulers_i()->psi;
guidance_h.sp.heading = guidance_h.rc_sp.psi;

/* reset speed setting */
guidance_h_set_guided_vel(0., 0.);
}

static void guidance_h_nav_enter(void)
{
ClearBit(guidance_h.sp.mask, 4);
ClearBit(guidance_h.sp.mask, 5);
ClearBit(guidance_h.sp.mask, 7);

/* horizontal position setpoint from navigation/flightplan */
INT32_VECT2_NED_OF_ENU(guidance_h.sp.pos, navigation_carrot);
Expand Down Expand Up @@ -645,7 +658,6 @@ void guidance_h_set_igain(uint32_t igain)
bool guidance_h_set_guided_pos(float x, float y)
{
if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED) {
ClearBit(guidance_h.sp.mask, 4);
ClearBit(guidance_h.sp.mask, 5);
guidance_h.sp.pos.x = POS_BFP_OF_REAL(x);
guidance_h.sp.pos.y = POS_BFP_OF_REAL(y);
Expand All @@ -668,11 +680,20 @@ bool guidance_h_set_guided_heading(float heading)
bool guidance_h_set_guided_vel(float vx, float vy)
{
if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED) {
SetBit(guidance_h.sp.mask, 4);
SetBit(guidance_h.sp.mask, 5);
guidance_h.sp.speed.x = SPEED_BFP_OF_REAL(vx);
guidance_h.sp.speed.y = SPEED_BFP_OF_REAL(vy);
return true;
}
return false;
}

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);
return true;
}
return false;
}
9 changes: 8 additions & 1 deletion sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
View file
Expand Up @@ -70,7 +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
uint8_t mask; ///< bit 4: vx, bit 5: vy, bit 6: vz, bit 7: vyaw
int32_t heading_rate; ///< with #INT32_RATE_FRAC
uint8_t mask; ///< bit 5: vx & vy, bit 6: vz, bit 7: vyaw
};

struct HorizontalGuidanceReference {
Expand Down Expand Up @@ -133,6 +134,12 @@ extern bool guidance_h_set_guided_heading(float heading);
*/
extern bool guidance_h_set_guided_vel(float vx, float vy);

/** Set heading rate setpoint in GUIDED mode.
* @param rate Heading rate in radians.
* @return TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED)
*/
extern bool guidance_h_set_guided_heading_rate(float rate);

/* Make sure that ref can only be temporarily disabled for testing,
* but not enabled if GUIDANCE_H_USE_REF was defined to FALSE.
*/
Expand Down
13 changes: 11 additions & 2 deletions sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
View file
Expand Up @@ -229,11 +229,20 @@ void guidance_v_mode_changed(uint8_t new_mode)
}

switch (new_mode) {
case GUIDANCE_V_MODE_HOVER:
case GUIDANCE_V_MODE_GUIDED:
guidance_v_z_sp = stateGetPositionNed_i()->z; // set current altitude as setpoint
case GUIDANCE_V_MODE_HOVER:
/* disable vertical velocity setpoints */
guidance_v_guided_vel_enabled = false;

/* set current altitude as setpoint */
guidance_v_z_sp = stateGetPositionNed_i()->z;

/* reset guidance reference */
guidance_v_z_sum_err = 0;
GuidanceVSetRef(stateGetPositionNed_i()->z, 0, 0);

/* reset speed setting */
guidance_v_zd_sp = 0;
break;

case GUIDANCE_V_MODE_RC_CLIMB:
Expand Down
35 changes: 8 additions & 27 deletions sw/airborne/firmwares/rotorcraft/rotorcraft_datalink.c
View file
Expand Up @@ -71,33 +71,14 @@ void firmware_parse_msg(void)

case DL_GUIDED_SETPOINT_NED:
if (DL_GUIDED_SETPOINT_NED_ac_id(dl_buffer) != AC_ID) { break; }
uint8_t flags = DL_GUIDED_SETPOINT_NED_flags(dl_buffer);
float x = DL_GUIDED_SETPOINT_NED_x(dl_buffer);
float y = DL_GUIDED_SETPOINT_NED_y(dl_buffer);
float z = DL_GUIDED_SETPOINT_NED_z(dl_buffer);
float yaw = DL_GUIDED_SETPOINT_NED_yaw(dl_buffer);
switch (flags) {
case 0x00:
case 0x02:
/* local NED position setpoints */
autopilot_guided_goto_ned(x, y, z, yaw);
break;
case 0x01:
/* local NED offset position setpoints */
autopilot_guided_goto_ned_relative(x, y, z, yaw);
break;
case 0x03:
/* body NED offset position setpoints */
autopilot_guided_goto_body_relative(x, y, z, yaw);
break;
case 0x70:
/* local NED with x/y/z as velocity and yaw as absolute angle */
autopilot_guided_move_ned(x, y, z, yaw);
break;
default:
/* others not handled yet */
break;
}

autopilot_guided_update(DL_GUIDED_SETPOINT_NED_flags(dl_buffer),
DL_GUIDED_SETPOINT_NED_x(dl_buffer),
DL_GUIDED_SETPOINT_NED_y(dl_buffer),
DL_GUIDED_SETPOINT_NED_z(dl_buffer),
DL_GUIDED_SETPOINT_NED_yaw(dl_buffer));
break;

default:
break;
}
Expand Down
2 changes: 1 addition & 1 deletion sw/ext/pprzlink
Submodule pprzlink updated 1 files
+12 −11 message_definitions/v1.0/messages.xml

0 comments on commit 6686c9a

Please sign in to comment.