zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_MotorsTailsitter: add mixer and update to use standard AP_motors functions

mission-4.1.18
IamPete1 6 years ago committed by Randy Mackay
parent
1a98989ac6
commit
4a7b2e8f8f
2 changed files with 144 additions and 52 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 176
      libraries/AP_Motors/AP_MotorsTailsitter.cpp
  2. 20
      libraries/AP_Motors/AP_MotorsTailsitter.h

176
libraries/AP_Motors/AP_MotorsTailsitter.cpp
Unescape View File

@ -14,7 +14,7 @@ @@ -14,7 +14,7 @@
*/
/*
* AP_MotorsTailsitter.cpp - ArduCopter motors library for tailsitters
* AP_MotorsTailsitter.cpp - ArduCopter motors library for tailsitters and bicopters
*
*/
@ -31,6 +31,16 @@ extern const AP_HAL::HAL& hal; @@ -31,6 +31,16 @@ extern const AP_HAL::HAL& hal;
// init
void AP_MotorsTailsitter::init(motor_frame_class frame_class, motor_frame_type frame_type)
{
// make sure 4 output channels are mapped
add_motor_num(AP_MOTORS_THROTTLE_LEFT);
add_motor_num(AP_MOTORS_THROTTLE_RIGHT);
add_motor_num(AP_MOTORS_TILT_LEFT);
add_motor_num(AP_MOTORS_TILT_RIGHT);
// set the motor_enabled flag so that the main ESC can be calibrated like other frame types
motor_enabled[AP_MOTORS_THROTTLE_LEFT] = true;
motor_enabled[AP_MOTORS_THROTTLE_RIGHT] = true;
// record successful initialisation if what we setup was the desired frame_class
_flags.initialised_ok = (frame_class == MOTOR_FRAME_TAILSITTER);
}
@ -44,56 +54,55 @@ AP_MotorsTailsitter::AP_MotorsTailsitter(uint16_t loop_rate, uint16_t speed_hz) @@ -44,56 +54,55 @@ AP_MotorsTailsitter::AP_MotorsTailsitter(uint16_t loop_rate, uint16_t speed_hz)
SRV_Channels::set_rc_frequency(SRV_Channel::k_throttleRight, speed_hz);
}
// set update rate to motors - a value in hertz
void AP_MotorsTailsitter::set_update_rate( uint16_t speed_hz )
{
// record requested speed
_speed_hz = speed_hz;
uint32_t mask =
1U << AP_MOTORS_THROTTLE_LEFT |
1U << AP_MOTORS_THROTTLE_RIGHT;
rc_set_freq(mask, _speed_hz);
}
void AP_MotorsTailsitter::output_to_motors()
{
if (!_flags.initialised_ok) {
return;
}
float throttle = _throttle;
float throttle_left = 0;
float throttle_right = 0;
float throttle = 0.0f;
switch (_spool_mode) {
case SHUT_DOWN:
throttle = 0;
// set limits flags
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
limit.throttle_upper = true;
throttle = get_pwm_output_min();
rc_write(AP_MOTORS_THROTTLE_LEFT, get_pwm_output_min());
rc_write(AP_MOTORS_THROTTLE_RIGHT, get_pwm_output_min());
break;
case SPIN_WHEN_ARMED:
// sends output to motors when armed but not flying
throttle = constrain_float(_spin_up_ratio, 0.0f, 1.0f) * _spin_min;
// set limits flags
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
limit.throttle_upper = true;
rc_write(AP_MOTORS_THROTTLE_LEFT, calc_spin_up_to_pwm());
rc_write(AP_MOTORS_THROTTLE_RIGHT, calc_spin_up_to_pwm());
break;
case SPOOL_UP:
case THROTTLE_UNLIMITED:
case SPOOL_DOWN: {
throttle = _spin_min + throttle * (1 - _spin_min);
throttle_left = constrain_float(throttle + _rudder*0.5, _spin_min, 1);
throttle_right = constrain_float(throttle - _rudder*0.5, _spin_min, 1);
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
case SPOOL_DOWN:
throttle = calc_thrust_to_pwm(_throttle);
rc_write(AP_MOTORS_THROTTLE_LEFT, calc_thrust_to_pwm(_thrust_left));
rc_write(AP_MOTORS_THROTTLE_RIGHT, calc_thrust_to_pwm(_thrust_right));
break;
}
}
// outputs are setup here, and written to the HAL by the plane servos loop
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, _aileron*SERVO_OUTPUT_RANGE);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, _elevator*SERVO_OUTPUT_RANGE);
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, _rudder*SERVO_OUTPUT_RANGE);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle*THROTTLE_RANGE);
// also support differential roll with twin motors
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, throttle_left*THROTTLE_RANGE);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, throttle_right*THROTTLE_RANGE);
// Always output to tilts
rc_write_angle(AP_MOTORS_TILT_LEFT, _tilt_left*SERVO_OUTPUT_RANGE);
rc_write_angle(AP_MOTORS_TILT_RIGHT, _tilt_right*SERVO_OUTPUT_RANGE);
// plane outputs for Qmodes are setup here, and written to the HAL by the plane servos loop
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, -_yaw_in*SERVO_OUTPUT_RANGE);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, _pitch_in*SERVO_OUTPUT_RANGE);
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, _roll_in*SERVO_OUTPUT_RANGE);
SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, throttle);
#if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
SRV_Channels::calc_pwm();
@ -101,24 +110,103 @@ void AP_MotorsTailsitter::output_to_motors() @@ -101,24 +110,103 @@ void AP_MotorsTailsitter::output_to_motors()
#endif
}
// 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_MotorsTailsitter::get_motor_mask()
{
uint32_t motor_mask =
1U << AP_MOTORS_THROTTLE_LEFT |
1U << AP_MOTORS_THROTTLE_RIGHT |
1U << AP_MOTORS_TILT_LEFT |
1U << AP_MOTORS_TILT_RIGHT;
uint16_t mask = rc_map_mask(motor_mask);
// add parent's mask
mask |= AP_MotorsMulticopter::get_motor_mask();
return mask;
}
// calculate outputs to the motors
void AP_MotorsTailsitter::output_armed_stabilizing()
{
_aileron = -_yaw_in;
_elevator = _pitch_in;
_rudder = _roll_in;
_throttle = get_throttle();
float roll_thrust; // roll thrust input value, +/- 1.0
float pitch_thrust; // pitch thrust input value, +/- 1.0
float yaw_thrust; // yaw thrust input value, +/- 1.0
float throttle_thrust; // throttle thrust input value, 0.0 - 1.0
float thrust_max; // highest motor value
float thr_adj = 0.0f; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
// apply voltage and air pressure compensation
const float compensation_gain = get_compensation_gain();
roll_thrust = _roll_in * compensation_gain;
pitch_thrust = _pitch_in * compensation_gain;
yaw_thrust = _yaw_in * compensation_gain;
throttle_thrust = get_throttle() * compensation_gain;
// sanity check throttle is above zero and below current limited throttle
if (_throttle <= 0.0f) {
_throttle = 0.0f;
if (throttle_thrust <= 0.0f) {
throttle_thrust = 0.0f;
limit.throttle_lower = true;
}
if (_throttle >= _throttle_thrust_max) {
_throttle = _throttle_thrust_max;
if (throttle_thrust >= _throttle_thrust_max) {
throttle_thrust = _throttle_thrust_max;
limit.throttle_upper = true;
}
_throttle = constrain_float(_throttle, 0.1, 1);
// caculate left and right throttle outputs
_thrust_left = throttle_thrust + roll_thrust*0.5;
_thrust_right = throttle_thrust - roll_thrust*0.5;
// if max thrust is more than one reduce average throttle
thrust_max = MAX(_thrust_right,_thrust_left);
if (thrust_max > 1.0f) {
thr_adj = 1.0f - thrust_max;
limit.throttle_upper = true;
limit.roll_pitch = true;
}
// Add ajustment to reduce average throttle
_thrust_left = constrain_float(_thrust_left + thr_adj, 0.0f, 1.0f);
_thrust_right = constrain_float(_thrust_right + thr_adj, 0.0f, 1.0f);
_throttle = throttle_thrust + thr_adj;
// thrust vectoring
_tilt_left = pitch_thrust - yaw_thrust;
_tilt_right = pitch_thrust + yaw_thrust;
}
// output_test_seq - 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_MotorsTailsitter::output_test_seq(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:
// throttle left
rc_write(AP_MOTORS_THROTTLE_LEFT, pwm);
break;
case 2:
// throttle right
rc_write(AP_MOTORS_THROTTLE_RIGHT, pwm);
break;
case 3:
// tilt left
rc_write(AP_MOTORS_TILT_LEFT, pwm);
break;
case 4:
// tilt right
rc_write(AP_MOTORS_TILT_RIGHT, pwm);
break;
default:
// do nothing
break;
}
}

