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

Rover: Whitespace/formatting change ONLY for Steering.cpp

This commit is contained in:
Grant Morphett 2015-11-05 14:34:24 +11:00 committed by Andrew Tridgell
parent bbbbd07935
commit 039ea59d7e
1 changed files with 51 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

109
APMrover2/Steering.cpp
View File

@ -3,12 +3,11 @@
#include "Rover.h"
/*****************************************
* Throttle slew limit
Throttle slew limit
*****************************************/
void Rover::throttle_slew_limit(int16_t last_throttle)
{
void Rover::throttle_slew_limit(int16_t last_throttle) {
// if slew limit rate is set to zero then do not slew limit
if (g.throttle_slewrate && last_throttle != 0) {
if (g.throttle_slewrate && last_throttle != 0) {
// limit throttle change by the given percentage per second
float temp = g.throttle_slewrate * G_Dt * 0.01f * fabsf(channel_throttle->radio_max - channel_throttle->radio_min);
// allow a minimum change of 1 PWM per cycle
@ -20,10 +19,9 @@ void Rover::throttle_slew_limit(int16_t last_throttle)
}
/*
check for triggering of start of auto mode
*/
bool Rover::auto_check_trigger(void)
{
check for triggering of start of auto mode
*/
bool Rover::auto_check_trigger(void) {
// only applies to AUTO mode
if (control_mode != AUTO) {
return true;
@ -33,7 +31,7 @@ bool Rover::auto_check_trigger(void)
if (auto_triggered && g.auto_trigger_pin != -1 && check_digital_pin(g.auto_trigger_pin) == 1) {
gcs_send_text(MAV_SEVERITY_WARNING, "AUTO triggered off");
auto_triggered = false;
return false;
return false;
}
// if already triggered, then return true, so you don't
@ -47,11 +45,11 @@ bool Rover::auto_check_trigger(void)
auto_triggered = true;
return true;
}
if (g.auto_trigger_pin != -1 && check_digital_pin(g.auto_trigger_pin) == 0) {
gcs_send_text(MAV_SEVERITY_WARNING, "Triggered AUTO with pin");
auto_triggered = true;
return true;
return true;
}
if (!is_zero(g.auto_kickstart)) {
@ -59,18 +57,17 @@ bool Rover::auto_check_trigger(void)
if (xaccel >= g.auto_kickstart) {
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Triggered AUTO xaccel=%.1f", (double)xaccel);
auto_triggered = true;
return true;
return true;
}
}
return false;
return false;
}
/*
work out if we are going to use pivot steering
*/
bool Rover::use_pivot_steering(void)
{
work out if we are going to use pivot steering
*/
bool Rover::use_pivot_steering(void) {
if (control_mode >= AUTO && g.skid_steer_out && g.pivot_turn_angle != 0) {
int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100;
if (abs(bearing_error) > g.pivot_turn_angle) {
@ -82,10 +79,9 @@ bool Rover::use_pivot_steering(void)
/*
calculate the throtte for auto-throttle modes
*/
void Rover::calc_throttle(float target_speed)
{
calculate the throtte for auto-throttle modes
*/
void Rover::calc_throttle(float target_speed) {
// If not autostarting OR we are loitering at a waypoint
// then set the throttle to minimum
if (!auto_check_trigger() || ((loiter_time > 0) && (control_mode == AUTO))) {
@ -94,11 +90,11 @@ void Rover::calc_throttle(float target_speed)
}
float throttle_base = (fabsf(target_speed) / g.speed_cruise) * g.throttle_cruise;
int throttle_target = throttle_base + throttle_nudge;
int throttle_target = throttle_base + throttle_nudge;
/*
reduce target speed in proportion to turning rate, up to the
SPEED_TURN_GAIN percentage.
reduce target speed in proportion to turning rate, up to the
SPEED_TURN_GAIN percentage.
*/
float steer_rate = fabsf(lateral_acceleration / (g.turn_max_g*GRAVITY_MSS));
steer_rate = constrain_float(steer_rate, 0.0f, 1.0f);
@ -110,7 +106,7 @@ void Rover::calc_throttle(float target_speed)
float speed_turn_reduction = (100 - g.speed_turn_gain) * speed_turn_ratio * 0.01f;
float reduction = 1.0f - steer_rate*speed_turn_reduction;
if (control_mode >= AUTO && wp_distance <= g.speed_turn_dist) {
// in auto-modes we reduce speed when approaching waypoints
float reduction2 = 1.0f - speed_turn_reduction;
@ -118,12 +114,12 @@ void Rover::calc_throttle(float target_speed)
reduction = reduction2;
}
}
// reduce the target speed by the reduction factor
target_speed *= reduction;
groundspeed_error = fabsf(target_speed) - ground_speed;
groundspeed_error = fabsf(target_speed) - ground_speed;
throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100) / 100);
// also reduce the throttle by the reduction factor. This gives a
@ -152,18 +148,17 @@ void Rover::calc_throttle(float target_speed)
// go negative
set_reverse(true);
}
if (use_pivot_steering()) {
channel_throttle->servo_out = 0;
}
}
/*****************************************
* Calculate desired turn angles (in medium freq loop)
Calculate desired turn angles (in medium freq loop)
*****************************************/
void Rover::calc_lateral_acceleration()
{
void Rover::calc_lateral_acceleration() {
switch (control_mode) {
case AUTO:
nav_controller->update_waypoint(prev_WP, next_WP);
@ -178,10 +173,10 @@ void Rover::calc_lateral_acceleration()
return;
}
// Calculate the required turn of the wheels
// Calculate the required turn of the wheels
// negative error = left turn
// positive error = right turn
// positive error = right turn
lateral_acceleration = nav_controller->lateral_acceleration();
if (use_pivot_steering()) {
int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100;
@ -194,10 +189,9 @@ void Rover::calc_lateral_acceleration()
}
/*
calculate steering angle given lateral_acceleration
*/
void Rover::calc_nav_steer()
{
calculate steering angle given lateral_acceleration
*/
void Rover::calc_nav_steer() {
// check to see if the rover is loitering
if ((loiter_time > 0) && (control_mode == AUTO)) {
channel_steer->servo_out = 0;
@ -214,16 +208,15 @@ void Rover::calc_nav_steer()
}
/*****************************************
* Set the flight control servos based on the current calculated values
Set the flight control servos based on the current calculated values
*****************************************/
void Rover::set_servos(void)
{
void Rover::set_servos(void) {
static int16_t last_throttle;
// support a separate steering channel
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_steering, channel_steer->pwm_to_angle_dz(0));
if (control_mode == MANUAL || control_mode == LEARNING) {
if (control_mode == MANUAL || control_mode == LEARNING) {
// do a direct pass through of radio values
channel_steer->radio_out = channel_steer->read();
channel_throttle->radio_out = channel_throttle->read();
@ -231,16 +224,16 @@ void Rover::set_servos(void)
// suppress throttle if in failsafe and manual
channel_throttle->radio_out = channel_throttle->radio_trim;
}
} else {
} else {
channel_steer->calc_pwm();
if (in_reverse) {
channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out,
-g.throttle_max,
-g.throttle_min);
channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out,
-g.throttle_max,
-g.throttle_min);
} else {
channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out,
g.throttle_min.get(),
g.throttle_max.get());
channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out,
g.throttle_min.get(),
g.throttle_max.get());
}
if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) {
@ -265,12 +258,12 @@ void Rover::set_servos(void)
if (g.skid_steer_out) {
// convert the two radio_out values to skid steering values
/*
mixing rule:
steering = motor1 - motor2
throttle = 0.5*(motor1 + motor2)
motor1 = throttle + 0.5*steering
motor2 = throttle - 0.5*steering
*/
mixing rule:
steering = motor1 - motor2
throttle = 0.5*(motor1 + motor2)
motor1 = throttle + 0.5*steering
motor2 = throttle - 0.5*steering
*/
float steering_scaled = channel_steer->norm_output();
float throttle_scaled = channel_throttle->norm_output();
float motor1 = throttle_scaled + 0.5f*steering_scaled;
@ -302,9 +295,9 @@ void Rover::set_servos(void)
}
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
// send values to the PWM timers for output
// ----------------------------------------
channel_steer->output();
// send values to the PWM timers for output
// ----------------------------------------
channel_steer->output();
channel_throttle->output();
RC_Channel_aux::output_ch_all();
#endif