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.
82 lines
2.9 KiB
C++
82 lines
2.9 KiB
C++
#include "Copter.h"
|
|
|
|
#if FRAME_CONFIG == HELI_FRAME
|
|
/*
|
|
* Init and run calls for stabilize flight mode for trad heli
|
|
*/
|
|
|
|
// stabilize_init - initialise stabilize controller
|
|
bool ModeStabilize_Heli::init(bool ignore_checks)
|
|
{
|
|
// set stab collective true to use stabilize scaled collective pitch range
|
|
copter.input_manager.set_use_stab_col(true);
|
|
|
|
return true;
|
|
}
|
|
|
|
// stabilize_run - runs the main stabilize controller
|
|
// should be called at 100hz or more
|
|
void ModeStabilize_Heli::run()
|
|
{
|
|
float target_roll, target_pitch;
|
|
float target_yaw_rate;
|
|
float pilot_throttle_scaled;
|
|
|
|
// apply SIMPLE mode transform to pilot inputs
|
|
update_simple_mode();
|
|
|
|
// convert pilot input to lean angles
|
|
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max);
|
|
|
|
// get pilot's desired yaw rate
|
|
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
|
|
|
|
// get pilot's desired throttle
|
|
pilot_throttle_scaled = copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in());
|
|
|
|
// Tradheli should not reset roll, pitch, yaw targets when motors are not runup while flying, because
|
|
// we may be in autorotation flight. This is so that the servos move in a realistic fashion while disarmed
|
|
// for operational checks. Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero
|
|
// so the swash servos move.
|
|
|
|
if (!motors->armed()) {
|
|
// Motors should be Stopped
|
|
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
|
|
} else {
|
|
// heli will not let the spool state progress to THROTTLE_UNLIMITED until motor interlock is enabled
|
|
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
|
|
if (motors->init_targets_on_arming()) {
|
|
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);
|
|
}
|
|
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 - note that TradHeli does not used angle-boost
|
|
attitude_control->set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);
|
|
}
|
|
|
|
#endif //HELI_FRAME
|