Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-10 06:59:54 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

[fix] update generated rotorcraft autopilot for new guidance (#3012)

Browse Source
This commit is contained in:
Gautier Hattenberger
2023-03-17 21:44:20 +01:00
committed by GitHub
parent 1d7b6dff57
commit 767333a02c
3 changed files with 16 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/autopilot/rotorcraft_autopilot.xml
+12 -9
View File
@@ -39,18 +39,19 @@
<control_block name="throttle_direct">
<call fun="guidance_v_read_rc()"/>
<call fun="guidance_v_z_sp = stateGetPositionNed_i()->z"/>
<call fun="stabilization_cmd[COMMAND_THRUST] = guidance_v_rc_delta_t"/>
<call fun="guidance_v_set_z(stateGetPositionNed_i()->z)"/>
<call fun="stabilization_cmd[COMMAND_THRUST] = guidance_v.rc_delta_t"/>
</control_block>
<control_block name="altitude_loop">
<call fun="gv_update_ref_from_z_sp(guidance_v_z_sp)"/>
<call fun="run_hover_loop(autopilot_in_flight())"/>
<call fun="gv_update_ref_from_z_sp(guidance_v.z_sp)"/>
<call fun="guidance_v.delta_t = guidance_v_run_pos(autopilot_in_flight(), &guidance_v)"/>
<call fun="stabilization_cmd[COMMAND_THRUST] = guidance_v.rc_delta_t"/>
<!--call fun="SaturateThrottle(rc_values)"/-->
</control_block>
<exceptions>
<exception cond="too_far_from_home" deroute="HOME"/>
<exception cond="nav.too_far_from_home" deroute="HOME"/>
<exception cond="kill_switch_is_on()" deroute="KILL"/>
</exceptions>
@@ -112,7 +113,7 @@
<on_enter>
<call fun="guidance_h_hover_enter()"/>
<call fun="stabilization_attitude_enter()"/>
<call fun="guidance_v_mode = GUIDANCE_V_MODE_GUIDED"/>
<call fun="guidance_v.mode = GUIDANCE_V_MODE_GUIDED"/>
<call fun="guidance_v_guided_enter()"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
@@ -152,13 +153,15 @@
<on_enter>
<call fun="stabilization_attitude_set_failsafe_setpoint()"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB)"/>
<call fun="guidance_v_zd_sp = SPEED_BFP_OF_REAL(FAILSAFE_DESCENT_SPEED)"/>
<call fun="guidance_v_set_z(SPEED_BFP_OF_REAL(FAILSAFE_DESCENT_SPEED))"/>
</on_enter>
<control>
<call fun="stabilization_attitude_set_failsafe_setpoint()"/>
<call fun="stabilization_attitude_run(autopilot_in_flight())"/>
<call fun="gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z)"/>
<call fun="run_hover_loop(autopilot_in_flight())"/>
<call fun="gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z)"/>
<call fun="guidance_v_update_ref()"/>
<call fun="guidance_v.delta_t = guidance_v_run_speed(autopilot_in_flight(), &guidance_v)"/>
<call fun="stabilization_cmd[COMMAND_THRUST] = guidance_v.rc_delta_t"/>
<call_block name="set_commands"/>
</control>
<exception cond="!GpsIsLost()" deroute="$LAST_MODE"/>
sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+2 -2
View File
@@ -199,7 +199,7 @@ static int32_t get_vertical_thrust_coeff(void)
}
static void guidance_v_thrust_adapt(bool in_flight)
void guidance_v_thrust_adapt(bool in_flight)
{
guidance_v.thrust_coeff = get_vertical_thrust_coeff();
@@ -302,7 +302,7 @@ void guidance_v_set_ref(int32_t pos, int32_t speed, int32_t accel)
}
static void guidance_v_update_ref(void)
void guidance_v_update_ref(void)
{
/* convert our reference to generic representation */
int64_t tmp = gv_z_ref >> (GV_Z_REF_FRAC - INT32_POS_FRAC);
sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
+2
View File
@@ -111,6 +111,8 @@ extern void guidance_v_init(void);
extern void guidance_v_read_rc(void);
extern void guidance_v_mode_changed(uint8_t new_mode);
extern void guidance_v_notify_in_flight(bool in_flight);
extern void guidance_v_thrust_adapt(bool in_flight);
extern void guidance_v_update_ref(void);
extern void guidance_v_run(bool in_flight);
extern void guidance_v_z_enter(void);