mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-09 09:24:01 -04:00
Copter: add coax heli support
Motors should be attached to RCOutputs 1 and 2, Flaps should be connected to RC outputs 3 and 4
This commit is contained in:
Dneault
Randy Mackay
parent
dc6433d63c
commit
f48e106271
@ -6,8 +6,7 @@
|
||||
// valid! You should switch to using a HAL_BOARD flag in your local config.mk.
|
||||
|
||||
//#define FRAME_CONFIG QUAD_FRAME
|
||||
/*
|
||||
* options:
|
||||
/* options:
|
||||
* QUAD_FRAME
|
||||
* TRI_FRAME
|
||||
* HEXA_FRAME
|
||||
@ -16,6 +15,7 @@
|
||||
* OCTA_QUAD_FRAME
|
||||
* HELI_FRAME
|
||||
* SINGLE_FRAME
|
||||
* COAX_FRAME
|
||||
*/
|
||||
|
||||
// uncomment the lines below to save on flash space if compiling for the APM using Arduino IDE. Top items save the most flash space
|
||||
|
||||
@ -465,6 +465,8 @@ static struct {
|
||||
#define MOTOR_CLASS AP_MotorsHeli
|
||||
#elif FRAME_CONFIG == SINGLE_FRAME
|
||||
#define MOTOR_CLASS AP_MotorsSingle
|
||||
#elif FRAME_CONFIG == COAX_FRAME
|
||||
#define MOTOR_CLASS AP_MotorsCoax
|
||||
#else
|
||||
#error Unrecognised frame type
|
||||
#endif
|
||||
@ -475,6 +477,8 @@ static MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_7, &g.rc_8,
|
||||
static MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_7);
|
||||
#elif FRAME_CONFIG == SINGLE_FRAME // single constructor requires extra servos for flaps
|
||||
static MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.single_servo_1, &g.single_servo_2, &g.single_servo_3, &g.single_servo_4);
|
||||
#elif FRAME_CONFIG == COAX_FRAME // single constructor requires extra servos for flaps
|
||||
static MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.single_servo_1, &g.single_servo_2, &g.single_servo_3, &g.single_servo_4);
|
||||
#else
|
||||
static MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4);
|
||||
#endif
|
||||
|
||||
@ -102,6 +102,8 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
|
||||
MAV_TYPE_HELICOPTER,
|
||||
#elif (FRAME_CONFIG == SINGLE_FRAME) //because mavlink did not define a singlecopter, we use a rocket
|
||||
MAV_TYPE_ROCKET,
|
||||
#elif (FRAME_CONFIG == COAX_FRAME) //because mavlink did not define a singlecopter, we use a rocket
|
||||
MAV_TYPE_ROCKET,
|
||||
#else
|
||||
#error Unrecognised frame type
|
||||
#endif
|
||||
|
||||
@ -378,6 +378,11 @@ public:
|
||||
RC_Channel single_servo_1, single_servo_2, single_servo_3, single_servo_4; // servos for four flaps
|
||||
#endif
|
||||
|
||||
#if FRAME_CONFIG == COAX_FRAME
|
||||
// Single
|
||||
RC_Channel single_servo_1, single_servo_2, single_servo_3, single_servo_4; // servos for four flaps
|
||||
#endif
|
||||
|
||||
// RC channels
|
||||
RC_Channel rc_1;
|
||||
RC_Channel rc_2;
|
||||
@ -441,6 +446,13 @@ public:
|
||||
single_servo_4 (CH_4),
|
||||
#endif
|
||||
|
||||
#if FRAME_CONFIG == COAX_FRAME
|
||||
single_servo_1 (CH_1),
|
||||
single_servo_2 (CH_2),
|
||||
single_servo_3 (CH_3),
|
||||
single_servo_4 (CH_4),
|
||||
#endif
|
||||
|
||||
rc_1 (CH_1),
|
||||
rc_2 (CH_2),
|
||||
rc_3 (CH_3),
|
||||
|
||||
@ -508,6 +508,20 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||
GGROUP(single_servo_4, "SS4_", RC_Channel),
|
||||
#endif
|
||||
|
||||
#if FRAME_CONFIG == COAX_FRAME
|
||||
// @Group: SS1_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
|
||||
GGROUP(single_servo_1, "SS1_", RC_Channel),
|
||||
// @Group: SS2_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
|
||||
GGROUP(single_servo_2, "SS2_", RC_Channel),
|
||||
// @Group: SS3_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
|
||||
GGROUP(single_servo_3, "SS3_", RC_Channel),
|
||||
// @Group: SS4_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
|
||||
GGROUP(single_servo_4, "SS4_", RC_Channel),
|
||||
#endif
|
||||
|
||||
// RC channel
|
||||
//-----------
|
||||
@ -1103,6 +1117,23 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||
// @Path: ../libraries/AP_Motors/AP_MotorsSingle.cpp
|
||||
GOBJECT(motors, "MOT_", AP_MotorsSingle),
|
||||
|
||||
#elif FRAME_CONFIG == COAX_FRAME
|
||||
// @Group: SS1_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
|
||||
GGROUP(single_servo_1, "SS1_", RC_Channel),
|
||||
// @Group: SS2_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
|
||||
GGROUP(single_servo_2, "SS2_", RC_Channel),
|
||||
// @Group: SS3_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
|
||||
GGROUP(single_servo_3, "SS3_", RC_Channel),
|
||||
// @Group: SS4_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
|
||||
GGROUP(single_servo_4, "SS4_", RC_Channel),
|
||||
// @Group: MOT_
|
||||
// @Path: ../libraries/AP_Motors/AP_MotorsSingle.cpp
|
||||
GOBJECT(motors, "MOT_", AP_MotorsSingle),
|
||||
|
||||
#else
|
||||
// @Group: MOT_
|
||||
// @Path: ../libraries/AP_Motors/AP_Motors_Class.cpp
|
||||
|
||||
@ -89,6 +89,7 @@
|
||||
#define HELI_FRAME 6
|
||||
#define OCTA_QUAD_FRAME 7
|
||||
#define SINGLE_FRAME 8
|
||||
#define COAX_FRAME 9
|
||||
|
||||
// Internal defines, don't edit and expect things to work
|
||||
// -------------------------------------------------------
|
||||
|
||||
@ -556,7 +556,7 @@ static void servo_write(uint8_t ch, uint16_t pwm)
|
||||
#if (FRAME_CONFIG == QUAD_FRAME)
|
||||
// Quads can use RC5 and higher as servos
|
||||
if (ch >= CH_5) servo_ok = true;
|
||||
#elif (FRAME_CONFIG == TRI_FRAME || FRAME_CONFIG == SINGLE_FRAME)
|
||||
#elif (FRAME_CONFIG == TRI_FRAME || FRAME_CONFIG == SINGLE_FRAME || FRAME_CONFIG == COAX_FRAME )
|
||||
// Tri's and Singles can use RC5, RC6, RC8 and higher
|
||||
if (ch == CH_5 || ch == CH_6 || ch >= CH_8) servo_ok = true;
|
||||
#elif (FRAME_CONFIG == HEXA_FRAME || FRAME_CONFIG == Y6_FRAME)
|
||||
|
||||
@ -41,6 +41,18 @@ static void init_rc_in()
|
||||
g.single_servo_4.set_angle(DEFAULT_ANGLE_MAX);
|
||||
#endif
|
||||
|
||||
#if FRAME_CONFIG == COAX_FRAME
|
||||
// we set four servos to angle
|
||||
g.single_servo_1.set_type(RC_CHANNEL_TYPE_ANGLE);
|
||||
g.single_servo_2.set_type(RC_CHANNEL_TYPE_ANGLE);
|
||||
g.single_servo_3.set_type(RC_CHANNEL_TYPE_ANGLE);
|
||||
g.single_servo_4.set_type(RC_CHANNEL_TYPE_ANGLE);
|
||||
g.single_servo_1.set_angle(DEFAULT_ANGLE_MAX);
|
||||
g.single_servo_2.set_angle(DEFAULT_ANGLE_MAX);
|
||||
g.single_servo_3.set_angle(DEFAULT_ANGLE_MAX);
|
||||
g.single_servo_4.set_angle(DEFAULT_ANGLE_MAX);
|
||||
#endif
|
||||
|
||||
//set auxiliary servo ranges
|
||||
g.rc_5.set_range(0,1000);
|
||||
g.rc_6.set_range(0,1000);
|
||||
|
||||
@ -13,5 +13,6 @@
|
||||
#include "AP_MotorsOctaQuad.h"
|
||||
#include "AP_MotorsHeli.h"
|
||||
#include "AP_MotorsSingle.h"
|
||||
#include "AP_MotorsCoax.h"
|
||||
|
||||
#endif // __AP_MOTORS_H__
|
||||
|
||||
357
libraries/AP_Motors/AP_MotorsCoax.cpp
Normal file
357
libraries/AP_Motors/AP_MotorsCoax.cpp
Normal file
@ -0,0 +1,357 @@
|
||||
// -*- 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_MotorsSingle.cpp - ArduCopter motors library
|
||||
* Code by RandyMackay. DIYDrones.com
|
||||
*
|
||||
*/
|
||||
|
||||
#include <AP_HAL.h>
|
||||
#include <AP_Math.h>
|
||||
#include "AP_MotorsCoax.h"
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
|
||||
const AP_Param::GroupInfo AP_MotorsCoax::var_info[] PROGMEM = {
|
||||
// 0 was used by TB_RATIO
|
||||
|
||||
// @Param: TCRV_ENABLE
|
||||
// @DisplayName: Thrust Curve Enable
|
||||
// @Description: Controls whether a curve is used to linearize the thrust produced by the motors
|
||||
// @User: Advanced
|
||||
// @Values: 0:Disabled,1:Enable
|
||||
AP_GROUPINFO("TCRV_ENABLE", 1, AP_MotorsCoax, _throttle_curve_enabled, THROTTLE_CURVE_ENABLED),
|
||||
|
||||
// @Param: TCRV_MIDPCT
|
||||
// @DisplayName: Thrust Curve mid-point percentage
|
||||
// @Description: Set the pwm position that produces half the maximum thrust of the motors
|
||||
// @User: Advanced
|
||||
// @Range: 20 80
|
||||
// @Increment: 1
|
||||
AP_GROUPINFO("TCRV_MIDPCT", 2, AP_MotorsCoax, _throttle_curve_mid, THROTTLE_CURVE_MID_THRUST),
|
||||
|
||||
// @Param: TCRV_MAXPCT
|
||||
// @DisplayName: Thrust Curve max thrust percentage
|
||||
// @Description: Set to the lowest pwm position that produces the maximum thrust of the motors. Most motors produce maximum thrust below the maximum pwm value that they accept.
|
||||
// @User: Advanced
|
||||
// @Range: 20 80
|
||||
// @Increment: 1
|
||||
AP_GROUPINFO("TCRV_MAXPCT", 3, AP_MotorsCoax, _throttle_curve_max, THROTTLE_CURVE_MAX_THRUST),
|
||||
|
||||
// @Param: SPIN_ARMED
|
||||
// @DisplayName: Motors always spin when armed
|
||||
// @Description: Controls whether motors always spin when armed (must be below THR_MIN)
|
||||
// @Values: 0:Do Not Spin,70:VerySlow,100:Slow,130:Medium,150:Fast
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("SPIN_ARMED", 5, AP_MotorsCoax, _spin_when_armed, AP_MOTORS_SPIN_WHEN_ARMED),
|
||||
|
||||
// @Param: REV_ROLL
|
||||
// @DisplayName: Reverse roll feedback
|
||||
// @Description: Ensure the feedback is negative
|
||||
// @Values: -1:Opposite direction,1:Same direction
|
||||
AP_GROUPINFO("REV_ROLL", 6, AP_MotorsCoax, _rev_roll, AP_MOTORS_COAX_POSITIVE),
|
||||
|
||||
// @Param: REV_PITCH
|
||||
// @DisplayName: Reverse roll feedback
|
||||
// @Description: Ensure the feedback is negative
|
||||
// @Values: -1:Opposite direction,1:Same direction
|
||||
AP_GROUPINFO("REV_PITCH", 7, AP_MotorsCoax, _rev_pitch, AP_MOTORS_COAX_POSITIVE),
|
||||
|
||||
// @Param: REV_ROLL
|
||||
// @DisplayName: Reverse roll feedback
|
||||
// @Description: Ensure the feedback is negative
|
||||
// @Values: -1:Opposite direction,1:Same direction
|
||||
AP_GROUPINFO("REV_YAW", 8, AP_MotorsCoax, _rev_yaw, AP_MOTORS_COAX_POSITIVE),
|
||||
|
||||
// @Param: SV_SPEED
|
||||
// @DisplayName: Servo speed
|
||||
// @Description: Servo update speed
|
||||
// @Values: -1:Opposite direction,1:Same direction
|
||||
AP_GROUPINFO("SV_SPEED", 9, AP_MotorsCoax, _servo_speed, AP_MOTORS_SINGLE_SPEED_DIGITAL_SERVOS),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
// init
|
||||
void AP_MotorsCoax::Init()
|
||||
{
|
||||
// call parent Init function to set-up throttle curve
|
||||
AP_Motors::Init();
|
||||
|
||||
// set update rate for the 3 motors (but not the servo on channel 7)
|
||||
set_update_rate(_speed_hz);
|
||||
|
||||
// set the motor_enabled flag so that the ESCs can be calibrated like other frame types
|
||||
motor_enabled[AP_MOTORS_MOT_1] = true;
|
||||
motor_enabled[AP_MOTORS_MOT_2] = true;
|
||||
motor_enabled[AP_MOTORS_MOT_3] = true;
|
||||
motor_enabled[AP_MOTORS_MOT_4] = true;
|
||||
}
|
||||
|
||||
// set update rate to motors - a value in hertz
|
||||
void AP_MotorsCoax::set_update_rate( uint16_t speed_hz )
|
||||
{
|
||||
// record requested speed
|
||||
_speed_hz = speed_hz;
|
||||
|
||||
// set update rate for the 3 motors (but not the servo on channel 7)
|
||||
// uint32_t mask =
|
||||
// 1U << _motor_to_channel_map[AP_MOTORS_MOT_1] |
|
||||
// 1U << _motor_to_channel_map[AP_MOTORS_MOT_2] |
|
||||
// 1U << _motor_to_channel_map[AP_MOTORS_MOT_3] |
|
||||
// 1U << _motor_to_channel_map[AP_MOTORS_MOT_4] ;
|
||||
// hal.rcout->set_freq(mask, _servo_speed);
|
||||
//uint32_t mask2 = 1U << _motor_to_channel_map[AP_MOTORS_MOT_7];
|
||||
//hal.rcout->set_freq(mask2, _speed_hz);
|
||||
|
||||
uint32_t mask =
|
||||
1U << _motor_to_channel_map[AP_MOTORS_MOT_3] |
|
||||
1U << _motor_to_channel_map[AP_MOTORS_MOT_4] ;
|
||||
hal.rcout->set_freq(mask, _servo_speed);
|
||||
uint32_t mask2 =
|
||||
1U << _motor_to_channel_map[AP_MOTORS_MOT_1] |
|
||||
1U << _motor_to_channel_map[AP_MOTORS_MOT_2] ;
|
||||
hal.rcout->set_freq(mask2, _speed_hz);
|
||||
}
|
||||
|
||||
// enable - starts allowing signals to be sent to motors
|
||||
void AP_MotorsCoax::enable()
|
||||
{
|
||||
// enable output channels
|
||||
// hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_1]);
|
||||
// hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_2]);
|
||||
// hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_3]);
|
||||
// hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_4]);
|
||||
//hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_7]);
|
||||
|
||||
hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_1]);
|
||||
hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_2]);
|
||||
hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_3]);
|
||||
hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_4]);
|
||||
}
|
||||
|
||||
// output_min - sends minimum values out to the motor and trim values to the servos
|
||||
void AP_MotorsCoax::output_min()
|
||||
{
|
||||
// fill the motor_out[] array for HIL use
|
||||
|
||||
// motor_out[AP_MOTORS_MOT_1] = _servo1->radio_trim;
|
||||
// motor_out[AP_MOTORS_MOT_2] = _servo2->radio_trim;
|
||||
// motor_out[AP_MOTORS_MOT_3] = _servo3->radio_trim;
|
||||
// motor_out[AP_MOTORS_MOT_4] = _servo4->radio_trim;
|
||||
// motor_out[AP_MOTORS_MOT_7] = _rc_throttle->radio_min;
|
||||
|
||||
motor_out[AP_MOTORS_MOT_1] = _rc_throttle->radio_min;
|
||||
motor_out[AP_MOTORS_MOT_2] = _rc_throttle->radio_min;
|
||||
motor_out[AP_MOTORS_MOT_3] = _servo3->radio_trim;
|
||||
motor_out[AP_MOTORS_MOT_4] = _servo4->radio_trim;
|
||||
|
||||
|
||||
// send minimum value to each motor
|
||||
|
||||
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min);
|
||||
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min);
|
||||
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo3->radio_trim);
|
||||
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo4->radio_trim);
|
||||
|
||||
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo1->radio_trim);
|
||||
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo2->radio_trim);
|
||||
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo3->radio_trim);
|
||||
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo4->radio_trim);
|
||||
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_7], _rc_throttle->radio_min);
|
||||
|
||||
}
|
||||
|
||||
// output_armed - sends commands to the motors
|
||||
void AP_MotorsCoax::output_armed()
|
||||
{
|
||||
int16_t out_min = _rc_throttle->radio_min + _min_throttle;
|
||||
|
||||
// Throttle is 0 to 1000 only
|
||||
_rc_throttle->servo_out = constrain_int16(_rc_throttle->servo_out, 0, _max_throttle);
|
||||
|
||||
// capture desired throttle from receiver
|
||||
_rc_throttle->calc_pwm();
|
||||
|
||||
// if we are not sending a throttle output, we cut the motors
|
||||
if(_rc_throttle->servo_out == 0) {
|
||||
// range check spin_when_armed
|
||||
if (_spin_when_armed < 0) {
|
||||
_spin_when_armed = 0;
|
||||
}
|
||||
if (_spin_when_armed > _min_throttle) {
|
||||
_spin_when_armed = _min_throttle;
|
||||
}
|
||||
motor_out[AP_MOTORS_MOT_1] = _rc_throttle->radio_min + _spin_when_armed;
|
||||
motor_out[AP_MOTORS_MOT_2] = _rc_throttle->radio_min + _spin_when_armed;
|
||||
|
||||
//motor_out[AP_MOTORS_MOT_7] = _rc_throttle->radio_min + _spin_when_armed;
|
||||
}else{
|
||||
|
||||
//motors
|
||||
motor_out[AP_MOTORS_MOT_1] = _rev_yaw*_rc_yaw->servo_out + _rc_throttle->radio_out;
|
||||
motor_out[AP_MOTORS_MOT_2] = -_rev_yaw*_rc_yaw->servo_out +_rc_throttle->radio_out;
|
||||
//Front
|
||||
_servo3->servo_out = _rev_roll*_rc_roll->servo_out;
|
||||
//right
|
||||
_servo4->servo_out = _rev_pitch*_rc_pitch->servo_out;
|
||||
|
||||
|
||||
|
||||
////motor
|
||||
//motor_out[AP_MOTORS_MOT_7] = _rc_throttle->radio_out;
|
||||
// //front
|
||||
// _servo1->servo_out = _rev_roll*_rc_roll->servo_out + _rev_yaw*_rc_yaw->servo_out;
|
||||
////right
|
||||
// _servo2->servo_out = _rev_pitch*_rc_pitch->servo_out + _rev_yaw*_rc_yaw->servo_out;
|
||||
////rear
|
||||
//_servo3->servo_out = -_rev_roll*_rc_roll->servo_out + _rev_yaw*_rc_yaw->servo_out; // - before _rev to reverse servo, and below
|
||||
////left
|
||||
//_servo4->servo_out = -_rev_pitch*_rc_pitch->servo_out + _rev_yaw*_rc_yaw->servo_out;
|
||||
|
||||
|
||||
_servo3->calc_pwm();
|
||||
_servo4->calc_pwm();
|
||||
|
||||
motor_out[AP_MOTORS_MOT_3] = _servo3->radio_out;
|
||||
motor_out[AP_MOTORS_MOT_4] = _servo4->radio_out;
|
||||
|
||||
|
||||
//_servo1->calc_pwm();
|
||||
//_servo2->calc_pwm();
|
||||
//_servo3->calc_pwm();
|
||||
//_servo4->calc_pwm();
|
||||
|
||||
//motor_out[AP_MOTORS_MOT_1] = _servo1->radio_out;
|
||||
//motor_out[AP_MOTORS_MOT_2] = _servo2->radio_out;
|
||||
//motor_out[AP_MOTORS_MOT_3] = _servo3->radio_out;
|
||||
//motor_out[AP_MOTORS_MOT_4] = _servo4->radio_out;
|
||||
|
||||
// adjust for throttle curve
|
||||
if( _throttle_curve_enabled ) {
|
||||
|
||||
motor_out[AP_MOTORS_MOT_1] = _throttle_curve.get_y(motor_out[AP_MOTORS_MOT_1]);
|
||||
motor_out[AP_MOTORS_MOT_2] = _throttle_curve.get_y(motor_out[AP_MOTORS_MOT_2]);
|
||||
//motor_out[AP_MOTORS_MOT_7] = _throttle_curve.get_y(motor_out[AP_MOTORS_MOT_7]);
|
||||
}
|
||||
|
||||
// ensure motors don't drop below a minimum value and stop
|
||||
motor_out[AP_MOTORS_MOT_1] = max(motor_out[AP_MOTORS_MOT_1], out_min);
|
||||
motor_out[AP_MOTORS_MOT_2] = max(motor_out[AP_MOTORS_MOT_2], out_min);
|
||||
//motor_out[AP_MOTORS_MOT_7] = max(motor_out[AP_MOTORS_MOT_7], out_min);
|
||||
}
|
||||
|
||||
// send output to each motor
|
||||
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], motor_out[AP_MOTORS_MOT_1]);
|
||||
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], motor_out[AP_MOTORS_MOT_2]);
|
||||
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], motor_out[AP_MOTORS_MOT_3]);
|
||||
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], motor_out[AP_MOTORS_MOT_4]);
|
||||
|
||||
|
||||
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], motor_out[AP_MOTORS_MOT_1]);
|
||||
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], motor_out[AP_MOTORS_MOT_2]);
|
||||
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], motor_out[AP_MOTORS_MOT_3]);
|
||||
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], motor_out[AP_MOTORS_MOT_4]);
|
||||
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_7], motor_out[AP_MOTORS_MOT_7]);
|
||||
|
||||
}
|
||||
|
||||
// output_disarmed - sends commands to the motors
|
||||
void AP_MotorsCoax::output_disarmed()
|
||||
{
|
||||
// Send minimum values to all motors
|
||||
output_min();
|
||||
}
|
||||
|
||||
// output_test - spin each motor for a moment to allow the user to confirm the motor order and spin direction
|
||||
void AP_MotorsCoax::output_test()
|
||||
{
|
||||
// Send minimum values to all motors
|
||||
output_min();
|
||||
|
||||
// spin motor 1
|
||||
hal.scheduler->delay(1000);
|
||||
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min + _min_throttle);
|
||||
hal.scheduler->delay(1000);
|
||||
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min);
|
||||
hal.scheduler->delay(2000);
|
||||
|
||||
// spin motor 2
|
||||
hal.scheduler->delay(1000);
|
||||
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min + _min_throttle);
|
||||
hal.scheduler->delay(1000);
|
||||
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min);
|
||||
hal.scheduler->delay(2000);
|
||||
|
||||
// flap servo 3
|
||||
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo3->radio_min);
|
||||
hal.scheduler->delay(1000);
|
||||
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo3->radio_max);
|
||||
hal.scheduler->delay(1000);
|
||||
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo3->radio_trim);
|
||||
hal.scheduler->delay(2000);
|
||||
|
||||
// flap servo 4
|
||||
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo4->radio_min);
|
||||
hal.scheduler->delay(1000);
|
||||
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo4->radio_max);
|
||||
hal.scheduler->delay(1000);
|
||||
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo4->radio_trim);
|
||||
|
||||
|
||||
|
||||
// // spin main motor
|
||||
// hal.scheduler->delay(1000);
|
||||
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_7], _rc_throttle->radio_min + _min_throttle);
|
||||
// hal.scheduler->delay(1000);
|
||||
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_7], _rc_throttle->radio_min);
|
||||
// hal.scheduler->delay(2000);
|
||||
|
||||
// // flap servo 1
|
||||
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo1->radio_min);
|
||||
// hal.scheduler->delay(1000);
|
||||
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo1->radio_max);
|
||||
// hal.scheduler->delay(1000);
|
||||
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo1->radio_trim);
|
||||
// hal.scheduler->delay(2000);
|
||||
|
||||
// // flap servo 2
|
||||
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo2->radio_min);
|
||||
// hal.scheduler->delay(1000);
|
||||
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo2->radio_max);
|
||||
// hal.scheduler->delay(1000);
|
||||
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo2->radio_trim);
|
||||
// hal.scheduler->delay(2000);
|
||||
|
||||
// // flap servo 3
|
||||
//hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo3->radio_min);
|
||||
// hal.scheduler->delay(1000);
|
||||
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo3->radio_max);
|
||||
// hal.scheduler->delay(1000);
|
||||
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo3->radio_trim);
|
||||
// hal.scheduler->delay(2000);
|
||||
|
||||
// // flap servo 4
|
||||
//hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo4->radio_min);
|
||||
// hal.scheduler->delay(1000);
|
||||
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo4->radio_max);
|
||||
// hal.scheduler->delay(1000);
|
||||
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo4->radio_trim);
|
||||
|
||||
// Send minimum values to all motors
|
||||
output_min();
|
||||
}
|
||||
69
libraries/AP_Motors/AP_MotorsCoax.h
Normal file
69
libraries/AP_Motors/AP_MotorsCoax.h
Normal file
@ -0,0 +1,69 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
/// @file AP_MotorsSingle.h
|
||||
/// @brief Motor and Servo control class for Singlecopters
|
||||
|
||||
#ifndef __AP_MOTORS_COAX_H__
|
||||
#define __AP_MOTORS_COAX_H__
|
||||
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
||||
#include <RC_Channel.h> // RC Channel Library
|
||||
#include "AP_Motors.h"
|
||||
|
||||
// feedback direction
|
||||
#define AP_MOTORS_COAX_POSITIVE 1
|
||||
#define AP_MOTORS_COAX_NEGATIVE -1
|
||||
|
||||
#define AP_MOTORS_SINGLE_SPEED_DIGITAL_SERVOS 250 // update rate for digital servos
|
||||
#define AP_MOTORS_SINGLE_SPEED_ANALOG_SERVOS 125 // update rate for analog servos
|
||||
|
||||
/// @class AP_MotorsSingle
|
||||
class AP_MotorsCoax : public AP_Motors {
|
||||
public:
|
||||
|
||||
/// Constructor
|
||||
AP_MotorsCoax( RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, RC_Channel* servo1, RC_Channel* servo2, RC_Channel* servo3, RC_Channel* servo4, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
|
||||
AP_Motors(rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz),
|
||||
_servo1(servo1),
|
||||
_servo2(servo2),
|
||||
_servo3(servo3),
|
||||
_servo4(servo4)
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
};
|
||||
|
||||
// init
|
||||
virtual void Init();
|
||||
|
||||
// set update rate to motors - a value in hertz
|
||||
void set_update_rate( uint16_t speed_hz );
|
||||
|
||||
// enable - starts allowing signals to be sent to motors
|
||||
virtual void enable();
|
||||
|
||||
// output_test - spin each motor for a moment to allow the user to confirm the motor order and spin direction
|
||||
virtual void output_test();
|
||||
|
||||
// output_min - sends minimum values out to the motors
|
||||
virtual void output_min();
|
||||
|
||||
// var_info for holding Parameter information
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
protected:
|
||||
// output - sends commands to the motors
|
||||
virtual void output_armed();
|
||||
virtual void output_disarmed();
|
||||
|
||||
AP_Int8 _rev_roll; // REV Roll feedback
|
||||
AP_Int8 _rev_pitch; // REV pitch feedback
|
||||
AP_Int8 _rev_yaw; // REV yaw feedback
|
||||
AP_Int16 _servo_speed; // servo speed
|
||||
RC_Channel* _servo1;
|
||||
RC_Channel* _servo2;
|
||||
RC_Channel* _servo3;
|
||||
RC_Channel* _servo4;
|
||||
};
|
||||
|
||||
#endif // AP_MOTORSSINGLE
|
||||
Loading…
Reference in New Issue
Block a user