Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

ArduCopter - AP_Motors library - added new library which has few advantages over current code:

Browse Source 
1. it's step towards rearchitecting the current code base ahead of a move to RTOS.
     2. internally it uses a MatrixTable for the Quad, Octa, OctaQuad, Y6 frames.
     3. it implements the missing stability patch for Octa and OctaQuads (still missing for Y6)

Later check-ins will incorporate into the main ArduCopter code.
This commit is contained in:
rmackay9 2012-04-02 17:26:37 +09:00
parent 40d7b07789
commit 71ad185238
19 changed files with 1815 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
libraries/AP_Motors/AP_Motors.cpp Normal file
View File

@ -0,0 +1,56 @@
/*
AP_Motors.cpp - ArduCopter motors library
Code by RandyMackay. DIYDrones.com
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
*/
#include "AP_Motors.h"
// parameters for the motor class
const AP_Param::GroupInfo AP_Motors::var_info[] PROGMEM = {
AP_GROUPINFO("TB_RATIO", 0, AP_Motors, top_bottom_ratio), // not used
AP_GROUPEND
};
// Constructor
AP_Motors::AP_Motors( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz ) :
top_bottom_ratio(AP_MOTORS_TOP_BOTTOM_RATIO),
_rc(rc_out),
_rc_roll(rc_roll),
_rc_pitch(rc_pitch),
_rc_throttle(rc_throttle),
_rc_yaw(rc_yaw),
_speed_hz(speed_hz),
_armed(false),
_auto_armed(false),
_frame_orientation(0),
_min_throttle(AP_MOTORS_DEFAULT_MIN_THROTTLE),
_max_throttle(AP_MOTORS_DEFAULT_MAX_THROTTLE)
{
uint8_t i;
// initialise motor map
if( APM_version == AP_MOTORS_APM1 ) {
set_motor_to_channel_map(APM1_MOTOR_TO_CHANNEL_MAP);
} else {
set_motor_to_channel_map(APM2_MOTOR_TO_CHANNEL_MAP);
}
// clear output arrays
for(i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
motor_out[i] = 0;
}
};
// throttle_pass_through - passes throttle through to motors - dangerous but used for initialising ESCs
void AP_Motors::throttle_pass_through() {
if( armed() ) {
for( int16_t i=0; i < AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
_rc->OutputCh(_motor_to_channel_map[i], _rc_throttle->radio_in);
}
}
}

137
libraries/AP_Motors/AP_Motors.h Normal file
View File

@ -0,0 +1,137 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file AxisController.h
/// @brief Generic PID algorithm, with EEPROM-backed storage of constants.
#ifndef AP_MOTORS
#define AP_MOTORS
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library
#include <APM_RC.h> // ArduPilot Mega RC Library
// offsets for motors in motor_out, _motor_filtered and _motor_to_channel_map arrays
#define AP_MOTORS_MOT_1 0
#define AP_MOTORS_MOT_2 1
#define AP_MOTORS_MOT_3 2
#define AP_MOTORS_MOT_4 3
#define AP_MOTORS_MOT_5 4
#define AP_MOTORS_MOT_6 5
#define AP_MOTORS_MOT_7 6
#define AP_MOTORS_MOT_8 7
#define APM1_MOTOR_TO_CHANNEL_MAP CH_1,CH_2,CH_3,CH_4,CH_7,CH_8,CH_10,CH_11
#define APM2_MOTOR_TO_CHANNEL_MAP CH_1,CH_2,CH_3,CH_4,CH_5,CH_6,CH_7,CH_8
#define AP_MOTORS_MAX_NUM_MOTORS 8
#define AP_MOTORS_DEFAULT_MIN_THROTTLE 130
#define AP_MOTORS_DEFAULT_MAX_THROTTLE 850
// APM board definitions
#define AP_MOTORS_APM1 1
#define AP_MOTORS_APM2 2
// frame definitions
#define AP_MOTORS_PLUS_FRAME 0
#define AP_MOTORS_X_FRAME 1
#define AP_MOTORS_V_FRAME 2
// motor update rates
#define AP_MOTORS_SPEED_DEFAULT 490
#define AP_MOTORS_SPEED_INSTANT_PWM 0
// top-bottom ratio (for Y6)
#define AP_MOTORS_TOP_BOTTOM_RATIO 1.0
/// @class AP_Motors
class AP_Motors {
public:
// Constructor
AP_Motors( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT);
// init
virtual void Init() {};
// set mapping from motor number to RC channel
virtual void set_motor_to_channel_map( uint8_t mot_1, uint8_t mot_2, uint8_t mot_3, uint8_t mot_4, uint8_t mot_5, uint8_t mot_6, uint8_t mot_7, uint8_t mot_8 ) {
_motor_to_channel_map[AP_MOTORS_MOT_1] = mot_1;
_motor_to_channel_map[AP_MOTORS_MOT_2] = mot_2;
_motor_to_channel_map[AP_MOTORS_MOT_3] = mot_3;
_motor_to_channel_map[AP_MOTORS_MOT_4] = mot_4;
_motor_to_channel_map[AP_MOTORS_MOT_5] = mot_5;
_motor_to_channel_map[AP_MOTORS_MOT_6] = mot_6;
_motor_to_channel_map[AP_MOTORS_MOT_7] = mot_7;
_motor_to_channel_map[AP_MOTORS_MOT_8] = mot_8;
}
// set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm
virtual void set_update_rate( uint16_t speed_hz ) { _speed_hz = speed_hz; };
// set frame orientation (normally + or X)
virtual void set_frame_orientation( uint8_t new_orientation ) { _frame_orientation = new_orientation; };
// enable - starts allowing signals to be sent to motors
virtual void enable() {};
// arm, disarm or check status status of motors
virtual bool armed() { return _armed; };
virtual void armed(bool armed) { _armed = armed; };
// check or set status of auto_armed - controls whether autopilot can take control of throttle
// Note: this should probably be moved out of this class as it has little to do with the motors
virtual bool auto_armed() { return _auto_armed; };
virtual void auto_armed(bool armed) { _auto_armed = armed; };
// set_min_throttle - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle)
virtual void set_min_throttle(uint16_t min_throttle) { _min_throttle = min_throttle; };
virtual void set_max_throttle(uint16_t max_throttle) { _max_throttle = max_throttle; };
// output - sends commands to the motors
virtual void output() { if( _armed && _auto_armed ) { output_armed(); }else{ output_disarmed(); } };
// output_min - sends minimum values out to the motors
virtual void output_min() {};
// get basic information about the platform
virtual uint8_t get_num_motors() { return 0; };
// motor test
virtual void output_test() {};
// throttle_pass_through - passes throttle through to motors - dangerous but required for initialising ESCs
virtual void throttle_pass_through();
// 1 if motor is enabled, 0 otherwise
AP_Int8 motor_enabled[AP_MOTORS_MAX_NUM_MOTORS];
// final output values sent to the motors. public (for now) so that they can be access for logging
int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS];
// power ratio of upper vs lower motors (only used by y6 and octa quad copters)
AP_Float top_bottom_ratio;
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
protected:
// output functions that should be overloaded by child classes
virtual void output_armed() {};
virtual void output_disarmed() {};
APM_RC_Class* _rc; // APM_RC class used to send updates to ESCs/Servos
RC_Channel* _rc_roll, *_rc_pitch, *_rc_throttle, *_rc_yaw; // input in from users
uint8_t _motor_to_channel_map[AP_MOTORS_MAX_NUM_MOTORS]; // mapping of motor number (as received from upper APM code) to RC channel output - used to account for differences between APM1 and APM2
uint16_t _speed_hz; // speed in hz to send updates to motors
bool _armed; // true if motors are armed
bool _auto_armed; // true is throttle is above zero, allows auto pilot to take control of throttle
uint8_t _frame_orientation; // PLUS_FRAME 0, X_FRAME 1, V_FRAME 2
int16_t _min_throttle; // the minimum throttle to be sent to the engines when they're on (prevents issues with some motors on while other off at very low throttle)
int16_t _max_throttle; // the minimum throttle to be sent to the engines when they're on (prevents issues with some motors on while other off at very low throttle)
};
#endif // AP_MOTORS

