From e10a1e1eecc0985e6626b53292c0d5c00fec2d05 Mon Sep 17 00:00:00 2001 From: jaxxzer Date: Mon, 11 Jan 2016 23:37:04 -0500 Subject: [PATCH] Motors update --- libraries/AP_Motors/AP_Motors6DOF.cpp | 77 ++++++--------------------- libraries/AP_Motors/AP_Motors6DOF.h | 14 ++--- libraries/AP_Motors/AP_Motors_Class.h | 8 +++ 3 files changed, 30 insertions(+), 69 deletions(-) diff --git a/libraries/AP_Motors/AP_Motors6DOF.cpp b/libraries/AP_Motors/AP_Motors6DOF.cpp index 14bd685ffb..503da6ef68 100644 --- a/libraries/AP_Motors/AP_Motors6DOF.cpp +++ b/libraries/AP_Motors/AP_Motors6DOF.cpp @@ -25,18 +25,20 @@ extern const AP_HAL::HAL& hal; -AP_Motors6DOF::add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, float throttle_fac, float forward_fac, float strafe_fac, uint8_t testing_order) { - //Parent takes care of enabling output and setting up masks - AP_MotorsMatrix::add_motor_raw(motor_num, roll_fac, pitch_fac, yaw_fac, testing_order); +//void AP_Motors6DOF::add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, float throttle_fac, float forward_fac, float strafe_fac, uint8_t testing_order) { +// //Parent takes care of enabling output and setting up masks +// AP_MotorsMatrix::add_motor_raw(motor_num, roll_fac, pitch_fac, yaw_fac, testing_order); +// +// //These are additional parameters for an ROV +// _throttle_factor[motor_num] = throttle_fac; +// _forward_factor[motor_num] = forward_fac; +// _strafe_factor[motor_num] = strafe_fac; +//} + - //These are additional parameters for an ROV - _throttle_factor[motor_num] = throttle_fac; - _forward_factor[motor_num] = forward_fac; - _strafe_factor[motor_num] = strafe_fac; -} // output_min - sends minimum values out to the motors -void AP_MotorsMatrix::output_min() +void AP_Motors6DOF::output_min() { int8_t i; @@ -62,55 +64,7 @@ void AP_MotorsMatrix::output_min() //ToDo: call in control_rov, to mix inputs with no stabilization control void AP_Motors6DOF::output_armed_not_stabilizing() { -// uint8_t i; -// int16_t throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900 -// int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS]; // final outputs sent to the motors -// int16_t out_min_pwm = _throttle_radio_min + _min_throttle; // minimum pwm value we can send to the motors -// int16_t out_max_pwm = _throttle_radio_max; // maximum pwm value we can send to the motors -// -// // initialize limits flags -// limit.roll_pitch = true; -// limit.yaw = true; -// limit.throttle_lower = false; -// limit.throttle_upper = false; -// -// int16_t thr_in_min = rel_pwm_to_thr_range(_spin_when_armed_ramped); -// if (_throttle_control_input <= thr_in_min) { -// _throttle_control_input = thr_in_min; -// limit.throttle_lower = true; -// } -// -// if (_throttle_control_input >= _hover_out) { -// _throttle_control_input = _hover_out; -// limit.throttle_upper = true; -// } -// -// throttle_radio_output = calc_throttle_radio_output(); -// -// // set output throttle -// for (i=0; i= out_min_pwm) { -// // apply thrust curve and voltage scaling -// for (i=0; icork(); -// for( i=0; ipush(); + output_min(); //cut the motors for now } // output_armed - sends commands to the motors @@ -128,12 +82,15 @@ void AP_Motors6DOF::output_armed_stabilizing() int16_t strafe_pwm; int16_t out_min_pwm = 1100; // minimum pwm value we can send to the motors int16_t out_max_pwm = 1900; // maximum pwm value we can send to the motors - int16_t out_mid_pwm = 1500; // mid pwm value we can send to the motors +// int16_t out_mid_pwm = 1500; // mid pwm value we can send to the motors // the is the best throttle we can come up which provides good control without climbing - float rpy_scale = 1.0; // this is used to scale the roll, pitch and yaw to fit within the motor limits +// float rpy_scale = 1.0; // this is used to scale the roll, pitch and yaw to fit within the motor limits + + int16_t rpy_out[AP_MOTORS_MAX_NUM_MOTORS]; // buffer so we don't have to multiply coefficients multiple times. + int16_t linear_out[AP_MOTORS_MAX_NUM_MOTORS]; // 3 linear DOF mix for each motor int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS]; // final outputs sent to the motors int16_t rpy_low = 0; // lowest motor value diff --git a/libraries/AP_Motors/AP_Motors6DOF.h b/libraries/AP_Motors/AP_Motors6DOF.h index 11ca41b107..e4cc8937d0 100644 --- a/libraries/AP_Motors/AP_Motors6DOF.h +++ b/libraries/AP_Motors/AP_Motors6DOF.h @@ -22,20 +22,16 @@ public: AP_MotorsMatrix(loop_rate, speed_hz) {}; - void set_forward(float forward_in) { _forward_in = constrain_float(2*(forward_in-1500),-1000.0f,1000.0f); }; // range 0 ~ 1000 - void set_strafe(float strafe_in) { _strafe_in = constrain_float(2*(strafe_in-1500),-1000.0f,1000.0f); }; // range 0 ~ 1000 - - float get_forward() const { return _forward_in; } // range 1100~1900 this is raw pwm value from rc - float get_strafe() const { return _strafe_in; } // range 1100~1900 this is raw pwm value from rc - + void output_min() override; protected: //Override MotorsMatrix method - void add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, float climb_fac, float forward_fac, float strafe_fac, uint8_t testing_order) + //void add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, float climb_fac, float forward_fac, float strafe_fac, uint8_t testing_order); + + void output_armed_not_stabilizing() override; + void output_armed_stabilizing() override; - float _forward_in; // last forward input from set_forward caller, raw pwm - float _strafe_in; // last strafe input from set_strafe caller, raw pwm float _throttle_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to throttle (climb/descent) float _forward_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to forward/backward diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 33b79317e7..0f8e60ec20 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -65,6 +65,8 @@ public: void set_pitch(int16_t pitch_in) { _pitch_control_input = pitch_in; }; // range -4500 ~ 4500 void set_yaw(int16_t yaw_in) { _yaw_control_input = yaw_in; }; // range -4500 ~ 4500 void set_throttle(float throttle_in) { _throttle_in = constrain_float(throttle_in,-100.0f,1100.0f); }; // range 0 ~ 1000 + void set_forward(float forward_in) { _strafe_in = constrain_float(forward_in,-100.0f,1100.0f); }; // range 0 ~ 1000 + void set_strafe(float strafe_in) { _strafe_in = constrain_float(strafe_in,-100.0f,1100.0f); }; // range 0 ~ 1000 void set_stabilizing(bool stabilizing) { _flags.stabilizing = stabilizing; } // accessors for roll, pitch, yaw and throttle inputs to motors @@ -72,6 +74,8 @@ public: float get_pitch() const { return _pitch_control_input; } float get_yaw() const { return _yaw_control_input; } float get_throttle() const { return _throttle_control_input; } + float get_forward() const { return _forward_control_input; } + float get_strafe() const { return _strafe_control_input; } void set_throttle_filter_cutoff(float filt_hz) { _throttle_filter.set_cutoff_frequency(filt_hz); } @@ -151,6 +155,8 @@ protected: float _roll_control_input; // desired roll control from attitude controllers, +/- 4500 float _pitch_control_input; // desired pitch control from attitude controller, +/- 4500 float _throttle_control_input; // desired throttle (thrust) control from attitude controller, 0-1000 + float _forward_control_input; // desired throttle (forward) control from attitude controller, 0-1000 + float _strafe_control_input; // desired throttle (strafe) control from attitude controller, 0-1000 float _yaw_control_input; // desired yaw control from attitude controller, +/- 4500 float _throttle_pwm_scalar; // scalar used to convert throttle channel pwm range into 0-1000 range, ~0.8 - 1.0 float _rpy_pwm_scalar; // scaler used to convert roll, pitch, yaw inputs to pwm range @@ -159,6 +165,8 @@ protected: int16_t _throttle_radio_min; // minimum radio channel pwm int16_t _throttle_radio_max; // maximum radio channel pwm float _throttle_in; // last throttle input from set_throttle caller + float _forward_in; // last forward input from set_forward caller + float _strafe_in; // last strafe input from set_strafe caller LowPassFilterFloat _throttle_filter; // throttle input filter // battery voltage, current and air pressure compensation variables