Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: add custom config support

This commit is contained in:
Ammarf 2018-05-31 18:26:07 +09:00 committed by Randy Mackay
parent 9fff6d1e71
commit 430ed9bd3d
5 changed files with 208 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

248
APMrover2/AP_MotorsUGV.cpp
View File

@ -115,6 +115,11 @@ void AP_MotorsUGV::init()
// set safety output
setup_safety_output();
// setup motors for custom configs
if (rover.get_frame_type() != FRAME_TYPE_UNDEFINED) {
setup_motors();
}
}
// setup output in case of main CPU failure
@ -158,10 +163,92 @@ void AP_MotorsUGV::setup_servo_output()
SRV_Channels::set_angle(SRV_Channel::k_throttleLeft, 1000);
SRV_Channels::set_angle(SRV_Channel::k_throttleRight, 1000);
// k_motor1, k_motor2 and k_motor3 are in power percent so -100 ... 100
SRV_Channels::set_angle(SRV_Channel::k_motor1, 100);
SRV_Channels::set_angle(SRV_Channel::k_motor2, 100);
SRV_Channels::set_angle(SRV_Channel::k_motor3, 100);
// custom config motors set in power percent so -100 ... 100
for (uint8_t i=0; i<AP_MOTORS_NUM_MOTORS_MAX; i++) {
SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(i);
SRV_Channels::set_angle(function, 100);
}
}
// config for frames with vectored motors and custom motor configurations
void AP_MotorsUGV::setup_motors()
{
// remove existing motors
for (int8_t i=0; i<AP_MOTORS_NUM_MOTORS_MAX; i++) {
clear_motors(i);
}
// hard coded factor configuration
switch (rover.get_frame_type()) {
// FRAME TYPE NAME
case FRAME_TYPE_UNDEFINED:
break;
case FRAME_TYPE_OMNI3:
_motors_num = 3;
add_motor(0, 1.0f, 1.0f, -1.0f);
add_motor(1, 0.0f, 1.0f, 1.0f);
add_motor(2, 1.0f, 1.0f, 1.0f);
break;
case FRAME_TYPE_OMNIX:
_motors_num = 4,
add_motor(0, -1.0f, 1.0f, 1.0f);
add_motor(1, -1.0f, -1.0f, -1.0f);
add_motor(2, 1.0f, -1.0f, 1.0f);
add_motor(3, 1.0f, 1.0f, -1.0f);
break;
case FRAME_TYPE_OMNIPLUS:
_motors_num = 4;
add_motor(0, 0.0f, 1.0f, 1.0f);
add_motor(1, 0.0f, -1.0f, 1.0f);
add_motor(2, 1.0f, 0.0f, 0.0f);
add_motor(3, 1.0f, 0.0f, 0.0f);
break;
}
}
// add motor using separate throttle, steering and lateral factors for frames with custom motor configurations
void AP_MotorsUGV::add_motor(int8_t motor_num, float throttle_factor, float steering_factor, float lateral_factor)
{
// ensure valid motor number is provided
if (motor_num >= 0 && motor_num < AP_MOTORS_NUM_MOTORS_MAX) {
// set throttle, steering and lateral factors
_throttle_factor[motor_num] = throttle_factor;
_steering_factor[motor_num] = steering_factor;
_lateral_factor[motor_num] = lateral_factor;
add_motor_num(motor_num);
}
}
// add a motor and set up default output function
void AP_MotorsUGV::add_motor_num(int8_t motor_num)
{
// ensure a valid motor number is provided
if (motor_num >= 0 && motor_num < AP_MOTORS_NUM_MOTORS_MAX) {
uint8_t chan;
SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(motor_num);
SRV_Channels::set_aux_channel_default(function, motor_num);
if (!SRV_Channels::find_channel(function, chan)) {
gcs().send_text(MAV_SEVERITY_ERROR, "Motors: unable to setup motor %u", motor_num);
}
}
}
// disable motor and remove all throttle, steering and lateral factor for this motor
void AP_MotorsUGV::clear_motors(int8_t motor_num)
{
// ensure valid motor number is provided
if (motor_num >= 0 && motor_num < AP_MOTORS_NUM_MOTORS_MAX) {
// disable the motor and set factors to zero
_throttle_factor[motor_num] = 0;
_steering_factor[motor_num] = 0;
_lateral_factor[motor_num] = 0;
}
}
// set steering as a value from -4500 to +4500
@ -216,17 +303,6 @@ bool AP_MotorsUGV::have_skid_steering() const
return false;
}
// returns true if vehicle is capable of lateral movement
bool AP_MotorsUGV::has_lateral_control() const
{
if (SRV_Channels::function_assigned(SRV_Channel::k_motor1) &&
SRV_Channels::function_assigned(SRV_Channel::k_motor2) &&
SRV_Channels::function_assigned(SRV_Channel::k_motor3)) {
return true;
}
return false;
}
void AP_MotorsUGV::output(bool armed, float ground_speed, float dt)
{
// soft-armed overrides passed in armed status
@ -244,12 +320,12 @@ void AP_MotorsUGV::output(bool armed, float ground_speed, float dt)
// output for regular steering/throttle style frames
output_regular(armed, ground_speed, _steering, _throttle);
// output for omni style frames
output_omni(armed, _steering, _throttle, _lateral);
// output for skid steering style frames
output_skid_steering(armed, _steering, _throttle);
// output for frames with vectored and custom motor configurations
output_custom_config(armed, _steering, _throttle, _lateral);
// send values to the PWM timers for output
SRV_Channels::calc_pwm();
SRV_Channels::cork();
@ -379,12 +455,17 @@ bool AP_MotorsUGV::pre_arm_check(bool report) const
}
return false;
}
// check if only one of the omni rover outputs has been configured
if ((SRV_Channels::function_assigned(SRV_Channel::k_motor1)) != (SRV_Channels::function_assigned(SRV_Channel::k_motor2)) ||
(SRV_Channels::function_assigned(SRV_Channel::k_motor1)) != (SRV_Channels::function_assigned(SRV_Channel::k_motor3)) ||
(SRV_Channels::function_assigned(SRV_Channel::k_motor2)) != (SRV_Channels::function_assigned(SRV_Channel::k_motor3))) {
if (report) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: check motor 1, motor2 and motor3 config");
// check if one of custom config motors hasn't been configured
for (uint8_t i=0; i<_motors_num; i++)
{
SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(i);
if (SRV_Channels::function_assigned(function)) {
return true;
} else {
if (report) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: check %u", function);
return false;
}
}
}
return true;
@ -407,9 +488,9 @@ void AP_MotorsUGV::setup_pwm_type()
motor_mask |= SRV_Channels::get_output_channel_mask(SRV_Channel::k_throttle);
motor_mask |= SRV_Channels::get_output_channel_mask(SRV_Channel::k_throttleLeft);
motor_mask |= SRV_Channels::get_output_channel_mask(SRV_Channel::k_throttleRight);
motor_mask |= SRV_Channels::get_output_channel_mask(SRV_Channel::k_motor1);
motor_mask |= SRV_Channels::get_output_channel_mask(SRV_Channel::k_motor2);
motor_mask |= SRV_Channels::get_output_channel_mask(SRV_Channel::k_motor3);
for (uint8_t i=0; i<_motors_num; i++) {
motor_mask |= SRV_Channels::get_output_channel_mask(SRV_Channels::get_motor_function(i));
}
switch (_pwm_type) {
case PWM_TYPE_ONESHOT:
@ -484,64 +565,6 @@ void AP_MotorsUGV::output_regular(bool armed, float ground_speed, float steering
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering);
}
// output for omni style frames
void AP_MotorsUGV::output_omni(bool armed, float steering, float throttle, float lateral)
{
if (!has_lateral_control()) {
return;
}
// clear and set limits based on input
set_limits_from_input(armed, steering, throttle);
// constrain steering
steering = constrain_float(steering, -4500.0f, 4500.0f);
if (armed) {
// scale throttle, steering and lateral to -1 ~ 1
const float scaled_throttle = throttle / 100.0f;
const float scaled_steering = steering / 4500.0f;
const float scaled_lateral = lateral / 100.0f;
// calculate desired vehicle speed and direction
const float magnitude = safe_sqrt((scaled_throttle*scaled_throttle)+(scaled_lateral*scaled_lateral));
const float theta = atan2f(scaled_throttle,scaled_lateral);
// calculate X and Y vectors using the following the equations: vx = cos(theta) * magnitude and vy = sin(theta) * magnitude
const float Vx = -(cosf(theta)*magnitude);
const float Vy = -(sinf(theta)*magnitude);
// calculate output throttle for each motor. Output is multiplied by 0.5 to bring the range generally within -1 ~ 1
// First wheel (motor 1) moves only parallel to x-axis so only X component is taken. Normal range is -2 ~ 2 with the steering
// motor_2 and motor_3 utilizes both X and Y components.
// safe_sqrt((3)/2) used because the motors are 120 degrees apart in the frame, this setup is mandatory
float motor_1 = 0.5 * ((-Vx) + scaled_steering);
float motor_2 = 0.5 * (((0.5*Vx)-((safe_sqrt(3)/2)*Vy)) + scaled_steering);
float motor_3 = 0.5 * (((0.5*Vx)+((safe_sqrt(3)/2)*Vy)) + scaled_steering);
// apply constraints
motor_1 = constrain_float(motor_1, -1.0f, 1.0f);
motor_2 = constrain_float(motor_2, -1.0f, 1.0f);
motor_3 = constrain_float(motor_3, -1.0f, 1.0f);
// scale back and send pwm value to each motor
output_throttle(SRV_Channel::k_motor1, 100.0f * motor_1);
output_throttle(SRV_Channel::k_motor2, 100.0f * motor_2);
output_throttle(SRV_Channel::k_motor3, 100.0f * motor_3);
} else {
// handle disarmed case
if (_disarm_disable_pwm) {
SRV_Channels::set_output_limit(SRV_Channel::k_motor1, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
SRV_Channels::set_output_limit(SRV_Channel::k_motor2, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
SRV_Channels::set_output_limit(SRV_Channel::k_motor3, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
} else {
SRV_Channels::set_output_limit(SRV_Channel::k_motor1, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::set_output_limit(SRV_Channel::k_motor2, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::set_output_limit(SRV_Channel::k_motor3, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
}
}
}
// output to skid steering channels
void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float throttle)
{
@ -591,11 +614,60 @@ void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float thrott
output_throttle(SRV_Channel::k_throttleRight, 100.0f * motor_right);
}
// output for custom configurations
void AP_MotorsUGV::output_custom_config(bool armed, float steering, float throttle, float lateral)
{
// exit immediately if the frame type is set to UNDEFINED
if (rover.get_frame_type() == FRAME_TYPE_UNDEFINED) {
return;
}
if (armed) {
// clear and set limits based on input
set_limits_from_input(armed, steering, throttle);
// constrain steering
steering = constrain_float(steering, -4500.0f, 4500.0f);
// scale throttle, steering and lateral inputs to -1 to 1
const float scaled_throttle = throttle / 100.0f;
const float scaled_steering = steering / 4500.0f;
const float scaled_lateral = lateral / 100.0f;
float thr_str_ltr_out;
float thr_str_ltr_max = 1;
for (uint8_t i=0; i<AP_MOTORS_NUM_MOTORS_MAX; i++) {
thr_str_ltr_out = (scaled_throttle * _throttle_factor[i]) +
(scaled_steering * _steering_factor[i]) +
(scaled_lateral * _lateral_factor[i]);
if (fabsf(thr_str_ltr_out) > thr_str_ltr_max) {
thr_str_ltr_max = fabsf(thr_str_ltr_out);
}
float output_vectored = (thr_str_ltr_out / thr_str_ltr_max);
// send output for each motor
output_throttle(SRV_Channels::get_motor_function(i), 100.0f * output_vectored);
}
} else {
// handle disarmed case
if (_disarm_disable_pwm) {
for (uint8_t i=0; i<_motors_num; i++) {
SRV_Channels::set_output_limit(SRV_Channels::get_motor_function(i), SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
}
} else {
for (uint8_t i=0; i<_motors_num; i++) {
SRV_Channels::set_output_limit(SRV_Channels::get_motor_function(i), SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
}
}
}
}
// output throttle value to main throttle channel, left throttle or right throttle. throttle should be scaled from -100 to 100
void AP_MotorsUGV::output_throttle(SRV_Channel::Aux_servo_function_t function, float throttle)
{
// sanity check servo function
if (function != SRV_Channel::k_throttle && function != SRV_Channel::k_throttleLeft && function != SRV_Channel::k_throttleRight && function != SRV_Channel::k_motor1 && function != SRV_Channel::k_motor2 && function != SRV_Channel::k_motor3) {
if (function != SRV_Channel::k_throttle && function != SRV_Channel::k_throttleLeft && function != SRV_Channel::k_throttleRight && function != SRV_Channel::k_motor1 && function != SRV_Channel::k_motor2 && function != SRV_Channel::k_motor3 && function!= SRV_Channel::k_motor4) {
return;
}
@ -625,6 +697,9 @@ void AP_MotorsUGV::output_throttle(SRV_Channel::Aux_servo_function_t function, f
case SRV_Channel::k_motor3:
_relayEvents.do_set_relay(2, relay_high);
break;
case SRV_Channel::k_motor4:
_relayEvents.do_set_relay(3, relay_high);
break;
default:
// do nothing
break;
@ -639,6 +714,7 @@ void AP_MotorsUGV::output_throttle(SRV_Channel::Aux_servo_function_t function, f
case SRV_Channel::k_motor1:
case SRV_Channel::k_motor2:
case SRV_Channel::k_motor3:
case SRV_Channel::k_motor4:
SRV_Channels::set_output_scaled(function, throttle);
break;
case SRV_Channel::k_throttleLeft:

39
APMrover2/AP_MotorsUGV.h
View File

@ -25,6 +25,14 @@ public:
MOTOR_TEST_THROTTLE_RIGHT = 4,
};
// supported custom motor configurations
enum frame_type {
FRAME_TYPE_UNDEFINED = 0,
FRAME_TYPE_OMNI3 = 1,
FRAME_TYPE_OMNIX = 2,
FRAME_TYPE_OMNIPLUS = 3,
};
// initialise motors
void init();
@ -37,6 +45,18 @@ public:
// setup servo output ranges
void setup_servo_output();
// config for frames with vectored motors
void setup_motors();
// add motor using separate throttle, steering and lateral factors for frames with custom motor configuration
void add_motor(int8_t motor_num, float throttle_factor, float steering_factor, float lateral_factor);
// add a motor and set up output function
void add_motor_num(int8_t motor_num);
// disable motor and remove all throttle, steering and lateral factor for this motor
void clear_motors(int8_t motor_num);
// get or set steering as a value from -4500 to +4500
// apply_scaling should be set to false for manual modes where
// no scaling by speed or angle should e performed
@ -58,9 +78,6 @@ public:
// true if vehicle is capable of skid steering
bool have_skid_steering() const;
//true if vehicle is capable of lateral movement
bool has_lateral_control() const;
// true if vehicle has vectored thrust (i.e. boat with motor on steering servo)
bool have_vectored_thrust() const { return is_positive(_vector_throttle_base); }
@ -101,12 +118,12 @@ protected:
// output to regular steering and throttle channels
void output_regular(bool armed, float ground_speed, float steering, float throttle);
// output for omni style frames
void output_omni(bool armed, float steering, float throttle, float lateral);
// output to skid steering channels
void output_skid_steering(bool armed, float steering, float throttle);
// output for vectored and custom motors configuration
void output_custom_config(bool armed, float steering, float throttle, float lateral);
// output throttle (-100 ~ +100) to a throttle channel. Sets relays if required
void output_throttle(SRV_Channel::Aux_servo_function_t function, float throttle);
@ -122,6 +139,8 @@ protected:
// external references
AP_ServoRelayEvents &_relayEvents;
static const int8_t AP_MOTORS_NUM_MOTORS_MAX = 4;
// parameters
AP_Int8 _pwm_type; // PWM output type
AP_Int8 _pwm_freq; // PWM output freq for brushed motors
@ -138,5 +157,11 @@ protected:
float _throttle; // requested throttle as a value from -100 to 100
float _throttle_prev; // throttle input from previous iteration
bool _scale_steering = true; // true if we should scale steering by speed or angle
float _lateral; // requested lateral input as a value from -4500 to +4500
float _lateral; // requested lateral input as a value from -100 to +100
// custom config variables
float _throttle_factor[AP_MOTORS_NUM_MOTORS_MAX];
float _steering_factor[AP_MOTORS_NUM_MOTORS_MAX];
float _lateral_factor[AP_MOTORS_NUM_MOTORS_MAX];
uint8_t _motors_num;
};

8
APMrover2/Parameters.cpp
View File

@ -556,6 +556,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Path: ../libraries/AP_Follow/AP_Follow.cpp
AP_SUBGROUPINFO(follow, "FOLL", 23, ParametersG2, AP_Follow),
// @Param: FRAME_TYPE
// @DisplayName: Frame Type
// @Description: Frame Type
// @Values: 0:Undefined,1:Omni3,2:OmniX,3:OmniPlus
// @User: Standard
// @RebootRequired: True
AP_GROUPINFO("FRAME_TYPE", 24, ParametersG2, frame_type, 0),
AP_GROUPEND
};

3
APMrover2/Parameters.h
View File

@ -351,6 +351,9 @@ public:
// follow mode library
AP_Follow follow;
// frame type for vehicle (used for vectored motor vehicles and custom motor configs)
AP_Int8 frame_type;
};
extern const AP_Param::Info var_info[];

3
APMrover2/Rover.h
View File

@ -563,6 +563,9 @@ public:
bool mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc, uint8_t motor_seq, uint8_t throttle_type, int16_t throttle_value);
MAV_RESULT mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type, int16_t throttle_value, float timeout_sec);
void motor_test_stop();
// frame type
uint8_t get_frame_type() { return g2.frame_type.get(); }
};
extern const AP_HAL::HAL& hal;