Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 23:43:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: ported to AP_HAL

Browse Source
This commit is contained in:
Pat Hickey 2012-10-26 16:59:07 -07:00 committed by Andrew Tridgell
parent 95a13bdbd2
commit c6fe5e5340
18 changed files with 381 additions and 397 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

196
libraries/AP_Motors/AP_Motors.h
View File

@ -1,187 +1,15 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file AxisController.h #ifndef __AP_MOTORS_H__
/// @brief Generic PID algorithm, with EEPROM-backed storage of constants. #define __AP_MOTORS_H__
#ifndef AP_MOTORS #include "AP_Motors_Class.h"
#define AP_MOTORS #include "AP_MotorsMatrix.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 <FastSerial.h> #endif // __AP_MOTORS_H__
#include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <AP_Curve.h> // Curve used to linearlise throttle pwm to thrust
#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 rate
#define AP_MOTORS_SPEED_DEFAULT 490
// top-bottom ratio (for Y6)
#define AP_MOTORS_TOP_BOTTOM_RATIO 1.0
#define THROTTLE_CURVE_ENABLED 1 // throttle curve disabled by default
#define THROTTLE_CURVE_MID_THRUST 52 // throttle which produces 1/2 the maximum thrust. expressed as a percentage of the full throttle range (i.e 0 ~ 100)
#define THROTTLE_CURVE_MAX_THRUST 93 // throttle which produces the maximum thrust. expressed as a percentage of the full throttle range (i.e 0 ~ 100)
// bit mask for recording which limits we have reached when outputting to motors
#define AP_MOTOR_NO_LIMITS_REACHED 0x00
#define AP_MOTOR_ROLLPITCH_LIMIT 0x01
#define AP_MOTOR_YAW_LIMIT 0x02
#define AP_MOTOR_THROTTLE_LIMIT 0x04
#define AP_MOTOR_ANY_LIMIT 0xFF
/// @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
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 arm) {
_armed = arm;
};
// 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 arm) {
_auto_armed = arm;
};
// 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() {
};
// reached_limits - return whether we hit the limits of the motors
virtual uint8_t reached_limit( uint8_t which_limit = AP_MOTOR_ANY_LIMIT ) {
return _reached_limit & which_limit;
}
// 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();
// setup_throttle_curve - used to linearlise thrust output by motors
// returns true if curve is created successfully
virtual bool setup_throttle_curve();
// 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)
AP_CurveInt16_Size4 _throttle_curve; // curve used to linearize the pwm->thrust
AP_Int8 _throttle_curve_enabled; // enable throttle curve
AP_Int8 _throttle_curve_mid; // throttle which produces 1/2 the maximum thrust. expressed as a percentage (i.e. 0 ~ 100 ) of the full throttle range
AP_Int8 _throttle_curve_max; // throttle which produces the maximum thrust. expressed as a percentage (i.e. 0 ~ 100 ) of the full throttle range
uint8_t _reached_limit; // bit mask to record which motor limits we hit (if any) during most recent output. Used to provide feedback to attitude controllers
};
#endif // AP_MOTORS

