aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Motors update

Browse Source
This commit is contained in:
jaxxzer 2016-01-11 23:37:04 -05:00 committed by Rustom Jehangir
parent 633ae6539f
commit e10a1e1eec
3 changed files with 30 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

77
libraries/AP_Motors/AP_Motors6DOF.cpp
View File

@ -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<AP_MOTORS_MAX_NUM_MOTORS; i++) {
// if (motor_enabled[i]) {
// motor_out[i] = throttle_radio_output;
// }
// }
//
// if(throttle_radio_output >= out_min_pwm) {
// // apply thrust curve and voltage scaling
// for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
// if (motor_enabled[i]) {
// motor_out[i] = apply_thrust_curve_and_volt_scaling(motor_out[i], out_min_pwm, out_max_pwm);
// }
// }
// }
//
// // send output to each motor
// hal.rcout->cork();
// for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
// if( motor_enabled[i] ) {
// rc_write(i, motor_out[i]);
// }
// }
// hal.rcout->push();
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

14
libraries/AP_Motors/AP_Motors6DOF.h
View File

@ -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

8
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -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