Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 20:48:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: move set_servos_idle functionality to ModeAuto

Browse Source
This commit is contained in:
Iampete1 2023-10-28 21:05:57 +01:00 committed by Andrew Tridgell
parent 220ab515b7
commit 8061b1b4c2
5 changed files with 18 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduPlane/Plane.h
View File

@ -494,11 +494,6 @@ private:
// have we checked for an auto-land?
bool checked_for_autoland;
// Altitude threshold to complete a takeoff command in autonomous modes. Centimeters
// are we in idle mode? used for balloon launch to stop servo
// movement until altitude is reached
bool idle_mode;
// are we in VTOL mode in AUTO?
bool vtol_mode;
@ -930,7 +925,6 @@ private:
void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
void do_loiter_turns(const AP_Mission::Mission_Command& cmd);
void do_loiter_time(const AP_Mission::Mission_Command& cmd);
void do_altitude_wait(const AP_Mission::Mission_Command& cmd);
void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd);
void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd);
void do_vtol_takeoff(const AP_Mission::Mission_Command& cmd);

10
ArduPlane/commands_logic.cpp
View File

@ -23,9 +23,6 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
// except in a takeoff
auto_state.takeoff_complete = true;
// start non-idle
auto_state.idle_mode = false;
nav_controller->set_data_is_stale();
// reset loiter start time. New command is a new loiter
@ -93,7 +90,6 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
break;
case MAV_CMD_NAV_ALTITUDE_WAIT:
do_altitude_wait(cmd);
break;
#if HAL_QUADPLANE_ENABLED
@ -519,12 +515,6 @@ void Plane::do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd)
reset_offset_altitude();
}
void Plane::do_altitude_wait(const AP_Mission::Mission_Command& cmd)
{
// set all servos to trim until we reach altitude or descent speed
auto_state.idle_mode = true;
}
void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
{
//set target alt

2
ArduPlane/mode.h
View File

@ -225,6 +225,8 @@ public:
void do_nav_delay(const AP_Mission::Mission_Command& cmd);
bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
void run() override;
protected:
bool _enter() override;

16
ArduPlane/mode_auto.cpp
View File

@ -164,3 +164,19 @@ bool ModeAuto::is_landing() const
{
return (plane.flight_stage == AP_FixedWing::FlightStage::LAND);
}
void ModeAuto::run()
{
if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_ALTITUDE_WAIT) {
// Wiggle servos
plane.set_servos_idle();
// Relax attitude control
reset_controllers();
} else {
// Normal flight, run base class
Mode::run();
}
}

8
ArduPlane/servos.cpp
View File

@ -373,7 +373,6 @@ void Plane::set_servos_idle(void)
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, servo_value);
SRV_Channels::set_output_to_trim(SRV_Channel::k_throttle);
SRV_Channels::output_ch_all();
}
@ -815,13 +814,6 @@ void Plane::set_servos(void)
quadplane.update();
#endif
if (control_mode == &mode_auto && auto_state.idle_mode) {
// special handling for balloon launch
set_servos_idle();
servos_output();
return;
}
if (control_mode != &mode_manual) {
set_servos_controlled();
set_takeoff_expected();