370
libraries/AP_Motors/AP_MotorsHeli.cpp Normal file
View File

@ -0,0 +1,370 @@
/*
AP_MotorsHeli.cpp - ArduCopter motors library
Code by RandyMackay. DIYDrones.com
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
*/
#include "AP_MotorsHeli.h"
const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = {
AP_NESTEDGROUPINFO(AP_Motors, 0),
AP_GROUPINFO("SV1_POS", 1, AP_MotorsHeli, servo1_pos),
AP_GROUPINFO("SV2_POS", 2, AP_MotorsHeli, servo2_pos),
AP_GROUPINFO("SV3_POS", 3, AP_MotorsHeli, servo3_pos),
AP_GROUPINFO("ROL_MAX", 4, AP_MotorsHeli, roll_max),
AP_GROUPINFO("PIT_MAX", 5, AP_MotorsHeli, pitch_max),
AP_GROUPINFO("COL_MIN", 6, AP_MotorsHeli, collective_min),
AP_GROUPINFO("COL_MAX", 7, AP_MotorsHeli, collective_max),
AP_GROUPINFO("COL_MID", 8, AP_MotorsHeli, collective_mid),
AP_GROUPINFO("GYR_ENABLE", 9, AP_MotorsHeli, ext_gyro_enabled),
AP_GROUPINFO("SWASH_TYPE", 10, AP_MotorsHeli, swash_type), // changed from trunk
AP_GROUPINFO("GYR_GAIN", 11, AP_MotorsHeli, ext_gyro_gain),
AP_GROUPINFO("SV_MAN", 12, AP_MotorsHeli, servo_manual),
AP_GROUPINFO("PHANG", 13, AP_MotorsHeli, phase_angle), // changed from trunk
AP_GROUPINFO("COLYAW", 14, AP_MotorsHeli, collective_yaw_effect), // changed from trunk
AP_GROUPEND
};
// init
void AP_MotorsHeli::Init()
{
// set update rate
set_update_rate(_speed_hz);
}
// set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm
void AP_MotorsHeli::set_update_rate( uint16_t speed_hz )
{
// record requested speed
_speed_hz = speed_hz;
// setup fast channels
if( _speed_hz != AP_MOTORS_SPEED_INSTANT_PWM ) {
_rc->SetFastOutputChannels(_BV(_motor_to_channel_map[AP_MOTORS_MOT_1]) | _BV(_motor_to_channel_map[AP_MOTORS_MOT_2]) | _BV(_motor_to_channel_map[AP_MOTORS_MOT_3]) | _BV(_motor_to_channel_map[AP_MOTORS_MOT_4]), _speed_hz);
}
}
// enable - starts allowing signals to be sent to motors
void AP_MotorsHeli::enable()
{
// enable output channels
_rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_1]); // swash servo 1
_rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_2]); // swash servo 2
_rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_3]); // swash servo 3
_rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_4]); // yaw
_rc->enable_out(AP_MOTORS_HELI_EXT_GYRO); // for external gyro
}
// output_min - sends minimum values out to the motors
void AP_MotorsHeli::output_min()
{
// move swash to mid
move_swash(0,0,500,0);
}
// output_armed - sends commands to the motors
void AP_MotorsHeli::output_armed()
{
// if manual override (i.e. when setting up swash), pass pilot commands straight through to swash
if( servo_manual == 1 ) {
_rc_roll->servo_out = _rc_roll->control_in;
_rc_pitch->servo_out = _rc_pitch->control_in;
_rc_throttle->servo_out = _rc_throttle->control_in;
_rc_yaw->servo_out = _rc_yaw->control_in;
}
//static int counter = 0;
_rc_roll->calc_pwm();
_rc_pitch->calc_pwm();
_rc_throttle->calc_pwm();
_rc_yaw->calc_pwm();
move_swash( _rc_roll->servo_out, _rc_pitch->servo_out, _rc_throttle->servo_out, _rc_yaw->servo_out );
}
// output_disarmed - sends commands to the motors
void AP_MotorsHeli::output_disarmed()
{
if(_rc_throttle->control_in > 0){
// we have pushed up the throttle
// remove safety
_auto_armed = true;
}
// for helis - armed or disarmed we allow servos to move
output_armed();
}
// output_disarmed - sends commands to the motors
void AP_MotorsHeli::output_test()
{
int16_t i;
// Send minimum values to all motors
output_min();
// servo 1
for( i=0; i<5; i++ ) {
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_trim + 100);
delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_trim - 100);
delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_trim + 0);
delay(300);
}
// servo 2
for( i=0; i<5; i++ ) {
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo_2->radio_trim + 100);
delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo_2->radio_trim - 100);
delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo_2->radio_trim + 0);
delay(300);
}
// servo 3
for( i=0; i<5; i++ ) {
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_3->radio_trim + 100);
delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_3->radio_trim - 100);
delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_3->radio_trim + 0);
delay(300);
}
// external gyro
if( ext_gyro_enabled ) {
_rc->OutputCh(AP_MOTORS_HELI_EXT_GYRO, ext_gyro_gain);
}
// servo 4
for( i=0; i<5; i++ ) {
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo_4->radio_trim + 100);
delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo_4->radio_trim - 100);
delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo_4->radio_trim + 0);
delay(300);
}
// Send minimum values to all motors
output_min();
}
// reset_swash - free up swash for maximum movements. Used for set-up
void AP_MotorsHeli::reset_swash()
{
// free up servo ranges
_servo_1->radio_min = 1000;
_servo_1->radio_max = 2000;
_servo_2->radio_min = 1000;
_servo_2->radio_max = 2000;
_servo_3->radio_min = 1000;
_servo_3->radio_max = 2000;
if( swash_type == AP_MOTORS_HELI_SWASH_CCPM ) { //CCPM Swashplate, perform servo control mixing
// roll factors
_rollFactor[CH_1] = cos(radians(servo1_pos + 90 - phase_angle));
_rollFactor[CH_2] = cos(radians(servo2_pos + 90 - phase_angle));
_rollFactor[CH_3] = cos(radians(servo3_pos + 90 - phase_angle));
// pitch factors
_pitchFactor[CH_1] = cos(radians(servo1_pos - phase_angle));
_pitchFactor[CH_2] = cos(radians(servo2_pos - phase_angle));
_pitchFactor[CH_3] = cos(radians(servo3_pos - phase_angle));
// collective factors
_collectiveFactor[CH_1] = 1;
_collectiveFactor[CH_2] = 1;
_collectiveFactor[CH_3] = 1;
}else{ //H1 Swashplate, keep servo outputs seperated
// roll factors
_rollFactor[CH_1] = 1;
_rollFactor[CH_2] = 0;
_rollFactor[CH_3] = 0;
// pitch factors
_pitchFactor[CH_1] = 0;
_pitchFactor[CH_2] = 1;
_pitchFactor[CH_3] = 0;
// collective factors
_collectiveFactor[CH_1] = 0;
_collectiveFactor[CH_2] = 0;
_collectiveFactor[CH_3] = 1;
}
// set roll, pitch and throttle scaling
_roll_scaler = 1.0;
_pitch_scaler = 1.0;
_collective_scalar = ((float)(_rc_throttle->radio_max - _rc_throttle->radio_min))/1000.0;
// we must be in set-up mode so mark swash as uninitialised
_swash_initialised = false;
}
// init_swash - initialise the swash plate
void AP_MotorsHeli::init_swash()
{
// swash servo initialisation
_servo_1->set_range(0,1000);
_servo_2->set_range(0,1000);
_servo_3->set_range(0,1000);
_servo_4->set_angle(4500);
// ensure _coll values are reasonable
if( collective_min >= collective_max ) {
collective_min = 1000;
collective_max = 2000;
}
collective_mid = constrain(collective_mid, collective_min, collective_max);
// calculate throttle mid point
throttle_mid = ((float)(collective_mid-collective_min))/((float)(collective_max-collective_min))*1000.0;
// determine roll, pitch and throttle scaling
_roll_scaler = (float)roll_max/4500.0;
_pitch_scaler = (float)pitch_max/4500.0;
_collective_scalar = ((float)(collective_max-collective_min))/1000.0;
if( swash_type == AP_MOTORS_HELI_SWASH_CCPM ) { //CCPM Swashplate, perform control mixing
// roll factors
_rollFactor[CH_1] = cos(radians(servo1_pos + 90 - phase_angle));
_rollFactor[CH_2] = cos(radians(servo2_pos + 90 - phase_angle));
_rollFactor[CH_3] = cos(radians(servo3_pos + 90 - phase_angle));
// pitch factors
_pitchFactor[CH_1] = cos(radians(servo1_pos - phase_angle));
_pitchFactor[CH_2] = cos(radians(servo2_pos - phase_angle));
_pitchFactor[CH_3] = cos(radians(servo3_pos - phase_angle));
// collective factors
_collectiveFactor[CH_1] = 1;
_collectiveFactor[CH_2] = 1;
_collectiveFactor[CH_3] = 1;
}else{ //H1 Swashplate, keep servo outputs seperated
// roll factors
_rollFactor[CH_1] = 1;
_rollFactor[CH_2] = 0;
_rollFactor[CH_3] = 0;
// pitch factors
_pitchFactor[CH_1] = 0;
_pitchFactor[CH_2] = 1;
_pitchFactor[CH_3] = 0;
// collective factors
_collectiveFactor[CH_1] = 0;
_collectiveFactor[CH_2] = 0;
_collectiveFactor[CH_3] = 1;
}
// servo min/max values
_servo_1->radio_min = 1000;
_servo_1->radio_max = 2000;
_servo_2->radio_min = 1000;
_servo_2->radio_max = 2000;
_servo_3->radio_min = 1000;
_servo_3->radio_max = 2000;
// mark swash as initialised
_swash_initialised = true;
}
//
// heli_move_swash - moves swash plate to attitude of parameters passed in
// - expected ranges:
// roll : -4500 ~ 4500
// pitch: -4500 ~ 4500
// collective: 0 ~ 1000
// yaw: -4500 ~ 4500
//
void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll_out, int16_t yaw_out)
{
int16_t yaw_offset = 0;
int16_t coll_out_scaled;
if( servo_manual == 1 ) { // are we in manual servo mode? (i.e. swash set-up mode)?
// check if we need to free up the swash
if( _swash_initialised ) {
reset_swash();
}
coll_out_scaled = coll_out * _collective_scalar + _rc_throttle->radio_min - 1000;
}else{ // regular flight mode
// check if we need to reinitialise the swash
if( !_swash_initialised ) {
init_swash();
}
// rescale roll_out and pitch-out into the min and max ranges to provide linear motion
// across the input range instead of stopping when the input hits the constrain value
// these calculations are based on an assumption of the user specified roll_max and pitch_max
// coming into this equation at 4500 or less, and based on the original assumption of the
// total _servo_x.servo_out range being -4500 to 4500.
roll_out = roll_out * _roll_scaler;
roll_out = constrain(roll_out, (int16_t)-roll_max, (int16_t)roll_max);
pitch_out = pitch_out * _pitch_scaler;
pitch_out = constrain(pitch_out, (int16_t)-pitch_max, (int16_t)pitch_max);
// scale collective pitch
coll_out = constrain(coll_out, 0, 1000);
coll_out_scaled = coll_out * _collective_scalar + collective_min - 1000;
// rudder feed forward based on collective
if( !ext_gyro_enabled ) {
yaw_offset = collective_yaw_effect * abs(coll_out_scaled - collective_mid);
}
}
// swashplate servos
_servo_1->servo_out = (_rollFactor[CH_1] * roll_out + _pitchFactor[CH_1] * pitch_out)/10 + _collectiveFactor[CH_1] * coll_out_scaled + (_servo_1->radio_trim-1500);
_servo_2->servo_out = (_rollFactor[CH_2] * roll_out + _pitchFactor[CH_2] * pitch_out)/10 + _collectiveFactor[CH_2] * coll_out_scaled + (_servo_2->radio_trim-1500);
if( swash_type == AP_MOTORS_HELI_SWASH_H1 ) {
_servo_1->servo_out += 500;
_servo_2->servo_out += 500;
}
_servo_3->servo_out = (_rollFactor[CH_3] * roll_out + _pitchFactor[CH_3] * pitch_out)/10 + _collectiveFactor[CH_3] * coll_out_scaled + (_servo_3->radio_trim-1500);
_servo_4->servo_out = yaw_out + yaw_offset;
// use servo_out to calculate pwm_out and radio_out
_servo_1->calc_pwm();
_servo_2->calc_pwm();
_servo_3->calc_pwm();
_servo_4->calc_pwm();
// actually move the servos
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_out);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo_2->radio_out);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_3->radio_out);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo_4->radio_out);
// to be compatible with other frame types
motor_out[AP_MOTORS_MOT_1] = _servo_1->radio_out;
motor_out[AP_MOTORS_MOT_2] = _servo_2->radio_out;
motor_out[AP_MOTORS_MOT_3] = _servo_3->radio_out;
motor_out[AP_MOTORS_MOT_4] = _servo_4->radio_out;
// output gyro value
if( ext_gyro_enabled ) {
_rc->OutputCh(AP_MOTORS_HELI_EXT_GYRO, ext_gyro_gain);
}
// InstantPWM
if( _speed_hz == AP_MOTORS_SPEED_INSTANT_PWM ) {
_rc->Force_Out0_Out1();
_rc->Force_Out2_Out3();
}
}

