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

[guidance] Expansion of the guided protocol (#1694)

Browse Source 
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
This commit is contained in:
Kirk Scheper
2016-06-10 14:20:10 +02:00
committed by Felix Ruess
parent 2a7ab468f6
commit 6686c9a0ce
8 changed files with 189 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/autopilot.c
+76 -9
View File
@@ -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)
@@ -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) {
@@ -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;
}
}
@@ -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
sw/airborne/firmwares/rotorcraft/autopilot.h
+21
View File
@@ -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 */
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+38 -17
View File
@@ -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;
@@ -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();
@@ -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;
@@ -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;
@@ -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);
@@ -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.)
@@ -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);
@@ -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);
}
@@ -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);
@@ -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);
@@ -668,7 +680,6 @@ 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);
@@ -676,3 +687,13 @@ bool guidance_h_set_guided_vel(float vx, float vy)
}
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;
}
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
+8 -1
View File
@@ -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 {
@@ -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.
*/
sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+11 -2
View File
@@ -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:
sw/airborne/firmwares/rotorcraft/rotorcraft_datalink.c
+8 -27
View File
@@ -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;
}
sw/ext/pprzlink
+1 -1
Submodule sw/ext/pprzlink updated: 0a44f55835...971a4b71d0
sw/ground_segment/python/guided_mode_example.py
+26 -10
View File
@@ -91,7 +91,7 @@ class Guidance(object):
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0x01
msg['flags'] = 0x0D
msg['x'] = north
msg['y'] = east
msg['z'] = down
@@ -105,7 +105,7 @@ class Guidance(object):
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0x03
msg['flags'] = 0x0E
msg['x'] = forward
msg['y'] = right
msg['z'] = down
@@ -113,36 +113,52 @@ class Guidance(object):
print("goto body relative: %s" % msg)
self._interface.send_raw_datalink(msg)
def move_at_vel(self, north=0.0, east=0.0, down=0.0, yaw=0.0):
def move_at_ned_vel(self, north=0.0, east=0.0, down=0.0, yaw=0.0):
"""
move at specified velocity in meters/sec with absolute heading (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0x70
msg['flags'] = 0x60
msg['x'] = north
msg['y'] = east
msg['z'] = down
msg['yaw'] = yaw
print("move at vel NED: %s" % msg)
self._interface.send_raw_datalink(msg)
def move_at_body_vel(self, forward=0.0, right=0.0, down=0.0, yaw=0.0):
"""
move at specified velocity in meters/sec with absolute heading (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0x62
msg['x'] = forward
msg['y'] = right
msg['z'] = down
msg['yaw'] = yaw
print("move at vel body: %s" % msg)
self._interface.send_raw_datalink(msg)
if __name__ == '__main__':
ac_id = 40
ac_id = 11
try:
g = Guidance(ac_id)
sleep(0.1)
g.set_guided_mode()
sleep(0.2)
g.goto_ned(north=10.0, east=5.0, down=-5.0, heading=radians(90))
g.goto_ned(north=2.0, east=2.0, down=-3.0, heading=radians(90))
sleep(10)
g.goto_ned_relative(north=-5.0, east=-5.0, down=-2.0, yaw=-radians(45))
g.goto_ned_relative(north=-2.0, east=-2.0, down=1.0, yaw=-radians(45))
sleep(10)
g.goto_body_relative(forward=0.0, right=5.0, down=2.0)
sleep(10)
g.move_at_vel(north=3.0)
g.goto_body_relative(forward=0.0, right=1.0, down=0.0)
sleep(10)
g.move_at_ned_vel(north=0.5)
sleep(3)
g.move_at_body_vel(forward=-0.5)
sleep(3)
g.set_nav_mode()
sleep(0.2)
except KeyboardInterrupt: