ardupilot/libraries/AP_Motors/AP_MotorsHeli.cpp

392 lines
14 KiB
C++
Raw Normal View History

2013-05-29 20:51:34 -03:00
// -*- 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/>.
*/
/*
* AP_MotorsHeli.cpp - ArduCopter motors library
* Code by RandyMackay. DIYDrones.com
*
*/
2012-10-26 20:59:07 -03:00
#include <stdlib.h>
#include <AP_HAL/AP_HAL.h>
#include "AP_MotorsHeli.h"
#include <GCS_MAVLink/GCS.h>
2012-10-26 20:59:07 -03:00
extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = {
// 1 was ROL_MAX which has been replaced by CYC_MAX
// 2 was PIT_MAX which has been replaced by CYC_MAX
// @Param: COL_MIN
// @DisplayName: Collective Pitch Minimum
// @Description: Lowest possible servo position for the swashplate
// @Range: 1000 2000
// @Units: PWM
// @Increment: 1
// @User: Standard
AP_GROUPINFO("COL_MIN", 3, AP_MotorsHeli, _collective_min, AP_MOTORS_HELI_COLLECTIVE_MIN),
// @Param: COL_MAX
// @DisplayName: Collective Pitch Maximum
// @Description: Highest possible servo position for the swashplate
// @Range: 1000 2000
// @Units: PWM
// @Increment: 1
// @User: Standard
AP_GROUPINFO("COL_MAX", 4, AP_MotorsHeli, _collective_max, AP_MOTORS_HELI_COLLECTIVE_MAX),
// @Param: COL_MID
// @DisplayName: Collective Pitch Mid-Point
2016-05-12 13:59:30 -03:00
// @Description: Swash servo position corresponding to zero collective pitch (or zero lift for Asymmetrical blades)
// @Range: 1000 2000
// @Units: PWM
// @Increment: 1
// @User: Standard
AP_GROUPINFO("COL_MID", 5, AP_MotorsHeli, _collective_mid, AP_MOTORS_HELI_COLLECTIVE_MID),
// @Param: SV_MAN
// @DisplayName: Manual Servo Mode
// @Description: Manual servo override for swash set-up. Do not set this manually!
// @Values: 0:Disabled,1:Passthrough,2:Max collective,3:Mid collective,4:Min collective
// @User: Standard
AP_GROUPINFO("SV_MAN", 6, AP_MotorsHeli, _servo_mode, SERVO_CONTROL_MODE_AUTOMATED),
// @Param: RSC_SETPOINT
// @DisplayName: External Motor Governor Setpoint
// @Description: PWM passed to the external motor governor when external governor is enabled
// @Range: 0 1000
// @Units: PWM
// @Increment: 10
// @User: Standard
AP_GROUPINFO("RSC_SETPOINT", 7, AP_MotorsHeli, _rsc_setpoint, AP_MOTORS_HELI_RSC_SETPOINT),
// @Param: RSC_MODE
// @DisplayName: Rotor Speed Control Mode
// @Description: Determines the method of rotor speed control
// @Values: 1:Ch8 Input, 2:SetPoint, 3:Throttle Curve
// @User: Standard
AP_GROUPINFO("RSC_MODE", 8, AP_MotorsHeli, _rsc_mode, (int8_t)ROTOR_CONTROL_MODE_SPEED_PASSTHROUGH),
2013-11-06 08:39:10 -04:00
// @Param: LAND_COL_MIN
// @DisplayName: Landing Collective Minimum
// @Description: Minimum collective position while landed or landing
// @Range: 0 500
// @Units: pwm
// @Increment: 1
// @User: Standard
AP_GROUPINFO("LAND_COL_MIN", 9, AP_MotorsHeli, _land_collective_min, AP_MOTORS_HELI_LAND_COLLECTIVE_MIN),
2013-11-06 08:39:10 -04:00
// @Param: RSC_RAMP_TIME
// @DisplayName: RSC Ramp Time
// @Description: Time in seconds for the output to the main rotor's ESC to reach full speed
// @Range: 0 60
// @Units: Seconds
// @User: Standard
AP_GROUPINFO("RSC_RAMP_TIME", 10, AP_MotorsHeli, _rsc_ramp_time, AP_MOTORS_HELI_RSC_RAMP_TIME),
// @Param: RSC_RUNUP_TIME
// @DisplayName: RSC Runup Time
// @Description: Time in seconds for the main rotor to reach full speed. Must be longer than RSC_RAMP_TIME
// @Range: 0 60
// @Units: Seconds
// @User: Standard
AP_GROUPINFO("RSC_RUNUP_TIME", 11, AP_MotorsHeli, _rsc_runup_time, AP_MOTORS_HELI_RSC_RUNUP_TIME),
// @Param: RSC_CRITICAL
// @DisplayName: Critical Rotor Speed
// @Description: Rotor speed below which flight is not possible
// @Range: 0 1000
// @Increment: 10
// @User: Standard
AP_GROUPINFO("RSC_CRITICAL", 12, AP_MotorsHeli, _rsc_critical, AP_MOTORS_HELI_RSC_CRITICAL),
2015-08-07 20:58:49 -03:00
// @Param: RSC_IDLE
// @DisplayName: Rotor Speed Output at Idle
// @Description: Rotor speed output while armed but rotor control speed is not engaged
// @Range: 0 500
// @Increment: 10
// @User: Standard
AP_GROUPINFO("RSC_IDLE", 13, AP_MotorsHeli, _rsc_idle_output, AP_MOTORS_HELI_RSC_IDLE_DEFAULT),
// @Param: RSC_POWER_LOW
// @DisplayName: Throttle Servo Low Power Position
// @Description: Throttle output at zero collective pitch. This is on a scale from 0 to 1000, where 1000 is full throttle and 0 is zero throttle. Actual PWM values are controlled by H_RSC_PWM_MIN and H_RSC_PWM_MAX. Zero collective pitch is defined by H_COL_MID.
// @Range: 0 1000
// @Increment: 10
// @User: Standard
AP_GROUPINFO("RSC_POWER_LOW", 14, AP_MotorsHeli, _rsc_power_low, AP_MOTORS_HELI_RSC_POWER_LOW_DEFAULT),
// @Param: RSC_POWER_HIGH
// @DisplayName: Throttle Servo High Power Position
// @Description: Throttle output at maximum collective pitch. This is on a scale from 0 to 1000, where 1000 is full throttle and 0 is zero throttle. Actual PWM values are controlled by H_RSC_PWM_MIN and H_RSC_PWM_MAX.
// @Range: 0 1000
// @Increment: 10
// @User: Standard
AP_GROUPINFO("RSC_POWER_HIGH", 15, AP_MotorsHeli, _rsc_power_high, AP_MOTORS_HELI_RSC_POWER_HIGH_DEFAULT),
2015-08-07 20:58:49 -03:00
// @Param: CYC_MAX
// @DisplayName: Cyclic Pitch Angle Max
// @Description: Maximum pitch angle of the swash plate
// @Range: 0 18000
// @Units: Centi-Degrees
// @Increment: 100
// @User: Advanced
AP_GROUPINFO("CYC_MAX", 16, AP_MotorsHeli, _cyclic_max, AP_MOTORS_HELI_SWASH_CYCLIC_MAX),
// @Param: SV_TEST
// @DisplayName: Boot-up Servo Test Cycles
// @Description: Number of cycles to run servo test on boot-up
// @Range: 0 10
// @Increment: 1
// @User: Standard
AP_GROUPINFO("SV_TEST", 17, AP_MotorsHeli, _servo_test, 0),
// @Param: RSC_POWER_NEGC
// @DisplayName: Throttle servo negative collective power position
// @Description: Throttle output at full negative collective pitch. This is on a scale from 0 to 1000, where 1000 is full throttle and 0 is zero throttle. Actual PWM values are controlled by H_RSC_PWM_MIN and H_RSC_PWM_MAX. If this is equal to H_RSC_POWER_HIGH then you will have a symmetric V-curve for the throttle response.
// @Range: 1 1000
// @Increment: 10
// @User: Standard
AP_GROUPINFO("RSC_POWER_NEGC", 18, AP_MotorsHeli, _rsc_power_negc, AP_MOTORS_HELI_RSC_POWER_HIGH_DEFAULT),
// @Param: RSC_SLEWRATE
// @DisplayName: Throttle servo slew rate
// @Description: This controls the maximum rate at which the throttle output can change, as a percentage per second. A value of 100 means the throttle can change over its full range in one second. A value of zero gives unlimited slew rate.
// @Range: 0 500
// @Increment: 10
// @User: Standard
AP_GROUPINFO("RSC_SLEWRATE", 19, AP_MotorsHeli, _rsc_slewrate, 0),
AP_GROUPEND
};
//
// public methods
//
// 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 radio passthrough for collective to middle
_throttle_radio_passthrough = 0.5f;
// initialise Servo/PWM ranges and endpoints
init_outputs();
// calculate all scalars
calculate_scalars();
}
// output_min - sets servos to neutral point with motors stopped
void AP_MotorsHeli::output_min()
{
// move swash to mid
move_actuators(0.0f,0.0f,0.5f,0.0f);
update_motor_control(ROTOR_CONTROL_STOP);
// override limits flags
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
limit.throttle_upper = false;
}
// output - sends commands to the servos
void AP_MotorsHeli::output()
{
// update throttle filter
update_throttle_filter();
if (_flags.armed) {
calculate_armed_scalars();
if (!_flags.interlock) {
output_armed_zero_throttle();
} else {
output_armed_stabilizing();
}
} else {
output_disarmed();
}
};
// sends commands to the motors
void AP_MotorsHeli::output_armed_stabilizing()
{
// if manual override active after arming, deactivate it and reinitialize servos
if (_servo_mode != SERVO_CONTROL_MODE_AUTOMATED) {
reset_flight_controls();
}
move_actuators(_roll_in, _pitch_in, get_throttle(), _yaw_in);
update_motor_control(ROTOR_CONTROL_ACTIVE);
}
// output_armed_zero_throttle - sends commands to the motors
void AP_MotorsHeli::output_armed_zero_throttle()
{
// if manual override active after arming, deactivate it and reinitialize servos
if (_servo_mode != SERVO_CONTROL_MODE_AUTOMATED) {
reset_flight_controls();
}
move_actuators(_roll_in, _pitch_in, get_throttle(), _yaw_in);
update_motor_control(ROTOR_CONTROL_IDLE);
}
// 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_in = _roll_radio_passthrough;
_pitch_in = _pitch_radio_passthrough;
_throttle_filter.reset(_throttle_radio_passthrough);
_yaw_in = _yaw_radio_passthrough;
break;
case SERVO_CONTROL_MODE_MANUAL_CENTER:
// fixate mid collective
_roll_in = 0.0f;
_pitch_in = 0.0f;
_throttle_filter.reset(_collective_mid_pct);
_yaw_in = 0.0f;
break;
case SERVO_CONTROL_MODE_MANUAL_MAX:
// fixate max collective
_roll_in = 0.0f;
_pitch_in = 0.0f;
_throttle_filter.reset(1.0f);
_yaw_in = 1.0f;
break;
case SERVO_CONTROL_MODE_MANUAL_MIN:
// fixate min collective
_roll_in = 0.0f;
_pitch_in = 0.0f;
_throttle_filter.reset(0.0f);
_yaw_in = -1.0f;
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_in, _pitch_in, get_throttle(), _yaw_in);
update_motor_control(ROTOR_CONTROL_STOP);
}
// parameter_check - check if helicopter specific parameters are sensible
bool AP_MotorsHeli::parameter_check(bool display_msg) const
{
// returns false if _rsc_setpoint is not higher than _rsc_critical as this would not allow rotor_runup_complete to ever return true
if (_rsc_critical >= _rsc_setpoint) {
if (display_msg) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: H_RSC_CRITICAL too large");
}
return false;
}
// returns false if RSC Mode is not set to a valid control mode
if (_rsc_mode <= (int8_t)ROTOR_CONTROL_MODE_DISABLED || _rsc_mode > (int8_t)ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT) {
if (display_msg) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: H_RSC_MODE invalid");
}
return false;
}
// returns false if RSC Runup Time is less than Ramp time as this could cause undesired behaviour of rotor speed estimate
if (_rsc_runup_time <= _rsc_ramp_time){
if (display_msg) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: H_RUNUP_TIME too small");
}
return false;
}
// returns false if idle output is higher than critical rotor speed as this could block runup_complete from going false
if ( _rsc_idle_output >= _rsc_critical){
if (display_msg) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: H_RSC_IDLE too large");
}
return false;
}
// all other cases parameters are OK
return true;
}
// reset_swash_servo
void AP_MotorsHeli::reset_swash_servo(RC_Channel& servo)
{
servo.set_range_out(0, 1000);
// swash servos always use full endpoints as restricting them would lead to scaling errors
AP_Motors: Fix up after RC_Channels refactor 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:28:21 -03:00
servo.set_radio_min(1000);
servo.set_radio_max(2000);
}
// update the throttle input filter
void AP_MotorsHeli::update_throttle_filter()
{
_throttle_filter.apply(_throttle_in, 1.0f/_loop_rate);
// constrain filtered throttle
if (_throttle_filter.get() < 0.0f) {
_throttle_filter.reset(0.0f);
}
if (_throttle_filter.get() > 1.0f) {
_throttle_filter.reset(1.0f);
}
}
// reset_flight_controls - resets all controls and scalars to flight status
void AP_MotorsHeli::reset_flight_controls()
{
_servo_mode = SERVO_CONTROL_MODE_AUTOMATED;
init_outputs();
calculate_scalars();
}