97
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -7,9 +7,12 @@
* License as published by the Free Software Foundation; either * License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version. * version 2.1 of the License, or (at your option) any later version.
*/ */
#include <stdlib.h>
#include <AP_HAL.h>
#include "AP_MotorsHeli.h" #include "AP_MotorsHeli.h"
extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = {
@ -195,19 +198,22 @@ void AP_MotorsHeli::set_update_rate( uint16_t speed_hz )
_speed_hz = speed_hz; _speed_hz = speed_hz;
// setup fast channels // setup fast channels
_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); hal.rcout->set_freq(_motor_to_channel_map[AP_MOTORS_MOT_1], _speed_hz);
hal.rcout->set_freq(_motor_to_channel_map[AP_MOTORS_MOT_2], _speed_hz);
hal.rcout->set_freq(_motor_to_channel_map[AP_MOTORS_MOT_3], _speed_hz);
hal.rcout->set_freq(_motor_to_channel_map[AP_MOTORS_MOT_4], _speed_hz);
} }
// enable - starts allowing signals to be sent to motors // enable - starts allowing signals to be sent to motors
void AP_MotorsHeli::enable() void AP_MotorsHeli::enable()
{ {
// enable output channels // enable output channels
_rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_1]); // swash servo 1 hal.rcout->enable_ch(_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 hal.rcout->enable_ch(_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 hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_3]); // swash servo 3
_rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_4]); // yaw hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_4]); // yaw
_rc->enable_out(AP_MOTORS_HELI_EXT_GYRO); // for external gyro hal.rcout->enable_ch(AP_MOTORS_HELI_EXT_GYRO); // for external gyro
_rc->enable_out(AP_MOTORS_HELI_EXT_RSC); // for external RSC hal.rcout->enable_ch(AP_MOTORS_HELI_EXT_RSC); // for external RSC
} }
// output_min - sends minimum values out to the motors // output_min - sends minimum values out to the motors
@ -261,47 +267,47 @@ void AP_MotorsHeli::output_test()
// servo 1 // servo 1
for( i=0; i<5; i++ ) { for( i=0; i<5; i++ ) {
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_trim + 100); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_trim + 100);
delay(300); hal.scheduler->delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_trim - 100); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_trim - 100);
delay(300); hal.scheduler->delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_trim + 0); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_trim + 0);
delay(300); hal.scheduler->delay(300);
} }
// servo 2 // servo 2
for( i=0; i<5; i++ ) { for( i=0; i<5; i++ ) {
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo_2->radio_trim + 100); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo_2->radio_trim + 100);
delay(300); hal.scheduler->delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo_2->radio_trim - 100); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo_2->radio_trim - 100);
delay(300); hal.scheduler->delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo_2->radio_trim + 0); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo_2->radio_trim + 0);
delay(300); hal.scheduler->delay(300);
} }
// servo 3 // servo 3
for( i=0; i<5; i++ ) { for( i=0; i<5; i++ ) {
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_3->radio_trim + 100); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_3->radio_trim + 100);
delay(300); hal.scheduler->delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_3->radio_trim - 100); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_3->radio_trim - 100);
delay(300); hal.scheduler->delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_3->radio_trim + 0); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_3->radio_trim + 0);
delay(300); hal.scheduler->delay(300);
} }
// external gyro // external gyro
if( ext_gyro_enabled ) { if( ext_gyro_enabled ) {
_rc->OutputCh(AP_MOTORS_HELI_EXT_GYRO, ext_gyro_gain); hal.rcout->write(AP_MOTORS_HELI_EXT_GYRO, ext_gyro_gain);
} }
// servo 4 // servo 4
for( i=0; i<5; i++ ) { for( i=0; i<5; i++ ) {
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo_4->radio_trim + 100); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo_4->radio_trim + 100);
delay(300); hal.scheduler->delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo_4->radio_trim - 100); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo_4->radio_trim - 100);
delay(300); hal.scheduler->delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo_4->radio_trim + 0); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo_4->radio_trim + 0);
delay(300); hal.scheduler->delay(300);
} }
// Send minimum values to all motors // Send minimum values to all motors
@ -379,6 +385,7 @@ void AP_MotorsHeli::init_swash()
collective_min = 1000; collective_min = 1000;
collective_max = 2000; collective_max = 2000;
} }
collective_mid = constrain(collective_mid, collective_min, collective_max); collective_mid = constrain(collective_mid, collective_min, collective_max);
// calculate throttle mid point // calculate throttle mid point
@ -504,10 +511,10 @@ void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll
_servo_4->calc_pwm(); _servo_4->calc_pwm();
// actually move the servos // actually move the servos
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_out); hal.rcout->write(_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); hal.rcout->write(_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); hal.rcout->write(_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); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo_4->radio_out);
// to be compatible with other frame types // to be compatible with other frame types
motor_out[AP_MOTORS_MOT_1] = _servo_1->radio_out; motor_out[AP_MOTORS_MOT_1] = _servo_1->radio_out;
@ -517,13 +524,17 @@ void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll
// output gyro value // output gyro value
if( ext_gyro_enabled ) { if( ext_gyro_enabled ) {
_rc->OutputCh(AP_MOTORS_HELI_EXT_GYRO, ext_gyro_gain); hal.rcout->write(AP_MOTORS_HELI_EXT_GYRO, ext_gyro_gain);
} }
} }
void AP_MotorsHeli::rsc_control() static long map(long x, long in_min, long in_max, long out_min, long out_max)
{ {
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
void AP_MotorsHeli::rsc_control() {
switch ( rsc_mode ) { switch ( rsc_mode ) {
case AP_MOTORSHELI_RSC_MODE_CH8_PASSTHROUGH: case AP_MOTORSHELI_RSC_MODE_CH8_PASSTHROUGH:
@ -541,7 +552,7 @@ void AP_MotorsHeli::rsc_control()
} }
rsc_output = _rc_8->radio_min; rsc_output = _rc_8->radio_min;
} }
_rc->OutputCh(AP_MOTORS_HELI_EXT_RSC, rsc_output); hal.rcout->write(AP_MOTORS_HELI_EXT_RSC, rsc_output);
break; break;
case AP_MOTORSHELI_RSC_MODE_EXT_GOV: case AP_MOTORSHELI_RSC_MODE_EXT_GOV:
@ -560,7 +571,7 @@ void AP_MotorsHeli::rsc_control()
} }
rsc_output = 1000; //Just to be sure RSC output is 0 rsc_output = 1000; //Just to be sure RSC output is 0
} }
_rc->OutputCh(AP_MOTORS_HELI_EXT_RSC, rsc_output); hal.rcout->write(AP_MOTORS_HELI_EXT_RSC, rsc_output);
break; break;
// case 3: // Open Loop ESC Control // case 3: // Open Loop ESC Control
@ -583,7 +594,7 @@ void AP_MotorsHeli::rsc_control()
// ext_esc_ramp = 0; //Return ESC Ramp to 0 // ext_esc_ramp = 0; //Return ESC Ramp to 0
// ext_esc_output = 1000; //Just to be sure ESC output is 0 // ext_esc_output = 1000; //Just to be sure ESC output is 0
//} //}
// _rc->OutputCh(AP_MOTORS_HELI_EXT_ESC, ext_esc_output); // hal.rcout->write(AP_MOTORS_HELI_EXT_ESC, ext_esc_output);
// break; // break;

