Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: move training stabilize function to training mode

Browse Source
This commit is contained in:
Iampete1 2023-02-12 13:38:37 +00:00 committed by Andrew Tridgell
parent 549dd3875d
commit 6bb0096b9d
3 changed files with 16 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
ArduPlane/Attitude.cpp
View File

@ -338,34 +338,34 @@ void Plane::stabilize_yaw()
/* /*
a special stabilization function for training mode a special stabilization function for training mode
*/ */
void Plane::stabilize_training() void ModeTraining::run()
{ {
const float rexpo = roll_in_expo(false); const float rexpo = plane.roll_in_expo(false);
const float pexpo = pitch_in_expo(false); const float pexpo = plane.pitch_in_expo(false);
if (training_manual_roll) { if (plane.training_manual_roll) {
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rexpo); SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rexpo);
} else { } else {
// calculate what is needed to hold // calculate what is needed to hold
stabilize_roll(); plane.stabilize_roll();
if ((nav_roll_cd > 0 && rexpo < SRV_Channels::get_output_scaled(SRV_Channel::k_aileron)) || if ((plane.nav_roll_cd > 0 && rexpo < SRV_Channels::get_output_scaled(SRV_Channel::k_aileron)) ||
(nav_roll_cd < 0 && rexpo > SRV_Channels::get_output_scaled(SRV_Channel::k_aileron))) { (plane.nav_roll_cd < 0 && rexpo > SRV_Channels::get_output_scaled(SRV_Channel::k_aileron))) {
// allow user to get out of the roll // allow user to get out of the roll
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rexpo); SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rexpo);
} }
} }
if (training_manual_pitch) { if (plane.training_manual_pitch) {
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pexpo); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pexpo);
} else { } else {
stabilize_pitch(); plane.stabilize_pitch();
if ((nav_pitch_cd > 0 && pexpo < SRV_Channels::get_output_scaled(SRV_Channel::k_elevator)) || if ((plane.nav_pitch_cd > 0 && pexpo < SRV_Channels::get_output_scaled(SRV_Channel::k_elevator)) ||
(nav_pitch_cd < 0 && pexpo > SRV_Channels::get_output_scaled(SRV_Channel::k_elevator))) { (plane.nav_pitch_cd < 0 && pexpo > SRV_Channels::get_output_scaled(SRV_Channel::k_elevator))) {
// allow user to get back to level // allow user to get back to level
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pexpo); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pexpo);
} }
} }
stabilize_yaw(); plane.stabilize_yaw();
} }
@ -594,7 +594,7 @@ void Plane::stabilize()
last_stabilize_ms = now; last_stabilize_ms = now;
if (control_mode == &mode_training) { if (control_mode == &mode_training) {
stabilize_training(); plane.control_mode->run();
#if AP_SCRIPTING_ENABLED #if AP_SCRIPTING_ENABLED
} else if (nav_scripting_active()) { } else if (nav_scripting_active()) {
// scripting is in control of roll and pitch rates and throttle // scripting is in control of roll and pitch rates and throttle

1
ArduPlane/Plane.h
View File

@ -868,7 +868,6 @@ private:
void stabilize_stick_mixing_direct(); void stabilize_stick_mixing_direct();
void stabilize_stick_mixing_fbw(); void stabilize_stick_mixing_fbw();
void stabilize_yaw(); void stabilize_yaw();
void stabilize_training();
void stabilize_acro(); void stabilize_acro();
void stabilize_acro_quaternion(); void stabilize_acro_quaternion();
void calc_nav_yaw_coordinated(); void calc_nav_yaw_coordinated();

3
ArduPlane/mode.h
View File

@ -391,6 +391,9 @@ public:
// methods that affect movement of the vehicle in this mode // methods that affect movement of the vehicle in this mode
void update() override; void update() override;
void run() override;
}; };
class ModeInitializing : public Mode class ModeInitializing : public Mode