140
libraries/AP_Motors/AP_MotorsHeli.h Normal file
View File

@ -0,0 +1,140 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file AP_MotorsHeli.h
/// @brief Motor control class for Traditional Heli
#ifndef AP_MOTORSHELI
#define AP_MOTORSHELI
#include <inttypes.h>
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <AP_Motors.h>
#define AP_MOTORS_HELI_SPEED_DEFAULT 125 // default servo update rate for helicopters
#define AP_MOTORS_HELI_SPEED_DIGITAL_SERVOS 125 // update rate for digital servos
#define AP_MOTORS_HELI_SPEED_ANALOG_SERVOS 125 // update rate for analog servos
#define AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS 3
// tail servo uses channel 7
#define AP_MOTORS_HELI_EXT_GYRO CH_7
// frame definitions
#define AP_MOTORS_HELI_SWASH_CCPM 0
#define AP_MOTORS_HELI_SWASH_H1 1
/// @class AP_MotorsHeli
class AP_MotorsHeli : public AP_Motors {
public:
/// Constructor
AP_MotorsHeli( uint8_t APM_version,
APM_RC_Class* rc_out,
RC_Channel* rc_roll,
RC_Channel* rc_pitch,
RC_Channel* rc_throttle,
RC_Channel* rc_yaw,
RC_Channel* swash_servo_1,
RC_Channel* swash_servo_2,
RC_Channel* swash_servo_3,
RC_Channel* yaw_servo,
uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) :
AP_Motors(APM_version, rc_out, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz),
_servo_1(swash_servo_1),
_servo_2(swash_servo_2),
_servo_3(swash_servo_3),
_servo_4(yaw_servo),
swash_type(AP_MOTORS_HELI_SWASH_CCPM),
servo1_pos (-60),
servo2_pos (60),
servo3_pos (180),
roll_max (4500),
pitch_max (4500),
collective_min (1250),
collective_max (1750),
collective_mid (1500),
ext_gyro_enabled (0),
ext_gyro_gain (1350),
phase_angle (0),
collective_yaw_effect (0),
servo_manual (0),
throttle_mid(0),
_roll_scaler(1),
_pitch_scaler(1),
_collective_scalar(1),
_swash_initialised(false)
{};
// init
virtual void Init();
// set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm
// you must have setup_motors before calling this
virtual void set_update_rate( uint16_t speed_hz );
// enable - starts allowing signals to be sent to motors
virtual void enable();
// get basic information about the platform
virtual uint8_t get_num_motors() { return 4; };
// motor test
virtual void output_test();
// output_min - sends minimum values out to the motors
virtual void output_min();
// reset_swash - free up swash for maximum movements. Used for set-up
virtual void reset_swash();
// init_swash - initialise the swash plate
virtual void init_swash();
// heli_move_swash - moves swash plate to attitude of parameters passed in
virtual void move_swash(int roll_out, int pitch_out, int coll_out, int yaw_out);
// 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();
float _rollFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS];
float _pitchFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS];
float _collectiveFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS];
RC_Channel *_servo_1;
RC_Channel *_servo_2;
RC_Channel *_servo_3;
RC_Channel *_servo_4;
AP_Int8 swash_type;
AP_Int16 servo1_pos;
AP_Int16 servo2_pos;
AP_Int16 servo3_pos;
AP_Int16 roll_max;
AP_Int16 pitch_max;
AP_Int16 collective_min;
AP_Int16 collective_max;
AP_Int16 collective_mid;
AP_Int16 ext_gyro_enabled;
AP_Int16 ext_gyro_gain;
AP_Int16 phase_angle;
AP_Int16 collective_yaw_effect;
AP_Int8 servo_manual; // used to trigger swash reset from mission planner
// internally used variables
int16_t throttle_mid; // throttle mid point in pwm form (i.e. 0 ~ 1000)
float _roll_scaler; // scaler to convert roll input from radio (i.e. -4500 ~ 4500) to max roll range
float _pitch_scaler; // scaler to convert pitch input from radio (i.e. -4500 ~ 4500) to max pitch range
float _collective_scalar; // throttle scalar to convert pwm form (i.e. 0 ~ 1000) passed in to actual servo range (i.e 1250~1750 would be 500)
bool _swash_initialised; // true if swash has been initialised
};
#endif // AP_MOTORSHELI