18
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -3,23 +3,14 @@
/// @file AP_MotorsHeli.h /// @file AP_MotorsHeli.h
/// @brief Motor control class for Traditional Heli /// @brief Motor control class for Traditional Heli
#ifndef AP_MOTORSHELI #ifndef __AP_MOTORS_HELI_H__
#define AP_MOTORSHELI #define __AP_MOTORS_HELI_H__
#include <inttypes.h> #include <inttypes.h>
#include <FastSerial.h>
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library #include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library #include <RC_Channel.h> // RC Channel Library
#include <APM_RC.h> // ArduPilot Mega RC Library #include "AP_Motors.h"
#include <AP_Motors.h>
// below is required to make "map" function available to this library
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
#define AP_MOTORS_HELI_SPEED_DEFAULT 125 // default servo update rate for helicopters #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_DIGITAL_SERVOS 125 // update rate for digital servos
@ -52,7 +43,6 @@ public:
/// Constructor /// Constructor
AP_MotorsHeli( uint8_t APM_version, AP_MotorsHeli( uint8_t APM_version,
APM_RC_Class* rc_out,
RC_Channel* rc_roll, RC_Channel* rc_roll,
RC_Channel* rc_pitch, RC_Channel* rc_pitch,
RC_Channel* rc_throttle, RC_Channel* rc_throttle,
@ -63,7 +53,7 @@ public:
RC_Channel* swash_servo_3, RC_Channel* swash_servo_3,
RC_Channel* yaw_servo, RC_Channel* yaw_servo,
uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) : 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), AP_Motors(APM_version, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz),
_servo_1(swash_servo_1), _servo_1(swash_servo_1),
_servo_2(swash_servo_2), _servo_2(swash_servo_2),
_servo_3(swash_servo_3), _servo_3(swash_servo_3),

10
libraries/AP_Motors/AP_MotorsHexa.h
View File

@ -3,22 +3,20 @@
/// @file AP_MotorsHexa.h /// @file AP_MotorsHexa.h
/// @brief Motor control class for Hexacopters /// @brief Motor control class for Hexacopters
#ifndef AP_MOTORSHEXA #ifndef __AP_MOTORS_HEXA_H__
#define AP_MOTORSHEXA #define __AP_MOTORS_HEXA_H__
#include <FastSerial.h>
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library #include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library #include <RC_Channel.h> // RC Channel Library
#include <APM_RC.h> // ArduPilot Mega RC Library #include "AP_MotorsMatrix.h" // Parent Motors Matrix library
#include <AP_MotorsMatrix.h> // Parent Motors Matrix library
/// @class AP_MotorsHexa /// @class AP_MotorsHexa
class AP_MotorsHexa : public AP_MotorsMatrix { class AP_MotorsHexa : public AP_MotorsMatrix {
public: public:
/// Constructor /// 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) { AP_MotorsHexa( uint8_t APM_version, 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_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {
}; };
// setup_motors - configures the motors for a quad // setup_motors - configures the motors for a quad

