mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
676d75c391
This makes us look like Rover and Plane in terms of namespacing for the Mode classes, and removes a wart where we #include mode.h in the middle of the Mode class. This was done mechanically for the most part. I've had to remove the convenience reference for ap as part of this.
62 lines
2.1 KiB
C++
62 lines
2.1 KiB
C++
#include "Copter.h"
|
|
|
|
/*
|
|
* Init and run calls for stabilize flight mode
|
|
*/
|
|
|
|
// stabilize_run - runs the main stabilize controller
|
|
// should be called at 100hz or more
|
|
void ModeStabilize::run()
|
|
{
|
|
// apply simple mode transform to pilot inputs
|
|
update_simple_mode();
|
|
|
|
// convert pilot input to lean angles
|
|
float target_roll, target_pitch;
|
|
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max);
|
|
|
|
// get pilot's desired yaw rate
|
|
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
|
|
|
|
if (!motors->armed()) {
|
|
// Motors should be Stopped
|
|
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
|
|
} else if (copter.ap.throttle_zero) {
|
|
// Attempting to Land
|
|
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
|
|
} else {
|
|
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
|
}
|
|
|
|
switch (motors->get_spool_state()) {
|
|
case AP_Motors::SpoolState::SHUT_DOWN:
|
|
// Motors Stopped
|
|
attitude_control->set_yaw_target_to_current_heading();
|
|
attitude_control->reset_rate_controller_I_terms();
|
|
break;
|
|
case AP_Motors::SpoolState::GROUND_IDLE:
|
|
// Landed
|
|
attitude_control->set_yaw_target_to_current_heading();
|
|
attitude_control->reset_rate_controller_I_terms();
|
|
break;
|
|
case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
|
|
// clear landing flag above zero throttle
|
|
if (!motors->limit.throttle_lower) {
|
|
set_land_complete(false);
|
|
}
|
|
break;
|
|
case AP_Motors::SpoolState::SPOOLING_UP:
|
|
case AP_Motors::SpoolState::SPOOLING_DOWN:
|
|
// do nothing
|
|
break;
|
|
}
|
|
|
|
// call attitude controller
|
|
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
|
|
|
|
// output pilot's throttle
|
|
attitude_control->set_throttle_out(get_pilot_desired_throttle(),
|
|
true,
|
|
g.throttle_filt);
|
|
}
|