Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: Heli: move swashplate output state and code into swash lib

This commit is contained in:
Iampete1 2023-05-05 00:41:08 +01:00 committed by Randy Mackay
parent 6717da708c
commit b61b761141
8 changed files with 79 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -535,17 +535,6 @@ void AP_MotorsHeli::reset_flight_controls()
calculate_scalars();
}
// convert input in -1 to +1 range to pwm output for swashplate servo.
// The value 0 corresponds to the trim value of the servo. Swashplate
// servo travel range is fixed to 1000 pwm and therefore the input is
// multiplied by 500 to get PWM output.
void AP_MotorsHeli::rc_write_swash(uint8_t chan, float swash_in)
{
uint16_t pwm = (uint16_t)(1500 + 500 * swash_in);
SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(chan);
SRV_Channels::set_output_pwm_trimmed(function, pwm);
}
// update the collective input filter. should be called at 100hz
void AP_MotorsHeli::update_throttle_hover(float dt)
{

3
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -223,9 +223,6 @@ protected:
// to be overloaded by child classes, different vehicle types would have different movement patterns
virtual void servo_test() = 0;
// write to a swash servo. output value is pwm
void rc_write_swash(uint8_t chan, float swash_in);
// save parameters as part of disarming
void save_params_on_disarm() override;

33
libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
View File

@ -557,21 +557,9 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c
float swash2_roll = get_swashplate(2, AP_MOTORS_HELI_DUAL_SWASH_AXIS_ROLL, pitch_out, roll_out, yaw_out, collective2_out_scaled);
float swash2_coll = get_swashplate(2, AP_MOTORS_HELI_DUAL_SWASH_AXIS_COLL, pitch_out, roll_out, yaw_out, collective2_out_scaled);
// get servo positions from swashplate library
_servo_out[CH_1] = _swashplate1.get_servo_out(CH_1,swash1_pitch,swash1_roll,swash1_coll);
_servo_out[CH_2] = _swashplate1.get_servo_out(CH_2,swash1_pitch,swash1_roll,swash1_coll);
_servo_out[CH_3] = _swashplate1.get_servo_out(CH_3,swash1_pitch,swash1_roll,swash1_coll);
if (_swashplate1.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate1.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
_servo_out[CH_7] = _swashplate1.get_servo_out(CH_4,swash1_pitch,swash1_roll,swash1_coll);
}
// get servo positions from swashplate library
_servo_out[CH_4] = _swashplate2.get_servo_out(CH_1,swash2_pitch,swash2_roll,swash2_coll);
_servo_out[CH_5] = _swashplate2.get_servo_out(CH_2,swash2_pitch,swash2_roll,swash2_coll);
_servo_out[CH_6] = _swashplate2.get_servo_out(CH_3,swash2_pitch,swash2_roll,swash2_coll);
if (_swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
_servo_out[CH_8] = _swashplate2.get_servo_out(CH_4,swash2_pitch,swash2_roll,swash2_coll);
}
// Calculate servo positions in swashplate library
_swashplate1.calculate(swash1_roll, swash1_pitch, swash1_coll);
_swashplate2.calculate(swash2_roll, swash2_pitch, swash2_coll);
}
@ -580,19 +568,10 @@ void AP_MotorsHeli_Dual::output_to_motors()
if (!initialised_ok()) {
return;
}
// actually move the servos. PWM is sent based on nominal 1500 center. servo output shifts center based on trim value.
for (uint8_t i=0; i<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {
rc_write_swash(i, _servo_out[CH_1+i]);
}
// write to servo for 4 servo of 4 servo swashplate
if (_swashplate1.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate1.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
rc_write_swash(AP_MOTORS_MOT_7, _servo_out[CH_7]);
}
// write to servo for 4 servo of 4 servo swashplate
if (_swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
rc_write_swash(AP_MOTORS_MOT_8, _servo_out[CH_8]);
}
// Write swashplate outputs
_swashplate1.output();
_swashplate2.output();
switch (_spool_state) {
case SpoolState::SHUT_DOWN:

4
libraries/AP_Motors/AP_MotorsHeli_Dual.h
View File

@ -87,8 +87,8 @@ protected:
const char* _get_frame_string() const override { return "HELI_DUAL"; }
// objects we depend upon
AP_MotorsHeli_Swash _swashplate1; // swashplate1
AP_MotorsHeli_Swash _swashplate2; // swashplate2
AP_MotorsHeli_Swash _swashplate1 { CH_1, CH_2, CH_3, CH_7 }; // swashplate1
AP_MotorsHeli_Swash _swashplate2 { CH_4, CH_5, CH_6, CH_8 }; // swashplate2
// internal variables
float _oscillate_angle = 0.0f; // cyclic oscillation angle, used by servo_test function

20
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

@ -460,13 +460,8 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
float collective_scalar = ((float)(_collective_max-_collective_min))*0.001f;
float collective_out_scaled = collective_out * collective_scalar + (_collective_min - 1000)*0.001f;
// get servo positions from swashplate library
_servo1_out = _swashplate.get_servo_out(CH_1,pitch_out,roll_out,collective_out_scaled);
_servo2_out = _swashplate.get_servo_out(CH_2,pitch_out,roll_out,collective_out_scaled);
_servo3_out = _swashplate.get_servo_out(CH_3,pitch_out,roll_out,collective_out_scaled);
if (_swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
_servo5_out = _swashplate.get_servo_out(CH_4,pitch_out,roll_out,collective_out_scaled);
}
// Caculate servo positions from swashplate library
_swashplate.calculate(roll_out, pitch_out, collective_out_scaled);
// update the yaw rate using the tail rotor/servo
move_yaw(yaw_out + yaw_offset);
@ -494,14 +489,9 @@ void AP_MotorsHeli_Single::output_to_motors()
return;
}
// actually move the servos. PWM is sent based on nominal 1500 center. servo output shifts center based on trim value.
rc_write_swash(AP_MOTORS_MOT_1, _servo1_out);
rc_write_swash(AP_MOTORS_MOT_2, _servo2_out);
rc_write_swash(AP_MOTORS_MOT_3, _servo3_out);
// get servo positions from swashplate library and write to servo for 4 servo of 4 servo swashplate
if (_swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
rc_write_swash(AP_MOTORS_MOT_5, _servo5_out);
}
// Write swashplate outputs
_swashplate.output();
if (_tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW && _tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW){
rc_write_angle(AP_MOTORS_MOT_4, _servo4_out * YAW_SERVO_MAX_ANGLE);
}

6
libraries/AP_Motors/AP_MotorsHeli_Single.h
View File

@ -42,7 +42,7 @@ public:
AP_MotorsHeli_Single(uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) :
AP_MotorsHeli(speed_hz),
_tail_rotor(SRV_Channel::k_heli_tail_rsc, AP_MOTORS_HELI_SINGLE_TAILRSC),
_swashplate()
_swashplate(AP_MOTORS_MOT_1, AP_MOTORS_MOT_2, AP_MOTORS_MOT_3, AP_MOTORS_MOT_5)
{
AP_Param::setup_object_defaults(this, var_info);
};
@ -117,11 +117,7 @@ protected:
float _roll_test = 0.0f; // over-ride for roll output, used by servo_test function
float _pitch_test = 0.0f; // over-ride for pitch output, used by servo_test function
float _yaw_test = 0.0f; // over-ride for yaw output, used by servo_test function
float _servo1_out = 0.0f; // output value sent to motor
float _servo2_out = 0.0f; // output value sent to motor
float _servo3_out = 0.0f; // output value sent to motor
float _servo4_out = 0.0f; // output value sent to motor
float _servo5_out = 0.0f; // output value sent to motor
uint16_t _ddfp_pwm_min = 0; // minimum ddfp servo min
uint16_t _ddfp_pwm_max = 0; // minimum ddfp servo max
uint16_t _ddfp_pwm_trim = 0; // minimum ddfp servo trim

67
libraries/AP_Motors/AP_MotorsHeli_Swash.cpp
View File

@ -15,6 +15,7 @@
#include <stdlib.h>
#include <AP_HAL/AP_HAL.h>
#include <SRV_Channel/SRV_Channel.h>
#include "AP_MotorsHeli_Swash.h"
@ -86,6 +87,16 @@ const AP_Param::GroupInfo AP_MotorsHeli_Swash::var_info[] = {
AP_GROUPEND
};
AP_MotorsHeli_Swash::AP_MotorsHeli_Swash(uint8_t mot_0, uint8_t mot_1, uint8_t mot_2, uint8_t mot_3)
{
_motor_num[0] = mot_0;
_motor_num[1] = mot_1;
_motor_num[2] = mot_2;
_motor_num[3] = mot_3;
AP_Param::setup_object_defaults(this, var_info);
}
// configure - configure the swashplate settings for any updated parameters
void AP_MotorsHeli_Swash::configure()
{
@ -103,6 +114,7 @@ void AP_MotorsHeli_Swash::calculate_roll_pitch_collective_factors()
{
// Clear existing setup
for (uint8_t i = 0; i < _max_num_servos; i++) {
_enabled[i] = false;
_rollFactor[i] = 0.0;
_pitchFactor[i] = 0.0;
_collectiveFactor[i] = 0.0;
@ -182,33 +194,40 @@ void AP_MotorsHeli_Swash::add_servo_raw(uint8_t num, float roll, float pitch, fl
return;
}
_enabled[num] = true;
_rollFactor[num] = roll * 0.45;
_pitchFactor[num] = pitch * 0.45;
_collectiveFactor[num] = collective;
}
// get_servo_out - calculates servo output
float AP_MotorsHeli_Swash::get_servo_out(int8_t ch_num, float pitch, float roll, float collective) const
// calculates servo output
void AP_MotorsHeli_Swash::calculate(float roll, float pitch, float collective)
{
// Collective control direction. Swash moves up for negative collective pitch, down for positive collective pitch
if (_collective_direction == COLLECTIVE_DIRECTION_REVERSED){
collective = 1 - collective;
}
float servo = (_rollFactor[ch_num] * roll) + (_pitchFactor[ch_num] * pitch) + _collectiveFactor[ch_num] * collective;
if (_swash_type == SWASHPLATE_TYPE_H1 && (ch_num == CH_1 || ch_num == CH_2)) {
servo += 0.5f;
for (uint8_t i = 0; i < _max_num_servos; i++) {
if (!_enabled[i]) {
// This servo is not enabled
continue;
}
_output[i] = (_rollFactor[i] * roll) + (_pitchFactor[i] * pitch) + _collectiveFactor[i] * collective;
if (_swash_type == SWASHPLATE_TYPE_H1 && (i == CH_1 || i == CH_2)) {
_output[i] += 0.5f;
}
// rescale from -1..1, so we can use the pwm calc that includes trim
_output[i] = 2.0f * _output[i] - 1.0f;
if (_make_servo_linear) {
_output[i] = get_linear_servo_output(_output[i]);
}
}
// rescale from -1..1, so we can use the pwm calc that includes trim
servo = 2.0f * servo - 1.0f;
if (_make_servo_linear) {
servo = get_linear_servo_output(servo);
}
return servo;
}
// set_linear_servo_out - sets swashplate servo output to be linear
@ -222,3 +241,23 @@ float AP_MotorsHeli_Swash::get_linear_servo_output(float input) const
}
// Output calculated values to servos
void AP_MotorsHeli_Swash::output()
{
for (uint8_t i = 0; i < _max_num_servos; i++) {
if (_enabled[i]) {
rc_write(_motor_num[i], _output[i]);
}
}
}
// convert input in -1 to +1 range to pwm output for swashplate servo.
// The value 0 corresponds to the trim value of the servo. Swashplate
// servo travel range is fixed to 1000 pwm and therefore the input is
// multiplied by 500 to get PWM output.
void AP_MotorsHeli_Swash::rc_write(uint8_t chan, float swash_in)
{
uint16_t pwm = (uint16_t)(1500 + 500 * swash_in);
SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(chan);
SRV_Channels::set_output_pwm_trimmed(function, pwm);
}

18
libraries/AP_Motors/AP_MotorsHeli_Swash.h
View File

@ -19,10 +19,7 @@ enum SwashPlateType {
class AP_MotorsHeli_Swash {
public:
AP_MotorsHeli_Swash()
{
AP_Param::setup_object_defaults(this, var_info);
};
AP_MotorsHeli_Swash(uint8_t mot_0, uint8_t mot_1, uint8_t mot_2, uint8_t mot_3);
// configure - configure the swashplate settings for any updated parameters
void configure();
@ -30,8 +27,11 @@ public:
// get_swash_type - gets swashplate type
SwashPlateType get_swash_type() const { return _swash_type; }
// get_servo_out - calculates servo output
float get_servo_out(int8_t servo_num, float pitch, float roll, float collective) const;
// calculates servo output
void calculate(float roll, float pitch, float collective);
// Output calculated values to servos
void output();
// get_phase_angle - returns the rotor phase angle
int16_t get_phase_angle() const { return _phase_angle; }
@ -51,6 +51,9 @@ private:
void add_servo_angle(uint8_t num, float angle, float collective);
void add_servo_raw(uint8_t num, float roll, float pitch, float collective);
// write to a swash servo. output value is pwm
void rc_write(uint8_t chan, float swash_in);
enum CollectiveDirection {
COLLECTIVE_DIRECTION_NORMAL = 0,
COLLECTIVE_DIRECTION_REVERSED
@ -64,9 +67,12 @@ private:
bool _make_servo_linear; // Sets servo output to be linearized
// Internal variables
bool _enabled[_max_num_servos]; // True if this output servo is enabled
float _rollFactor[_max_num_servos]; // Roll axis scaling of servo output based on servo position
float _pitchFactor[_max_num_servos]; // Pitch axis scaling of servo output based on servo position
float _collectiveFactor[_max_num_servos]; // Collective axis scaling of servo output based on servo position
float _output[_max_num_servos]; // Servo output value
uint8_t _motor_num[_max_num_servos]; // Motor function to use for output
// parameters
AP_Int8 _swashplate_type; // Swash Type Setting