ardupilot/libraries/AP_Motors/AP_MotorsHeli_Single.cpp
Andrew Tridgell 11975223dd AP_Motors: added H_RSC_POWER_NEGC and H_RSC_SLEWRATE
this gives more control over throttle for petrol
helis. H_RSC_POWER_NEGC allows for a asymmetric V-curve, which allows
for less power being put into the head when landing or when sitting on
the ground. That can lead to significantly less vibration and chance
of ground oscillation. A heli not being flown with aerobatics does not
need to use high throttle at negative collective pitch.

The H_RSC_SLEWRATE allows for a maximum throttle slew rate to be
set. Some petrol motors can cut if the throttle is moved too
quickly. We had this happen at a height of 6m when switching from
ALT_HOLD to STABILIZE mode. It also lowers the chance of the blades
skewing in their holders with the sudden change of power when the heli
is disarmed. In general it is a bad idea to do instantaneous large
movements of a IC engine throttle.
2016-07-03 18:29:05 +10:00

581 lines
23 KiB
C++

// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdlib.h>
#include <AP_HAL/AP_HAL.h>
#include <RC_Channel/RC_Channel.h>
#include "AP_MotorsHeli_Single.h"
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = {
AP_NESTEDGROUPINFO(AP_MotorsHeli, 0),
// @Param: SV1_POS
// @DisplayName: Servo 1 Position
// @Description: Angular location of swash servo #1
// @Range: -180 180
// @Units: Degrees
// @User: Standard
// @Increment: 1
AP_GROUPINFO("SV1_POS", 1, AP_MotorsHeli_Single, _servo1_pos, AP_MOTORS_HELI_SINGLE_SERVO1_POS),
// @Param: SV2_POS
// @DisplayName: Servo 2 Position
// @Description: Angular location of swash servo #2
// @Range: -180 180
// @Units: Degrees
// @User: Standard
// @Increment: 1
AP_GROUPINFO("SV2_POS", 2, AP_MotorsHeli_Single, _servo2_pos, AP_MOTORS_HELI_SINGLE_SERVO2_POS),
// @Param: SV3_POS
// @DisplayName: Servo 3 Position
// @Description: Angular location of swash servo #3
// @Range: -180 180
// @Units: Degrees
// @User: Standard
// @Increment: 1
AP_GROUPINFO("SV3_POS", 3, AP_MotorsHeli_Single, _servo3_pos, AP_MOTORS_HELI_SINGLE_SERVO3_POS),
// @Param: TAIL_TYPE
// @DisplayName: Tail Type
// @Description: Tail type selection. Simpler yaw controller used if external gyro is selected
// @Values: 0:Servo only,1:Servo with ExtGyro,2:DirectDrive VarPitch,3:DirectDrive FixedPitch
// @User: Standard
AP_GROUPINFO("TAIL_TYPE", 4, AP_MotorsHeli_Single, _tail_type, AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO),
// @Param: SWASH_TYPE
// @DisplayName: Swash Type
// @Description: Swash Type Setting - either 3-servo CCPM or H1 Mechanical Mixing
// @Values: 0:3-Servo CCPM, 1:H1 Mechanical Mixing
// @User: Standard
AP_GROUPINFO("SWASH_TYPE", 5, AP_MotorsHeli_Single, _swash_type, AP_MOTORS_HELI_SINGLE_SWASH_CCPM),
// @Param: GYR_GAIN
// @DisplayName: External Gyro Gain
// @Description: PWM sent to external gyro on ch7 when tail type is Servo w/ ExtGyro
// @Range: 0 1000
// @Units: PWM
// @Increment: 1
// @User: Standard
AP_GROUPINFO("GYR_GAIN", 6, AP_MotorsHeli_Single, _ext_gyro_gain_std, AP_MOTORS_HELI_SINGLE_EXT_GYRO_GAIN),
// @Param: PHANG
// @DisplayName: Swashplate Phase Angle Compensation
// @Description: Phase angle correction for rotor head. If pitching the swash forward induces a roll, this can be correct the problem
// @Range: -90 90
// @Units: Degrees
// @User: Advanced
// @Increment: 1
AP_GROUPINFO("PHANG", 7, AP_MotorsHeli_Single, _phase_angle, 0),
// @Param: COLYAW
// @DisplayName: Collective-Yaw Mixing
// @Description: Feed-forward compensation to automatically add rudder input when collective pitch is increased. Can be positive or negative depending on mechanics.
// @Range: -10 10
// @Increment: 0.1
AP_GROUPINFO("COLYAW", 8, AP_MotorsHeli_Single, _collective_yaw_effect, 0),
// @Param: FLYBAR_MODE
// @DisplayName: Flybar Mode Selector
// @Description: Flybar present or not. Affects attitude controller used during ACRO flight mode
// @Values: 0:NoFlybar,1:Flybar
// @User: Standard
AP_GROUPINFO("FLYBAR_MODE", 9, AP_MotorsHeli_Single, _flybar_mode, AP_MOTORS_HELI_NOFLYBAR),
// @Param: TAIL_SPEED
// @DisplayName: Direct Drive VarPitch Tail ESC speed
// @Description: Direct Drive VarPitch Tail ESC speed. Only used when TailType is DirectDrive VarPitch
// @Range: 0 1000
// @Units: PWM
// @Increment: 1
// @User: Standard
AP_GROUPINFO("TAIL_SPEED", 10, AP_MotorsHeli_Single, _direct_drive_tailspeed, AP_MOTORS_HELI_SINGLE_DDVPT_SPEED_DEFAULT),
// @Param: GYR_GAIN_ACRO
// @DisplayName: External Gyro Gain for ACRO
// @Description: PWM sent to external gyro on ch7 when tail type is Servo w/ ExtGyro. A value of zero means to use H_GYR_GAIN
// @Range: 0 1000
// @Units: PWM
// @Increment: 1
// @User: Standard
AP_GROUPINFO("GYR_GAIN_ACRO", 11, AP_MotorsHeli_Single, _ext_gyro_gain_acro, 0),
// @Group: SV1_
// @Path: ../RC_Channel/RC_Channel.cpp
AP_SUBGROUPINFO(_swash_servo_1, "SV1_", 12, AP_MotorsHeli_Single, RC_Channel),
// @Group: SV2_
// @Path: ../RC_Channel/RC_Channel.cpp
AP_SUBGROUPINFO(_swash_servo_2, "SV2_", 13, AP_MotorsHeli_Single, RC_Channel),
// @Group: SV3_
// @Path: ../RC_Channel/RC_Channel.cpp
AP_SUBGROUPINFO(_swash_servo_3, "SV3_", 14, AP_MotorsHeli_Single, RC_Channel),
// @Group: SV4_
// @Path: ../RC_Channel/RC_Channel.cpp
AP_SUBGROUPINFO(_yaw_servo, "SV4_", 15, AP_MotorsHeli_Single, RC_Channel),
// @Param: RSC_PWM_MIN
// @DisplayName: RSC PWM output miniumum
// @Description: This sets the PWM output on RSC channel for maximum rotor speed
// @Range: 0 2000
// @User: Standard
AP_GROUPINFO("RSC_PWM_MIN", 16, AP_MotorsHeli_Single, _main_rotor._pwm_min, 1000),
// @Param: RSC_PWM_MAX
// @DisplayName: RSC PWM output maxiumum
// @Description: This sets the PWM output on RSC channel for miniumum rotor speed
// @Range: 0 2000
// @User: Standard
AP_GROUPINFO("RSC_PWM_MAX", 17, AP_MotorsHeli_Single, _main_rotor._pwm_max, 2000),
// @Param: RSC_PWM_REV
// @DisplayName: RSC PWM reversal
// @Description: This controls reversal of the RSC channel output
// @Values: -1:Reversed,1:Normal
// @User: Standard
AP_GROUPINFO("RSC_PWM_REV", 18, AP_MotorsHeli_Single, _main_rotor._pwm_rev, 1),
// parameters up to and including 29 are reserved for tradheli
AP_GROUPEND
};
// set update rate to motors - a value in hertz
void AP_MotorsHeli_Single::set_update_rate( uint16_t speed_hz )
{
// record requested speed
_speed_hz = speed_hz;
// setup fast channels
uint32_t mask =
1U << AP_MOTORS_MOT_1 |
1U << AP_MOTORS_MOT_2 |
1U << AP_MOTORS_MOT_3 |
1U << AP_MOTORS_MOT_4;
rc_set_freq(mask, _speed_hz);
}
// enable - starts allowing signals to be sent to motors and servos
void AP_MotorsHeli_Single::enable()
{
// enable output channels
rc_enable_ch(AP_MOTORS_MOT_1); // swash servo 1
rc_enable_ch(AP_MOTORS_MOT_2); // swash servo 2
rc_enable_ch(AP_MOTORS_MOT_3); // swash servo 3
rc_enable_ch(AP_MOTORS_MOT_4); // yaw
rc_enable_ch(AP_MOTORS_HELI_SINGLE_AUX); // output for gyro gain or direct drive variable pitch tail motor
rc_enable_ch(AP_MOTORS_HELI_SINGLE_RSC); // output for main rotor esc
}
// init_outputs - initialise Servo/PWM ranges and endpoints
void AP_MotorsHeli_Single::init_outputs()
{
// reset swash servo range and endpoints
reset_swash_servo (_swash_servo_1);
reset_swash_servo (_swash_servo_2);
reset_swash_servo (_swash_servo_3);
_yaw_servo.set_angle(4500);
// set main rotor servo range
// tail rotor servo use range as set in vehicle code for rc7
_main_rotor.init_servo();
}
// output_test - spin a motor at the pwm value specified
// motor_seq is the motor's sequence number from 1 to the number of motors on the frame
// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
void AP_MotorsHeli_Single::output_test(uint8_t motor_seq, int16_t pwm)
{
// exit immediately if not armed
if (!armed()) {
return;
}
// output to motors and servos
switch (motor_seq) {
case 1:
// swash servo 1
rc_write(AP_MOTORS_MOT_1, pwm);
break;
case 2:
// swash servo 2
rc_write(AP_MOTORS_MOT_2, pwm);
break;
case 3:
// swash servo 3
rc_write(AP_MOTORS_MOT_3, pwm);
break;
case 4:
// external gyro & tail servo
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
if (_acro_tail && _ext_gyro_gain_acro > 0) {
write_aux(_ext_gyro_gain_acro/1000.0f);
} else {
write_aux(_ext_gyro_gain_std/1000.0f);
}
}
rc_write(AP_MOTORS_MOT_4, pwm);
break;
case 5:
// main rotor
rc_write(AP_MOTORS_HELI_SINGLE_RSC, pwm);
break;
default:
// do nothing
break;
}
}
// set_desired_rotor_speed
void AP_MotorsHeli_Single::set_desired_rotor_speed(float desired_speed)
{
_main_rotor.set_desired_speed(desired_speed);
// always send desired speed to tail rotor control, will do nothing if not DDVPT not enabled
_tail_rotor.set_desired_speed(_direct_drive_tailspeed/1000.0f);
}
// calculate_scalars - recalculates various scalers used.
void AP_MotorsHeli_Single::calculate_armed_scalars()
{
_main_rotor.set_ramp_time(_rsc_ramp_time);
_main_rotor.set_runup_time(_rsc_runup_time);
_main_rotor.set_critical_speed(_rsc_critical/1000.0f);
_main_rotor.set_idle_output(_rsc_idle_output/1000.0f);
_main_rotor.set_power_output_range(_rsc_power_low/1000.0f, _rsc_power_high/1000.0f, _rsc_power_negc/1000.0f, (uint16_t)_rsc_slewrate.get());
}
// calculate_scalars - recalculates various scalers used.
void AP_MotorsHeli_Single::calculate_scalars()
{
// range check collective min, max and mid
if( _collective_min >= _collective_max ) {
_collective_min = AP_MOTORS_HELI_COLLECTIVE_MIN;
_collective_max = AP_MOTORS_HELI_COLLECTIVE_MAX;
}
_collective_mid = constrain_int16(_collective_mid, _collective_min, _collective_max);
// calculate collective mid point as a number from 0 to 1
_collective_mid_pct = ((float)(_collective_mid-_collective_min))/((float)(_collective_max-_collective_min));
// calculate factors based on swash type and servo position
calculate_roll_pitch_collective_factors();
// send setpoints to main rotor controller and trigger recalculation of scalars
_main_rotor.set_control_mode(static_cast<RotorControlMode>(_rsc_mode.get()));
calculate_armed_scalars();
// send setpoints to tail rotor controller and trigger recalculation of scalars
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
_tail_rotor.set_control_mode(ROTOR_CONTROL_MODE_SPEED_SETPOINT);
_tail_rotor.set_ramp_time(AP_MOTORS_HELI_SINGLE_DDVPT_RAMP_TIME);
_tail_rotor.set_runup_time(AP_MOTORS_HELI_SINGLE_DDVPT_RUNUP_TIME);
_tail_rotor.set_critical_speed(_rsc_critical/1000.0f);
_tail_rotor.set_idle_output(_rsc_idle_output/1000.0f);
} else {
_tail_rotor.set_control_mode(ROTOR_CONTROL_MODE_DISABLED);
_tail_rotor.set_ramp_time(0);
_tail_rotor.set_runup_time(0);
_tail_rotor.set_critical_speed(0);
_tail_rotor.set_idle_output(0);
}
}
// calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position
void AP_MotorsHeli_Single::calculate_roll_pitch_collective_factors()
{
if (_swash_type == AP_MOTORS_HELI_SINGLE_SWASH_CCPM) { //CCPM Swashplate, perform control mixing
// roll factors
_rollFactor[CH_1] = cosf(radians(_servo1_pos + 90 - _phase_angle));
_rollFactor[CH_2] = cosf(radians(_servo2_pos + 90 - _phase_angle));
_rollFactor[CH_3] = cosf(radians(_servo3_pos + 90 - _phase_angle));
// pitch factors
_pitchFactor[CH_1] = cosf(radians(_servo1_pos - _phase_angle));
_pitchFactor[CH_2] = cosf(radians(_servo2_pos - _phase_angle));
_pitchFactor[CH_3] = cosf(radians(_servo3_pos - _phase_angle));
// collective factors
_collectiveFactor[CH_1] = 1;
_collectiveFactor[CH_2] = 1;
_collectiveFactor[CH_3] = 1;
}else{ //H1 Swashplate, keep servo outputs separated
// roll factors
_rollFactor[CH_1] = 1;
_rollFactor[CH_2] = 0;
_rollFactor[CH_3] = 0;
// pitch factors
_pitchFactor[CH_1] = 0;
_pitchFactor[CH_2] = 1;
_pitchFactor[CH_3] = 0;
// collective factors
_collectiveFactor[CH_1] = 0;
_collectiveFactor[CH_2] = 0;
_collectiveFactor[CH_3] = 1;
}
}
// get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
uint16_t AP_MotorsHeli_Single::get_motor_mask()
{
// heli uses channels 1,2,3,4,7 and 8
return rc_map_mask(1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << AP_MOTORS_HELI_SINGLE_AUX | 1U << AP_MOTORS_HELI_SINGLE_RSC);
}
// update_motor_controls - sends commands to motor controllers
void AP_MotorsHeli_Single::update_motor_control(RotorControlState state)
{
// Send state update to motors
_tail_rotor.output(state);
_main_rotor.output(state);
if (state == ROTOR_CONTROL_STOP){
// set engine run enable aux output to not run position to kill engine when disarmed
RC_Channel_aux::set_radio_to_min(RC_Channel_aux::k_engine_run_enable);
} else {
// else if armed, set engine run enable output to run position
RC_Channel_aux::set_radio_to_max(RC_Channel_aux::k_engine_run_enable);
}
// Check if both rotors are run-up, tail rotor controller always returns true if not enabled
_heliflags.rotor_runup_complete = ( _main_rotor.is_runup_complete() && _tail_rotor.is_runup_complete() );
}
//
// move_actuators - moves swash plate and tail rotor
// - expected ranges:
// roll : -1 ~ +1
// pitch: -1 ~ +1
// collective: 0 ~ 1
// yaw: -1 ~ +1
//
void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float coll_in, float yaw_out)
{
float yaw_offset = 0.0f;
// initialize limits flag
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// rescale roll_out and pitch_out into the min and max ranges to provide linear motion
// across the input range instead of stopping when the input hits the constrain value
// these calculations are based on an assumption of the user specified cyclic_max
// coming into this equation at 4500 or less
float total_out = norm(pitch_out, roll_out);
if (total_out > (_cyclic_max/4500.0f)) {
float ratio = (float)(_cyclic_max/4500.0f) / total_out;
roll_out *= ratio;
pitch_out *= ratio;
limit.roll_pitch = true;
}
// constrain collective input
float collective_out = coll_in;
if (collective_out <= 0.0f) {
collective_out = 0.0f;
limit.throttle_lower = true;
}
if (collective_out >= 1.0f) {
collective_out = 1.0f;
limit.throttle_upper = true;
}
// ensure not below landed/landing collective
if (_heliflags.landing_collective && collective_out < (_land_collective_min/1000.0f)) {
collective_out = (_land_collective_min/1000.0f);
limit.throttle_lower = true;
}
// if servo output not in manual mode, process pre-compensation factors
if (_servo_mode == SERVO_CONTROL_MODE_AUTOMATED) {
// rudder feed forward based on collective
// the feed-forward is not required when the motor is stopped or at idle, and thus not creating torque
// also not required if we are using external gyro
if ((_main_rotor.get_control_output() > _main_rotor.get_idle_output()) && _tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
// sanity check collective_yaw_effect
_collective_yaw_effect = constrain_float(_collective_yaw_effect, -AP_MOTORS_HELI_SINGLE_COLYAW_RANGE, AP_MOTORS_HELI_SINGLE_COLYAW_RANGE);
yaw_offset = _collective_yaw_effect * fabsf(collective_out - _collective_mid_pct);
}
} else {
yaw_offset = 0.0f;
}
// feed power estimate into main rotor controller
// ToDo: include tail rotor power?
// ToDo: add main rotor cyclic power?
if (collective_out > _collective_mid_pct) {
// +ve motor load for +ve collective
_main_rotor.set_motor_load((collective_out - _collective_mid_pct) / (1.0f - _collective_mid_pct));
} else {
// -ve motor load for -ve collective
_main_rotor.set_motor_load((collective_out - _collective_mid_pct) / _collective_mid_pct);
}
// swashplate servos
float collective_scalar = ((float)(_collective_max-_collective_min))/1000.0f;
float coll_out_scaled = collective_out * collective_scalar + (_collective_min - 1000)/1000.0f;
float servo1_out = ((_rollFactor[CH_1] * roll_out) + (_pitchFactor[CH_1] * pitch_out))*0.45f + _collectiveFactor[CH_1] * coll_out_scaled;
float servo2_out = ((_rollFactor[CH_2] * roll_out) + (_pitchFactor[CH_2] * pitch_out))*0.45f + _collectiveFactor[CH_2] * coll_out_scaled;
if (_swash_type == AP_MOTORS_HELI_SINGLE_SWASH_H1) {
servo1_out += 0.5f;
servo2_out += 0.5f;
}
float servo3_out = ((_rollFactor[CH_3] * roll_out) + (_pitchFactor[CH_3] * pitch_out))*0.45f + _collectiveFactor[CH_3] * coll_out_scaled;
hal.rcout->cork();
// actually move the servos
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_0to1(servo1_out, _swash_servo_1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_0to1(servo2_out, _swash_servo_2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_0to1(servo3_out, _swash_servo_3));
// update the yaw rate using the tail rotor/servo
move_yaw(yaw_out + yaw_offset);
hal.rcout->push();
}
// move_yaw
void AP_MotorsHeli_Single::move_yaw(float yaw_out)
{
// sanity check yaw_out
if (yaw_out < -1.0f) {
yaw_out = -1.0f;
limit.yaw = true;
}
if (yaw_out > 1.0f) {
yaw_out = 1.0f;
limit.yaw = true;
}
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(yaw_out, _yaw_servo));
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
// output gain to exernal gyro
if (_acro_tail && _ext_gyro_gain_acro > 0) {
write_aux(_ext_gyro_gain_acro/1000.0f);
} else {
write_aux(_ext_gyro_gain_std/1000.0f);
}
} else if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH && _main_rotor.get_desired_speed() > 0.0f) {
// output yaw servo to tail rsc
// To-Do: fix this messy calculation
write_aux(yaw_out*0.5f+1.0f);
}
}
// write_aux - converts servo_out parameter value (0 to 1 range) to pwm and outputs to aux channel (ch7)
void AP_MotorsHeli_Single::write_aux(float servo_out)
{
rc_write(AP_MOTORS_HELI_SINGLE_AUX, calc_pwm_output_0to1(servo_out, _servo_aux));
}
// servo_test - move servos through full range of movement
void AP_MotorsHeli_Single::servo_test()
{
_servo_test_cycle_time += 1.0f / _loop_rate;
if ((_servo_test_cycle_time >= 0.0f && _servo_test_cycle_time < 0.5f)|| // Tilt swash back
(_servo_test_cycle_time >= 6.0f && _servo_test_cycle_time < 6.5f)){
_pitch_test += (1.0f / (_loop_rate/2));
_oscillate_angle += 8 * M_PI / _loop_rate;
_yaw_test = 0.5f * sinf(_oscillate_angle);
} else if ((_servo_test_cycle_time >= 0.5f && _servo_test_cycle_time < 4.5f)|| // Roll swash around
(_servo_test_cycle_time >= 6.5f && _servo_test_cycle_time < 10.5f)){
_oscillate_angle += M_PI / (2 * _loop_rate);
_roll_test = sinf(_oscillate_angle);
_pitch_test = cosf(_oscillate_angle);
_yaw_test = sinf(_oscillate_angle);
} else if ((_servo_test_cycle_time >= 4.5f && _servo_test_cycle_time < 5.0f)|| // Return swash to level
(_servo_test_cycle_time >= 10.5f && _servo_test_cycle_time < 11.0f)){
_pitch_test -= (1.0f / (_loop_rate/2));
_oscillate_angle += 8 * M_PI / _loop_rate;
_yaw_test = 0.5f * sinf(_oscillate_angle);
} else if (_servo_test_cycle_time >= 5.0f && _servo_test_cycle_time < 6.0f){ // Raise swash to top
_collective_test += (1.0f / _loop_rate);
_oscillate_angle += 2 * M_PI / _loop_rate;
_yaw_test = sinf(_oscillate_angle);
} else if (_servo_test_cycle_time >= 11.0f && _servo_test_cycle_time < 12.0f){ // Lower swash to bottom
_collective_test -= (1.0f / _loop_rate);
_oscillate_angle += 2 * M_PI / _loop_rate;
_yaw_test = sinf(_oscillate_angle);
} else { // reset cycle
_servo_test_cycle_time = 0.0f;
_oscillate_angle = 0.0f;
_collective_test = 0.0f;
_roll_test = 0.0f;
_pitch_test = 0.0f;
_yaw_test = 0.0f;
// decrement servo test cycle counter at the end of the cycle
if (_servo_test_cycle_counter > 0){
_servo_test_cycle_counter--;
}
}
// over-ride servo commands to move servos through defined ranges
_throttle_in = _collective_test;
_roll_in = _roll_test;
_pitch_in = _pitch_test;
_yaw_in = _yaw_test;
}
// parameter_check - check if helicopter specific parameters are sensible
bool AP_MotorsHeli_Single::parameter_check(bool display_msg) const
{
// returns false if Phase Angle is outside of range
if ((_phase_angle > 90) || (_phase_angle < -90)){
if (display_msg) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: H_PHANG out of range");
}
return false;
}
// returns false if Acro External Gyro Gain is outside of range
if ((_ext_gyro_gain_acro < 0) || (_ext_gyro_gain_acro > 1000)){
if (display_msg) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: H_GYR_GAIN_ACRO out of range");
}
return false;
}
// returns false if Standard External Gyro Gain is outside of range
if ((_ext_gyro_gain_std < 0) || (_ext_gyro_gain_std > 1000)){
if (display_msg) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: H_GYR_GAIN out of range");
}
return false;
}
// check parent class parameters
return AP_MotorsHeli::parameter_check(display_msg);
}