37
libraries/AP_Motors/AP_MotorsHexa.cpp Normal file
View File

@ -0,0 +1,37 @@
/*
AP_MotorsHexa.cpp - ArduCopter motors library
Code by RandyMackay. DIYDrones.com
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
*/
#include "AP_MotorsHexa.h"
// setup_motors - configures the motors for a hexa
void AP_MotorsHexa::setup_motors()
{
// call parent
AP_MotorsMatrix::setup_motors();
// hard coded config for supported frames
if( _frame_orientation == AP_MOTORS_PLUS_FRAME ) {
// plus frame set-up
add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_2, 1);
add_motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_1, 4);
add_motor(AP_MOTORS_MOT_3,-120, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_4, 5);
add_motor(AP_MOTORS_MOT_4, 60, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_3, 2);
add_motor(AP_MOTORS_MOT_5, -60, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_6, 6);
add_motor(AP_MOTORS_MOT_6, 120, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_5, 3);
}else{
// X frame set-up
add_motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_2, 2);
add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_1, 5);
add_motor(AP_MOTORS_MOT_3, -30, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_4, 6);
add_motor(AP_MOTORS_MOT_4, 150, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_3, 3);
add_motor(AP_MOTORS_MOT_5, 30, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_6, 1);
add_motor(AP_MOTORS_MOT_6,-150, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_5, 4);
}
}

30
libraries/AP_Motors/AP_MotorsHexa.h Normal file
View File

