2015-05-29 23:12:49 -03:00
|
|
|
#include "Copter.h"
|
|
|
|
|
2014-01-30 08:13:17 -04:00
|
|
|
#if FRAME_CONFIG == HELI_FRAME
|
|
|
|
/*
|
2016-07-25 15:45:29 -03:00
|
|
|
* 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
|
2017-12-10 23:51:13 -04:00
|
|
|
bool Copter::ModeAcro_Heli::init(bool ignore_checks)
|
2014-01-30 08:13:17 -04:00
|
|
|
{
|
2014-07-03 17:18:48 -03:00
|
|
|
// if heli is equipped with a flybar, then tell the attitude controller to pass through controls directly to servos
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->use_flybar_passthrough(motors->has_flybar(), motors->supports_yaw_passthrough());
|
2014-07-03 17:18:48 -03:00
|
|
|
|
2017-01-09 03:31:26 -04:00
|
|
|
motors->set_acro_tail(true);
|
2015-08-09 08:03:11 -03:00
|
|
|
|
2015-10-12 22:05:49 -03:00
|
|
|
// set stab collective false to use full collective pitch range
|
2016-03-21 01:20:54 -03:00
|
|
|
_copter.input_manager.set_use_stab_col(false);
|
2015-10-12 22:05:49 -03:00
|
|
|
|
2014-06-06 00:05:09 -03:00
|
|
|
// 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
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeAcro_Heli::run()
|
2014-01-30 08:13:17 -04:00
|
|
|
{
|
2014-02-09 21:50:43 -04:00
|
|
|
float target_roll, target_pitch, target_yaw;
|
2016-01-04 00:49:05 -04:00
|
|
|
float pilot_throttle_scaled;
|
2014-05-11 13:21:27 -03:00
|
|
|
|
|
|
|
// 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
|
|
|
|
|
2017-01-09 03:31:26 -04:00
|
|
|
if(!motors->armed()) {
|
2016-03-21 01:20:54 -03:00
|
|
|
_copter.heli_flags.init_targets_on_arming=true;
|
2017-10-26 19:53:49 -03:00
|
|
|
attitude_control->set_attitude_target_to_current_attitude();
|
|
|
|
attitude_control->reset_rate_controller_I_terms();
|
2014-01-30 08:45:01 -04:00
|
|
|
}
|
2015-07-08 16:10:50 -03:00
|
|
|
|
2016-03-21 01:20:54 -03:00
|
|
|
if(motors->armed() && _copter.heli_flags.init_targets_on_arming) {
|
2017-10-26 19:53:49 -03:00
|
|
|
attitude_control->set_attitude_target_to_current_attitude();
|
|
|
|
attitude_control->reset_rate_controller_I_terms();
|
|
|
|
if (motors->get_interlock()) {
|
2016-03-21 01:20:54 -03:00
|
|
|
_copter.heli_flags.init_targets_on_arming=false;
|
2015-07-08 16:10:50 -03:00
|
|
|
}
|
2014-05-11 13:21:27 -03:00
|
|
|
}
|
|
|
|
|
2016-08-05 01:36:39 -03:00
|
|
|
// clear landing flag above zero throttle
|
2017-01-09 03:31:26 -04:00
|
|
|
if (motors->armed() && motors->get_interlock() && motors->rotor_runup_complete() && !ap.throttle_zero) {
|
2016-08-05 01:36:39 -03:00
|
|
|
set_land_complete(false);
|
|
|
|
}
|
|
|
|
|
2017-01-09 03:31:26 -04:00
|
|
|
if (!motors->has_flybar()){
|
2014-07-03 21:55:17 -03:00
|
|
|
// 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);
|
2015-11-20 15:51:21 -04:00
|
|
|
|
2017-01-09 03:31:26 -04:00
|
|
|
if (motors->supports_yaw_passthrough()) {
|
2015-11-20 15:51:21 -04:00
|
|
|
// 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();
|
2015-11-20 15:51:21 -04:00
|
|
|
}
|
|
|
|
|
2014-08-20 10:14:48 -03:00
|
|
|
// run attitude controller
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
|
2014-07-03 21:55:17 -03:00
|
|
|
}else{
|
2015-07-26 23:49:03 -03:00
|
|
|
/*
|
|
|
|
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();
|
2015-07-26 23:49:03 -03:00
|
|
|
float yaw_in;
|
|
|
|
|
2017-01-09 03:31:26 -04:00
|
|
|
if (motors->supports_yaw_passthrough()) {
|
2015-07-26 23:49:03 -03:00
|
|
|
// 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();
|
2015-07-26 23:49:03 -03:00
|
|
|
} 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());
|
2015-07-26 23:49:03 -03:00
|
|
|
}
|
|
|
|
|
2014-08-20 10:14:48 -03:00
|
|
|
// run attitude controller
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->passthrough_bf_roll_pitch_rate_yaw(roll_in, pitch_in, yaw_in);
|
2014-07-03 21:55:17 -03:00
|
|
|
}
|
2014-01-30 08:13:17 -04:00
|
|
|
|
2015-10-12 22:05:49 -03:00
|
|
|
// get pilot's desired throttle
|
2016-03-21 01:20:54 -03:00
|
|
|
pilot_throttle_scaled = _copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in());
|
2015-10-12 22:05:49 -03:00
|
|
|
|
2014-01-30 20:55:20 -04:00
|
|
|
// output pilot's throttle without angle boost
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);
|
2014-01-30 08:13:17 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
#endif //HELI_FRAME
|