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

Rover: add support for lateral control input

This commit is contained in:
Ammarf 2018-05-10 16:10:34 +09:00 committed by Randy Mackay
parent 0baee6f82b
commit 26c25daa36
8 changed files with 76 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
APMrover2/AP_MotorsUGV.cpp
View File

@ -177,6 +177,12 @@ void AP_MotorsUGV::set_throttle(float throttle)
_throttle = constrain_float(throttle, -_throttle_max, _throttle_max);
}
// set lateral input as a value from -100 to +100
void AP_MotorsUGV::set_lateral(float lateral)
{
_lateral = constrain_float(lateral, -100.0f, 100.0f);
}
// get slew limited throttle
// used by manual mode to avoid bad steering behaviour during transitions from forward to reverse
// same as private slew_limit_throttle method (see below) but does not update throttle state
@ -202,8 +208,8 @@ bool AP_MotorsUGV::have_skid_steering() const
return false;
}
// returns true if omni rover
bool AP_MotorsUGV::is_omni_rover() const
// 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) &&
@ -234,7 +240,7 @@ void AP_MotorsUGV::output(bool armed, float ground_speed, float dt)
output_regular(armed, ground_speed, _steering, _throttle);
// output for omni style frames
output_omni(armed, _steering, _throttle);
output_omni(armed, _steering, _throttle, _lateral);
// output for skid steering style frames
output_skid_steering(armed, _steering, _throttle);
@ -464,36 +470,42 @@ void AP_MotorsUGV::output_regular(bool armed, float ground_speed, float steering
}
// output for omni style frames
void AP_MotorsUGV::output_omni(bool armed, float steering, float throttle)
void AP_MotorsUGV::output_omni(bool armed, float steering, float throttle, float lateral)
{
if (!is_omni_rover()) {
if (!has_lateral_control()) {
return;
}
if (armed) {
// scale throttle and steering to a 1000 to 2000 range for motor throttle calculations
const float scaled_throttle = (throttle - (100)) * (2000 - 1000) / (-100 - (100)) + 1000;
const float scaled_steering = (steering - (-4500)) * (2000 - 1000) / (4500 - (-4500)) + 1000;
// 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
// 1500 is a place-holder value for lateral movement input
const float magnitude = safe_sqrt((scaled_throttle*scaled_throttle)+(1500*1500));
const float theta = atan2f(scaled_throttle,1500);
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(?) * magnitude and vy = sin(?) * magnitude
// 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 and scale it back to a -100 to 100 range
// calculations are done using the following equations: MOTOR1 = –vx, MOTOR2 = 0.5 * v – v(3/2) * vy, MOTOR 3 = 0.5 * vx + v(3/2) * vy
// 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
const int16_t motor_1 = (((-Vx) + scaled_steering) - (2500)) * (100 - (-100)) / (3500 - (2500)) + (-100);
const int16_t motor_2 = ((((0.5*Vx)-((safe_sqrt(3)/2)*Vy)) + scaled_steering) - (1121)) * (100 - (-100)) / (2973 - (1121)) + (-100);
const int16_t motor_3 = ((((0.5*Vx)+((safe_sqrt(3)/2)*Vy)) + scaled_steering) - (-1468)) * (100 - (-100)) / (383 - (-1468)) + (-100);
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);
// send pwm value to each motor
output_throttle(SRV_Channel::k_motor1, motor_1);
output_throttle(SRV_Channel::k_motor2, motor_2);
output_throttle(SRV_Channel::k_motor3, motor_3);
// 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) {

10
APMrover2/AP_MotorsUGV.h
View File

@ -44,6 +44,9 @@ public:
float get_throttle() const { return _throttle; }
void set_throttle(float throttle);
// set lateral input as a value from -100 to +100
void set_lateral(float lateral);
// get slew limited throttle
// used by manual mode to avoid bad steering behaviour during transitions from forward to reverse
// same as private slew_limit_throttle method (see below) but does not update throttle state
@ -52,8 +55,8 @@ public:
// true if vehicle is capable of skid steering
bool have_skid_steering() const;
//true if vehicle is an omni rover
bool is_omni_rover() 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); }
@ -96,7 +99,7 @@ protected:
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);
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);
@ -131,4 +134,5 @@ 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
};