@ -0,0 +1,30 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file AP_MotorsHexa.h
/// @brief Motor control class for Hexacopters
#ifndef AP_MOTORSHEXA
#define AP_MOTORSHEXA
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <AP_MotorsMatrix.h> // Parent Motors Matrix library
/// @class AP_MotorsHexa
class AP_MotorsHexa : public AP_MotorsMatrix {
public:
/// Constructor
AP_MotorsHexa( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(APM_version, rc_out, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {};
// setup_motors - configures the motors for a quad
virtual void setup_motors();
protected:
};
#endif // AP_MOTORSHEXA

330
libraries/AP_Motors/AP_MotorsMatrix.cpp Normal file
View File

@ -0,0 +1,330 @@
/*
AP_MotorsMatrix.cpp - ArduCopter motors library
Code by RandyMackay. DIYDrones.com
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
*/
#include "AP_MotorsMatrix.h"
// Init
void AP_MotorsMatrix::Init()
{
int8_t i;
// setup the motors
setup_motors();
// double check that opposite motor definitions are ok
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( opposite_motor[i] <= 0 || opposite_motor[i] >= AP_MOTORS_MAX_NUM_MOTORS || !motor_enabled[opposite_motor[i]] )
opposite_motor[i] = AP_MOTORS_MATRIX_MOTOR_UNDEFINED;
}
// enable fast channels or instant pwm
set_update_rate(_speed_hz);
}
// set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm
void AP_MotorsMatrix::set_update_rate( uint16_t speed_hz )
{
uint32_t fast_channel_mask = 0;
int8_t i;
// record requested speed
_speed_hz = speed_hz;
// initialise instant_pwm booleans. they will be set again below
instant_pwm_force01 = false;
instant_pwm_force23 = false;
instant_pwm_force67 = false;
// check each enabled motor
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
// set-up fast channel mask
fast_channel_mask |= _BV(_motor_to_channel_map[i]); // add to fast channel map
// and instant pwm
if( _motor_to_channel_map[i] == 0 || _motor_to_channel_map[i] == 1 )
instant_pwm_force01 = true;
if( _motor_to_channel_map[i] == 2 || _motor_to_channel_map[i] == 3 )
instant_pwm_force23 = true;
if( _motor_to_channel_map[i] == 6 || _motor_to_channel_map[i] == 7 )
instant_pwm_force67 = true;
}
}
// enable fast channels
if( _speed_hz != AP_MOTORS_SPEED_INSTANT_PWM ) {
_rc->SetFastOutputChannels(fast_channel_mask, _speed_hz);
}
}
// set frame orientation (normally + or X)
void AP_MotorsMatrix::set_frame_orientation( uint8_t new_orientation )
{
// return if nothing has changed
if( new_orientation == _frame_orientation ) {
return;
}
// call parent
AP_Motors::set_frame_orientation( new_orientation );
// setup the motors
setup_motors();
// enable fast channels or instant pwm
set_update_rate(_speed_hz);
}
// enable - starts allowing signals to be sent to motors
void AP_MotorsMatrix::enable()
{
int8_t i;
// enable output channels
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
_rc->enable_out(_motor_to_channel_map[i]);
}
}
}
// output_min - sends minimum values out to the motors
void AP_MotorsMatrix::output_min()
{
int8_t i;
// fill the motor_out[] array for HIL use and send minimum value to each motor
for( int8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
motor_out[i] = _rc_throttle->radio_min;
_rc->OutputCh(_motor_to_channel_map[i], motor_out[i]);
}
}
// Force output if instant pwm
if( _speed_hz == AP_MOTORS_SPEED_INSTANT_PWM ) {
if( instant_pwm_force01 )
_rc->Force_Out0_Out1();
if( instant_pwm_force23 )
_rc->Force_Out2_Out3();
if( instant_pwm_force67 )
_rc->Force_Out6_Out7();
}
}
// output_armed - sends commands to the motors
void AP_MotorsMatrix::output_armed()
{
int8_t i;
int16_t out_min = _rc_throttle->radio_min;
int16_t out_max = _rc_throttle->radio_max;
// Throttle is 0 to 1000 only
_rc_throttle->servo_out = constrain(_rc_throttle->servo_out, 0, _max_throttle);
if(_rc_throttle->servo_out > 0)
out_min = _rc_throttle->radio_min + _min_throttle;
// capture desired roll, pitch, yaw and throttle from receiver
_rc_roll->calc_pwm();
_rc_pitch->calc_pwm();
_rc_throttle->calc_pwm();
_rc_yaw->calc_pwm();
// mix roll, pitch, yaw, throttle into output for each motor
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
motor_out[i] = _rc_throttle->radio_out +
_rc_roll->pwm_out * _roll_factor[i] +
_rc_pitch->pwm_out * _pitch_factor[i] +
_rc_yaw->pwm_out*_yaw_factor[i];
}
// ensure motor is not below the minimum
motor_out[AP_MOTORS_MOT_1] = max(motor_out[AP_MOTORS_MOT_1], out_min);
}
// stability patch
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] && opposite_motor[i] != AP_MOTORS_MATRIX_MOTOR_UNDEFINED && motor_out[i] > out_max ) {
motor_out[opposite_motor[i]] -= motor_out[i] - out_max;
motor_out[i] = out_max;
}
}
// ensure motors are not below the minimum
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
motor_out[i] = max(motor_out[i], out_min);
}
}
#if CUT_MOTORS == ENABLED
// if we are not sending a throttle output, we cut the motors
if(_rc_throttle->servo_out == 0){
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
motor_out[i] = _rc_throttle->radio_min;
}
}
}
#endif
// send output to each motor
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
_rc->OutputCh(_motor_to_channel_map[i], motor_out[i]);
}
}
// InstantPWM
if( _speed_hz == AP_MOTORS_SPEED_INSTANT_PWM ) {
if( instant_pwm_force01 )
_rc->Force_Out0_Out1();
if( instant_pwm_force23 )
_rc->Force_Out2_Out3();
if( instant_pwm_force67 )
_rc->Force_Out6_Out7();
}
}
// output_disarmed - sends commands to the motors
void AP_MotorsMatrix::output_disarmed()
{
if(_rc_throttle->control_in > 0){
// we have pushed up the throttle
// remove safety for auto pilot
_auto_armed = true;
}
// Send minimum values to all motors
output_min();
}
// output_disarmed - sends commands to the motors
void AP_MotorsMatrix::output_test()
{
int8_t min_order, max_order;
int8_t i,j;
// find min and max orders
min_order = test_order[0];
max_order = test_order[0];
for(i=1; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( test_order[i] < min_order )
min_order = test_order[i];
if( test_order[i] > max_order )
max_order = test_order[i];
}
// shut down all motors
output_min();
// first delay is longer
delay(4000);
// loop through all the possible orders spinning any motors that match that description
for( i=min_order; i<=max_order; i++ ) {
for( j=0; j<AP_MOTORS_MAX_NUM_MOTORS; j++ ) {
if( motor_enabled[j] && test_order[j] == i ) {
// turn on this motor and wait 1/3rd of a second
_rc->OutputCh(_motor_to_channel_map[j], _rc_throttle->radio_min + 100);
delay(300);
_rc->OutputCh(_motor_to_channel_map[j], _rc_throttle->radio_min);
delay(2000);
}
}
}
// shut down all motors
output_min();
}
// add_motor
void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, int8_t opposite_motor_num, int8_t testing_order)
{
// ensure valid motor number is provided
if( motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS ) {
// increment number of motors if this motor is being newly motor_enabled
if( !motor_enabled[motor_num] ) {
motor_enabled[motor_num] = true;
_num_motors++;
}
// set roll, pitch, thottle factors and opposite motor (for stability patch)
_roll_factor[motor_num] = roll_fac;
_pitch_factor[motor_num] = pitch_fac;
_yaw_factor[motor_num] = yaw_fac;
// set opposite motor after checking it's somewhat valid
if( opposite_motor_num == AP_MOTORS_MATRIX_MOTOR_UNDEFINED || (opposite_motor_num >=0 && opposite_motor_num < AP_MOTORS_MAX_NUM_MOTORS) ) {
opposite_motor[motor_num] = opposite_motor_num;
}else{
opposite_motor[motor_num] = AP_MOTORS_MATRIX_MOTOR_UNDEFINED;
}
// set order that motor appears in test
if( testing_order == AP_MOTORS_MATRIX_ORDER_UNDEFINED ) {
test_order[motor_num] = motor_num;
}else{
test_order[motor_num] = testing_order;
}
}
}
// add_motor using just position and prop direction
void AP_MotorsMatrix::add_motor(int8_t motor_num, float angle_degrees, int8_t direction, int8_t opposite_motor_num, int8_t testing_order)
{
// call raw motor set-up method
add_motor_raw(
motor_num,
cos(radians(angle_degrees + 90)), // roll factor
cos(radians(angle_degrees)), // pitch factor
(float)direction, // yaw factor
opposite_motor_num,
testing_order);
}
// remove_motor - disabled motor and clears all roll, pitch, throttle factors for this motor
void AP_MotorsMatrix::remove_motor(int8_t motor_num)
{
int8_t i;
// ensure valid motor number is provided
if( motor_num >= 0 && motor_num <= AP_MOTORS_MAX_NUM_MOTORS ) {
// if the motor was enabled decrement the number of motors
if( motor_enabled[motor_num] )
_num_motors--;
// disable the motor, set all factors to zero
motor_enabled[motor_num] = false;
_roll_factor[motor_num] = 0;
_pitch_factor[motor_num] = 0;
_yaw_factor[motor_num] = 0;
opposite_motor[motor_num] = AP_MOTORS_MATRIX_MOTOR_UNDEFINED;
}
// if another motor has referred to this motor as it's opposite, remove that reference
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( opposite_motor[i] == motor_num )
opposite_motor[i] = AP_MOTORS_MATRIX_MOTOR_UNDEFINED;
}
}
// remove_all_motors - removes all motor definitions
void AP_MotorsMatrix::remove_all_motors()
{
for( int8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
remove_motor(i);
}
_num_motors = 0;
}

93
libraries/AP_Motors/AP_MotorsMatrix.h Normal file
View File