27
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -7,9 +7,11 @@
* License as published by the Free Software Foundation; either * License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version. * version 2.1 of the License, or (at your option) any later version.
*/ */
#include <AP_HAL.h>
#include "AP_MotorsMatrix.h" #include "AP_MotorsMatrix.h"
extern const AP_HAL::HAL& hal;
// Init // Init
void AP_MotorsMatrix::Init() void AP_MotorsMatrix::Init()
{ {
@ -34,7 +36,6 @@ void AP_MotorsMatrix::Init()
// set update rate to motors - a value in hertz // set update rate to motors - a value in hertz
void AP_MotorsMatrix::set_update_rate( uint16_t speed_hz ) void AP_MotorsMatrix::set_update_rate( uint16_t speed_hz )
{ {
uint32_t fast_channel_mask = 0;
int8_t i; int8_t i;
// record requested speed // record requested speed
@ -43,13 +44,9 @@ void AP_MotorsMatrix::set_update_rate( uint16_t speed_hz )
// check each enabled motor // check each enabled motor
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) { for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) { if( motor_enabled[i] ) {
// set-up fast channel mask hal.rcout->set_freq( _motor_to_channel_map[i], _speed_hz );
fast_channel_mask |= _BV(_motor_to_channel_map[i]); // add to fast channel map
} }
} }
// enable fast channels
_rc->SetFastOutputChannels(fast_channel_mask, _speed_hz);
} }
// set frame orientation (normally + or X) // set frame orientation (normally + or X)
@ -78,7 +75,7 @@ void AP_MotorsMatrix::enable()
// enable output channels // enable output channels
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) { for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) { if( motor_enabled[i] ) {
_rc->enable_out(_motor_to_channel_map[i]); hal.rcout->enable_ch(_motor_to_channel_map[i]);
} }
} }
} }
@ -92,7 +89,7 @@ void AP_MotorsMatrix::output_min()
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) { for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) { if( motor_enabled[i] ) {
motor_out[i] = _rc_throttle->radio_min; motor_out[i] = _rc_throttle->radio_min;
_rc->OutputCh(_motor_to_channel_map[i], motor_out[i]); hal.rcout->write(_motor_to_channel_map[i], motor_out[i]);
} }
} }
} }
@ -273,7 +270,7 @@ void AP_MotorsMatrix::output_armed()
// send output to each motor // send output to each motor
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) { for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) { if( motor_enabled[i] ) {
_rc->OutputCh(_motor_to_channel_map[i], motor_out[i]); hal.rcout->write(_motor_to_channel_map[i], motor_out[i]);
} }
} }
} }
@ -311,17 +308,17 @@ void AP_MotorsMatrix::output_test()
output_min(); output_min();
// first delay is longer // first delay is longer
delay(4000); hal.scheduler->delay(4000);
// loop through all the possible orders spinning any motors that match that description // loop through all the possible orders spinning any motors that match that description
for( i=min_order; i<=max_order; i++ ) { for( i=min_order; i<=max_order; i++ ) {
for( j=0; j<AP_MOTORS_MAX_NUM_MOTORS; j++ ) { for( j=0; j<AP_MOTORS_MAX_NUM_MOTORS; j++ ) {
if( motor_enabled[j] && test_order[j] == i ) { if( motor_enabled[j] && test_order[j] == i ) {
// turn on this motor and wait 1/3rd of a second // turn on this motor and wait 1/3rd of a second
_rc->OutputCh(_motor_to_channel_map[j], _rc_throttle->radio_min + 100); hal.rcout->write(_motor_to_channel_map[j], _rc_throttle->radio_min + 100);
delay(300); hal.scheduler->delay(300);
_rc->OutputCh(_motor_to_channel_map[j], _rc_throttle->radio_min); hal.rcout->write(_motor_to_channel_map[j], _rc_throttle->radio_min);
delay(2000); hal.scheduler->delay(2000);
} }
} }
} }

12
libraries/AP_Motors/AP_MotorsMatrix.h
View File

