From c6fe5e534052f42976eee6a251e22bc37af13ee4 Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Fri, 26 Oct 2012 16:59:07 -0700 Subject: [PATCH] AP_Motors: ported to AP_HAL --- libraries/AP_Motors/AP_Motors.h | 196 ++---------------- libraries/AP_Motors/AP_MotorsHeli.cpp | 97 +++++---- libraries/AP_Motors/AP_MotorsHeli.h | 18 +- libraries/AP_Motors/AP_MotorsHexa.h | 12 +- libraries/AP_Motors/AP_MotorsMatrix.cpp | 29 ++- libraries/AP_Motors/AP_MotorsMatrix.h | 14 +- libraries/AP_Motors/AP_MotorsOcta.h | 12 +- libraries/AP_Motors/AP_MotorsOctaQuad.h | 12 +- libraries/AP_Motors/AP_MotorsQuad.h | 12 +- libraries/AP_Motors/AP_MotorsTri.cpp | 67 +++--- libraries/AP_Motors/AP_MotorsTri.h | 14 +- libraries/AP_Motors/AP_MotorsY6.h | 10 +- .../{AP_Motors.cpp => AP_Motors_Class.cpp} | 14 +- libraries/AP_Motors/AP_Motors_Class.h | 180 ++++++++++++++++ .../AP_Motors_test/AP_Motors_test.pde | 86 +++----- .../examples/AP_Motors_test/Arduino.h | 0 .../examples/AP_Motors_test/Makefile | 5 + .../examples/AP_Motors_test/nocore.inoflag | 0 18 files changed, 381 insertions(+), 397 deletions(-) rename libraries/AP_Motors/{AP_Motors.cpp => AP_Motors_Class.cpp} (91%) create mode 100644 libraries/AP_Motors/AP_Motors_Class.h create mode 100644 libraries/AP_Motors/examples/AP_Motors_test/Arduino.h create mode 100644 libraries/AP_Motors/examples/AP_Motors_test/Makefile create mode 100644 libraries/AP_Motors/examples/AP_Motors_test/nocore.inoflag diff --git a/libraries/AP_Motors/AP_Motors.h b/libraries/AP_Motors/AP_Motors.h index 18e775410e..28d2564cb6 100644 --- a/libraries/AP_Motors/AP_Motors.h +++ b/libraries/AP_Motors/AP_Motors.h @@ -1,187 +1,15 @@ -// -*- 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_H__ +#define __AP_MOTORS_H__ -#ifndef AP_MOTORS -#define AP_MOTORS +#include "AP_Motors_Class.h" +#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 -#include -#include // ArduPilot Mega Vector/Matrix math Library -#include // Curve used to linearlise throttle pwm to thrust -#include // RC Channel Library -#include // ArduPilot Mega RC Library - -// offsets for motors in motor_out, _motor_filtered and _motor_to_channel_map arrays -#define AP_MOTORS_MOT_1 0 -#define AP_MOTORS_MOT_2 1 -#define AP_MOTORS_MOT_3 2 -#define AP_MOTORS_MOT_4 3 -#define AP_MOTORS_MOT_5 4 -#define AP_MOTORS_MOT_6 5 -#define AP_MOTORS_MOT_7 6 -#define AP_MOTORS_MOT_8 7 - -#define APM1_MOTOR_TO_CHANNEL_MAP CH_1,CH_2,CH_3,CH_4,CH_7,CH_8,CH_10,CH_11 -#define APM2_MOTOR_TO_CHANNEL_MAP CH_1,CH_2,CH_3,CH_4,CH_5,CH_6,CH_7,CH_8 - -#define AP_MOTORS_MAX_NUM_MOTORS 8 - -#define AP_MOTORS_DEFAULT_MIN_THROTTLE 130 -#define AP_MOTORS_DEFAULT_MAX_THROTTLE 850 - -// APM board definitions -#define AP_MOTORS_APM1 1 -#define AP_MOTORS_APM2 2 - -// frame definitions -#define AP_MOTORS_PLUS_FRAME 0 -#define AP_MOTORS_X_FRAME 1 -#define AP_MOTORS_V_FRAME 2 - -// motor update 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 \ No newline at end of file +#endif // __AP_MOTORS_H__ diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 3c3bd074ff..faf432c669 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -7,9 +7,12 @@ * License as published by the Free Software Foundation; either * version 2.1 of the License, or (at your option) any later version. */ - +#include +#include #include "AP_MotorsHeli.h" +extern const AP_HAL::HAL& hal; + 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; // 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 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 - _rc->enable_out(AP_MOTORS_HELI_EXT_RSC); // for external RSC + hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_1]); // swash servo 1 + hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_2]); // swash servo 2 + hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_3]); // swash servo 3 + hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_4]); // yaw + hal.rcout->enable_ch(AP_MOTORS_HELI_EXT_GYRO); // for external gyro + hal.rcout->enable_ch(AP_MOTORS_HELI_EXT_RSC); // for external RSC } // output_min - sends minimum values out to the motors @@ -261,47 +267,47 @@ void AP_MotorsHeli::output_test() // 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); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_trim + 100); + hal.scheduler->delay(300); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_trim - 100); + hal.scheduler->delay(300); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_trim + 0); + hal.scheduler->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); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo_2->radio_trim + 100); + hal.scheduler->delay(300); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo_2->radio_trim - 100); + hal.scheduler->delay(300); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo_2->radio_trim + 0); + hal.scheduler->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); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_3->radio_trim + 100); + hal.scheduler->delay(300); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_3->radio_trim - 100); + hal.scheduler->delay(300); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_3->radio_trim + 0); + hal.scheduler->delay(300); } // external gyro 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 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); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo_4->radio_trim + 100); + hal.scheduler->delay(300); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo_4->radio_trim - 100); + hal.scheduler->delay(300); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo_4->radio_trim + 0); + hal.scheduler->delay(300); } // Send minimum values to all motors @@ -379,6 +385,7 @@ void AP_MotorsHeli::init_swash() collective_min = 1000; collective_max = 2000; } + collective_mid = constrain(collective_mid, collective_min, collective_max); // 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(); // 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); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_out); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo_2->radio_out); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_3->radio_out); + hal.rcout->write(_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; @@ -517,13 +524,17 @@ void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll // output gyro value 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 ) { case AP_MOTORSHELI_RSC_MODE_CH8_PASSTHROUGH: @@ -541,7 +552,7 @@ void AP_MotorsHeli::rsc_control() } 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; 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 } - _rc->OutputCh(AP_MOTORS_HELI_EXT_RSC, rsc_output); + hal.rcout->write(AP_MOTORS_HELI_EXT_RSC, rsc_output); break; // 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_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; diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 4def9f4fe2..3497febee7 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -3,23 +3,14 @@ /// @file AP_MotorsHeli.h /// @brief Motor control class for Traditional Heli -#ifndef AP_MOTORSHELI -#define AP_MOTORSHELI +#ifndef __AP_MOTORS_HELI_H__ +#define __AP_MOTORS_HELI_H__ #include -#include #include #include // ArduPilot Mega Vector/Matrix math Library #include // RC Channel Library -#include // ArduPilot Mega RC Library -#include - -// below is required to make "map" function available to this library -#if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" -#else - #include "WProgram.h" -#endif +#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 @@ -52,7 +43,6 @@ 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, @@ -63,7 +53,7 @@ public: 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), + AP_Motors(APM_version, 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), diff --git a/libraries/AP_Motors/AP_MotorsHexa.h b/libraries/AP_Motors/AP_MotorsHexa.h index 9fd37b0722..e09654e133 100644 --- a/libraries/AP_Motors/AP_MotorsHexa.h +++ b/libraries/AP_Motors/AP_MotorsHexa.h @@ -3,22 +3,20 @@ /// @file AP_MotorsHexa.h /// @brief Motor control class for Hexacopters -#ifndef AP_MOTORSHEXA -#define AP_MOTORSHEXA +#ifndef __AP_MOTORS_HEXA_H__ +#define __AP_MOTORS_HEXA_H__ -#include #include #include // ArduPilot Mega Vector/Matrix math Library #include // RC Channel Library -#include // ArduPilot Mega RC Library -#include // Parent Motors Matrix 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) { + 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 @@ -28,4 +26,4 @@ protected: }; -#endif // AP_MOTORSHEXA \ No newline at end of file +#endif // AP_MOTORSHEXA diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index 0f5420be23..8e0c3e783a 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -7,9 +7,11 @@ * License as published by the Free Software Foundation; either * version 2.1 of the License, or (at your option) any later version. */ - +#include #include "AP_MotorsMatrix.h" +extern const AP_HAL::HAL& hal; + // Init void AP_MotorsMatrix::Init() { @@ -34,7 +36,6 @@ void AP_MotorsMatrix::Init() // set update rate to motors - a value in hertz void AP_MotorsMatrix::set_update_rate( uint16_t speed_hz ) { - uint32_t fast_channel_mask = 0; int8_t i; // record requested speed @@ -43,13 +44,9 @@ void AP_MotorsMatrix::set_update_rate( uint16_t speed_hz ) // check each enabled motor for( i=0; iset_freq( _motor_to_channel_map[i], _speed_hz ); } } - - // enable fast channels - _rc->SetFastOutputChannels(fast_channel_mask, _speed_hz); } // set frame orientation (normally + or X) @@ -78,7 +75,7 @@ void AP_MotorsMatrix::enable() // enable output channels for( i=0; ienable_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; iradio_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 for( i=0; iOutputCh(_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(); // first delay is longer - delay(4000); + hal.scheduler->delay(4000); // loop through all the possible orders spinning any motors that match that description for( i=min_order; i<=max_order; i++ ) { for( j=0; jOutputCh(_motor_to_channel_map[j], _rc_throttle->radio_min + 100); - delay(300); - _rc->OutputCh(_motor_to_channel_map[j], _rc_throttle->radio_min); - delay(2000); + hal.rcout->write(_motor_to_channel_map[j], _rc_throttle->radio_min + 100); + hal.scheduler->delay(300); + hal.rcout->write(_motor_to_channel_map[j], _rc_throttle->radio_min); + hal.scheduler->delay(2000); } } } @@ -411,4 +408,4 @@ void AP_MotorsMatrix::remove_all_motors() remove_motor(i); } _num_motors = 0; -} \ No newline at end of file +} diff --git a/libraries/AP_Motors/AP_MotorsMatrix.h b/libraries/AP_Motors/AP_MotorsMatrix.h index f36ad34a94..1ff8494683 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.h +++ b/libraries/AP_Motors/AP_MotorsMatrix.h @@ -3,15 +3,13 @@ /// @file AP_MotorsMatrix.h /// @brief Motor control class for Matrixcopters -#ifndef AP_MOTORSMATRIX -#define AP_MOTORSMATRIX +#ifndef __AP_MOTORS_MATRIX_H__ +#define __AP_MOTORS_MATRIX_H__ -#include #include #include // ArduPilot Mega Vector/Matrix math Library #include // RC Channel Library -#include // ArduPilot Mega RC Library -#include +#include "AP_Motors_Class.h" #define AP_MOTORS_MATRIX_MOTOR_UNDEFINED -1 #define AP_MOTORS_MATRIX_ORDER_UNDEFINED -1 @@ -26,8 +24,8 @@ 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), + 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_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz), _num_motors(0) { }; @@ -87,4 +85,4 @@ protected: float _yaw_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to yaw (normally 1 or -1) }; -#endif // AP_MOTORSMATRIX \ No newline at end of file +#endif // AP_MOTORSMATRIX diff --git a/libraries/AP_Motors/AP_MotorsOcta.h b/libraries/AP_Motors/AP_MotorsOcta.h index 3a23b3e4bd..f39022b331 100644 --- a/libraries/AP_Motors/AP_MotorsOcta.h +++ b/libraries/AP_Motors/AP_MotorsOcta.h @@ -3,22 +3,20 @@ /// @file AP_MotorsOcta.h /// @brief Motor control class for Octacopters -#ifndef AP_MOTORSOCTA -#define AP_MOTORSOCTA +#ifndef __AP_MOTORS_OCTA_H__ +#define __AP_MOTORS_OCTA_H__ -#include #include #include // ArduPilot Mega Vector/Matrix math Library #include // RC Channel Library -#include // ArduPilot Mega RC Library -#include // Parent Motors Matrix 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) { + 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 @@ -28,4 +26,4 @@ protected: }; -#endif // AP_MOTORSOCTA \ No newline at end of file +#endif // AP_MOTORSOCTA diff --git a/libraries/AP_Motors/AP_MotorsOctaQuad.h b/libraries/AP_Motors/AP_MotorsOctaQuad.h index 390498288c..46d196d07a 100644 --- a/libraries/AP_Motors/AP_MotorsOctaQuad.h +++ b/libraries/AP_Motors/AP_MotorsOctaQuad.h @@ -3,22 +3,20 @@ /// @file AP_MotorsOctaQuad.h /// @brief Motor control class for OctaQuadcopters -#ifndef AP_MOTORSOCTAQUAD -#define AP_MOTORSOCTAQUAD +#ifndef __AP_MOTORS_OCTA_QUAD_H__ +#define __AP_MOTORS_OCTA_QUAD_H__ -#include #include #include // ArduPilot Mega Vector/Matrix math Library #include // RC Channel Library -#include // ArduPilot Mega RC Library -#include // Parent Motors Matrix 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) { + 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 @@ -28,4 +26,4 @@ protected: }; -#endif // AP_MOTORSOCTAQUAD \ No newline at end of file +#endif // AP_MOTORSOCTAQUAD diff --git a/libraries/AP_Motors/AP_MotorsQuad.h b/libraries/AP_Motors/AP_MotorsQuad.h index 14edee0a7c..7a5e188041 100644 --- a/libraries/AP_Motors/AP_MotorsQuad.h +++ b/libraries/AP_Motors/AP_MotorsQuad.h @@ -3,22 +3,20 @@ /// @file AP_MotorsQuad.h /// @brief Motor control class for Quadcopters -#ifndef AP_MOTORSQUAD -#define AP_MOTORSQUAD +#ifndef __AP_MOTORS_QUAD_H__ +#define __AP_MOTORS_QUAD_H__ -#include #include #include // ArduPilot Mega Vector/Matrix math Library #include // RC Channel Library -#include // ArduPilot Mega RC Library -#include // Parent Motors Matrix 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) { + 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 @@ -28,4 +26,4 @@ protected: }; -#endif // AP_MOTORSQUAD \ No newline at end of file +#endif // AP_MOTORSQUAD diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index eb96b5978d..7be68e54d1 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -7,9 +7,12 @@ * License as published by the Free Software Foundation; either * version 2.1 of the License, or (at your option) any later version. */ - +#include +#include #include "AP_MotorsTri.h" +extern const AP_HAL::HAL& hal; + // init void AP_MotorsTri::Init() { @@ -27,17 +30,19 @@ void AP_MotorsTri::set_update_rate( uint16_t speed_hz ) _speed_hz = speed_hz; // 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 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); + hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_1]); + hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_2]); + hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_4]); + hal.rcout->enable_ch(AP_MOTORS_CH_TRI_YAW); } // 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; // 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_yaw->radio_trim); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _rc_throttle->radio_min); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_CH_TRI_YAW], _rc_yaw->radio_trim); } // output_armed - sends commands to the motors @@ -124,18 +129,18 @@ void AP_MotorsTri::output_armed() #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]); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], motor_out[AP_MOTORS_MOT_1]); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], motor_out[AP_MOTORS_MOT_2]); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_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 // 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 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{ - _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 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); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min); + hal.scheduler->delay(4000); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min + 100); + hal.scheduler->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); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min); + hal.scheduler->delay(2000); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _rc_throttle->radio_min + 100); + hal.scheduler->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); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _rc_throttle->radio_min); + hal.scheduler->delay(2000); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min + 100); + hal.scheduler->delay(300); - _rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], motor_out[AP_MOTORS_MOT_1]); - _rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], motor_out[AP_MOTORS_MOT_2]); - _rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], motor_out[AP_MOTORS_MOT_4]); -} \ No newline at end of file + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], motor_out[AP_MOTORS_MOT_1]); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], motor_out[AP_MOTORS_MOT_2]); + hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], motor_out[AP_MOTORS_MOT_4]); +} diff --git a/libraries/AP_Motors/AP_MotorsTri.h b/libraries/AP_Motors/AP_MotorsTri.h index e4c6a4e24c..f92614c7a0 100644 --- a/libraries/AP_Motors/AP_MotorsTri.h +++ b/libraries/AP_Motors/AP_MotorsTri.h @@ -3,15 +3,13 @@ /// @file AP_MotorsTri.h /// @brief Motor control class for Tricopters -#ifndef AP_MOTORSTRI -#define AP_MOTORSTRI +#ifndef __AP_MOTORS_TRI_H__ +#define __AP_MOTORS_TRI_H__ -#include #include #include // ArduPilot Mega Vector/Matrix math Library #include // RC Channel Library -#include // ArduPilot Mega RC Library -#include +#include "AP_Motors.h" // tail servo uses channel 7 #define AP_MOTORS_CH_TRI_YAW CH_7 @@ -21,8 +19,8 @@ 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, 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_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_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz), _rc_tail(rc_tail) { }; @@ -54,4 +52,4 @@ protected: RC_Channel* _rc_tail; // REV parameter used from this channel to determine direction of tail servo movement }; -#endif // AP_MOTORSTRI \ No newline at end of file +#endif // AP_MOTORSTRI diff --git a/libraries/AP_Motors/AP_MotorsY6.h b/libraries/AP_Motors/AP_MotorsY6.h index db93e3f44a..8de5d53a60 100644 --- a/libraries/AP_Motors/AP_MotorsY6.h +++ b/libraries/AP_Motors/AP_MotorsY6.h @@ -3,14 +3,12 @@ /// @file AP_MotorsY6.h /// @brief Motor control class for Y6 frames -#ifndef AP_MOTORSY6 -#define AP_MOTORSY6 +#ifndef __AP_MOTORS_Y6_H__ +#define __AP_MOTORS_Y6_H__ -#include #include #include // ArduPilot Mega Vector/Matrix math Library #include // RC Channel Library -#include // ArduPilot Mega RC Library #include // Parent Motors Matrix library #define AP_MOTORS_Y6_YAW_DIRECTION 1 // this really should be a user selectable parameter @@ -20,7 +18,7 @@ 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) { + 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 @@ -30,4 +28,4 @@ protected: }; -#endif // AP_MOTORSY6 \ No newline at end of file +#endif // AP_MOTORSY6 diff --git a/libraries/AP_Motors/AP_Motors.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp similarity index 91% rename from libraries/AP_Motors/AP_Motors.cpp rename to libraries/AP_Motors/AP_Motors_Class.cpp index 5a435ac145..495d1c37c6 100644 --- a/libraries/AP_Motors/AP_Motors.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -8,7 +8,9 @@ * version 2.1 of the License, or (at your option) any later version. */ -#include "AP_Motors.h" +#include "AP_Motors_Class.h" +#include +extern const AP_HAL::HAL& hal; // parameters for the motor class const AP_Param::GroupInfo AP_Motors::var_info[] PROGMEM = { @@ -39,8 +41,7 @@ const AP_Param::GroupInfo AP_Motors::var_info[] PROGMEM = { }; // 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 ) : - _rc(rc_out), +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_roll(rc_roll), _rc_pitch(rc_pitch), _rc_throttle(rc_throttle), @@ -80,8 +81,9 @@ void AP_Motors::Init() void AP_Motors::throttle_pass_through() { if( armed() ) { + // XXX 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,8 +119,8 @@ bool AP_Motors::setup_throttle_curve() // disable throttle curve if not set-up corrrectly if( !retval ) { _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; -} \ No newline at end of file +} diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h new file mode 100644 index 0000000000..14fdebfefa --- /dev/null +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -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 +#include // ArduPilot Mega Vector/Matrix math Library +#include // Curve used to linearlise throttle pwm to thrust +#include // 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__ diff --git a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.pde b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.pde index 683a7e95b7..8a82d38ba7 100644 --- a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.pde +++ b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.pde @@ -3,66 +3,35 @@ * Code by Randy Mackay. DIYDrones.com */ -// AVR runtime -#include -#include -#include -#include - // Libraries -#include #include +#include +#include #include -#include #include // ArduPilot Mega Vector/Matrix math Library #include // RC Channel Library -#include // ArduPilot Mega RC Library #include -#include -#include -#include -#include -#include -#include -#include -#include +#include -//////////////////////////////////////////////////////////////////////////////// -// Serial ports -//////////////////////////////////////////////////////////////////////////////// -// -// Note that FastSerial port buffers are allocated at ::begin time, -// so there is not much of a penalty to defining ports that we don't -// use. -// -FastSerialPort0(Serial); // FTDI/console +#include +const AP_HAL::HAL& hal = AP_HAL_AVR_APM2; -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(CH_1), rc2(CH_2), rc3(CH_3), rc4(CH_4); +RC_Channel rc1(0), rc2(1), rc3(2), rc4(3); // 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); +//AP_MotorsTri motors(AP_MOTORS_APM1, &rc1, &rc2, &rc3, &rc4); +AP_MotorsQuad motors(AP_MOTORS_APM2, &rc1, &rc2, &rc3, &rc4); +//AP_MotorsHexa motors(AP_MOTORS_APM1, &rc1, &rc2, &rc3, &rc4); +//AP_MotorsY6 motors(AP_MOTORS_APM1, &rc1, &rc2, &rc3, &rc4); +//AP_MotorsOcta motors(AP_MOTORS_APM1, &rc1, &rc2, &rc3, &rc4); +//AP_MotorsOctaQuad motors(AP_MOTORS_APM1, &rc1, &rc2, &rc3, &rc4); +//AP_MotorsHeli motors(AP_MOTORS_APM1, &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 + hal.console->println("AP_Motors library test ver 1.0"); // motor initialisation motors.set_update_rate(490); @@ -71,10 +40,19 @@ void setup() motors.set_max_throttle(850); 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.output_min(); - delay(1000); + hal.scheduler->delay(1000); } // loop @@ -83,23 +61,23 @@ void loop() int value; // 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 - while( !Serial.available() ) { - delay(20); + while( !hal.console->available() ) { + hal.scheduler->delay(20); } // get character from user - value = Serial.read(); + value = hal.console->read(); // test motors if( value == 't' || value == 'T' ) { - Serial.println("testing motors..."); + hal.console->println("testing motors..."); motors.armed(true); motors.output_test(); motors.armed(false); - Serial.println("finished test."); + hal.console->println("finished test."); } } @@ -109,7 +87,9 @@ void print_motor_output() int8_t i; for(i=0; iprintf("\t%d %d",i,motors.motor_out[i]); } } } + +AP_HAL_MAIN(); diff --git a/libraries/AP_Motors/examples/AP_Motors_test/Arduino.h b/libraries/AP_Motors/examples/AP_Motors_test/Arduino.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_Motors/examples/AP_Motors_test/Makefile b/libraries/AP_Motors/examples/AP_Motors_test/Makefile new file mode 100644 index 0000000000..bd5f874ba2 --- /dev/null +++ b/libraries/AP_Motors/examples/AP_Motors_test/Makefile @@ -0,0 +1,5 @@ +BOARD = mega +include ../../../AP_Common/Arduino.mk + +apm2: + make -f Makefile EXTRAFLAGS="-DAPM2_HARDWARE=1" diff --git a/libraries/AP_Motors/examples/AP_Motors_test/nocore.inoflag b/libraries/AP_Motors/examples/AP_Motors_test/nocore.inoflag new file mode 100644 index 0000000000..e69de29bb2