@ -0,0 +1,93 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file AP_MotorsMatrix.h
/// @brief Motor control class for Matrixcopters
#ifndef AP_MOTORSMATRIX
#define AP_MOTORSMATRIX
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <AP_Motors.h>
#define AP_MOTORS_MATRIX_MOTOR_UNDEFINED -1
#define AP_MOTORS_MATRIX_ORDER_UNDEFINED -1
#define AP_MOTORS_MATRIX_MOTOR_CW -1
#define AP_MOTORS_MATRIX_MOTOR_CCW 1
/// @class AP_MotorsMatrix
class AP_MotorsMatrix : public AP_Motors {
public:
/// Constructor
AP_MotorsMatrix( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
AP_Motors(APM_version, rc_out, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz),
instant_pwm_force01(false),
instant_pwm_force23(false),
instant_pwm_force67(false),
_num_motors(0) {};
// init
virtual void Init();
// set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm
// you must have setup_motors before calling this
virtual void set_update_rate( uint16_t speed_hz );
// set frame orientation (normally + or X)
virtual void set_frame_orientation( uint8_t new_orientation );
// enable - starts allowing signals to be sent to motors
virtual void enable();
// get basic information about the platform
virtual uint8_t get_num_motors() { return _num_motors; };
// motor test
virtual void output_test();
// output_min - sends minimum values out to the motors
virtual void output_min();
// add_motor using just position and prop direction
virtual void add_motor(int8_t motor_num, float angle_degrees, int8_t direction, int8_t opposite_motor_num = AP_MOTORS_MATRIX_MOTOR_UNDEFINED, int8_t testing_order = AP_MOTORS_MATRIX_ORDER_UNDEFINED);
// remove_motor
virtual void remove_motor(int8_t motor_num);
// remove_all_motors - removes all motor definitions
virtual void remove_all_motors();
// setup_motors - configures the motors for a given frame type - should be overwritten by child classes
virtual void setup_motors() { remove_all_motors(); };
// matrix
AP_Int16 angle[AP_MOTORS_MAX_NUM_MOTORS]; // angle in degrees from the front of the copter
AP_Int8 direction[AP_MOTORS_MAX_NUM_MOTORS]; // direction of rotation of the motor (-1 = clockwise, +1 = counter clockwise)
AP_Int8 opposite_motor[AP_MOTORS_MAX_NUM_MOTORS]; // motor number of the opposite motor
AP_Int8 test_order[AP_MOTORS_MAX_NUM_MOTORS]; // order of the motors in the test sequence
// used for instant_pwm only
bool instant_pwm_force01;
bool instant_pwm_force23;
bool instant_pwm_force67;
protected:
// output - sends commands to the motors
virtual void output_armed();
virtual void output_disarmed();
// add_motor using raw roll, pitch, throttle and yaw factors
virtual void add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, int8_t opposite_motor_num = AP_MOTORS_MATRIX_MOTOR_UNDEFINED, int8_t testing_order = AP_MOTORS_MATRIX_ORDER_UNDEFINED);
int8_t _num_motors; // not a very useful variable as you really need to check the motor_enabled array to see which motors are enabled
float _roll_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to roll
float _pitch_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to pitch
float _yaw_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to yaw (normally 1 or -1)
};
#endif // AP_MOTORSMATRIX

53
libraries/AP_Motors/AP_MotorsOcta.cpp Normal file
View File

@ -0,0 +1,53 @@
/*
AP_MotorsOcta.cpp - ArduCopter motors library
Code by RandyMackay. DIYDrones.com
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
*/
#include "AP_MotorsOcta.h"
// setup_motors - configures the motors for a octa
void AP_MotorsOcta::setup_motors()
{
// call parent
AP_MotorsMatrix::setup_motors();
// hard coded config for supported frames
if( _frame_orientation == AP_MOTORS_PLUS_FRAME ) {
// plus frame set-up
add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_2, 1);
add_motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_1, 5);
add_motor(AP_MOTORS_MOT_3, 45, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_6, 2);
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_5, 4);
add_motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_4, 8);
add_motor(AP_MOTORS_MOT_6, -135, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_3, 6);
add_motor(AP_MOTORS_MOT_7, -90, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_8, 7);
add_motor(AP_MOTORS_MOT_8, 90, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_7, 3);
}else if( _frame_orientation == AP_MOTORS_V_FRAME ) {
// V frame set-up
add_motor_raw(AP_MOTORS_MOT_1, 1.0, 0.34, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_2, 7);
add_motor_raw(AP_MOTORS_MOT_2, -1.0, -0.32, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_1, 3);
add_motor_raw(AP_MOTORS_MOT_3, 1.0, -0.32, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_6, 6);
add_motor_raw(AP_MOTORS_MOT_4, -0.5, -1.0, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_5, 8);
add_motor_raw(AP_MOTORS_MOT_5, 1.0, 1.0, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_4, 8);
add_motor_raw(AP_MOTORS_MOT_6, -1.0, 0.34, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_3, 2);
add_motor_raw(AP_MOTORS_MOT_7, -1.0, 1.0, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_8, 1);
add_motor_raw(AP_MOTORS_MOT_8, 0.5, -1.0, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_7, 5);
}else {
// X frame set-up
add_motor(AP_MOTORS_MOT_1, 22.5, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_2, 1);
add_motor(AP_MOTORS_MOT_2, -157.5, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_1, 5);
add_motor(AP_MOTORS_MOT_3, 67.5, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_6, 2);
add_motor(AP_MOTORS_MOT_4, 157.5, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_5, 4);
add_motor(AP_MOTORS_MOT_5, -22.5, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_4, 8);
add_motor(AP_MOTORS_MOT_6, -112.5, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_3, 6);
add_motor(AP_MOTORS_MOT_7, -67.5, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_8, 7);
add_motor(AP_MOTORS_MOT_8, 112.5, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_7, 3);
}
}

30
libraries/AP_Motors/AP_MotorsOcta.h Normal file
View File

@ -0,0 +1,30 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file AP_MotorsOcta.h
/// @brief Motor control class for Octacopters
#ifndef AP_MOTORSOCTA
#define AP_MOTORSOCTA
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <AP_MotorsMatrix.h> // Parent Motors Matrix library
/// @class AP_MotorsOcta
class AP_MotorsOcta : public AP_MotorsMatrix {
public:
/// Constructor
AP_MotorsOcta( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(APM_version, rc_out, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {};
// setup_motors - configures the motors for a quad
virtual void setup_motors();
protected:
};
#endif // AP_MOTORSOCTA

41
libraries/AP_Motors/AP_MotorsOctaQuad.cpp Normal file
View File

@ -0,0 +1,41 @@
/*
AP_MotorsOctaQuad.cpp - ArduCopter motors library
Code by RandyMackay. DIYDrones.com
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
*/
#include "AP_MotorsOctaQuad.h"
// setup_motors - configures the motors for a octa
void AP_MotorsOctaQuad::setup_motors()
{
// call parent
AP_MotorsMatrix::setup_motors();
// hard coded config for supported frames
if( _frame_orientation == AP_MOTORS_PLUS_FRAME ) {
// plus frame set-up
add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_3, 1);
add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_4, 7);
add_motor(AP_MOTORS_MOT_3, 180, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_1, 5);
add_motor(AP_MOTORS_MOT_4, 90, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_2, 4);
add_motor(AP_MOTORS_MOT_5, -90, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_7, 8);
add_motor(AP_MOTORS_MOT_6, 0, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_8, 2);
add_motor(AP_MOTORS_MOT_7, 90, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_5, 4);
add_motor(AP_MOTORS_MOT_8, 180, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_6, 6);
}else{
// X frame set-up
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_3, 1);
add_motor(AP_MOTORS_MOT_2, -45, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_4, 7);
add_motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_1, 5);
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_2, 4);
add_motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_7, 8);
add_motor(AP_MOTORS_MOT_6, 45, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_8, 2);
add_motor(AP_MOTORS_MOT_7, 135, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_5, 4);
add_motor(AP_MOTORS_MOT_8, -135, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_6, 6);
}
}