@ -3,15 +3,13 @@
/// @file AP_MotorsMatrix.h /// @file AP_MotorsMatrix.h
/// @brief Motor control class for Matrixcopters /// @brief Motor control class for Matrixcopters
#ifndef AP_MOTORSMATRIX #ifndef __AP_MOTORS_MATRIX_H__
#define AP_MOTORSMATRIX #define __AP_MOTORS_MATRIX_H__
#include <FastSerial.h>
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library #include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library #include <RC_Channel.h> // RC Channel Library
#include <APM_RC.h> // ArduPilot Mega RC Library #include "AP_Motors_Class.h"
#include <AP_Motors.h>
#define AP_MOTORS_MATRIX_MOTOR_UNDEFINED -1 #define AP_MOTORS_MATRIX_MOTOR_UNDEFINED -1
#define AP_MOTORS_MATRIX_ORDER_UNDEFINED -1 #define AP_MOTORS_MATRIX_ORDER_UNDEFINED -1
@ -26,8 +24,8 @@ class AP_MotorsMatrix : public AP_Motors {
public: public:
/// Constructor /// 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_MotorsMatrix( uint8_t APM_version, 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), AP_Motors(APM_version, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz),
_num_motors(0) { _num_motors(0) {
}; };

10
libraries/AP_Motors/AP_MotorsOcta.h
View File

@ -3,22 +3,20 @@
/// @file AP_MotorsOcta.h /// @file AP_MotorsOcta.h
/// @brief Motor control class for Octacopters /// @brief Motor control class for Octacopters
#ifndef AP_MOTORSOCTA #ifndef __AP_MOTORS_OCTA_H__
#define AP_MOTORSOCTA #define __AP_MOTORS_OCTA_H__
#include <FastSerial.h>
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library #include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library #include <RC_Channel.h> // RC Channel Library
#include <APM_RC.h> // ArduPilot Mega RC Library #include "AP_MotorsMatrix.h" // Parent Motors Matrix library
#include <AP_MotorsMatrix.h> // Parent Motors Matrix library
/// @class AP_MotorsOcta /// @class AP_MotorsOcta
class AP_MotorsOcta : public AP_MotorsMatrix { class AP_MotorsOcta : public AP_MotorsMatrix {
public: public:
/// Constructor /// 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) { AP_MotorsOcta( uint8_t APM_version, 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_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {
}; };
// setup_motors - configures the motors for a quad // setup_motors - configures the motors for a quad

10
libraries/AP_Motors/AP_MotorsOctaQuad.h
View File

@ -3,22 +3,20 @@
/// @file AP_MotorsOctaQuad.h /// @file AP_MotorsOctaQuad.h
/// @brief Motor control class for OctaQuadcopters /// @brief Motor control class for OctaQuadcopters
#ifndef AP_MOTORSOCTAQUAD #ifndef __AP_MOTORS_OCTA_QUAD_H__
#define AP_MOTORSOCTAQUAD #define __AP_MOTORS_OCTA_QUAD_H__
#include <FastSerial.h>
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library #include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library #include <RC_Channel.h> // RC Channel Library
#include <APM_RC.h> // ArduPilot Mega RC Library #include "AP_MotorsMatrix.h" // Parent Motors Matrix library
#include <AP_MotorsMatrix.h> // Parent Motors Matrix library
/// @class AP_MotorsOcta /// @class AP_MotorsOcta
class AP_MotorsOctaQuad : public AP_MotorsMatrix { class AP_MotorsOctaQuad : public AP_MotorsMatrix {
public: public:
/// Constructor /// 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) { AP_MotorsOctaQuad( uint8_t APM_version, 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_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {
}; };
// setup_motors - configures the motors for a quad // setup_motors - configures the motors for a quad

10
libraries/AP_Motors/AP_MotorsQuad.h
View File

@ -3,22 +3,20 @@
/// @file AP_MotorsQuad.h /// @file AP_MotorsQuad.h
/// @brief Motor control class for Quadcopters /// @brief Motor control class for Quadcopters
#ifndef AP_MOTORSQUAD #ifndef __AP_MOTORS_QUAD_H__
#define AP_MOTORSQUAD #define __AP_MOTORS_QUAD_H__
#include <FastSerial.h>
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library #include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library #include <RC_Channel.h> // RC Channel Library
#include <APM_RC.h> // ArduPilot Mega RC Library #include "AP_MotorsMatrix.h" // Parent Motors Matrix library
#include <AP_MotorsMatrix.h> // Parent Motors Matrix library
/// @class AP_MotorsQuad /// @class AP_MotorsQuad
class AP_MotorsQuad : public AP_MotorsMatrix { class AP_MotorsQuad : public AP_MotorsMatrix {
public: public:
/// Constructor /// 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) { AP_MotorsQuad( uint8_t APM_version, 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_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {
}; };
// setup_motors - configures the motors for a quad // setup_motors - configures the motors for a quad

65
libraries/AP_Motors/AP_MotorsTri.cpp
View File

@ -7,9 +7,12 @@
* License as published by the Free Software Foundation; either * License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version. * version 2.1 of the License, or (at your option) any later version.
*/ */
#include <AP_HAL.h>
#include <AP_Math.h>
#include "AP_MotorsTri.h" #include "AP_MotorsTri.h"
extern const AP_HAL::HAL& hal;
// init // init
void AP_MotorsTri::Init() void AP_MotorsTri::Init()
{ {
@ -27,17 +30,19 @@ void AP_MotorsTri::set_update_rate( uint16_t speed_hz )
_speed_hz = speed_hz; _speed_hz = speed_hz;
// set update rate for the 3 motors (but not the servo on channel 7) // set update rate for the 3 motors (but not the servo on channel 7)
_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); hal.rcout->set_freq(_motor_to_channel_map[AP_MOTORS_MOT_1], _speed_hz);
hal.rcout->set_freq(_motor_to_channel_map[AP_MOTORS_MOT_2], _speed_hz);
hal.rcout->set_freq(_motor_to_channel_map[AP_MOTORS_MOT_4], _speed_hz);
} }
// enable - starts allowing signals to be sent to motors // enable - starts allowing signals to be sent to motors
void AP_MotorsTri::enable() void AP_MotorsTri::enable()
{ {
// enable output channels // enable output channels
_rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_1]); hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_1]);
_rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_2]); hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_2]);
_rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_4]); hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_4]);
_rc->enable_out(AP_MOTORS_CH_TRI_YAW); hal.rcout->enable_ch(AP_MOTORS_CH_TRI_YAW);
} }
// output_min - sends minimum values out to the motors // output_min - sends minimum values out to the motors
@ -49,10 +54,10 @@ void AP_MotorsTri::output_min()
motor_out[AP_MOTORS_MOT_4] = _rc_throttle->radio_min; motor_out[AP_MOTORS_MOT_4] = _rc_throttle->radio_min;
// send minimum value to each motor // send minimum value to each motor
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min); hal.rcout->write(_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); hal.rcout->write(_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); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _rc_throttle->radio_min);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_CH_TRI_YAW], _rc_yaw->radio_trim); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_CH_TRI_YAW], _rc_yaw->radio_trim);
} }
// output_armed - sends commands to the motors // output_armed - sends commands to the motors
@ -124,18 +129,18 @@ void AP_MotorsTri::output_armed()
#endif #endif
// send output to each motor // send output to each motor
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], motor_out[AP_MOTORS_MOT_1]); hal.rcout->write(_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]); hal.rcout->write(_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]); hal.rcout->write(_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) // 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 // 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
// note: we use _rc_tail's (aka channel 7's) REV parameter to control whether the servo is reversed or not but this is a bit nonsensical. // note: we use _rc_tail's (aka channel 7's) REV parameter to control whether the servo is reversed or not but this is a bit nonsensical.
// a separate servo object (including min, max settings etc) would be better or at least a separate parameter to specify the direction of the tail servo // a separate servo object (including min, max settings etc) would be better or at least a separate parameter to specify the direction of the tail servo
if( _rc_tail->get_reverse() == true ) { if( _rc_tail->get_reverse() == true ) {
_rc->OutputCh(AP_MOTORS_CH_TRI_YAW, _rc_yaw->radio_trim - (_rc_yaw->radio_out - _rc_yaw->radio_trim)); hal.rcout->write(AP_MOTORS_CH_TRI_YAW, _rc_yaw->radio_trim - (_rc_yaw->radio_out - _rc_yaw->radio_trim));
}else{ }else{
_rc->OutputCh(AP_MOTORS_CH_TRI_YAW, _rc_yaw->radio_out); hal.rcout->write(AP_MOTORS_CH_TRI_YAW, _rc_yaw->radio_out);
} }
} }
@ -163,22 +168,22 @@ void AP_MotorsTri::output_test()
// Send minimum values to all motors // Send minimum values to all motors
output_min(); output_min();
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min);
delay(4000); hal.scheduler->delay(4000);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min + 100); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min + 100);
delay(300); hal.scheduler->delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min);
delay(2000); hal.scheduler->delay(2000);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], _rc_throttle->radio_min + 100); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _rc_throttle->radio_min + 100);
delay(300); hal.scheduler->delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], _rc_throttle->radio_min); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _rc_throttle->radio_min);
delay(2000); hal.scheduler->delay(2000);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min + 100); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min + 100);
delay(300); hal.scheduler->delay(300);
_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], motor_out[AP_MOTORS_MOT_1]); hal.rcout->write(_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]); hal.rcout->write(_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]); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], motor_out[AP_MOTORS_MOT_4]);
} }

