Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 00:28:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_MotorsUGV: positive steering always rotates vehicle right

Browse Source 
also scale steering down with increased speed for regular rovers
add support for disabling scaling of steering
This commit is contained in:
Randy Mackay 2018-05-03 17:54:13 +09:00
parent 8137001a86
commit 3db2cc700e
2 changed files with 48 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
APMrover2/AP_MotorsUGV.cpp
View File

@ -12,6 +12,7 @@
*/
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include "SRV_Channel/SRV_Channel.h"
#include "AP_MotorsUGV.h"
#include "Rover.h"
@ -156,9 +157,12 @@ void AP_MotorsUGV::setup_servo_output()
}
// set steering as a value from -4500 to +4500
void AP_MotorsUGV::set_steering(float steering)
// apply_scaling should be set to false for manual modes where
// no scaling by speed or angle should be performed
void AP_MotorsUGV::set_steering(float steering, bool apply_scaling)
{
_steering = constrain_float(steering, -4500.0f, 4500.0f);
_scale_steering = apply_scaling;
}
// set throttle as a value from -100 to 100
@ -196,7 +200,7 @@ bool AP_MotorsUGV::is_omni_rover() const
return false;
}
void AP_MotorsUGV::output(bool armed, float dt)
void AP_MotorsUGV::output(bool armed, float ground_speed, float dt)
{
// soft-armed overrides passed in armed status
if (!hal.util->get_soft_armed()) {
@ -214,7 +218,7 @@ void AP_MotorsUGV::output(bool armed, float dt)
slew_limit_throttle(dt);
// output for regular steering/throttle style frames
output_regular(armed, _steering, _throttle);
output_regular(armed, ground_speed, _steering, _throttle);
// output for omni style frames
output_omni(armed, _steering, _throttle);
@ -399,15 +403,38 @@ void AP_MotorsUGV::setup_pwm_type()
}
// output to regular steering and throttle channels
void AP_MotorsUGV::output_regular(bool armed, float steering, float throttle)
void AP_MotorsUGV::output_regular(bool armed, float ground_speed, float steering, float throttle)
{
// output to throttle channels
if (armed) {
// vectored thrust handling
if (have_vectored_thrust() && (fabsf(throttle) > _vector_throttle_base)) {
// scale steering down linearly as throttle increases above _vector_throttle_base
const float steering_scalar = constrain_float(_vector_throttle_base / fabsf(throttle), 0.0f, 1.0f);
steering *= steering_scalar;
if (_scale_steering) {
// vectored thrust handling
if (have_vectored_thrust()) {
if (fabsf(throttle) > _vector_throttle_base) {
// scale steering down linearly as throttle increases above _vector_throttle_base
steering *= constrain_float(_vector_throttle_base / fabsf(throttle), 0.0f, 1.0f);
}
} else {
// scale steering down as speed increase above 1m/s
if (fabsf(ground_speed) > 1.0f) {
steering *= (1.0f / fabsf(ground_speed));
} else {
// regular steering rover at low speed so set limits to stop I-term build-up in controllers
if (!have_skid_steering()) {
limit.steer_left = true;
limit.steer_right = true;
}
}
// reverse steering output if backing up
if (is_negative(ground_speed)) {
steering *= -1.0f;
}
}
} else {
// reverse steering direction when backing up
if (is_negative(throttle)) {
steering *= -1.0f;
}
}
output_throttle(SRV_Channel::k_throttle, throttle);
} else {
@ -439,12 +466,12 @@ void AP_MotorsUGV::output_omni(bool armed, float steering, float throttle)
const float magnitude = safe_sqrt((scaled_throttle*scaled_throttle)+(1500*1500));
const float theta = atan2f(scaled_throttle,1500);
// 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(?) * magnitude and vy = sin(?) * 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 – √(3/2) * vy, MOTOR 3 = 0.5 * vx + √(3/2) * vy
// 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
// 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);
@ -502,12 +529,9 @@ void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float thrott
throttle_scaled = throttle_scaled / saturation_value;
}
// reverse steering direction if throttle is negative to mimic regular rovers
const float steering_dir = is_negative(throttle_scaled) ? -1.0f : 1.0f;
// add in throttle and steering
const float motor_left = throttle_scaled + (steering_dir * steering_scaled);
const float motor_right = throttle_scaled - (steering_dir * steering_scaled);
const float motor_left = throttle_scaled + steering_scaled;
const float motor_right = throttle_scaled - steering_scaled;
// send pwm value to each motor
output_throttle(SRV_Channel::k_throttleLeft, 100.0f * motor_left);

11
APMrover2/AP_MotorsUGV.h
View File

@ -35,8 +35,10 @@ public:
void setup_servo_output();
// 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
float get_steering() const { return _steering; }
void set_steering(float steering);
void set_steering(float steering, bool apply_scaling = true);
// get or set throttle as a value from -100 to 100
float get_throttle() const { return _throttle; }
@ -52,7 +54,9 @@ public:
bool have_vectored_thrust() const { return is_positive(_vector_throttle_base); }
// output to motors and steering servos
void output(bool armed, float dt);
// ground_speed should be the vehicle's speed over the surface in m/s
// dt should be expected time between calls to this function
void output(bool armed, float ground_speed, float dt);
// test steering or throttle output as a percentage of the total (range -100 to +100)
// used in response to DO_MOTOR_TEST mavlink command
@ -84,7 +88,7 @@ protected:
void setup_pwm_type();
// output to regular steering and throttle channels
void output_regular(bool armed, float steering, float throttle);
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);
@ -121,4 +125,5 @@ protected:
float _steering; // requested steering as a value from -4500 to +4500
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
};