30
libraries/AP_Motors/AP_MotorsOctaQuad.h Normal file
View File

@ -0,0 +1,30 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file AP_MotorsOctaQuad.h
/// @brief Motor control class for OctaQuadcopters
#ifndef AP_MOTORSOCTAQUAD
#define AP_MOTORSOCTAQUAD
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <AP_MotorsMatrix.h> // Parent Motors Matrix library
/// @class AP_MotorsOcta
class AP_MotorsOctaQuad : public AP_MotorsMatrix {
public:
/// Constructor
AP_MotorsOctaQuad( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(APM_version, rc_out, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {};
// setup_motors - configures the motors for a quad
virtual void setup_motors();
protected:
};
#endif // AP_MOTORSOCTAQUAD

33
libraries/AP_Motors/AP_MotorsQuad.cpp Normal file
View File

@ -0,0 +1,33 @@
/*
AP_MotorsQuad.cpp - ArduCopter motors library
Code by RandyMackay. DIYDrones.com
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
*/
#include "AP_MotorsQuad.h"
// setup_motors - configures the motors for a quad
void AP_MotorsQuad::setup_motors()
{
// call parent
AP_MotorsMatrix::setup_motors();
// hard coded config for supported frames
if( _frame_orientation == AP_MOTORS_PLUS_FRAME ) {
// plus frame set-up
add_motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_2, 2);
add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_1, 4);
add_motor(AP_MOTORS_MOT_3, 0, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_4, 1);
add_motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_3, 3);
}else{
// X frame set-up
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_2, 1);
add_motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_1, 3);
add_motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_4, 4);
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_3, 2);
}
}

30
libraries/AP_Motors/AP_MotorsQuad.h Normal file
View File

@ -0,0 +1,30 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file AP_MotorsQuad.h
/// @brief Motor control class for Quadcopters
#ifndef AP_MOTORSQUAD
#define AP_MOTORSQUAD
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <AP_MotorsMatrix.h> // Parent Motors Matrix library
/// @class AP_MotorsQuad
class AP_MotorsQuad : public AP_MotorsMatrix {
public:
/// Constructor
AP_MotorsQuad( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(APM_version, rc_out, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {};
// setup_motors - configures the motors for a quad
virtual void setup_motors();
protected:
};
#endif // AP_MOTORSQUAD

182
libraries/AP_Motors/AP_MotorsTri.cpp Normal file
View File

@ -0,0 +1,182 @@
/*
AP_MotorsTri.cpp - ArduCopter motors library
Code by RandyMackay. DIYDrones.com
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
*/
#include "AP_MotorsTri.h"
// init
void AP_MotorsTri::Init()
{
// set update rate for the 3 motors (but not the servo on channel 7)
set_update_rate(_speed_hz);
}
// set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm
void AP_MotorsTri::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)
if( _speed_hz != AP_MOTORS_SPEED_INSTANT_PWM ) {
_rc->SetFastOutputChannels(_BV(_motor_to_channel_map[AP_MOTORS_MOT_1]) | _BV(_motor_to_channel_map[AP_MOTORS_MOT_2]) | _BV(_motor_to_channel_map[AP_MOTORS_MOT_4]), _speed_hz);
}
}
// enable - starts allowing signals to be sent to motors
void AP_MotorsTri::enable()
{
// enable output channels
_rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_1]);
_rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_2]);
_rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_4]);
_rc->enable_out(AP_MOTORS_CH_TRI_YAW);
}
// output_min - sends minimum values out to the motors
void AP_MotorsTri::output_min()
{
// fill the motor_out[] array for HIL use
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_4] = _rc_throttle->radio_min;
// send minimum value to each motor
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], _rc_throttle->radio_min);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_CH_TRI_YAW], _rc_throttle->radio_trim);
// InstantPWM
if( _speed_hz == AP_MOTORS_SPEED_INSTANT_PWM ) {
_rc->Force_Out0_Out1();
_rc->Force_Out2_Out3();
}
}
// output_armed - sends commands to the motors
void AP_MotorsTri::output_armed()
{
int16_t out_min = _rc_throttle->radio_min;
int16_t out_max = _rc_throttle->radio_max;
// Throttle is 0 to 1000 only
_rc_throttle->servo_out = constrain(_rc_throttle->servo_out, 0, _max_throttle);
if(_rc_throttle->servo_out > 0)
out_min = _rc_throttle->radio_min + _min_throttle;
// capture desired roll, pitch, yaw and throttle from receiver
_rc_roll->calc_pwm();
_rc_pitch->calc_pwm();
_rc_throttle->calc_pwm();
_rc_yaw->calc_pwm();
int roll_out = (float)_rc_roll->pwm_out * .866;
int pitch_out = _rc_pitch->pwm_out / 2;
//left front
motor_out[AP_MOTORS_MOT_2] = _rc_throttle->radio_out + roll_out + pitch_out;
//right front
motor_out[AP_MOTORS_MOT_1] = _rc_throttle->radio_out - roll_out + pitch_out;
// rear
motor_out[AP_MOTORS_MOT_4] = _rc_throttle->radio_out - _rc_pitch->pwm_out;
// Tridge's stability patch
if(motor_out[AP_MOTORS_MOT_1] > out_max){
motor_out[AP_MOTORS_MOT_2] -= (motor_out[AP_MOTORS_MOT_1] - out_max) >> 1;
motor_out[AP_MOTORS_MOT_4] -= (motor_out[AP_MOTORS_MOT_1] - out_max) >> 1;
motor_out[AP_MOTORS_MOT_1] = out_max;
}
if(motor_out[AP_MOTORS_MOT_2] > out_max){
motor_out[AP_MOTORS_MOT_1] -= (motor_out[AP_MOTORS_MOT_2] - out_max) >> 1;
motor_out[AP_MOTORS_MOT_4] -= (motor_out[AP_MOTORS_MOT_2] - out_max) >> 1;
motor_out[AP_MOTORS_MOT_2] = out_max;
}
if(motor_out[AP_MOTORS_MOT_4] > out_max){
motor_out[AP_MOTORS_MOT_1] -= (motor_out[AP_MOTORS_MOT_4] - out_max) >> 1;
motor_out[AP_MOTORS_MOT_2] -= (motor_out[AP_MOTORS_MOT_4] - out_max) >> 1;
motor_out[AP_MOTORS_MOT_4] = out_max;
}
// 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_4] = max(motor_out[AP_MOTORS_MOT_4], out_min);
#if CUT_MOTORS == ENABLED
// if we are not sending a throttle output, we cut the motors
if(_rc_throttle->servo_out == 0){
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_4] = _rc_throttle->radio_min;
}
#endif
// send output to each motor
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], motor_out[AP_MOTORS_MOT_1]);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], motor_out[AP_MOTORS_MOT_2]);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], motor_out[AP_MOTORS_MOT_4]);
// also send out to tail command (we rely on any auto pilot to have updated the rc_yaw->radio_out to the correct value)
// note we do not save the radio_out to the motor_out array so it may not appear in the ch7out in the status screen of the mission planner
_rc->OutputCh(AP_MOTORS_CH_TRI_YAW, _rc_yaw->radio_out);
// InstantPWM
if( _speed_hz == AP_MOTORS_SPEED_INSTANT_PWM ) {
_rc->Force_Out0_Out1();
_rc->Force_Out2_Out3();
}
}
// output_disarmed - sends commands to the motors
void AP_MotorsTri::output_disarmed()
{
if(_rc_throttle->control_in > 0){
// we have pushed up the throttle
// remove safety
_auto_armed = true;
}
// fill the motor_out[] array for HIL use
for (unsigned char i = AP_MOTORS_MOT_1; i < AP_MOTORS_MOT_4; i++){
motor_out[i] = _rc_throttle->radio_min;
}
// Send minimum values to all motors
output_min();
}
// output_disarmed - sends commands to the motors
void AP_MotorsTri::output_test()
{
// Send minimum values to all motors
output_min();
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min);
delay(4000);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min + 100);
delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min);
delay(2000);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], _rc_throttle->radio_min + 100);
delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], _rc_throttle->radio_min);
delay(2000);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min + 100);
delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], motor_out[AP_MOTORS_MOT_1]);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], motor_out[AP_MOTORS_MOT_2]);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], motor_out[AP_MOTORS_MOT_4]);
}