12
libraries/AP_Motors/AP_MotorsTri.h
View File

@ -3,15 +3,13 @@
/// @file AP_MotorsTri.h /// @file AP_MotorsTri.h
/// @brief Motor control class for Tricopters /// @brief Motor control class for Tricopters
#ifndef AP_MOTORSTRI #ifndef __AP_MOTORS_TRI_H__
#define AP_MOTORSTRI #define __AP_MOTORS_TRI_H__
#include <FastSerial.h>
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library #include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library #include <RC_Channel.h> // RC Channel Library
#include <APM_RC.h> // ArduPilot Mega RC Library #include "AP_Motors.h"
#include <AP_Motors.h>
// tail servo uses channel 7 // tail servo uses channel 7
#define AP_MOTORS_CH_TRI_YAW CH_7 #define AP_MOTORS_CH_TRI_YAW CH_7
@ -21,8 +19,8 @@ class AP_MotorsTri : public AP_Motors {
public: public:
/// Constructor /// 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, RC_Channel* rc_tail, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsTri( uint8_t APM_version, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, RC_Channel* rc_tail, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
AP_Motors(APM_version, rc_out, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz), AP_Motors(APM_version, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz),
_rc_tail(rc_tail) { _rc_tail(rc_tail) {
}; };

8
libraries/AP_Motors/AP_MotorsY6.h
View File

@ -3,14 +3,12 @@
/// @file AP_MotorsY6.h /// @file AP_MotorsY6.h
/// @brief Motor control class for Y6 frames /// @brief Motor control class for Y6 frames
#ifndef AP_MOTORSY6 #ifndef __AP_MOTORS_Y6_H__
#define AP_MOTORSY6 #define __AP_MOTORS_Y6_H__
#include <FastSerial.h>
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library #include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library #include <RC_Channel.h> // RC Channel Library
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <AP_MotorsMatrix.h> // Parent Motors Matrix library #include <AP_MotorsMatrix.h> // Parent Motors Matrix library
#define AP_MOTORS_Y6_YAW_DIRECTION 1 // this really should be a user selectable parameter #define AP_MOTORS_Y6_YAW_DIRECTION 1 // this really should be a user selectable parameter
@ -20,7 +18,7 @@ class AP_MotorsY6 : public AP_MotorsMatrix {
public: public:
/// Constructor /// 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) { AP_MotorsY6( uint8_t APM_version, 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_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {
}; };
// setup_motors - configures the motors for a quad // setup_motors - configures the motors for a quad

12
libraries/AP_Motors/AP_Motors.cpp → libraries/AP_Motors/AP_Motors_Class.cpp
View File

