// init
void AP_MotorsHeli::init(motor_frame_class frame_class, motor_frame_type frame_type)
{
// remember frame type
_frame_type = frame_type;
// set update rate
set_update_rate(_speed_hz);
// load boot-up servo test cycles into counter to be consumed
_servo_test_cycle_counter = _servo_test;
// ensure inputs are not passed through to servos on start-up
_servo_mode = SERVO_CONTROL_MODE_AUTOMATED;
// initialise radio passthrough for collective to middle
_throttle_radio_passthrough = 0.5f;
// initialise Servo/PWM ranges and endpoints
if (!init_outputs()) {
// don't set initialised_ok
return;
}
// calculate all scalars
calculate_scalars();
// record successful initialisation if what we setup was the desired frame_class
_flags.initialised_ok = (frame_class == MOTOR_FRAME_HELI);
}
// reset_flight_controls - resets all controls and scalars to flight status
void AP_MotorsHeli::reset_flight_controls()
{
reset_radio_passthrough();
_servo_mode = SERVO_CONTROL_MODE_AUTOMATED;
init_outputs();
calculate_scalars();
}
// reset_flight_controls - resets all controls and scalars to flight status
void AP_MotorsHeli::reset_flight_controls()
{
reset_radio_passthrough();
_servo_manual = 0;
init_outputs();
calculate_scalars();
}
// output_disarmed - sends commands to the motors
void AP_MotorsHeli::output_disarmed()
{
if (_servo_test_cycle_counter > 0){
// perform boot-up servo test cycle if enabled
servo_test();
} else {
// manual override (i.e. when setting up swash)
switch (_servo_mode) {
case SERVO_CONTROL_MODE_MANUAL_PASSTHROUGH:
// pass pilot commands straight through to swash
_roll_control_input = _roll_radio_passthrough;
_pitch_control_input = _pitch_radio_passthrough;
_throttle_control_input = _throttle_radio_passthrough;
_yaw_control_input = _yaw_radio_passthrough;
break;
case SERVO_CONTROL_MODE_MANUAL_CENTER:
// fixate mid collective
_roll_control_input = 0;
_pitch_control_input = 0;
_throttle_control_input = _collective_mid_pwm;
_yaw_control_input = 0;
break;
case SERVO_CONTROL_MODE_MANUAL_MAX:
// fixate max collective
_roll_control_input = 0;
_pitch_control_input = 0;
_throttle_control_input = 1000;
_yaw_control_input = 4500;
break;
case SERVO_CONTROL_MODE_MANUAL_MIN:
// fixate min collective
_roll_control_input = 0;
_pitch_control_input = 0;
_throttle_control_input = 0;
_yaw_control_input = -4500;
break;
case SERVO_CONTROL_MODE_MANUAL_OSCILLATE:
// use servo_test function from child classes
servo_test();
break;
default:
// no manual override
break;
}
}
// ensure swash servo endpoints haven't been moved
init_outputs();
// continuously recalculate scalars to allow setup
calculate_scalars();
// helicopters always run stabilizing flight controls
move_actuators(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
update_motor_control(ROTOR_CONTROL_STOP);
}
// init
void AP_MotorsHeli::Init()
{
// set update rate
set_update_rate(_speed_hz);
// ensure inputs are not passed through to servos on start-up
_servo_manual = 0;
// initialise Servo/PWM ranges and endpoints
init_outputs();
// calculate all scalars
calculate_scalars();
}
// init
void AP_MotorsHeli::Init()
{
// set update rate
set_update_rate(_speed_hz);
// load boot-up servo test cycles into counter to be consumed
_servo_test_cycle_counter = _servo_test;
// ensure inputs are not passed through to servos on start-up
_servo_mode = SERVO_CONTROL_MODE_AUTOMATED;
// initialise Servo/PWM ranges and endpoints
init_outputs();
// calculate all scalars
calculate_scalars();
}
// output_disarmed - sends commands to the motors
void AP_MotorsHeli::output_disarmed()
{
// if manual override (i.e. when setting up swash), pass pilot commands straight through to swash
if (_servo_manual == 1) {
_roll_control_input = _roll_radio_passthrough;
_pitch_control_input = _pitch_radio_passthrough;
_throttle_control_input = _throttle_radio_passthrough;
_yaw_control_input = _yaw_radio_passthrough;
}
// ensure swash servo endpoints haven't been moved
init_outputs();
// continuously recalculate scalars to allow setup
calculate_scalars();
// helicopters always run stabilizing flight controls
move_actuators(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
update_motor_control(ROTOR_CONTROL_STOP);
}