51
libraries/AP_Motors/AP_MotorsTri.h Normal file
View File

@ -0,0 +1,51 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file AP_MotorsTri.h
/// @brief Motor control class for Tricopters
#ifndef AP_MOTORSTRI
#define AP_MOTORSTRI
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <AP_Motors.h>
// tail servo uses channel 7
#define AP_MOTORS_CH_TRI_YAW CH_7
/// @class AP_MotorsTri
class AP_MotorsTri : public AP_Motors {
public:
/// Constructor
AP_MotorsTri( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_Motors(APM_version, rc_out, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {};
// init
virtual void Init();
// set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm
void set_update_rate( uint16_t speed_hz );
// enable - starts allowing signals to be sent to motors
virtual void enable();
// get basic information about the platform
virtual uint8_t get_num_motors() { return 4; }; // 3 motors + 1 tail servo
// motor test
virtual void output_test();
// output_min - sends minimum values out to the motors
virtual void output_min();
protected:
// output - sends commands to the motors
virtual void output_armed();
virtual void output_disarmed();
};
#endif // AP_MOTORSTRI

26
libraries/AP_Motors/AP_MotorsY6.cpp Normal file
View File

@ -0,0 +1,26 @@
/*
AP_MotorsY6.cpp - ArduCopter motors library
Code by RandyMackay. DIYDrones.com
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
*/
#include "AP_MotorsY6.h"
// setup_motors - configures the motors for a hexa
void AP_MotorsY6::setup_motors()
{
// call parent
AP_MotorsMatrix::setup_motors();
// MultiWii set-up
add_motor_raw(AP_MOTORS_MOT_1, -1.0, 0.666, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MATRIX_MOTOR_UNDEFINED, 2);
add_motor_raw(AP_MOTORS_MOT_2, 1.0, 0.666, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MATRIX_MOTOR_UNDEFINED, 5);
add_motor_raw(AP_MOTORS_MOT_3, 1.0, 0.666, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MATRIX_MOTOR_UNDEFINED, 6);
add_motor_raw(AP_MOTORS_MOT_4, 0.0, -1.333, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MATRIX_MOTOR_UNDEFINED, 4);
add_motor_raw(AP_MOTORS_MOT_5, -1.0, 0.666, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MATRIX_MOTOR_UNDEFINED, 1);
add_motor_raw(AP_MOTORS_MOT_6, 0.0, -1.333, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MATRIX_MOTOR_UNDEFINED, 3);
}

32
libraries/AP_Motors/AP_MotorsY6.h Normal file
View File

@ -0,0 +1,32 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file AP_MotorsY6.h
/// @brief Motor control class for Y6 frames
#ifndef AP_MOTORSY6
#define AP_MOTORSY6
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <AP_MotorsMatrix.h> // Parent Motors Matrix library
#define AP_MOTORS_Y6_YAW_DIRECTION 1 // this really should be a user selectable parameter
/// @class AP_MotorsY6
class AP_MotorsY6 : public AP_MotorsMatrix {
public:
/// Constructor
AP_MotorsY6( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(APM_version, rc_out, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {};
// setup_motors - configures the motors for a quad
virtual void setup_motors();
protected:
};
#endif // AP_MOTORSY6

114
libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.pde Normal file
View File

@ -0,0 +1,114 @@
/*
Example of AP_Motors library.
Code by Randy Mackay. DIYDrones.com
*/
// AVR runtime
#include <avr/io.h>
#include <avr/eeprom.h>
#include <avr/pgmspace.h>
#include <math.h>
// Libraries
#include <FastSerial.h>
#include <AP_Common.h>
#include <Arduino_Mega_ISR_Registry.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <AP_Motors.h>
#include <AP_MotorsTri.h>
#include <AP_MotorsQuad.h>
#include <AP_MotorsHexa.h>
#include <AP_MotorsY6.h>
#include <AP_MotorsOcta.h>
#include <AP_MotorsOctaQuad.h>
#include <AP_MotorsHeli.h>
#include <AP_MotorsMatrix.h>
////////////////////////////////////////////////////////////////////////////////
// Serial ports
////////////////////////////////////////////////////////////////////////////////
//
// Note that FastSerial port buffers are allocated at ::begin time,
// so there is not much of a penalty to defining ports that we don't
// use.
//
FastSerialPort0(Serial); // FTDI/console
Arduino_Mega_ISR_Registry isr_registry;
// uncomment one row below depending upon whether you have an APM1 or APM2
//APM_RC_APM1 APM_RC;
APM_RC_APM2 APM_RC;
RC_Channel rc1, rc2, rc3, rc4;
// uncomment the row below depending upon what frame you are using
//AP_MotorsTri motors(AP_MOTORS_APM1, &APM_RC, &rc1, &rc2, &rc3, &rc4);
AP_MotorsQuad motors(AP_MOTORS_APM2, &APM_RC, &rc1, &rc2, &rc3, &rc4);
//AP_MotorsHexa motors(AP_MOTORS_APM1, &APM_RC, &rc1, &rc2, &rc3, &rc4);
//AP_MotorsY6 motors(AP_MOTORS_APM1, &APM_RC, &rc1, &rc2, &rc3, &rc4);
//AP_MotorsOcta motors(AP_MOTORS_APM1, &APM_RC, &rc1, &rc2, &rc3, &rc4);
//AP_MotorsOctaQuad motors(AP_MOTORS_APM1, &APM_RC, &rc1, &rc2, &rc3, &rc4);
//AP_MotorsHeli motors(AP_MOTORS_APM1, &APM_RC, &rc1, &rc2, &rc3, &rc4);
// setup
void setup()
{
Serial.begin(115200);
Serial.println("AP_Motors library test ver 1.0");
RC_Channel::set_apm_rc(&APM_RC);
APM_RC.Init( &isr_registry ); // APM Radio initialization
// motor initialisation
motors.set_update_rate(490);
motors.set_frame_orientation(AP_MOTORS_X_FRAME);
motors.set_min_throttle(130);
motors.set_max_throttle(850);
motors.Init(); // initialise motors
motors.enable();
motors.output_min();
delay(1000);
}
// loop
void loop()
{
int value;
// display help
Serial.println("Press 't' to test motors. Becareful they will spin!");
// wait for user to enter something
while( !Serial.available() ) {
delay(20);
}
// get character from user
value = Serial.read();
// test motors
if( value == 't' || value == 'T' ) {
Serial.println("testing motors...");
motors.armed(true);
motors.output_test();
motors.armed(false);
Serial.println("finished test.");
}
}
// print motor output
void print_motor_output()
{
int8_t i;
for(i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if( motors.motor_enabled[i] ) {
Serial.printf("\t%d %d",i,motors.motor_out[i]);
}
}
}