Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: refactor NAV_ALTITUDE_WAIT to AutoMode:: to match nav_delay()

This commit is contained in:
Tom Pittenger 2024-04-23 15:20:30 -07:00 committed by Tom Pittenger
parent 410521afb8
commit 1fe7f6b099
5 changed files with 47 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduPlane/Plane.h
View File

@ -480,10 +480,6 @@ private:
// Minimum pitch to hold during takeoff command execution. Hundredths of a degree
int16_t takeoff_pitch_cd;
// used to 'wiggle' servos in idle mode to prevent them freezing
// at high altitudes
uint8_t idle_wiggle_stage;
// Flag for using gps ground course instead of INS yaw. Set false when takeoff command in process.
bool takeoff_complete;
@ -1110,7 +1106,6 @@ private:
void avoidance_adsb_update(void);
// servos.cpp
void set_servos_idle(void);
void set_servos();
float apply_throttle_limits(float throttle_in);
void set_throttle(void);

21
ArduPlane/commands_logic.cpp
View File

@ -284,7 +284,7 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret
return verify_continue_and_change_alt();
case MAV_CMD_NAV_ALTITUDE_WAIT:
return verify_altitude_wait(cmd);
return mode_auto.verify_altitude_wait(cmd);
#if HAL_QUADPLANE_ENABLED
case MAV_CMD_NAV_VTOL_TAKEOFF:
@ -845,26 +845,25 @@ bool Plane::verify_continue_and_change_alt()
/*
see if we have reached altitude or descent speed
*/
bool Plane::verify_altitude_wait(const AP_Mission::Mission_Command &cmd)
bool ModeAuto::verify_altitude_wait(const AP_Mission::Mission_Command &cmd)
{
if (current_loc.alt > cmd.content.altitude_wait.altitude*100.0f) {
if (plane.current_loc.alt > cmd.content.altitude_wait.altitude*100.0f) {
gcs().send_text(MAV_SEVERITY_INFO,"Reached altitude");
return true;
}
if (auto_state.sink_rate > cmd.content.altitude_wait.descent_rate) {
gcs().send_text(MAV_SEVERITY_INFO, "Reached descent rate %.1f m/s", (double)auto_state.sink_rate);
if (plane.auto_state.sink_rate > cmd.content.altitude_wait.descent_rate) {
gcs().send_text(MAV_SEVERITY_INFO, "Reached descent rate %.1f m/s", (double)plane.auto_state.sink_rate);
return true;
}
// if requested, wiggle servos
if (cmd.content.altitude_wait.wiggle_time != 0) {
static uint32_t last_wiggle_ms;
if (auto_state.idle_wiggle_stage == 0 &&
AP_HAL::millis() - last_wiggle_ms > cmd.content.altitude_wait.wiggle_time*1000) {
auto_state.idle_wiggle_stage = 1;
last_wiggle_ms = AP_HAL::millis();
if (wiggle.stage == 0 &&
AP_HAL::millis() - wiggle.last_ms > cmd.content.altitude_wait.wiggle_time*1000) {
wiggle.stage = 1;
wiggle.last_ms = AP_HAL::millis();
// idle_wiggle_stage is updated in wiggle_servos()
}
// idle_wiggle_stage is updated in set_servos_idle()
}
return false;

8
ArduPlane/mode.h
View File

@ -233,6 +233,8 @@ public:
void do_nav_delay(const AP_Mission::Mission_Command& cmd);
bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
bool verify_altitude_wait(const AP_Mission::Mission_Command& cmd);
void run() override;
protected:
@ -249,6 +251,12 @@ private:
uint32_t time_start_ms;
} nav_delay;
// wiggle state and timer for NAV_ALTITUDE_WAIT
void wiggle_servos();
struct {
uint8_t stage;
uint32_t last_ms;
} wiggle;
};

12
ArduPlane/mode_auto.cpp
View File

@ -167,8 +167,16 @@ bool ModeAuto::is_landing() const
void ModeAuto::run()
{
if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_ALTITUDE_WAIT) {
// Wiggle servos
plane.set_servos_idle();
wiggle_servos();
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 0.0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 0.0);
SRV_Channels::set_output_to_trim(SRV_Channel::k_throttle);
SRV_Channels::set_output_to_trim(SRV_Channel::k_throttleLeft);
SRV_Channels::set_output_to_trim(SRV_Channel::k_throttleRight);
// Relax attitude control
reset_controllers();

41
ArduPlane/servos.cpp
View File

@ -348,43 +348,40 @@ void Plane::airbrake_update(void)
}
/*
setup servos for idle mode
setup servos for idle wiggle mode
Idle mode is used during balloon launch to keep servos still, apart
from occasional wiggle to prevent freezing up
*/
void Plane::set_servos_idle(void)
void ModeAuto::wiggle_servos()
{
// This is only active while in AUTO running NAV_ALTITUDE_WAIT with wiggle_time > 0
if (wiggle.last_ms == 0) {
return;
}
int16_t servo_value;
// move over full range for 2 seconds
if (auto_state.idle_wiggle_stage != 0) {
auto_state.idle_wiggle_stage += 2;
if (wiggle.stage != 0) {
wiggle.stage += 2;
}
if (auto_state.idle_wiggle_stage == 0) {
if (wiggle.stage == 0) {
servo_value = 0;
} else if (auto_state.idle_wiggle_stage < 50) {
servo_value = auto_state.idle_wiggle_stage * (4500 / 50);
} else if (auto_state.idle_wiggle_stage < 100) {
servo_value = (100 - auto_state.idle_wiggle_stage) * (4500 / 50);
} else if (auto_state.idle_wiggle_stage < 150) {
servo_value = (100 - auto_state.idle_wiggle_stage) * (4500 / 50);
} else if (auto_state.idle_wiggle_stage < 200) {
servo_value = (auto_state.idle_wiggle_stage-200) * (4500 / 50);
} else if (wiggle.stage < 50) {
servo_value = wiggle.stage * (4500 / 50);
} else if (wiggle.stage < 100) {
servo_value = (100 - wiggle.stage) * (4500 / 50);
} else if (wiggle.stage < 150) {
servo_value = (100 - wiggle.stage) * (4500 / 50);
} else if (wiggle.stage < 200) {
servo_value = (wiggle.stage-200) * (4500 / 50);
} else {
auto_state.idle_wiggle_stage = 0;
wiggle.stage = 0;
servo_value = 0;
}
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, servo_value);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, servo_value);
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, servo_value);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 0.0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 0.0);
SRV_Channels::set_output_to_trim(SRV_Channel::k_throttle);
SRV_Channels::set_output_to_trim(SRV_Channel::k_throttleLeft);
SRV_Channels::set_output_to_trim(SRV_Channel::k_throttleRight);
}