aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Motors: Move rotor speed control into AP_MotorsHeli_RSC.

Browse Source
This commit is contained in:
Fredrik Hedberg 2015-07-21 12:07:54 +02:00 committed by Randy Mackay
parent 1bb6b65ce0
commit ae9a16dc27
4 changed files with 255 additions and 209 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

246
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -346,7 +346,7 @@ void AP_MotorsHeli::output_test(uint8_t motor_seq, int16_t pwm)
bool AP_MotorsHeli::allow_arming() const
{
// returns false if main rotor speed is not zero
if (_rsc_mode != AP_MOTORS_HELI_RSC_MODE_NONE && _rotor_speed_estimate > 0) {
if (_rsc_mode != AP_MOTORS_HELI_RSC_MODE_NONE && _main_rotor.get_estimated_speed() > 0) {
return false;
}
@ -366,34 +366,50 @@ bool AP_MotorsHeli::parameter_check() const
return true;
}
// set_desired_rotor_speed
void AP_MotorsHeli::set_desired_rotor_speed(int16_t desired_speed)
{
_main_rotor.set_desired_speed(desired_speed);
if (desired_speed > 0 && _tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH) {
_tail_rotor.set_desired_speed(_direct_drive_tailspeed);
} else {
_tail_rotor.set_desired_speed(0);
}
}
// return true if the main rotor is up to speed
bool AP_MotorsHeli::rotor_runup_complete() const
{
// if we have no control of motors, assume pilot has spun them up
if (_rsc_mode == AP_MOTORS_HELI_RSC_MODE_NONE) {
return true;
}
return _heliflags.rotor_runup_complete;
}
// recalc_scalers - recalculates various scalers used. Should be called at about 1hz to allow users to see effect of changing parameters
void AP_MotorsHeli::recalc_scalers()
{
// recalculate rotor ramp up increment
if (_rsc_ramp_time <= 0) {
_rsc_ramp_time = 1;
if (_rsc_mode != AP_MOTORS_HELI_RSC_MODE_SETPOINT) {
_tail_rotor.set_ramp_time(0);
_tail_rotor.set_runup_time(0);
_tail_rotor.set_critical_speed(0);
} else {
_main_rotor.set_ramp_time(_rsc_ramp_time);
_main_rotor.set_runup_time(_rsc_runup_time);
_main_rotor.set_critical_speed(_rsc_critical);
}
_rsc_ramp_increment = 1000.0f / (_rsc_ramp_time * _loop_rate);
// recalculate rotor runup increment
if (_rsc_runup_time <= 0 ) {
_rsc_runup_time = 1;
_main_rotor.recalc_scalers();
if (_rsc_mode != AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH) {
_tail_rotor.set_ramp_time(0);
_tail_rotor.set_runup_time(0);
_tail_rotor.set_critical_speed(0);
} else {
_tail_rotor.set_ramp_time(_rsc_ramp_time);
_tail_rotor.set_runup_time(_rsc_runup_time);
_tail_rotor.set_critical_speed(_rsc_critical);
}
if (_rsc_runup_time < _rsc_ramp_time) {
_rsc_runup_time = _rsc_ramp_time;
}
_rsc_runup_increment = 1000.0f / (_rsc_runup_time * _loop_rate);
_tail_rotor.recalc_scalers();
}
// get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
@ -413,18 +429,21 @@ void AP_MotorsHeli::output_armed_not_stabilizing()
// sends commands to the motors
void AP_MotorsHeli::output_armed_stabilizing()
{
// 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;
}
move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
// update rotor and direct drive esc speeds
rsc_control();
if (_tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH) {
_tail_rotor.output_armed();
if (!_tail_rotor.is_runup_complete())
{
_heliflags.rotor_runup_complete = false;
return;
}
}
_main_rotor.output_armed();
_heliflags.rotor_runup_complete = _main_rotor.is_runup_complete();
}
// output_armed_zero_throttle - sends commands to the motors
@ -438,8 +457,15 @@ void AP_MotorsHeli::output_armed_zero_throttle()
// output_disarmed - sends commands to the motors
void AP_MotorsHeli::output_disarmed()
{
// stabilizing servos always operate for helicopters
output_armed_stabilizing();
move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
if (_tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH) {
_tail_rotor.output_disarmed();
}
_main_rotor.output_disarmed();
_heliflags.rotor_runup_complete = false;
}
// reset_swash - free up swash for maximum movements. Used for set-up
@ -555,6 +581,14 @@ void AP_MotorsHeli::calculate_roll_pitch_collective_factors()
//
void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll_in, int16_t yaw_out)
{
// 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;
}
int16_t yaw_offset = 0;
int16_t coll_out_scaled;
@ -628,7 +662,7 @@ void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll
// rudder feed forward based on collective
// the feed-forward is not required when the motor is shut down and not creating torque
// also not required if we are using external gyro
if ((_desired_rotor_speed > 0) && _tail_type != AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO) {
if ((_main_rotor.get_desired_speed() > 0) && _tail_type != AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO) {
// sanity check collective_yaw_effect
_collective_yaw_effect = constrain_float(_collective_yaw_effect, -AP_MOTOR_HELI_COLYAW_RANGE, AP_MOTOR_HELI_COLYAW_RANGE);
yaw_offset = _collective_yaw_effect * abs(_collective_out - _collective_mid_pwm);
@ -673,158 +707,6 @@ void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll
}
}
// rsc_control - update value to send to tail and main rotor's ESC
// desired_rotor_speed is a desired speed from 0 to 1000
void AP_MotorsHeli::rsc_control()
{
// if disarmed output minimums
if (!armed()) {
// shut down tail rotor
if (_tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH || _tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_FIXEDPITCH) {
_tail_direct_drive_out = 0;
write_aux(_tail_direct_drive_out);
}
// shut down main rotor
if (_rsc_mode != AP_MOTORS_HELI_RSC_MODE_NONE) {
_rotor_out = 0;
_rotor_speed_estimate = 0;
write_rsc(_rotor_out);
}
return;
}
// ramp up or down main rotor and tail
if (_desired_rotor_speed > 0) {
// ramp up tail rotor (this does nothing if not using direct drive variable pitch tail)
tail_ramp(_direct_drive_tailspeed);
// note: this always returns true if not using direct drive variable pitch tail
if (tail_rotor_runup_complete()) {
rotor_ramp(_desired_rotor_speed);
}
}else{
// shutting down main rotor
rotor_ramp(0);
// shut-down tail rotor. Note: this does nothing if not using direct drive vairable pitch tail
tail_ramp(0);
}
// direct drive fixed pitch tail servo gets copy of yaw servo out (ch4) while main rotor is running
if (_tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_FIXEDPITCH) {
// output fixed-pitch speed control if Ch8 is high
if (_desired_rotor_speed > 0 || _rotor_speed_estimate > 0) {
// copy yaw output to tail esc
write_aux(_servo_4.servo_out);
}else{
write_aux(0);
}
}
}
// rotor_ramp - ramps rotor towards target
// result put in _rotor_out and sent to ESC
void AP_MotorsHeli::rotor_ramp(int16_t rotor_target)
{
// return immediately if not ramping required
if (_rsc_mode == AP_MOTORS_HELI_RSC_MODE_NONE) {
_rotor_out = rotor_target;
return;
}
// range check rotor_target
rotor_target = constrain_int16(rotor_target,0,1000);
// ramp rotor esc output towards target
if (_rotor_out < rotor_target) {
// allow rotor out to jump to rotor's current speed
if (_rotor_out < _rotor_speed_estimate) {
_rotor_out = _rotor_speed_estimate;
}
// ramp up slowly to target
_rotor_out += _rsc_ramp_increment;
if (_rotor_out > rotor_target) {
_rotor_out = rotor_target;
}
}else{
// ramping down happens instantly
_rotor_out = rotor_target;
}
// ramp rotor speed estimate towards rotor out
if (_rotor_speed_estimate < _rotor_out) {
_rotor_speed_estimate += _rsc_runup_increment;
if (_rotor_speed_estimate > _rotor_out) {
_rotor_speed_estimate = _rotor_out;
}
}else{
_rotor_speed_estimate -= _rsc_runup_increment;
if (_rotor_speed_estimate < _rotor_out) {
_rotor_speed_estimate = _rotor_out;
}
}
// set runup complete flag
if (!_heliflags.rotor_runup_complete && rotor_target > 0 && _rotor_speed_estimate >= rotor_target) {
_heliflags.rotor_runup_complete = true;
}
if (_heliflags.rotor_runup_complete && _rotor_speed_estimate <= _rsc_critical) {
_heliflags.rotor_runup_complete = false;
}
// output to rsc servo
write_rsc(_rotor_out);
}
// tail_ramp - ramps tail motor towards target. Only used for direct drive variable pitch tails
// results put into _tail_direct_drive_out and sent to ESC
void AP_MotorsHeli::tail_ramp(int16_t tail_target)
{
// return immediately if not ramping required
if (_tail_type != AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH) {
_tail_direct_drive_out = tail_target;
return;
}
// range check tail_target
tail_target = constrain_int16(tail_target,0,1000);
// ramp towards target
if (_tail_direct_drive_out < tail_target) {
_tail_direct_drive_out += AP_MOTORS_HELI_TAIL_RAMP_INCREMENT;
if (_tail_direct_drive_out >= tail_target) {
_tail_direct_drive_out = tail_target;
}
}else if(_tail_direct_drive_out > tail_target) {
_tail_direct_drive_out -= AP_MOTORS_HELI_TAIL_RAMP_INCREMENT;
if (_tail_direct_drive_out < tail_target) {
_tail_direct_drive_out = tail_target;
}
}
// output to tail servo
write_aux(_tail_direct_drive_out);
}
// return true if the tail rotor is up to speed
bool AP_MotorsHeli::tail_rotor_runup_complete()
{
// always return true if not using direct drive variable pitch tails
if (_tail_type != AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH) {
return true;
}
// check speed
return (armed() && _tail_direct_drive_out >= _direct_drive_tailspeed);
}
// write_rsc - outputs pwm onto output rsc channel (ch8)
// servo_out parameter is of the range 0 ~ 1000
void AP_MotorsHeli::write_rsc(int16_t servo_out)
{
_servo_rsc.servo_out = servo_out;
_servo_rsc.calc_pwm();
hal.rcout->write(AP_MOTORS_HELI_RSC, _servo_rsc.radio_out);
}
// write_aux - outputs pwm onto output aux channel (ch7)
// servo_out parameter is of the range 0 ~ 1000
void AP_MotorsHeli::write_aux(int16_t servo_out)

38
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -11,6 +11,7 @@
#include <AP_Math/AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel/RC_Channel.h> // RC Channel Library
#include "AP_Motors.h"
#include "AP_MotorsHeli_RSC.h"
// maximum number of swashplate servos
#define AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS 3
@ -103,7 +104,9 @@ public:
_servo_1(swash_servo_1),
_servo_2(swash_servo_2),
_servo_3(swash_servo_3),
_servo_4(yaw_servo)
_servo_4(yaw_servo),
_main_rotor(servo_rotor, AP_MOTORS_HELI_RSC, loop_rate),
_tail_rotor(servo_aux, AP_MOTORS_HELI_AUX, loop_rate)
{
AP_Param::setup_object_defaults(this, var_info);
@ -170,19 +173,19 @@ public:
int16_t get_rsc_setpoint() const { return _rsc_setpoint; }
// set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1000
void set_desired_rotor_speed(int16_t desired_speed) { _desired_rotor_speed = desired_speed; }
void set_desired_rotor_speed(int16_t desired_speed);
// get_desired_rotor_speed - gets target rotor speed as a number from 0 ~ 1000
int16_t get_desired_rotor_speed() const { return _desired_rotor_speed; }
int16_t get_desired_rotor_speed() const { return _main_rotor.get_desired_speed(); }
// get_estimated_rotor_speed - gets estimated rotor speed as a number from 0 ~ 1000
int16_t get_estimated_rotor_speed() { return _rotor_speed_estimate; }
int16_t get_estimated_rotor_speed() { return _main_rotor.get_estimated_speed(); }
// return true if the main rotor is up to speed
bool rotor_runup_complete() const;
// rotor_speed_above_critical - return true if rotor speed is above that critical for flight
bool rotor_speed_above_critical() const { return _rotor_speed_estimate > _rsc_critical; }
bool rotor_speed_above_critical() const { return _main_rotor.get_estimated_speed() > _main_rotor.get_critical_speed(); }
// recalc_scalers - recalculates various scalers used. Should be called at about 1hz to allow users to see effect of changing parameters
void recalc_scalers();
@ -235,22 +238,6 @@ private:
// calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position
void calculate_roll_pitch_collective_factors();
// rsc_control - main function to update values to send to main rotor and tail rotor ESCs
void rsc_control();
// rotor_ramp - ramps rotor towards target. result put rotor_out and sent to ESC
void rotor_ramp(int16_t rotor_target);
// tail_ramp - ramps tail motor towards target. Only used for direct drive variable pitch tails
// results put into _tail_direct_drive_out and sent to ESC
void tail_ramp(int16_t tail_target);
// return true if the tail rotor is up to speed
bool tail_rotor_runup_complete();
// write_rsc - outputs pwm onto output rsc channel (ch8). servo_out parameter is of the range 0 ~ 1000
void write_rsc(int16_t servo_out);
// write_aux - outputs pwm onto output aux channel (ch7). servo_out parameter is of the range 0 ~ 1000
void write_aux(int16_t servo_out);
@ -262,6 +249,9 @@ private:
RC_Channel& _servo_3; // swash plate servo #3
RC_Channel& _servo_4; // tail servo
AP_MotorsHeli_RSC _main_rotor; // main rotor
AP_MotorsHeli_RSC _tail_rotor; // tail rotor
// flags bitmask
struct heliflags_type {
uint8_t swash_initialised : 1; // true if swash has been initialised
@ -303,12 +293,6 @@ private:
float _collective_scalar_manual = 1; // collective scalar to reduce the range of the collective movement while collective is being controlled manually (i.e. directly by the pilot)
int16_t _collective_out = 0; // actual collective pitch value. Required by the main code for calculating cruise throttle
int16_t _collective_mid_pwm = 0; // collective mid parameter value converted to pwm form (i.e. 0 ~ 1000)
int16_t _desired_rotor_speed = 0; // latest desired rotor speed from pilot
float _rotor_out = 0; // latest output sent to the main rotor or an estimate of the rotors actual speed (whichever is higher) (0 ~ 1000)
float _rsc_ramp_increment = 0.0f; // the amount we can increase the rotor output during each 100hz iteration
float _rsc_runup_increment = 0.0f; // the amount we can increase the rotor's estimated speed during each 100hz iteration
float _rotor_speed_estimate = 0.0f; // estimated speed of the main rotor (0~1000)
int16_t _tail_direct_drive_out = 0; // current ramped speed of output on ch7 when using direct drive variable pitch tail type
int16_t _delta_phase_angle = 0; // phase angle dynamic compensation
int16_t _roll_radio_passthrough = 0; // roll control PWM direct from radio, used for manual control
int16_t _pitch_radio_passthrough = 0; // pitch control PWM direct from radio, used for manual control

104
libraries/AP_Motors/AP_MotorsHeli_RSC.cpp Normal file
View File

@ -0,0 +1,104 @@
// -*- 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.h>
#include "AP_MotorsHeli_RSC.h"
extern const AP_HAL::HAL& hal;
// recalc_scalers - recalculates various scalers used. Should be called at about 1hz to allow users to see effect of changing parameters
void AP_MotorsHeli_RSC::recalc_scalers()
{
// recalculate rotor ramp up increment
if (_ramp_time <= 0) {
_ramp_time = 1;
}
_ramp_increment = 1000.0f / (_ramp_time * _loop_rate);
// recalculate rotor runup increment
if (_runup_time <= 0 ) {
_runup_time = 1;
}
if (_runup_time < _ramp_time) {
_runup_time = _ramp_time;
}
_runup_increment = 1000.0f / (_runup_time * _loop_rate);
}
void AP_MotorsHeli_RSC::output_armed()
{
// ramp rotor esc output towards target
if (_speed_out < _desired_speed) {
// allow rotor out to jump to rotor's current speed
if (_speed_out < _estimated_speed) {
_speed_out = _estimated_speed;
}
// ramp up slowly to target
_speed_out += _ramp_increment;
if (_speed_out > _desired_speed) {
_speed_out = _desired_speed;
}
} else {
// ramping down happens instantly
_speed_out = _desired_speed;
}
// ramp rotor speed estimate towards speed out
if (_estimated_speed < _speed_out) {
_estimated_speed += _runup_increment;
if (_estimated_speed > _speed_out) {
_estimated_speed = _speed_out;
}
} else {
_estimated_speed -= _runup_increment;
if (_estimated_speed < _speed_out) {
_estimated_speed = _speed_out;
}
}
// set runup complete flag
if (!_runup_complete && _desired_speed > 0 && _estimated_speed >= _desired_speed) {
_runup_complete = true;
}
if (_runup_complete && _estimated_speed <= _critical_speed) {
_runup_complete = false;
}
// output to rsc servo
write_rsc(_speed_out);
}
void AP_MotorsHeli_RSC::output_disarmed()
{
write_rsc(0);
}
// write_rsc - outputs pwm onto output rsc channel
// servo_out parameter is of the range 0 ~ 1000
void AP_MotorsHeli_RSC::write_rsc(int16_t servo_out)
{
_servo_output.servo_out = servo_out;
_servo_output.calc_pwm();
hal.rcout->write(_servo_output_channel, _servo_output.radio_out);
}

76
libraries/AP_Motors/AP_MotorsHeli_RSC.h Normal file
View File

@ -0,0 +1,76 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef __AP_MOTORS_HELI_RSC_H__
#define __AP_MOTORS_HELI_RSC_H__
#include <inttypes.h>
#include <AP_Common.h>
#include <AP_Math.h>
#include <RC_Channel.h>
class AP_MotorsHeli_RSC {
public:
AP_MotorsHeli_RSC(RC_Channel& servo_output,
int8_t servo_output_channel,
uint16_t loop_rate) :
_servo_output(servo_output),
_servo_output_channel(servo_output_channel),
_loop_rate(loop_rate)
{};
// set_critical_speed
void set_critical_speed(int16_t critical_speed) { _critical_speed = critical_speed; }
// get_critical_speed
int16_t get_critical_speed() const { return _critical_speed; }
// get_desired_speed
int16_t get_desired_speed() const { return _desired_speed; }
// set_desired_speed
void set_desired_speed(int16_t desired_speed) { _desired_speed = desired_speed; }
// get_estimated_speed
int16_t get_estimated_speed() const { return _estimated_speed; }
// is_runup_complete
bool is_runup_complete() const { return _runup_complete; }
// set_ramp_time
void set_ramp_time (int8_t ramp_time) { _ramp_time = ramp_time; }
// set_runup_time
void set_runup_time (int8_t runup_time) { _runup_time = runup_time; }
// recalc_scalers
void recalc_scalers();
// output_armed
void output_armed ();
// output_disarmed
void output_disarmed ();
private:
// external variables
RC_Channel& _servo_output;
int8_t _servo_output_channel; // output channel to rotor esc
float _loop_rate; // main loop rate
// internal variables
int16_t _critical_speed = 0; // rotor speed below which flight is not possible
int16_t _desired_speed = 0; // latest desired rotor speed from pilot
float _speed_out = 0; // latest output sent to the main rotor or an estimate of the rotors actual speed (whichever is higher) (0 ~ 1000)
float _estimated_speed = 0; // estimated speed of the main rotor (0~1000)
float _ramp_increment = 0; // the amount we can increase the rotor output during each 100hz iteration
int8_t _ramp_time = 0; // time in seconds for the output to the main rotor's ESC to reach full speed
int8_t _runup_time = 0; // time in seconds for the main rotor to reach full speed. Must be longer than _rsc_ramp_time
float _runup_increment = 0; // the amount we can increase the rotor's estimated speed during each 100hz iteration
bool _runup_complete = false; // flag for determining if runup is complete
// write_rsc - outputs pwm onto output rsc channel. servo_out parameter is of the range 0 ~ 1000
void write_rsc(int16_t servo_out);
};
#endif // AP_MOTORS_HELI_RSC_H