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

Added forward-vertical pitch coupling fix for vectored ROV.

Browse Source
This commit is contained in:
Rustom Jehangir 2016-02-07 16:06:22 -08:00
parent a5daa47453
commit e91bb2d444
2 changed files with 132 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

130
libraries/AP_Motors/AP_MotorsVectoredROV.cpp
View File

@ -18,8 +18,11 @@
*
*/
#include <AP_HAL/AP_HAL.h>
#include "AP_MotorsVectoredROV.h"
extern const AP_HAL::HAL& hal;
// setup_motors - configures the motors for the BlueROV
void AP_MotorsVectoredROV::setup_motors()
{
@ -84,3 +87,130 @@ void AP_MotorsVectoredROV::setup_motors()
add_motor_raw_6dof(AP_MOTORS_MOT_5, MOT_5_ROLL_FACTOR, MOT_5_PITCH_FACTOR, MOT_5_YAW_FACTOR, MOT_5_THROTTLE_FACTOR, MOT_5_FORWARD_FACTOR, MOT_5_STRAFE_FACTOR,5);
add_motor_raw_6dof(AP_MOTORS_MOT_6, MOT_6_ROLL_FACTOR, MOT_6_PITCH_FACTOR, MOT_6_YAW_FACTOR, MOT_6_THROTTLE_FACTOR, MOT_6_FORWARD_FACTOR, MOT_6_STRAFE_FACTOR,6);
}
// output_armed - sends commands to the motors
// includes new scaling stability patch
// TODO pull code that is common to output_armed_not_stabilizing into helper functions
// ToDo calculate headroom for rpy to be added for stabilization during full throttle/forward/strafe commands
void AP_MotorsVectoredROV::output_armed_stabilizing()
{
int8_t i;
int16_t roll_pwm; // roll pwm value, initially calculated by calc_roll_pwm() but may be modified after, +/- 400
int16_t pitch_pwm; // pitch pwm value, initially calculated by calc_roll_pwm() but may be modified after, +/- 400
int16_t yaw_pwm; // yaw pwm value, initially calculated by calc_yaw_pwm() but may be modified after, +/- 400
int16_t throttle_radio_output; // throttle pwm value, +/- 400
int16_t forward_pwm; // forward pwm value, +/- 400
int16_t strafe_pwm; // forward pwm value, +/- 400
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
// 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
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
int16_t rpy_high = 0; // highest motor value
int16_t yaw_allowed; // amount of yaw we can fit in
int16_t thr_adj; // the difference between the pilot's desired throttle and out_best_thr_pwm (the throttle that is actually provided)
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// Ensure throttle is within bounds of 0 to 1000
int16_t thr_in_min = rel_pwm_to_thr_range(_min_throttle);
if (_throttle_control_input <= thr_in_min) {
_throttle_control_input = thr_in_min;
limit.throttle_lower = true;
}
if (_throttle_control_input >= _max_throttle) {
_throttle_control_input = _max_throttle;
limit.throttle_upper = true;
}
roll_pwm = calc_roll_pwm();
pitch_pwm = calc_pitch_pwm();
yaw_pwm = calc_yaw_pwm();
throttle_radio_output = (calc_throttle_radio_output()-_throttle_radio_min-(_throttle_radio_max-_throttle_radio_min)/2);
forward_pwm = get_forward()*0.4;
strafe_pwm = get_strafe()*0.4;
// calculate roll, pitch and yaw for each motor
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
rpy_out[i] = roll_pwm * _roll_factor[i] +
pitch_pwm * _pitch_factor[i] +
yaw_pwm * _yaw_factor[i];
}
}
float forwardVerticalCouplingFactor = 1.0; // 0.0-1.5
int16_t forward_coupling_limit = max(forwardVerticalCouplingFactor*(400-fabs(throttle_radio_output)),0);
int8_t forward_coupling_direction[] = {-1,0,1,1,0,-1};
// calculate linear command for each motor
// linear factors should be 0.0 or 1.0 for now
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
float forward_pwm_limited;
// The following statements decouple forward/vertical hydrodynamic coupling on
// vectored ROVs. This is done by limiting the maximum output of the "rear" vectored
// thruster (where "rear" depends on direction of travel).
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
#define sign(x) ((x>0)-(x<0))
if ( sign(forward_pwm) == sign(forward_coupling_direction[i]) && forward_coupling_direction[i] != 0 ) {
forward_pwm_limited = constrain(forward_pwm,-forward_coupling_limit,forward_coupling_limit);
}
#undef constrain
linear_out[i] = throttle_radio_output * _throttle_factor[i] +
forward_pwm_limited * _forward_factor[i] +
strafe_pwm * _strafe_factor[i];
}
}
// Calculate final pwm output for each motor
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = 1500 + _motor_reverse[i]*(rpy_out[i] + linear_out[i]);
}
}
// // 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);
// }
// }
// clip motor output if required (shouldn't be)
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = constrain_int16(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();
}

2
libraries/AP_Motors/AP_MotorsVectoredROV.h
View File

@ -24,6 +24,8 @@ public:
// This takes care of all of the enabling and bitmask stuff seen in the Tricopter motors class
virtual void setup_motors();
void output_armed_stabilizing() override;
protected:
};