20
libraries/AP_Motors/AP_MotorsTailsitter.h
Unescape View File

@ -1,5 +1,5 @@ @@ -1,5 +1,5 @@
/// @file AP_MotorsTailsitter.h
/// @brief Motor control class for tailsitters
/// @brief Motor control class for tailsitters and bicopters
#pragma once
#include <AP_Common/AP_Common.h>
@ -19,23 +19,27 @@ public: @@ -19,23 +19,27 @@ public:
// set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
void set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type) override {}
void set_update_rate( uint16_t speed_hz ) override {}
virtual void output_test_seq(uint8_t motor_seq, int16_t pwm) override {}
// set update rate to motors - a value in hertz
void set_update_rate( uint16_t speed_hz ) override;
virtual void output_test_seq(uint8_t motor_seq, int16_t pwm) override;
// output_to_motors - sends output to named servos
void output_to_motors() override;
// return 0 motor mask
uint16_t get_motor_mask() override { return 0; }
// 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 get_motor_mask() override;
protected:
// calculate motor outputs
void output_armed_stabilizing() override;
// calculated outputs
float _aileron; // -1..1
float _elevator; // -1..1
float _rudder; // -1..1
float _throttle; // 0..1
float _tilt_left; // -1..1
float _tilt_right; // -1..1
float _thrust_left; // 0..1
float _thrust_right; // 0..1
};

Write Preview
Loading…
Cancel
Save