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.
mission-4.1.18
rmackay9
13 years ago
19 changed files with 1815 additions and 0 deletions
@ -0,0 +1,56 @@
@@ -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); |
||||
} |
||||
} |
||||
} |
||||
@ -0,0 +1,137 @@
@@ -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
|
||||
@ -0,0 +1,370 @@
@@ -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(); |
||||
} |
||||
} |
||||
@ -0,0 +1,140 @@
@@ -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
|
||||
@ -0,0 +1,37 @@
@@ -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); |
||||
} |
||||
} |
||||
@ -0,0 +1,30 @@
@@ -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
|
||||
@ -0,0 +1,330 @@
@@ -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; |
||||
} |
||||
@ -0,0 +1,93 @@
@@ -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
|
||||
@ -0,0 +1,53 @@
@@ -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); |
||||
} |
||||
} |
||||
@ -0,0 +1,30 @@
@@ -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
|
||||
@ -0,0 +1,41 @@
@@ -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); |
||||
} |
||||
} |
||||
@ -0,0 +1,30 @@
@@ -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
|
||||
@ -0,0 +1,33 @@
@@ -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); |
||||
} |
||||
} |
||||
@ -0,0 +1,30 @@
@@ -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
|
||||
@ -0,0 +1,182 @@
@@ -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]); |
||||
} |
||||
@ -0,0 +1,51 @@
@@ -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
|
||||
@ -0,0 +1,26 @@
@@ -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); |
||||
} |
||||
@ -0,0 +1,32 @@
@@ -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
|
||||
@ -0,0 +1,114 @@
@@ -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]); |
||||
} |
||||
} |
||||
} |
||||
Loading…
Reference in new issue