diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index 43f0f98b42..b8cddb62eb 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -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 diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.h b/sw/airborne/firmwares/rotorcraft/autopilot.h index a64f774a9c..3c7e35c2cf 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.h +++ b/sw/airborne/firmwares/rotorcraft/autopilot.h @@ -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 */ diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 33568e1de4..fe42815bfd 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -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; +} diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h index 3a592a1e2b..3bc4179d29 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h @@ -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. */ diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index 4f48c23cee..1963337833 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -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: diff --git a/sw/airborne/firmwares/rotorcraft/rotorcraft_datalink.c b/sw/airborne/firmwares/rotorcraft/rotorcraft_datalink.c index a2dcf995af..931ab3b1ef 100644 --- a/sw/airborne/firmwares/rotorcraft/rotorcraft_datalink.c +++ b/sw/airborne/firmwares/rotorcraft/rotorcraft_datalink.c @@ -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; } diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink index 0a44f55835..971a4b71d0 160000 --- a/sw/ext/pprzlink +++ b/sw/ext/pprzlink @@ -1 +1 @@ -Subproject commit 0a44f5583527ea71ee28d4f33cc97b2bd9c2df4b +Subproject commit 971a4b71d0f5e01af55986d9a3e65a4a1c5d0976 diff --git a/sw/ground_segment/python/guided_mode_example.py b/sw/ground_segment/python/guided_mode_example.py index 40eab4ebc9..42717faa0e 100755 --- a/sw/ground_segment/python/guided_mode_example.py +++ b/sw/ground_segment/python/guided_mode_example.py @@ -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: