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

Changed the majority of the BlueROV motor library file. Haven't compile tested yet because this commit was made by accident. Whoops. Will commit a fix if it breaks.

Browse Source
This commit is contained in:
Rustom Jehangir 2016-01-01 15:22:48 -08:00
parent 45699c845d
commit f1d7cb01b1
2 changed files with 107 additions and 109 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

212
libraries/AP_Motors/AP_MotorsBlueROV.cpp
View File

@ -80,10 +80,13 @@ void AP_MotorsBlueROV::Init()
// set the motor_enabled flag so that the ESCs can be calibrated like other frame types
motor_enabled[AP_MOTORS_MOT_1] = true;
motor_enabled[AP_MOTORS_MOT_2] = true;
motor_enabled[AP_MOTORS_MOT_3] = true;
motor_enabled[AP_MOTORS_MOT_4] = true;
motor_enabled[AP_MOTORS_MOT_5] = true;
motor_enabled[AP_MOTORS_MOT_6] = true;
// disable CH7 from being used as an aux output (i.e. for camera gimbal, etc)
RC_Channel_aux::disable_aux_channel(AP_MOTORS_CH_TRI_YAW);
// disable CH7 from being used as an aux output (i.e. for lights, etc)
RC_Channel_aux::disable_aux_channel(AP_MOTORS_CH_CAM_PITCH);
}
// set update rate to motors - a value in hertz
@ -96,7 +99,10 @@ void AP_MotorsBlueROV::set_update_rate( uint16_t speed_hz )
uint32_t mask =
1U << AP_MOTORS_MOT_1 |
1U << AP_MOTORS_MOT_2 |
1U << AP_MOTORS_MOT_4;
1U << AP_MOTORS_MOT_3 |
1U << AP_MOTORS_MOT_4 |
1U << AP_MOTORS_MOT_5 |
1U << AP_MOTORS_MOT_6;
hal.rcout->set_freq(mask, _speed_hz);
}
@ -106,8 +112,11 @@ void AP_MotorsBlueROV::enable()
// enable output channels
hal.rcout->enable_ch(AP_MOTORS_MOT_1);
hal.rcout->enable_ch(AP_MOTORS_MOT_2);
hal.rcout->enable_ch(AP_MOTORS_MOT_3);
hal.rcout->enable_ch(AP_MOTORS_MOT_4);
hal.rcout->enable_ch(AP_MOTORS_CH_TRI_YAW);
hal.rcout->enable_ch(AP_MOTORS_MOT_5);
hal.rcout->enable_ch(AP_MOTORS_MOT_6);
hal.rcout->enable_ch(AP_MOTORS_CH_CAM_PITCH);
}
// output_min - sends minimum values out to the motors
@ -116,12 +125,17 @@ void AP_MotorsBlueROV::output_min()
// set lower limit flag
limit.throttle_lower = true;
int16_t out_stopped = 1500;
// send minimum value to each motor
hal.rcout->cork();
hal.rcout->write(AP_MOTORS_MOT_1, _throttle_radio_min);
hal.rcout->write(AP_MOTORS_MOT_2, _throttle_radio_min);
hal.rcout->write(AP_MOTORS_MOT_4, _throttle_radio_min);
hal.rcout->write(AP_MOTORS_CH_TRI_YAW, _yaw_servo_trim);
hal.rcout->write(AP_MOTORS_MOT_1, out_stopped);
hal.rcout->write(AP_MOTORS_MOT_2, out_stopped);
hal.rcout->write(AP_MOTORS_MOT_3, out_stopped);
hal.rcout->write(AP_MOTORS_MOT_4, out_stopped);
hal.rcout->write(AP_MOTORS_MOT_5, out_stopped);
hal.rcout->write(AP_MOTORS_MOT_6, out_stopped);
hal.rcout->write(AP_MOTORS_CH_CAM_PITCH, _yaw_servo_trim);
hal.rcout->push();
}
@ -129,19 +143,23 @@ void AP_MotorsBlueROV::output_min()
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
uint16_t AP_MotorsBlueROV::get_motor_mask()
{
// tri copter uses channels 1,2,4 and 7
// BlueROV uses channels 1,2,3,4,5,6 and 7
return (1U << AP_MOTORS_MOT_1) |
(1U << AP_MOTORS_MOT_2) |
(1U << AP_MOTORS_MOT_3) |
(1U << AP_MOTORS_MOT_4) |
(1U << AP_MOTORS_CH_TRI_YAW);
(1U << AP_MOTORS_MOT_5) |
(1U << AP_MOTORS_MOT_6) |
(1U << AP_MOTORS_CH_CAM_PITCH);
}
void AP_MotorsBlueROV::output_armed_not_stabilizing()
{
int16_t throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900
int16_t out_min = _throttle_radio_min + _min_throttle;
int16_t out_max = _throttle_radio_max;
int16_t motor_out[AP_MOTORS_MOT_4+1];
int16_t throttle_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900
int16_t out_min = 1100;
int16_t out_max = 1900;
int16_t out_stopped = 1500;
int16_t motor_out[AP_MOTORS_MOT_6+1];
// initialize limits flags
limit.roll_pitch = true;
@ -160,28 +178,37 @@ void AP_MotorsBlueROV::output_armed_not_stabilizing()
limit.throttle_upper = true;
}
throttle_radio_output = calc_throttle_radio_output();
throttle_output = out_stopped + _throttle_control_input;
motor_out[AP_MOTORS_MOT_1] = throttle_radio_output;
motor_out[AP_MOTORS_MOT_2] = throttle_radio_output;
motor_out[AP_MOTORS_MOT_4] = throttle_radio_output;
motor_out[AP_MOTORS_MOT_1] = throttle_output;
motor_out[AP_MOTORS_MOT_2] = throttle_output;
motor_out[AP_MOTORS_MOT_3] = throttle_output;
motor_out[AP_MOTORS_MOT_4] = throttle_output;
motor_out[AP_MOTORS_MOT_5] = throttle_output;
motor_out[AP_MOTORS_MOT_6] = throttle_output;
if(throttle_radio_output >= out_min) {
//if(throttle_output >= out_min) {
// adjust for thrust curve and voltage scaling
motor_out[AP_MOTORS_MOT_1] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_1], out_min, out_max);
motor_out[AP_MOTORS_MOT_2] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_2], out_min, out_max);
motor_out[AP_MOTORS_MOT_4] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_4], out_min, out_max);
}
//motor_out[AP_MOTORS_MOT_1] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_1], out_min, out_max);
//motor_out[AP_MOTORS_MOT_2] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_2], out_min, out_max);
//motor_out[AP_MOTORS_MOT_3] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_3], out_min, out_max);
//motor_out[AP_MOTORS_MOT_4] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_4], out_min, out_max);
//motor_out[AP_MOTORS_MOT_5] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_5], out_min, out_max);
//motor_out[AP_MOTORS_MOT_6] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_6], out_min, out_max);
//}
hal.rcout->cork();
// send output to each motor
hal.rcout->write(AP_MOTORS_MOT_1, motor_out[AP_MOTORS_MOT_1]);
hal.rcout->write(AP_MOTORS_MOT_2, motor_out[AP_MOTORS_MOT_2]);
hal.rcout->write(AP_MOTORS_MOT_3, motor_out[AP_MOTORS_MOT_3]);
hal.rcout->write(AP_MOTORS_MOT_4, motor_out[AP_MOTORS_MOT_4]);
hal.rcout->write(AP_MOTORS_MOT_5, motor_out[AP_MOTORS_MOT_5]);
hal.rcout->write(AP_MOTORS_MOT_6, motor_out[AP_MOTORS_MOT_6]);
// send centering signal to yaw servo
hal.rcout->write(AP_MOTORS_CH_TRI_YAW, _yaw_servo_trim);
hal.rcout->write(AP_MOTORS_CH_CAM_PITCH, _yaw_servo_trim);
hal.rcout->push();
}
@ -192,11 +219,14 @@ void AP_MotorsBlueROV::output_armed_stabilizing()
{
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 throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900
int16_t throttle_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900
int16_t forward_output; // total forward value
int16_t strafe_output; // total strafe value
int16_t yaw_radio_output; // final yaw pwm value sent to motors, typically ~1100-1900
int16_t out_min = _throttle_radio_min + _min_throttle;
int16_t out_max = _throttle_radio_max;
int16_t motor_out[AP_MOTORS_MOT_4+1];
int16_t out_min = 1100;
int16_t out_max = 1900;
int16_t out_stopped = 1500;
int16_t motor_out[AP_MOTORS_MOT_6+1];
// initialize limits flags
limit.roll_pitch = false;
@ -217,88 +247,51 @@ void AP_MotorsBlueROV::output_armed_stabilizing()
// tricopters limit throttle to 80%
// To-Do: implement improved stability patch and remove this limit
if (_throttle_control_input > 800) {
_throttle_control_input = 800;
limit.throttle_upper = true;
}
//if (_throttle_control_input > 800) {
// _throttle_control_input = 800;
// limit.throttle_upper = true;
//}
roll_pwm = calc_roll_pwm();
pitch_pwm = calc_pitch_pwm();
throttle_radio_output = calc_throttle_radio_output();
throttle_output = _throttle_control_input;
forward_output = _forward_control_input; // ToDo: Clean this up to match others
strafe_output = _strafe_control_input;
yaw_radio_output = calc_yaw_radio_output();
// if we are not sending a throttle output, we cut the motors
if( is_zero(_throttle_control_input) ) {
// range check spin_when_armed
if (_spin_when_armed_ramped < 0) {
_spin_when_armed_ramped = 0;
}
if (_spin_when_armed_ramped > _min_throttle) {
_spin_when_armed_ramped = _min_throttle;
}
motor_out[AP_MOTORS_MOT_1] = _throttle_radio_min + _spin_when_armed_ramped;
motor_out[AP_MOTORS_MOT_2] = _throttle_radio_min + _spin_when_armed_ramped;
motor_out[AP_MOTORS_MOT_4] = _throttle_radio_min + _spin_when_armed_ramped;
//left front vertical
motor_out[AP_MOTORS_MOT_1] = out_stopped + _throttle_control_input + roll_pwm + 0.5*pitch_pwm + 0.2*strafe_output;
//left forward
motor_out[AP_MOTORS_MOT_2] = out_stopped + forward_output + yaw_radio_output;
//rear vertical
motor_out[AP_MOTORS_MOT_3] = out_stopped + _throttle_control_input + roll_pwm - pitch_pwm;
//right forward
motor_out[AP_MOTORS_MOT_4] = out_stopped + forward_output - yaw_radio_output;
//right front vertical
motor_out[AP_MOTORS_MOT_5] = out_stopped + _throttle_control_input - roll_pwm + 0.5*pitch_pwm - 0.2*strafe_output;
// strafe
motor_out[AP_MOTORS_MOT_6] = out_stopped + strafe_output;
}else{
int16_t roll_out = (float)(roll_pwm * 0.866f);
int16_t pitch_out = pitch_pwm / 2;
// check if throttle is below limit
if (_throttle_control_input <= _min_throttle) {
limit.throttle_lower = true;
_throttle_control_input = _min_throttle;
throttle_radio_output = calc_throttle_radio_output();
}
// TODO: set limits.roll_pitch and limits.yaw
//left front
motor_out[AP_MOTORS_MOT_2] = throttle_radio_output + roll_out + pitch_out;
//right front
motor_out[AP_MOTORS_MOT_1] = throttle_radio_output - roll_out + pitch_out;
// rear
motor_out[AP_MOTORS_MOT_4] = throttle_radio_output - pitch_pwm;
// Tridge's stability patch
if(motor_out[AP_MOTORS_MOT_1] > out_max) {
motor_out[AP_MOTORS_MOT_2] -= (motor_out[AP_MOTORS_MOT_1] - out_max);
motor_out[AP_MOTORS_MOT_4] -= (motor_out[AP_MOTORS_MOT_1] - out_max);
motor_out[AP_MOTORS_MOT_1] = out_max;
}
if(motor_out[AP_MOTORS_MOT_2] > out_max) {
motor_out[AP_MOTORS_MOT_1] -= (motor_out[AP_MOTORS_MOT_2] - out_max);
motor_out[AP_MOTORS_MOT_4] -= (motor_out[AP_MOTORS_MOT_2] - out_max);
motor_out[AP_MOTORS_MOT_2] = out_max;
}
if(motor_out[AP_MOTORS_MOT_4] > out_max) {
motor_out[AP_MOTORS_MOT_1] -= (motor_out[AP_MOTORS_MOT_4] - out_max);
motor_out[AP_MOTORS_MOT_2] -= (motor_out[AP_MOTORS_MOT_4] - out_max);
motor_out[AP_MOTORS_MOT_4] = out_max;
}
// adjust for thrust curve and voltage scaling
motor_out[AP_MOTORS_MOT_1] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_1], out_min, out_max);
motor_out[AP_MOTORS_MOT_2] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_2], out_min, out_max);
motor_out[AP_MOTORS_MOT_4] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_4], out_min, out_max);
// ensure motors don't drop below a minimum value and stop
motor_out[AP_MOTORS_MOT_1] = MAX(motor_out[AP_MOTORS_MOT_1], out_min);
motor_out[AP_MOTORS_MOT_2] = MAX(motor_out[AP_MOTORS_MOT_2], out_min);
motor_out[AP_MOTORS_MOT_4] = MAX(motor_out[AP_MOTORS_MOT_4], out_min);
}
// adjust for thrust curve and voltage scaling
//motor_out[AP_MOTORS_MOT_1] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_1], out_min, out_max);
//motor_out[AP_MOTORS_MOT_2] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_2], out_min, out_max);
//motor_out[AP_MOTORS_MOT_3] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_3], out_min, out_max);
//motor_out[AP_MOTORS_MOT_4] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_4], out_min, out_max);
//motor_out[AP_MOTORS_MOT_5] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_5], out_min, out_max);
//motor_out[AP_MOTORS_MOT_6] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_6], out_min, out_max);
hal.rcout->cork();
// send output to each motor
hal.rcout->write(AP_MOTORS_MOT_1, motor_out[AP_MOTORS_MOT_1]);
hal.rcout->write(AP_MOTORS_MOT_2, motor_out[AP_MOTORS_MOT_2]);
hal.rcout->write(AP_MOTORS_MOT_4, motor_out[AP_MOTORS_MOT_4]);
hal.rcout->write(AP_MOTORS_MOT_2, motor_out[AP_MOTORS_MOT_2]);
hal.rcout->write(AP_MOTORS_MOT_3, motor_out[AP_MOTORS_MOT_3]);
hal.rcout->write(AP_MOTORS_MOT_4, motor_out[AP_MOTORS_MOT_4]);
hal.rcout->write(AP_MOTORS_MOT_5, motor_out[AP_MOTORS_MOT_5]);
hal.rcout->write(AP_MOTORS_MOT_6, motor_out[AP_MOTORS_MOT_6]);
// send out to yaw command to tail servo
hal.rcout->write(AP_MOTORS_CH_TRI_YAW, yaw_radio_output);
hal.rcout->write(AP_MOTORS_CH_CAM_PITCH, yaw_radio_output);
hal.rcout->push();
}
@ -323,21 +316,26 @@ void AP_MotorsBlueROV::output_test(uint8_t motor_seq, int16_t pwm)
// output to motors and servos
switch (motor_seq) {
case 1:
// front right motor
hal.rcout->write(AP_MOTORS_MOT_1, pwm);
break;
case 2:
// back motor
hal.rcout->write(AP_MOTORS_MOT_4, pwm);
break;
case 3:
// back servo
hal.rcout->write(AP_MOTORS_CH_TRI_YAW, pwm);
break;
case 4:
// front left motor
hal.rcout->write(AP_MOTORS_MOT_2, pwm);
break;
case 3:
hal.rcout->write(AP_MOTORS_MOT_3, pwm);
break;
case 4:
hal.rcout->write(AP_MOTORS_MOT_4, pwm);
break;
case 5:
hal.rcout->write(AP_MOTORS_MOT_5, pwm);
break;
case 6:
hal.rcout->write(AP_MOTORS_MOT_6, pwm);
break;
case 7:
hal.rcout->write(AP_MOTORS_CH_CAM_PITCH, pwm);
break;
default:
// do nothing
break;

4
libraries/AP_Motors/AP_MotorsBlueROV.h
View File

@ -11,8 +11,8 @@
#include <RC_Channel/RC_Channel.h> // RC Channel Library
#include "AP_MotorsMulticopter.h"
// tail servo uses channel 7
#define AP_MOTORS_CH_TRI_YAW CH_7
// pitch servo uses channel 7
#define AP_MOTORS_CH_CAM_PITCH CH_7
/// @class AP_MotorsTri
class AP_MotorsBlueROV : public AP_MotorsMulticopter {