1
APMrover2/Rover.cpp
View File

@ -27,6 +27,7 @@ Rover::Rover(void) :
channel_steer(nullptr),
channel_throttle(nullptr),
channel_aux(nullptr),
channel_lateral(nullptr),
DataFlash{fwver.fw_string, g.log_bitmask},
modes(&g.mode1),
nav_controller(&L1_controller),

1
APMrover2/Rover.h
View File

@ -155,6 +155,7 @@ private:
RC_Channel *channel_steer;
RC_Channel *channel_throttle;
RC_Channel *channel_aux;
RC_Channel *channel_lateral;
DataFlash_Class DataFlash;

14
APMrover2/mode.cpp
View File

@ -7,6 +7,7 @@ Mode::Mode() :
g2(rover.g2),
channel_steer(rover.channel_steer),
channel_throttle(rover.channel_throttle),
channel_lateral(rover.channel_lateral),
mission(rover.mission),
attitude_control(rover.g2.attitude_control)
{ }
@ -124,6 +125,19 @@ void Mode::get_pilot_desired_steering_and_speed(float &steering_out, float &spee
speed_out = speed_out_limited;
}
// decode pilot lateral movement input and return in lateral_out argument
void Mode::get_pilot_desired_lateral(float &lateral_out)
{
// no RC input means no lateral input
if (rover.failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
lateral_out = 0;
return;
}
// get pilot lateral input
lateral_out = rover.channel_lateral->get_control_in();
}
// set desired location
void Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
{

8
APMrover2/mode.h
View File

@ -112,6 +112,9 @@ protected:
// decode pilot input steering and return steering_out and speed_out (in m/s)
void get_pilot_desired_steering_and_speed(float &steering_out, float &speed_out);
// decode pilot lateral movement input and return in lateral_out argument
void get_pilot_desired_lateral(float &lateral_out);
// calculate steering output to drive along line from origin to destination waypoint
void calc_steering_to_waypoint(const struct Location &origin, const struct Location &destination, bool reversed = false);
@ -159,6 +162,7 @@ protected:
class ParametersG2 &g2;
class RC_Channel *&channel_steer;
class RC_Channel *&channel_throttle;
class RC_Channel *&channel_lateral;
class AP_Mission &mission;
class AR_AttitudeControl &attitude_control;
@ -352,6 +356,10 @@ public:
// manual mode does not require position or velocity estimate
bool requires_position() const override { return false; }
bool requires_velocity() const override { return false; }
protected:
void _exit() override;
};

10
APMrover2/mode_manual.cpp
View File

@ -1,12 +1,20 @@
#include "mode.h"
#include "Rover.h"
void ModeManual::_exit()
{
// clear lateral when exiting manual mode
g2.motors.set_lateral(0);
}
void ModeManual::update()
{
float desired_steering, desired_throttle;
float desired_steering, desired_throttle, desired_lateral;
get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle);
get_pilot_desired_lateral(desired_lateral);
// copy RC scaled inputs to outputs
g2.motors.set_throttle(desired_throttle);
g2.motors.set_steering(desired_steering, false);
g2.motors.set_lateral(desired_lateral);
}

3
APMrover2/radio.cpp
View File

@ -9,10 +9,12 @@ void Rover::set_control_channels(void)
channel_steer = RC_Channels::rc_channel(rcmap.roll()-1);
channel_throttle = RC_Channels::rc_channel(rcmap.throttle()-1);
channel_aux = RC_Channels::rc_channel(g.aux_channel-1);
channel_lateral = RC_Channels::rc_channel(rcmap.yaw()-1);
// set rc channel ranges
channel_steer->set_angle(SERVO_MAX);
channel_throttle->set_angle(100);
channel_lateral->set_angle(100);
// Allow to reconfigure ouput when not armed
if (!arming.is_armed()) {
@ -32,6 +34,7 @@ void Rover::init_rc_in()
// set rc dead zones
channel_steer->set_default_dead_zone(30);
channel_throttle->set_default_dead_zone(30);
channel_lateral->set_default_dead_zone(30);
}
void Rover::init_rc_out()