From b71877366e878d55c08955d6c8886ea830a81363 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Mon, 2 Apr 2012 17:26:37 +0900 Subject: [PATCH] ArduCopter - AP_Motors library - added new library which has few advantages over current code: 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. --- libraries/AP_Motors/AP_Motors.cpp | 56 +++ libraries/AP_Motors/AP_Motors.h | 137 +++++++ libraries/AP_Motors/AP_MotorsHeli.cpp | 370 ++++++++++++++++++ libraries/AP_Motors/AP_MotorsHeli.h | 140 +++++++ libraries/AP_Motors/AP_MotorsHexa.cpp | 37 ++ libraries/AP_Motors/AP_MotorsHexa.h | 30 ++ libraries/AP_Motors/AP_MotorsMatrix.cpp | 330 ++++++++++++++++ libraries/AP_Motors/AP_MotorsMatrix.h | 93 +++++ libraries/AP_Motors/AP_MotorsOcta.cpp | 53 +++ libraries/AP_Motors/AP_MotorsOcta.h | 30 ++ libraries/AP_Motors/AP_MotorsOctaQuad.cpp | 41 ++ libraries/AP_Motors/AP_MotorsOctaQuad.h | 30 ++ libraries/AP_Motors/AP_MotorsQuad.cpp | 33 ++ libraries/AP_Motors/AP_MotorsQuad.h | 30 ++ libraries/AP_Motors/AP_MotorsTri.cpp | 182 +++++++++ libraries/AP_Motors/AP_MotorsTri.h | 51 +++ libraries/AP_Motors/AP_MotorsY6.cpp | 26 ++ libraries/AP_Motors/AP_MotorsY6.h | 32 ++ .../AP_Motors_test/AP_Motors_test.pde | 114 ++++++ 19 files changed, 1815 insertions(+) create mode 100644 libraries/AP_Motors/AP_Motors.cpp create mode 100644 libraries/AP_Motors/AP_Motors.h create mode 100644 libraries/AP_Motors/AP_MotorsHeli.cpp create mode 100644 libraries/AP_Motors/AP_MotorsHeli.h create mode 100644 libraries/AP_Motors/AP_MotorsHexa.cpp create mode 100644 libraries/AP_Motors/AP_MotorsHexa.h create mode 100644 libraries/AP_Motors/AP_MotorsMatrix.cpp create mode 100644 libraries/AP_Motors/AP_MotorsMatrix.h create mode 100644 libraries/AP_Motors/AP_MotorsOcta.cpp create mode 100644 libraries/AP_Motors/AP_MotorsOcta.h create mode 100644 libraries/AP_Motors/AP_MotorsOctaQuad.cpp create mode 100644 libraries/AP_Motors/AP_MotorsOctaQuad.h create mode 100644 libraries/AP_Motors/AP_MotorsQuad.cpp create mode 100644 libraries/AP_Motors/AP_MotorsQuad.h create mode 100644 libraries/AP_Motors/AP_MotorsTri.cpp create mode 100644 libraries/AP_Motors/AP_MotorsTri.h create mode 100644 libraries/AP_Motors/AP_MotorsY6.cpp create mode 100644 libraries/AP_Motors/AP_MotorsY6.h create mode 100644 libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.pde diff --git a/libraries/AP_Motors/AP_Motors.cpp b/libraries/AP_Motors/AP_Motors.cpp new file mode 100644 index 0000000000..02f414601d --- /dev/null +++ b/libraries/AP_Motors/AP_Motors.cpp @@ -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; iOutputCh(_motor_to_channel_map[i], _rc_throttle->radio_in); + } + } +} diff --git a/libraries/AP_Motors/AP_Motors.h b/libraries/AP_Motors/AP_Motors.h new file mode 100644 index 0000000000..1d5f9e7184 --- /dev/null +++ b/libraries/AP_Motors/AP_Motors.h @@ -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 +#include +#include // ArduPilot Mega Vector/Matrix math Library +#include // RC Channel Library +#include // 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 \ No newline at end of file diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp new file mode 100644 index 0000000000..61216dbf7f --- /dev/null +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -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(); + } +} \ No newline at end of file diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h new file mode 100644 index 0000000000..9ad39d9c59 --- /dev/null +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -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 +#include +#include +#include // ArduPilot Mega Vector/Matrix math Library +#include // RC Channel Library +#include // ArduPilot Mega RC Library +#include + +#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 \ No newline at end of file diff --git a/libraries/AP_Motors/AP_MotorsHexa.cpp b/libraries/AP_Motors/AP_MotorsHexa.cpp new file mode 100644 index 0000000000..c390298843 --- /dev/null +++ b/libraries/AP_Motors/AP_MotorsHexa.cpp @@ -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); + } +} \ No newline at end of file diff --git a/libraries/AP_Motors/AP_MotorsHexa.h b/libraries/AP_Motors/AP_MotorsHexa.h new file mode 100644 index 0000000000..ffad0106b1 --- /dev/null +++ b/libraries/AP_Motors/AP_MotorsHexa.h @@ -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 +#include +#include // ArduPilot Mega Vector/Matrix math Library +#include // RC Channel Library +#include // ArduPilot Mega RC Library +#include // 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 \ No newline at end of file diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp new file mode 100644 index 0000000000..851f06365b --- /dev/null +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -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 || !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; iSetFastOutputChannels(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; ienable_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; iradio_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; iradio_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 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; iservo_out == 0){ + for( i=0; iradio_min; + } + } + } + #endif + + // send output to each motor + for( i=0; iOutputCh(_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 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; jOutputCh(_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 +#include +#include // ArduPilot Mega Vector/Matrix math Library +#include // RC Channel Library +#include // ArduPilot Mega RC Library +#include + +#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 \ No newline at end of file diff --git a/libraries/AP_Motors/AP_MotorsOcta.cpp b/libraries/AP_Motors/AP_MotorsOcta.cpp new file mode 100644 index 0000000000..33c16963d3 --- /dev/null +++ b/libraries/AP_Motors/AP_MotorsOcta.cpp @@ -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); + } +} \ No newline at end of file diff --git a/libraries/AP_Motors/AP_MotorsOcta.h b/libraries/AP_Motors/AP_MotorsOcta.h new file mode 100644 index 0000000000..724391802f --- /dev/null +++ b/libraries/AP_Motors/AP_MotorsOcta.h @@ -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 +#include +#include // ArduPilot Mega Vector/Matrix math Library +#include // RC Channel Library +#include // ArduPilot Mega RC Library +#include // 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 \ No newline at end of file diff --git a/libraries/AP_Motors/AP_MotorsOctaQuad.cpp b/libraries/AP_Motors/AP_MotorsOctaQuad.cpp new file mode 100644 index 0000000000..55443fc111 --- /dev/null +++ b/libraries/AP_Motors/AP_MotorsOctaQuad.cpp @@ -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); + } +} \ No newline at end of file diff --git a/libraries/AP_Motors/AP_MotorsOctaQuad.h b/libraries/AP_Motors/AP_MotorsOctaQuad.h new file mode 100644 index 0000000000..e7179ebae7 --- /dev/null +++ b/libraries/AP_Motors/AP_MotorsOctaQuad.h @@ -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 +#include +#include // ArduPilot Mega Vector/Matrix math Library +#include // RC Channel Library +#include // ArduPilot Mega RC Library +#include // 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 \ No newline at end of file diff --git a/libraries/AP_Motors/AP_MotorsQuad.cpp b/libraries/AP_Motors/AP_MotorsQuad.cpp new file mode 100644 index 0000000000..91ed2a0e3b --- /dev/null +++ b/libraries/AP_Motors/AP_MotorsQuad.cpp @@ -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); + } +} diff --git a/libraries/AP_Motors/AP_MotorsQuad.h b/libraries/AP_Motors/AP_MotorsQuad.h new file mode 100644 index 0000000000..3da0b10332 --- /dev/null +++ b/libraries/AP_Motors/AP_MotorsQuad.h @@ -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 +#include +#include // ArduPilot Mega Vector/Matrix math Library +#include // RC Channel Library +#include // ArduPilot Mega RC Library +#include // 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 \ No newline at end of file diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp new file mode 100644 index 0000000000..1f973dd808 --- /dev/null +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -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]); +} \ No newline at end of file diff --git a/libraries/AP_Motors/AP_MotorsTri.h b/libraries/AP_Motors/AP_MotorsTri.h new file mode 100644 index 0000000000..14616ec7b1 --- /dev/null +++ b/libraries/AP_Motors/AP_MotorsTri.h @@ -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 +#include +#include // ArduPilot Mega Vector/Matrix math Library +#include // RC Channel Library +#include // ArduPilot Mega RC Library +#include + +// 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 \ No newline at end of file diff --git a/libraries/AP_Motors/AP_MotorsY6.cpp b/libraries/AP_Motors/AP_MotorsY6.cpp new file mode 100644 index 0000000000..00702574b0 --- /dev/null +++ b/libraries/AP_Motors/AP_MotorsY6.cpp @@ -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); +} \ No newline at end of file diff --git a/libraries/AP_Motors/AP_MotorsY6.h b/libraries/AP_Motors/AP_MotorsY6.h new file mode 100644 index 0000000000..712056cce4 --- /dev/null +++ b/libraries/AP_Motors/AP_MotorsY6.h @@ -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 +#include +#include // ArduPilot Mega Vector/Matrix math Library +#include // RC Channel Library +#include // ArduPilot Mega RC Library +#include // 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 \ No newline at end of file diff --git a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.pde b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.pde new file mode 100644 index 0000000000..e9148cf303 --- /dev/null +++ b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.pde @@ -0,0 +1,114 @@ +/* + Example of AP_Motors library. + Code by Randy Mackay. DIYDrones.com +*/ + +// AVR runtime +#include +#include +#include +#include + +// Libraries +#include +#include +#include +#include // ArduPilot Mega Vector/Matrix math Library +#include // RC Channel Library +#include // ArduPilot Mega RC Library +#include +#include +#include +#include +#include +#include +#include +#include +#include + +//////////////////////////////////////////////////////////////////////////////// +// 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