@ -8,7 +8,9 @@
* version 2.1 of the License, or (at your option) any later version. * version 2.1 of the License, or (at your option) any later version.
*/ */
#include "AP_Motors.h" #include "AP_Motors_Class.h"
#include <AP_HAL.h>
extern const AP_HAL::HAL& hal;
// parameters for the motor class // parameters for the motor class
const AP_Param::GroupInfo AP_Motors::var_info[] PROGMEM = { const AP_Param::GroupInfo AP_Motors::var_info[] PROGMEM = {
@ -39,8 +41,7 @@ const AP_Param::GroupInfo AP_Motors::var_info[] PROGMEM = {
}; };
// Constructor // 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 ) : AP_Motors::AP_Motors( uint8_t APM_version, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz ) :
_rc(rc_out),
_rc_roll(rc_roll), _rc_roll(rc_roll),
_rc_pitch(rc_pitch), _rc_pitch(rc_pitch),
_rc_throttle(rc_throttle), _rc_throttle(rc_throttle),
@ -80,8 +81,9 @@ void AP_Motors::Init()
void AP_Motors::throttle_pass_through() void AP_Motors::throttle_pass_through()
{ {
if( armed() ) { if( armed() ) {
// XXX
for( int16_t i=0; i < AP_MOTORS_MAX_NUM_MOTORS; i++ ) { for( int16_t i=0; i < AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
_rc->OutputCh(_motor_to_channel_map[i], _rc_throttle->radio_in); hal.rcout->write(_motor_to_channel_map[i], _rc_throttle->radio_in);
} }
} }
} }
@ -117,7 +119,7 @@ bool AP_Motors::setup_throttle_curve()
// disable throttle curve if not set-up corrrectly // disable throttle curve if not set-up corrrectly
if( !retval ) { if( !retval ) {
_throttle_curve_enabled = false; _throttle_curve_enabled = false;
Serial.printf_P(PSTR("AP_Motors: failed to create throttle curve")); hal.console->println_P(PSTR("AP_Motors: failed to create throttle curve"));
} }
return retval; return retval;

180
libraries/AP_Motors/AP_Motors_Class.h Normal file
View File

@ -0,0 +1,180 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#ifndef __AP_MOTORS_CLASS_H__
#define __AP_MOTORS_CLASS_H__
#include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <AP_Curve.h> // Curve used to linearlise throttle pwm to thrust
#include <RC_Channel.h> // RC Channel 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 rate
#define AP_MOTORS_SPEED_DEFAULT 490
// top-bottom ratio (for Y6)
#define AP_MOTORS_TOP_BOTTOM_RATIO 1.0
#define THROTTLE_CURVE_ENABLED 1 // throttle curve disabled by default
#define THROTTLE_CURVE_MID_THRUST 52 // throttle which produces 1/2 the maximum thrust. expressed as a percentage of the full throttle range (i.e 0 ~ 100)
#define THROTTLE_CURVE_MAX_THRUST 93 // throttle which produces the maximum thrust. expressed as a percentage of the full throttle range (i.e 0 ~ 100)
// bit mask for recording which limits we have reached when outputting to motors
#define AP_MOTOR_NO_LIMITS_REACHED 0x00
#define AP_MOTOR_ROLLPITCH_LIMIT 0x01
#define AP_MOTOR_YAW_LIMIT 0x02
#define AP_MOTOR_THROTTLE_LIMIT 0x04
#define AP_MOTOR_ANY_LIMIT 0xFF
/// @class AP_Motors
class AP_Motors {
public:
// Constructor
AP_Motors( uint8_t APM_version, 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
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 arm) {
_armed = arm;
};
// 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 arm) {
_auto_armed = arm;
};
// 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() {
};
// reached_limits - return whether we hit the limits of the motors
virtual uint8_t reached_limit( uint8_t which_limit = AP_MOTOR_ANY_LIMIT ) {
return _reached_limit & which_limit;
}
// 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();
// setup_throttle_curve - used to linearlise thrust output by motors
// returns true if curve is created successfully
virtual bool setup_throttle_curve();
// 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() {
};
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)
AP_CurveInt16_Size4 _throttle_curve; // curve used to linearize the pwm->thrust
AP_Int8 _throttle_curve_enabled; // enable throttle curve
AP_Int8 _throttle_curve_mid; // throttle which produces 1/2 the maximum thrust. expressed as a percentage (i.e. 0 ~ 100 ) of the full throttle range
AP_Int8 _throttle_curve_max; // throttle which produces the maximum thrust. expressed as a percentage (i.e. 0 ~ 100 ) of the full throttle range
uint8_t _reached_limit; // bit mask to record which motor limits we hit (if any) during most recent output. Used to provide feedback to attitude controllers
};
#endif // __AP_MOTORS_CLASS_H__

86
libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.pde
View File

