From 638f1364bea68e83f9edbacf0de7fe81497d11c8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 3 Jan 2017 20:56:57 +1100 Subject: [PATCH] AP_Motors: adapt to new RC_Channel API --- libraries/AP_Motors/AP_MotorsCoax.cpp | 36 +++++++--------- libraries/AP_Motors/AP_MotorsCoax.h | 15 +++---- libraries/AP_Motors/AP_MotorsHeli.cpp | 13 +++--- libraries/AP_Motors/AP_MotorsHeli.h | 7 ++-- libraries/AP_Motors/AP_MotorsHeli_RSC.cpp | 4 +- libraries/AP_Motors/AP_MotorsHeli_RSC.h | 7 ++-- libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 39 ++++++++---------- libraries/AP_Motors/AP_MotorsHeli_Single.h | 24 +++++------ libraries/AP_Motors/AP_MotorsSingle.cpp | 41 +++++++++---------- libraries/AP_Motors/AP_MotorsSingle.h | 14 +++---- libraries/AP_Motors/AP_Motors_Class.cpp | 29 ++++++------- libraries/AP_Motors/AP_Motors_Class.h | 6 +-- .../AP_Motors_test/AP_Motors_test.cpp | 2 +- 13 files changed, 112 insertions(+), 125 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsCoax.cpp b/libraries/AP_Motors/AP_MotorsCoax.cpp index ca32591009..e2c0971fdc 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.cpp +++ b/libraries/AP_Motors/AP_MotorsCoax.cpp @@ -22,6 +22,7 @@ #include #include #include "AP_MotorsCoax.h" +#include extern const AP_HAL::HAL& hal; @@ -44,19 +45,6 @@ const AP_Param::GroupInfo AP_MotorsCoax::var_info[] = { // @Units: Hz AP_GROUPINFO("SV_SPEED", 43, AP_MotorsCoax, _servo_speed, AP_MOTORS_SINGLE_SPEED_DIGITAL_SERVOS), - // @Group: SV1_ - // @Path: ../RC_Channel/RC_Channel.cpp - AP_SUBGROUPINFO(_servo1, "SV1_", 44, AP_MotorsCoax, RC_Channel), - // @Group: SV2_ - // @Path: ../RC_Channel/RC_Channel.cpp - AP_SUBGROUPINFO(_servo2, "SV2_", 45, AP_MotorsCoax, RC_Channel), - // @Group: SV3_ - // @Path: ../RC_Channel/RC_Channel.cpp - AP_SUBGROUPINFO(_servo3, "SV3_", 46, AP_MotorsCoax, RC_Channel), - // @Group: SV4_ - // @Path: ../RC_Channel/RC_Channel.cpp - AP_SUBGROUPINFO(_servo4, "SV4_", 47, AP_MotorsCoax, RC_Channel), - AP_GROUPEND }; // init @@ -65,19 +53,25 @@ void AP_MotorsCoax::init(motor_frame_class frame_class, motor_frame_type frame_t // set update rate for the 3 motors (but not the servo on channel 7) set_update_rate(_speed_hz); + _servo1 = SRV_Channels::get_channel_for(SRV_Channel::k_motor1, CH_1); + _servo2 = SRV_Channels::get_channel_for(SRV_Channel::k_motor2, CH_2); + _servo3 = SRV_Channels::get_channel_for(SRV_Channel::k_motor3, CH_3); + _servo4 = SRV_Channels::get_channel_for(SRV_Channel::k_motor4, CH_4); + if (!_servo1 || !_servo2 || !_servo3 || !_servo4) { + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_ERROR, "MotorsCoax: unable to setup output channels"); + // don't set initialised_ok + return; + } + // set the motor_enabled flag so that the main ESC can be calibrated like other frame types motor_enabled[AP_MOTORS_MOT_5] = true; motor_enabled[AP_MOTORS_MOT_6] = true; // we set four servos to angle - _servo1.set_type(RC_CHANNEL_TYPE_ANGLE); - _servo2.set_type(RC_CHANNEL_TYPE_ANGLE); - _servo3.set_type(RC_CHANNEL_TYPE_ANGLE); - _servo4.set_type(RC_CHANNEL_TYPE_ANGLE); - _servo1.set_angle(AP_MOTORS_COAX_SERVO_INPUT_RANGE); - _servo2.set_angle(AP_MOTORS_COAX_SERVO_INPUT_RANGE); - _servo3.set_angle(AP_MOTORS_COAX_SERVO_INPUT_RANGE); - _servo4.set_angle(AP_MOTORS_COAX_SERVO_INPUT_RANGE); + _servo1->set_angle(AP_MOTORS_COAX_SERVO_INPUT_RANGE); + _servo2->set_angle(AP_MOTORS_COAX_SERVO_INPUT_RANGE); + _servo3->set_angle(AP_MOTORS_COAX_SERVO_INPUT_RANGE); + _servo4->set_angle(AP_MOTORS_COAX_SERVO_INPUT_RANGE); // record successful initialisation if what we setup was the desired frame_class _flags.initialised_ok = (frame_class == MOTOR_FRAME_COAX); diff --git a/libraries/AP_Motors/AP_MotorsCoax.h b/libraries/AP_Motors/AP_MotorsCoax.h index d3f9302fb3..92a9de82aa 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.h +++ b/libraries/AP_Motors/AP_MotorsCoax.h @@ -4,7 +4,7 @@ #include #include // ArduPilot Mega Vector/Matrix math Library -#include // RC Channel Library +#include #include "AP_MotorsMulticopter.h" // feedback direction @@ -24,8 +24,7 @@ public: /// Constructor AP_MotorsCoax(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : - AP_MotorsMulticopter(loop_rate, speed_hz), - _servo1(CH_NONE), _servo2(CH_NONE), _servo3(CH_NONE), _servo4(CH_NONE) + AP_MotorsMulticopter(loop_rate, speed_hz) { AP_Param::setup_object_defaults(this, var_info); }; @@ -64,13 +63,11 @@ protected: // servo speed AP_Int16 _servo_speed; - // Allow the use of a 4 servo output to make it easy to test coax and single using same airframe - RC_Channel _servo1; - RC_Channel _servo2; - RC_Channel _servo3; - RC_Channel _servo4; - float _actuator_out[NUM_ACTUATORS]; // combined roll, pitch, yaw and throttle outputs to motors in 0~1 range float _thrust_yt_ccw; float _thrust_yt_cw; + SRV_Channel *_servo1; + SRV_Channel *_servo2; + SRV_Channel *_servo3; + SRV_Channel *_servo4; }; diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index d2d23a40eb..1e77358cff 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -194,7 +194,10 @@ void AP_MotorsHeli::init(motor_frame_class frame_class, motor_frame_type frame_t _throttle_radio_passthrough = 0.5f; // initialise Servo/PWM ranges and endpoints - init_outputs(); + if (!init_outputs()) { + // don't set initialised_ok + return; + } // calculate all scalars calculate_scalars(); @@ -367,13 +370,13 @@ bool AP_MotorsHeli::parameter_check(bool display_msg) const } // reset_swash_servo -void AP_MotorsHeli::reset_swash_servo(RC_Channel& servo) +void AP_MotorsHeli::reset_swash_servo(SRV_Channel *servo) { - servo.set_range_out(0, 1000); + servo->set_range(1000); // swash servos always use full endpoints as restricting them would lead to scaling errors - servo.set_radio_min(1000); - servo.set_radio_max(2000); + servo->set_output_min(1000); + servo->set_output_max(2000); } // update the throttle input filter diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 13d380e3b8..f4b381fa6a 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -6,7 +6,8 @@ #include #include // ArduPilot Mega Vector/Matrix math Library -#include // RC Channel Library +#include +#include #include "AP_Motors_Class.h" #include "AP_MotorsHeli_RSC.h" @@ -160,10 +161,10 @@ protected: virtual void move_actuators(float roll_out, float pitch_out, float coll_in, float yaw_out) = 0; // reset_swash_servo - free up swash servo for maximum movement - void reset_swash_servo(RC_Channel& servo); + void reset_swash_servo(SRV_Channel *servo); // init_outputs - initialise Servo/PWM ranges and endpoints - virtual void init_outputs() = 0; + virtual bool init_outputs() = 0; // calculate_armed_scalars - must be implemented by child classes virtual void calculate_armed_scalars() = 0; diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp index 3f9a0546ed..20728bf18f 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp @@ -24,7 +24,7 @@ extern const AP_HAL::HAL& hal; void AP_MotorsHeli_RSC::init_servo() { // setup RSC on specified channel by default - RC_Channel_aux::set_aux_channel_default(_aux_fn, _default_channel); + SRV_Channels::set_aux_channel_default(_aux_fn, _default_channel); } // set_power_output_range @@ -194,6 +194,6 @@ void AP_MotorsHeli_RSC::write_rsc(float servo_out) } else { pwm = _pwm_max - pwm; } - RC_Channel_aux::set_radio(_aux_fn, pwm); + SRV_Channels::set_output_pwm(_aux_fn, pwm); } } diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.h b/libraries/AP_Motors/AP_MotorsHeli_RSC.h index 5b0d02b018..938445e9ba 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.h +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.h @@ -2,7 +2,8 @@ #include #include // ArduPilot Mega Vector/Matrix math Library -#include // RC Channel Library +#include +#include // rotor controller states enum RotorControlState { @@ -24,7 +25,7 @@ class AP_MotorsHeli_RSC { public: friend class AP_MotorsHeli_Single; - AP_MotorsHeli_RSC(RC_Channel_aux::Aux_servo_function_t aux_fn, + AP_MotorsHeli_RSC(SRV_Channel::Aux_servo_function_t aux_fn, uint8_t default_channel) : _aux_fn(aux_fn), _default_channel(default_channel) @@ -80,7 +81,7 @@ private: uint64_t _last_update_us; // channel setup for aux function - RC_Channel_aux::Aux_servo_function_t _aux_fn; + SRV_Channel::Aux_servo_function_t _aux_fn; uint8_t _default_channel; // internal variables diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index d79246d01f..1bbefb6b08 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -15,7 +15,7 @@ #include #include -#include +#include #include "AP_MotorsHeli_Single.h" #include @@ -116,22 +116,6 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = { // @User: Standard AP_GROUPINFO("GYR_GAIN_ACRO", 11, AP_MotorsHeli_Single, _ext_gyro_gain_acro, 0), - // @Group: SV1_ - // @Path: ../RC_Channel/RC_Channel.cpp - AP_SUBGROUPINFO(_swash_servo_1, "SV1_", 12, AP_MotorsHeli_Single, RC_Channel), - - // @Group: SV2_ - // @Path: ../RC_Channel/RC_Channel.cpp - AP_SUBGROUPINFO(_swash_servo_2, "SV2_", 13, AP_MotorsHeli_Single, RC_Channel), - - // @Group: SV3_ - // @Path: ../RC_Channel/RC_Channel.cpp - AP_SUBGROUPINFO(_swash_servo_3, "SV3_", 14, AP_MotorsHeli_Single, RC_Channel), - - // @Group: SV4_ - // @Path: ../RC_Channel/RC_Channel.cpp - AP_SUBGROUPINFO(_yaw_servo, "SV4_", 15, AP_MotorsHeli_Single, RC_Channel), - // @Param: RSC_PWM_MIN // @DisplayName: RSC PWM output miniumum // @Description: This sets the PWM output on RSC channel for maximum rotor speed @@ -186,18 +170,31 @@ void AP_MotorsHeli_Single::enable() } // init_outputs - initialise Servo/PWM ranges and endpoints -void AP_MotorsHeli_Single::init_outputs() +bool AP_MotorsHeli_Single::init_outputs() { + if (!_flags.initialised_ok) { + _swash_servo_1 = SRV_Channels::get_channel_for(SRV_Channel::k_motor1, CH_1); + _swash_servo_2 = SRV_Channels::get_channel_for(SRV_Channel::k_motor2, CH_2); + _swash_servo_3 = SRV_Channels::get_channel_for(SRV_Channel::k_motor3, CH_3); + _yaw_servo = SRV_Channels::get_channel_for(SRV_Channel::k_motor4, CH_4); + _servo_aux = SRV_Channels::get_channel_for(SRV_Channel::k_motor7, CH_7); + if (!_swash_servo_1 || !_swash_servo_2 || !_swash_servo_3 || !_yaw_servo || !_servo_aux) { + return false; + } + } + // reset swash servo range and endpoints reset_swash_servo (_swash_servo_1); reset_swash_servo (_swash_servo_2); reset_swash_servo (_swash_servo_3); - _yaw_servo.set_angle(4500); + _yaw_servo->set_angle(4500); // set main rotor servo range // tail rotor servo use range as set in vehicle code for rc7 _main_rotor.init_servo(); + + return true; } // output_test - spin a motor at the pwm value specified @@ -357,10 +354,10 @@ void AP_MotorsHeli_Single::update_motor_control(RotorControlState state) if (state == ROTOR_CONTROL_STOP){ // set engine run enable aux output to not run position to kill engine when disarmed - RC_Channel_aux::set_radio_to_min(RC_Channel_aux::k_engine_run_enable); + SRV_Channels::set_output_limit(SRV_Channel::k_engine_run_enable, SRV_Channel::SRV_CHANNEL_LIMIT_MIN); } else { // else if armed, set engine run enable output to run position - RC_Channel_aux::set_radio_to_max(RC_Channel_aux::k_engine_run_enable); + SRV_Channels::set_output_limit(SRV_Channel::k_engine_run_enable, SRV_Channel::SRV_CHANNEL_LIMIT_MAX); } // Check if both rotors are run-up, tail rotor controller always returns true if not enabled diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h index 49c054329c..f51b0a26e3 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h @@ -4,7 +4,7 @@ #include #include // ArduPilot Mega Vector/Matrix math Library -#include // RC Channel Library +#include #include "AP_MotorsHeli.h" #include "AP_MotorsHeli_RSC.h" @@ -42,14 +42,11 @@ class AP_MotorsHeli_Single : public AP_MotorsHeli { public: // constructor - AP_MotorsHeli_Single(RC_Channel& servo_aux, - uint16_t loop_rate, + AP_MotorsHeli_Single(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) : AP_MotorsHeli(loop_rate, speed_hz), - _servo_aux(servo_aux), - _main_rotor(RC_Channel_aux::k_heli_rsc, AP_MOTORS_HELI_SINGLE_RSC), - _tail_rotor(RC_Channel_aux::k_heli_tail_rsc, AP_MOTORS_HELI_SINGLE_AUX), - _swash_servo_1(CH_NONE), _swash_servo_2(CH_NONE), _swash_servo_3(CH_NONE), _yaw_servo(CH_NONE) + _main_rotor(SRV_Channel::k_heli_rsc, AP_MOTORS_HELI_SINGLE_RSC), + _tail_rotor(SRV_Channel::k_heli_tail_rsc, AP_MOTORS_HELI_SINGLE_AUX) { AP_Param::setup_object_defaults(this, var_info); }; @@ -107,7 +104,7 @@ public: protected: // init_outputs - initialise Servo/PWM ranges and endpoints - void init_outputs(); + bool init_outputs() override; // update_motor_controls - sends commands to motor controllers void update_motor_control(RotorControlState state); @@ -128,7 +125,6 @@ protected: void servo_test(); // external objects we depend upon - RC_Channel& _servo_aux; // output to ext gyro gain and tail direct drive esc (ch7) AP_MotorsHeli_RSC _main_rotor; // main rotor AP_MotorsHeli_RSC _tail_rotor; // tail rotor @@ -152,10 +148,12 @@ protected: AP_Float _collective_yaw_effect; // Feed-forward compensation to automatically add rudder input when collective pitch is increased. Can be positive or negative depending on mechanics. AP_Int8 _flybar_mode; // Flybar present or not. Affects attitude controller used during ACRO flight mode AP_Int16 _direct_drive_tailspeed; // Direct Drive VarPitch Tail ESC speed (0 ~ 1000) - RC_Channel _swash_servo_1; // swash plate servo #1 - RC_Channel _swash_servo_2; // swash plate servo #2 - RC_Channel _swash_servo_3; // swash plate servo #3 - RC_Channel _yaw_servo; // tail servo + SRV_Channel *_swash_servo_1; + SRV_Channel *_swash_servo_2; + SRV_Channel *_swash_servo_3; + SRV_Channel *_yaw_servo; + SRV_Channel *_servo_aux; + bool _acro_tail = false; }; diff --git a/libraries/AP_Motors/AP_MotorsSingle.cpp b/libraries/AP_Motors/AP_MotorsSingle.cpp index da77d0370c..dd4c57c951 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.cpp +++ b/libraries/AP_Motors/AP_MotorsSingle.cpp @@ -22,6 +22,7 @@ #include #include #include "AP_MotorsSingle.h" +#include extern const AP_HAL::HAL& hal; @@ -44,19 +45,6 @@ const AP_Param::GroupInfo AP_MotorsSingle::var_info[] = { // @Values: 50, 125, 250 AP_GROUPINFO("SV_SPEED", 43, AP_MotorsSingle, _servo_speed, AP_MOTORS_SINGLE_SPEED_DIGITAL_SERVOS), - // @Group: SV1_ - // @Path: ../RC_Channel/RC_Channel.cpp - AP_SUBGROUPINFO(_servo1, "SV1_", 44, AP_MotorsSingle, RC_Channel), - // @Group: SV2_ - // @Path: ../RC_Channel/RC_Channel.cpp - AP_SUBGROUPINFO(_servo2, "SV2_", 45, AP_MotorsSingle, RC_Channel), - // @Group: SV3_ - // @Path: ../RC_Channel/RC_Channel.cpp - AP_SUBGROUPINFO(_servo3, "SV3_", 46, AP_MotorsSingle, RC_Channel), - // @Group: SV4_ - // @Path: ../RC_Channel/RC_Channel.cpp - AP_SUBGROUPINFO(_servo4, "SV4_", 47, AP_MotorsSingle, RC_Channel), - AP_GROUPEND }; // init @@ -69,15 +57,21 @@ void AP_MotorsSingle::init(motor_frame_class frame_class, motor_frame_type frame motor_enabled[AP_MOTORS_MOT_5] = true; motor_enabled[AP_MOTORS_MOT_6] = true; + _servo1 = SRV_Channels::get_channel_for(SRV_Channel::k_motor1, CH_1); + _servo2 = SRV_Channels::get_channel_for(SRV_Channel::k_motor2, CH_2); + _servo3 = SRV_Channels::get_channel_for(SRV_Channel::k_motor3, CH_3); + _servo4 = SRV_Channels::get_channel_for(SRV_Channel::k_motor4, CH_4); + if (!_servo1 || !_servo2 || !_servo3 || !_servo4) { + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_ERROR, "MotorsSingle: unable to setup output channels"); + // don't set initialised_ok + return; + } + // we set four servos to angle - _servo1.set_type(RC_CHANNEL_TYPE_ANGLE); - _servo2.set_type(RC_CHANNEL_TYPE_ANGLE); - _servo3.set_type(RC_CHANNEL_TYPE_ANGLE); - _servo4.set_type(RC_CHANNEL_TYPE_ANGLE); - _servo1.set_angle(AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); - _servo2.set_angle(AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); - _servo3.set_angle(AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); - _servo4.set_angle(AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); + _servo1->set_angle(AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); + _servo2->set_angle(AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); + _servo3->set_angle(AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); + _servo4->set_angle(AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); // allow mapping of motor7 add_motor_num(CH_7); @@ -89,7 +83,7 @@ void AP_MotorsSingle::init(motor_frame_class frame_class, motor_frame_type frame // set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus) void AP_MotorsSingle::set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type) { - _flags.initialised_ok = (frame_class == MOTOR_FRAME_SINGLE); + // nothing to do } // set update rate to motors - a value in hertz @@ -125,6 +119,9 @@ void AP_MotorsSingle::enable() void AP_MotorsSingle::output_to_motors() { + if (!_flags.initialised_ok) { + return; + } switch (_spool_mode) { case SHUT_DOWN: // sends minimum values out to the motors diff --git a/libraries/AP_Motors/AP_MotorsSingle.h b/libraries/AP_Motors/AP_MotorsSingle.h index 4350bafe70..8aa059ee38 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.h +++ b/libraries/AP_Motors/AP_MotorsSingle.h @@ -4,7 +4,7 @@ #include #include // ArduPilot Mega Vector/Matrix math Library -#include // RC Channel Library +#include #include "AP_MotorsMulticopter.h" // feedback direction @@ -24,8 +24,7 @@ public: /// Constructor AP_MotorsSingle(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : - AP_MotorsMulticopter(loop_rate, speed_hz), - _servo1(CH_NONE), _servo2(CH_NONE), _servo3(CH_NONE), _servo4(CH_NONE) + AP_MotorsMulticopter(loop_rate, speed_hz) { AP_Param::setup_object_defaults(this, var_info); }; @@ -64,12 +63,11 @@ protected: // servo speed AP_Int16 _servo_speed; - RC_Channel _servo1; - RC_Channel _servo2; - RC_Channel _servo3; - RC_Channel _servo4; - int16_t _throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900 float _actuator_out[NUM_ACTUATORS]; // combined roll, pitch, yaw and throttle outputs to motors in 0~1 range float _thrust_out; + SRV_Channel *_servo1; + SRV_Channel *_servo2; + SRV_Channel *_servo3; + SRV_Channel *_servo4; }; diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index 9bd1d84af8..b4c8785a7e 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -21,6 +21,8 @@ #include "AP_Motors_Class.h" #include +#include + extern const AP_HAL::HAL& hal; // Constructor @@ -153,39 +155,39 @@ uint32_t AP_Motors::rc_map_mask(uint32_t mask) const } // convert input in -1 to +1 range to pwm output -int16_t AP_Motors::calc_pwm_output_1to1(float input, const RC_Channel& servo) +int16_t AP_Motors::calc_pwm_output_1to1(float input, const SRV_Channel *servo) { int16_t ret; input = constrain_float(input, -1.0f, 1.0f); - if (servo.get_reverse()) { + if (servo->get_reversed()) { input = -input; } if (input >= 0.0f) { - ret = ((input * (servo.get_radio_max() - servo.get_radio_trim())) + servo.get_radio_trim()); + ret = ((input * (servo->get_output_max() - servo->get_trim())) + servo->get_trim()); } else { - ret = ((input * (servo.get_radio_trim() - servo.get_radio_min())) + servo.get_radio_trim()); + ret = ((input * (servo->get_trim() - servo->get_output_min())) + servo->get_trim()); } - return constrain_int16(ret, servo.get_radio_min(), servo.get_radio_max()); + return constrain_int16(ret, servo->get_output_min(), servo->get_output_max()); } // convert input in 0 to +1 range to pwm output -int16_t AP_Motors::calc_pwm_output_0to1(float input, const RC_Channel& servo) +int16_t AP_Motors::calc_pwm_output_0to1(float input, const SRV_Channel *servo) { int16_t ret; input = constrain_float(input, 0.0f, 1.0f); - if (servo.get_reverse()) { + if (servo->get_reversed()) { input = 1.0f-input; } - ret = input * (servo.get_radio_max() - servo.get_radio_min()) + servo.get_radio_min(); + ret = input * (servo->get_output_max() - servo->get_output_min()) + servo->get_output_min(); - return constrain_int16(ret, servo.get_radio_min(), servo.get_radio_max()); + return constrain_int16(ret, servo->get_output_min(), servo->get_output_max()); } /* @@ -196,13 +198,12 @@ void AP_Motors::add_motor_num(int8_t motor_num) // ensure valid motor number is provided if( motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS ) { uint8_t chan; - if (RC_Channel_aux::find_channel((RC_Channel_aux::Aux_servo_function_t)(RC_Channel_aux::k_motor1+motor_num), - chan)) { + SRV_Channel::Aux_servo_function_t function = (SRV_Channel::Aux_servo_function_t)(SRV_Channel::k_motor1+motor_num); + SRV_Channels::set_aux_channel_default(function, motor_num); + if (SRV_Channels::find_channel((SRV_Channel::Aux_servo_function_t)(SRV_Channel::k_motor1+motor_num), + chan) && chan != motor_num) { _motor_map[motor_num] = chan; _motor_map_mask |= 1U< #include // ArduPilot Mega Vector/Matrix math Library #include // Notify library -#include // RC Channel Library +#include #include // filter library // offsets for motors in motor_out and _motor_filtered arrays @@ -167,10 +167,10 @@ protected: virtual void save_params_on_disarm() {} // convert input in -1 to +1 range to pwm output - int16_t calc_pwm_output_1to1(float input, const RC_Channel& servo); + int16_t calc_pwm_output_1to1(float input, const SRV_Channel *servo); // convert input in 0 to +1 range to pwm output - int16_t calc_pwm_output_0to1(float input, const RC_Channel& servo); + int16_t calc_pwm_output_0to1(float input, const SRV_Channel *servo); // flag bitmask struct AP_Motors_flags { diff --git a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp index 780c0479b1..cd5fd29e1f 100644 --- a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp +++ b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp @@ -74,7 +74,7 @@ void setup() // set rc channel ranges rc1.set_angle(4500); rc2.set_angle(4500); - rc3.set_range(130, 1000); + rc3.set_range(1000); rc4.set_angle(4500); hal.scheduler->delay(1000);