ardupilot/ArduCopter/mode_acro_heli.cpp

101 lines
4.0 KiB
C++
Raw Normal View History

#include "Copter.h"
2014-01-30 08:13:17 -04:00
#if FRAME_CONFIG == HELI_FRAME
/*
* Init and run calls for acro flight mode for trad heli
2014-01-30 08:13:17 -04:00
*/
// heli_acro_init - initialise acro controller
bool Copter::ModeAcro_Heli::init(bool ignore_checks)
2014-01-30 08:13:17 -04:00
{
// if heli is equipped with a flybar, then tell the attitude controller to pass through controls directly to servos
attitude_control->use_flybar_passthrough(motors->has_flybar(), motors->supports_yaw_passthrough());
motors->set_acro_tail(true);
// set stab collective false to use full collective pitch range
_copter.input_manager.set_use_stab_col(false);
// always successfully enter acro
2014-01-30 08:13:17 -04:00
return true;
}
// heli_acro_run - runs the acro controller
// should be called at 100hz or more
void Copter::ModeAcro_Heli::run()
2014-01-30 08:13:17 -04:00
{
float target_roll, target_pitch, target_yaw;
float pilot_throttle_scaled;
// Tradheli should not reset roll, pitch, yaw targets when motors are not runup, because
// we may be in autorotation flight. These should be reset only when transitioning from disarmed
// to armed, because the pilot will have placed the helicopter down on the landing pad. 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()) {
_copter.heli_flags.init_targets_on_arming=true;
attitude_control->set_attitude_target_to_current_attitude();
attitude_control->reset_rate_controller_I_terms();
}
if(motors->armed() && _copter.heli_flags.init_targets_on_arming) {
attitude_control->set_attitude_target_to_current_attitude();
attitude_control->reset_rate_controller_I_terms();
if (motors->get_interlock()) {
_copter.heli_flags.init_targets_on_arming=false;
}
}
// clear landing flag above zero throttle
if (motors->armed() && motors->get_interlock() && motors->rotor_runup_complete() && !ap.throttle_zero) {
set_land_complete(false);
}
if (!motors->has_flybar()){
// convert the input to the desired body frame rate
ArduCopter: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step)
2016-05-08 05:46:59 -03:00
get_pilot_desired_angle_rates(channel_roll->get_control_in(), channel_pitch->get_control_in(), channel_yaw->get_control_in(), target_roll, target_pitch, target_yaw);
if (motors->supports_yaw_passthrough()) {
// if the tail on a flybar heli has an external gyro then
// also use no deadzone for the yaw control and
// pass-through the input direct to output.
2017-01-06 21:06:40 -04:00
target_yaw = channel_yaw->get_control_in_zero_dz();
}
// run attitude controller
attitude_control->input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
}else{
/*
for fly-bar passthrough use control_in values with no
deadzone. This gives true pass-through.
*/
2017-01-06 21:06:40 -04:00
float roll_in = channel_roll->get_control_in_zero_dz();
float pitch_in = channel_pitch->get_control_in_zero_dz();
float yaw_in;
if (motors->supports_yaw_passthrough()) {
// if the tail on a flybar heli has an external gyro then
// also use no deadzone for the yaw control and
// pass-through the input direct to output.
2017-01-06 21:06:40 -04:00
yaw_in = channel_yaw->get_control_in_zero_dz();
} else {
// if there is no external gyro then run the usual
// ACRO_YAW_P gain on the input control, including
// deadzone
ArduCopter: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step)
2016-05-08 05:46:59 -03:00
yaw_in = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
}
// run attitude controller
attitude_control->passthrough_bf_roll_pitch_rate_yaw(roll_in, pitch_in, yaw_in);
}
2014-01-30 08:13:17 -04:00
// get pilot's desired throttle
pilot_throttle_scaled = _copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in());
// output pilot's throttle without angle boost
attitude_control->set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);
2014-01-30 08:13:17 -04:00
}
#endif //HELI_FRAME