@ -3,66 +3,35 @@
* Code by Randy Mackay. DIYDrones.com * Code by Randy Mackay. DIYDrones.com
*/ */
// AVR runtime
#include <avr/io.h>
#include <avr/eeprom.h>
#include <avr/pgmspace.h>
#include <math.h>
// Libraries // Libraries
#include <FastSerial.h>
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_Progmem.h>
#include <AP_HAL.h>
#include <AP_Param.h> #include <AP_Param.h>
#include <Arduino_Mega_ISR_Registry.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library #include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library #include <RC_Channel.h> // RC Channel Library
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <AP_Motors.h> #include <AP_Motors.h>
#include <AP_MotorsTri.h> #include <AP_Curve.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>
//////////////////////////////////////////////////////////////////////////////// #include <AP_HAL_AVR.h>
// Serial ports const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
////////////////////////////////////////////////////////////////////////////////
//
// 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; RC_Channel rc1(0), rc2(1), rc3(2), rc4(3);
// 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(CH_1), rc2(CH_2), rc3(CH_3), rc4(CH_4);
// uncomment the row below depending upon what frame you are using // uncomment the row below depending upon what frame you are using
//AP_MotorsTri motors(AP_MOTORS_APM1, &APM_RC, &rc1, &rc2, &rc3, &rc4); //AP_MotorsTri motors(AP_MOTORS_APM1, &rc1, &rc2, &rc3, &rc4);
AP_MotorsQuad motors(AP_MOTORS_APM2, &APM_RC, &rc1, &rc2, &rc3, &rc4); AP_MotorsQuad motors(AP_MOTORS_APM2, &rc1, &rc2, &rc3, &rc4);
//AP_MotorsHexa motors(AP_MOTORS_APM1, &APM_RC, &rc1, &rc2, &rc3, &rc4); //AP_MotorsHexa motors(AP_MOTORS_APM1, &rc1, &rc2, &rc3, &rc4);
//AP_MotorsY6 motors(AP_MOTORS_APM1, &APM_RC, &rc1, &rc2, &rc3, &rc4); //AP_MotorsY6 motors(AP_MOTORS_APM1, &rc1, &rc2, &rc3, &rc4);
//AP_MotorsOcta motors(AP_MOTORS_APM1, &APM_RC, &rc1, &rc2, &rc3, &rc4); //AP_MotorsOcta motors(AP_MOTORS_APM1, &rc1, &rc2, &rc3, &rc4);
//AP_MotorsOctaQuad motors(AP_MOTORS_APM1, &APM_RC, &rc1, &rc2, &rc3, &rc4); //AP_MotorsOctaQuad motors(AP_MOTORS_APM1, &rc1, &rc2, &rc3, &rc4);
//AP_MotorsHeli motors(AP_MOTORS_APM1, &APM_RC, &rc1, &rc2, &rc3, &rc4); //AP_MotorsHeli motors(AP_MOTORS_APM1, &rc1, &rc2, &rc3, &rc4);
// setup // setup
void setup() void setup()
{ {
Serial.begin(115200); hal.console->println("AP_Motors library test ver 1.0");
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 // motor initialisation
motors.set_update_rate(490); motors.set_update_rate(490);
@ -71,10 +40,19 @@ void setup()
motors.set_max_throttle(850); motors.set_max_throttle(850);
motors.Init(); // initialise motors motors.Init(); // initialise motors
if (rc3.radio_min == 0) {
// cope with AP_Param not being loaded
rc3.radio_min = 1000;
}
if (rc3.radio_max == 0) {
// cope with AP_Param not being loaded
rc3.radio_max = 2000;
}
motors.enable(); motors.enable();
motors.output_min(); motors.output_min();
delay(1000); hal.scheduler->delay(1000);
} }
// loop // loop
@ -83,23 +61,23 @@ void loop()
int value; int value;
// display help // display help
Serial.println("Press 't' to test motors. Becareful they will spin!"); hal.console->println("Press 't' to test motors. Be careful they will spin!");
// wait for user to enter something // wait for user to enter something
while( !Serial.available() ) { while( !hal.console->available() ) {
delay(20); hal.scheduler->delay(20);
} }
// get character from user // get character from user
value = Serial.read(); value = hal.console->read();
// test motors // test motors
if( value == 't' || value == 'T' ) { if( value == 't' || value == 'T' ) {
Serial.println("testing motors..."); hal.console->println("testing motors...");
motors.armed(true); motors.armed(true);
motors.output_test(); motors.output_test();
motors.armed(false); motors.armed(false);
Serial.println("finished test."); hal.console->println("finished test.");
} }
} }
@ -109,7 +87,9 @@ void print_motor_output()
int8_t i; int8_t i;
for(i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) { for(i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if( motors.motor_enabled[i] ) { if( motors.motor_enabled[i] ) {
Serial.printf("\t%d %d",i,motors.motor_out[i]); hal.console->printf("\t%d %d",i,motors.motor_out[i]);
} }
} }
} }
AP_HAL_MAIN();

0
libraries/AP_Motors/examples/AP_Motors_test/Arduino.h Normal file
View File

5
libraries/AP_Motors/examples/AP_Motors_test/Makefile Normal file
View File

@ -0,0 +1,5 @@
BOARD = mega
include ../../../AP_Common/Arduino.mk
apm2:
make -f Makefile EXTRAFLAGS="-DAPM2_HARDWARE=1"

0
libraries/AP_Motors/examples/AP_Motors_test/nocore.inoflag Normal file
View File