2012-04-30 04:17:14 -03:00
|
|
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
|
2015-05-13 00:16:45 -03:00
|
|
|
#include "Rover.h"
|
|
|
|
|
2012-11-27 20:42:22 -04:00
|
|
|
/*****************************************
|
2015-11-04 23:34:24 -04:00
|
|
|
Throttle slew limit
|
2012-11-27 20:42:22 -04:00
|
|
|
*****************************************/
|
2015-11-04 23:34:24 -04:00
|
|
|
void Rover::throttle_slew_limit(int16_t last_throttle) {
|
2012-11-27 20:42:22 -04:00
|
|
|
// if slew limit rate is set to zero then do not slew limit
|
2015-11-04 23:34:24 -04:00
|
|
|
if (g.throttle_slewrate && last_throttle != 0) {
|
2012-11-27 20:42:22 -04:00
|
|
|
// limit throttle change by the given percentage per second
|
2013-06-03 06:33:59 -03:00
|
|
|
float temp = g.throttle_slewrate * G_Dt * 0.01f * fabsf(channel_throttle->radio_max - channel_throttle->radio_min);
|
2012-11-27 21:13:39 -04:00
|
|
|
// allow a minimum change of 1 PWM per cycle
|
|
|
|
if (temp < 1) {
|
|
|
|
temp = 1;
|
|
|
|
}
|
2013-06-03 06:33:59 -03:00
|
|
|
channel_throttle->radio_out = constrain_int16(channel_throttle->radio_out, last_throttle - temp, last_throttle + temp);
|
2012-11-27 20:42:22 -04:00
|
|
|
}
|
|
|
|
}
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2013-03-21 19:38:25 -03:00
|
|
|
/*
|
2015-11-04 23:34:24 -04:00
|
|
|
check for triggering of start of auto mode
|
|
|
|
*/
|
|
|
|
bool Rover::auto_check_trigger(void) {
|
2013-03-21 19:38:25 -03:00
|
|
|
// only applies to AUTO mode
|
|
|
|
if (control_mode != AUTO) {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2013-05-02 20:19:20 -03:00
|
|
|
// check for user pressing the auto trigger to off
|
|
|
|
if (auto_triggered && g.auto_trigger_pin != -1 && check_digital_pin(g.auto_trigger_pin) == 1) {
|
2015-10-24 19:36:35 -03:00
|
|
|
gcs_send_text(MAV_SEVERITY_WARNING, "AUTO triggered off");
|
2013-05-02 20:19:20 -03:00
|
|
|
auto_triggered = false;
|
2015-11-04 23:34:24 -04:00
|
|
|
return false;
|
2013-05-02 20:19:20 -03:00
|
|
|
}
|
|
|
|
|
2013-03-21 19:38:25 -03:00
|
|
|
// if already triggered, then return true, so you don't
|
|
|
|
// need to hold the switch down
|
|
|
|
if (auto_triggered) {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2015-05-04 23:34:02 -03:00
|
|
|
if (g.auto_trigger_pin == -1 && is_zero(g.auto_kickstart)) {
|
2013-03-21 19:38:25 -03:00
|
|
|
// no trigger configured - let's go!
|
|
|
|
auto_triggered = true;
|
|
|
|
return true;
|
|
|
|
}
|
2015-11-04 23:34:24 -04:00
|
|
|
|
2013-05-02 20:19:20 -03:00
|
|
|
if (g.auto_trigger_pin != -1 && check_digital_pin(g.auto_trigger_pin) == 0) {
|
2015-10-24 19:36:35 -03:00
|
|
|
gcs_send_text(MAV_SEVERITY_WARNING, "Triggered AUTO with pin");
|
2013-05-02 20:19:20 -03:00
|
|
|
auto_triggered = true;
|
2015-11-04 23:34:24 -04:00
|
|
|
return true;
|
2013-03-21 19:38:25 -03:00
|
|
|
}
|
|
|
|
|
2015-05-04 23:34:02 -03:00
|
|
|
if (!is_zero(g.auto_kickstart)) {
|
2013-03-21 21:54:04 -03:00
|
|
|
float xaccel = ins.get_accel().x;
|
|
|
|
if (xaccel >= g.auto_kickstart) {
|
2015-11-03 01:19:03 -04:00
|
|
|
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Triggered AUTO xaccel=%.1f", (double)xaccel);
|
2013-03-21 19:38:25 -03:00
|
|
|
auto_triggered = true;
|
2015-11-04 23:34:24 -04:00
|
|
|
return true;
|
2013-03-21 19:38:25 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2015-11-04 23:34:24 -04:00
|
|
|
return false;
|
2013-03-21 19:38:25 -03:00
|
|
|
}
|
|
|
|
|
2014-02-16 19:08:59 -04:00
|
|
|
/*
|
2015-11-04 23:34:24 -04:00
|
|
|
work out if we are going to use pivot steering
|
|
|
|
*/
|
|
|
|
bool Rover::use_pivot_steering(void) {
|
2014-02-23 17:23:20 -04:00
|
|
|
if (control_mode >= AUTO && g.skid_steer_out && g.pivot_turn_angle != 0) {
|
2014-02-16 19:08:59 -04:00
|
|
|
int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100;
|
|
|
|
if (abs(bearing_error) > g.pivot_turn_angle) {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2013-03-21 19:38:25 -03:00
|
|
|
|
2013-03-01 20:03:15 -04:00
|
|
|
/*
|
2015-11-04 23:34:24 -04:00
|
|
|
calculate the throtte for auto-throttle modes
|
|
|
|
*/
|
|
|
|
void Rover::calc_throttle(float target_speed) {
|
2015-07-27 09:10:37 -03:00
|
|
|
// 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))) {
|
2013-06-03 06:33:59 -03:00
|
|
|
channel_throttle->servo_out = g.throttle_min.get();
|
2013-03-21 19:38:25 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2013-11-17 19:58:22 -04:00
|
|
|
float throttle_base = (fabsf(target_speed) / g.speed_cruise) * g.throttle_cruise;
|
2015-11-04 23:34:24 -04:00
|
|
|
int throttle_target = throttle_base + throttle_nudge;
|
2013-03-01 07:32:57 -04:00
|
|
|
|
2013-03-01 20:03:15 -04:00
|
|
|
/*
|
2015-11-04 23:34:24 -04:00
|
|
|
reduce target speed in proportion to turning rate, up to the
|
|
|
|
SPEED_TURN_GAIN percentage.
|
2013-03-01 07:32:57 -04:00
|
|
|
*/
|
2013-06-16 20:50:53 -03:00
|
|
|
float steer_rate = fabsf(lateral_acceleration / (g.turn_max_g*GRAVITY_MSS));
|
2015-05-02 06:17:06 -03:00
|
|
|
steer_rate = constrain_float(steer_rate, 0.0f, 1.0f);
|
2014-04-06 20:30:39 -03:00
|
|
|
|
|
|
|
// use g.speed_turn_gain for a 90 degree turn, and in proportion
|
|
|
|
// for other turn angles
|
|
|
|
int32_t turn_angle = wrap_180_cd(next_navigation_leg_cd - ahrs.yaw_sensor);
|
|
|
|
float speed_turn_ratio = constrain_float(fabsf(turn_angle / 9000.0f), 0, 1);
|
|
|
|
float speed_turn_reduction = (100 - g.speed_turn_gain) * speed_turn_ratio * 0.01f;
|
|
|
|
|
2015-05-02 06:17:06 -03:00
|
|
|
float reduction = 1.0f - steer_rate*speed_turn_reduction;
|
2015-11-04 23:34:24 -04:00
|
|
|
|
2013-03-01 20:03:15 -04:00
|
|
|
if (control_mode >= AUTO && wp_distance <= g.speed_turn_dist) {
|
|
|
|
// in auto-modes we reduce speed when approaching waypoints
|
2015-05-02 06:17:06 -03:00
|
|
|
float reduction2 = 1.0f - speed_turn_reduction;
|
2013-03-01 20:03:15 -04:00
|
|
|
if (reduction2 < reduction) {
|
|
|
|
reduction = reduction2;
|
|
|
|
}
|
|
|
|
}
|
2015-11-04 23:34:24 -04:00
|
|
|
|
2013-03-01 20:03:15 -04:00
|
|
|
// reduce the target speed by the reduction factor
|
|
|
|
target_speed *= reduction;
|
|
|
|
|
2015-11-04 23:34:24 -04:00
|
|
|
groundspeed_error = fabsf(target_speed) - ground_speed;
|
|
|
|
|
2013-03-01 20:03:15 -04:00
|
|
|
throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100) / 100);
|
2013-03-01 07:32:57 -04:00
|
|
|
|
2013-03-01 20:03:15 -04:00
|
|
|
// also reduce the throttle by the reduction factor. This gives a
|
|
|
|
// much faster response in turns
|
|
|
|
throttle *= reduction;
|
2013-03-01 07:32:57 -04:00
|
|
|
|
2013-11-17 19:58:22 -04:00
|
|
|
if (in_reverse) {
|
|
|
|
channel_throttle->servo_out = constrain_int16(-throttle, -g.throttle_max, -g.throttle_min);
|
|
|
|
} else {
|
|
|
|
channel_throttle->servo_out = constrain_int16(throttle, g.throttle_min, g.throttle_max);
|
|
|
|
}
|
2014-03-30 18:44:19 -03:00
|
|
|
|
2014-04-06 19:42:54 -03:00
|
|
|
if (!in_reverse && g.braking_percent != 0 && groundspeed_error < -g.braking_speederr) {
|
2014-03-30 18:44:19 -03:00
|
|
|
// the user has asked to use reverse throttle to brake. Apply
|
|
|
|
// it in proportion to the ground speed error, but only when
|
2014-04-06 19:42:54 -03:00
|
|
|
// our ground speed error is more than BRAKING_SPEEDERR.
|
|
|
|
//
|
|
|
|
// We use a linear gain, with 0 gain at a ground speed error
|
|
|
|
// of braking_speederr, and 100% gain when groundspeed_error
|
|
|
|
// is 2*braking_speederr
|
|
|
|
float brake_gain = constrain_float(((-groundspeed_error)-g.braking_speederr)/g.braking_speederr, 0, 1);
|
2014-03-30 18:44:19 -03:00
|
|
|
int16_t braking_throttle = g.throttle_max * (g.braking_percent * 0.01f) * brake_gain;
|
|
|
|
channel_throttle->servo_out = constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min);
|
2014-10-08 18:59:26 -03:00
|
|
|
|
2014-03-30 18:44:19 -03:00
|
|
|
// temporarily set us in reverse to allow the PWM setting to
|
|
|
|
// go negative
|
|
|
|
set_reverse(true);
|
|
|
|
}
|
2015-11-04 23:34:24 -04:00
|
|
|
|
2014-02-16 19:08:59 -04:00
|
|
|
if (use_pivot_steering()) {
|
|
|
|
channel_throttle->servo_out = 0;
|
|
|
|
}
|
2012-04-30 04:17:14 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/*****************************************
|
2015-11-04 23:34:24 -04:00
|
|
|
Calculate desired turn angles (in medium freq loop)
|
2012-04-30 04:17:14 -03:00
|
|
|
*****************************************/
|
|
|
|
|
2015-11-04 23:34:24 -04:00
|
|
|
void Rover::calc_lateral_acceleration() {
|
2013-06-16 20:50:53 -03:00
|
|
|
switch (control_mode) {
|
|
|
|
case AUTO:
|
2014-03-17 04:44:25 -03:00
|
|
|
nav_controller->update_waypoint(prev_WP, next_WP);
|
2013-06-16 20:50:53 -03:00
|
|
|
break;
|
|
|
|
|
|
|
|
case RTL:
|
|
|
|
case GUIDED:
|
|
|
|
case STEERING:
|
2014-03-17 04:44:25 -03:00
|
|
|
nav_controller->update_waypoint(current_loc, next_WP);
|
2013-06-16 20:50:53 -03:00
|
|
|
break;
|
|
|
|
default:
|
|
|
|
return;
|
2013-03-28 19:14:58 -03:00
|
|
|
}
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2015-11-04 23:34:24 -04:00
|
|
|
// Calculate the required turn of the wheels
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2012-11-29 03:09:05 -04:00
|
|
|
// negative error = left turn
|
2015-11-04 23:34:24 -04:00
|
|
|
// positive error = right turn
|
2013-06-16 20:50:53 -03:00
|
|
|
lateral_acceleration = nav_controller->lateral_acceleration();
|
2014-02-16 19:08:59 -04:00
|
|
|
if (use_pivot_steering()) {
|
|
|
|
int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100;
|
|
|
|
if (bearing_error > 0) {
|
|
|
|
lateral_acceleration = g.turn_max_g*GRAVITY_MSS;
|
|
|
|
} else {
|
|
|
|
lateral_acceleration = -g.turn_max_g*GRAVITY_MSS;
|
|
|
|
}
|
|
|
|
}
|
2013-06-16 20:50:53 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
2015-11-04 23:34:24 -04:00
|
|
|
calculate steering angle given lateral_acceleration
|
|
|
|
*/
|
|
|
|
void Rover::calc_nav_steer() {
|
2015-07-27 09:10:37 -03:00
|
|
|
// check to see if the rover is loitering
|
|
|
|
if ((loiter_time > 0) && (control_mode == AUTO)) {
|
|
|
|
channel_steer->servo_out = 0;
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2013-06-16 20:50:53 -03:00
|
|
|
// add in obstacle avoidance
|
|
|
|
lateral_acceleration += (obstacle.turn_angle/45.0f) * g.turn_max_g;
|
|
|
|
|
|
|
|
// constrain to max G force
|
|
|
|
lateral_acceleration = constrain_float(lateral_acceleration, -g.turn_max_g*GRAVITY_MSS, g.turn_max_g*GRAVITY_MSS);
|
2012-06-13 15:43:35 -03:00
|
|
|
|
2013-10-03 08:54:27 -03:00
|
|
|
channel_steer->servo_out = steerController.get_steering_out_lat_accel(lateral_acceleration);
|
2012-04-30 04:17:14 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/*****************************************
|
2015-11-04 23:34:24 -04:00
|
|
|
Set the flight control servos based on the current calculated values
|
2012-04-30 04:17:14 -03:00
|
|
|
*****************************************/
|
2015-11-04 23:34:24 -04:00
|
|
|
void Rover::set_servos(void) {
|
2014-10-08 18:59:26 -03:00
|
|
|
static int16_t last_throttle;
|
2012-11-27 21:13:39 -04:00
|
|
|
|
2014-03-04 21:13:35 -04:00
|
|
|
// support a separate steering channel
|
|
|
|
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_steering, channel_steer->pwm_to_angle_dz(0));
|
|
|
|
|
2015-11-04 23:34:24 -04:00
|
|
|
if (control_mode == MANUAL || control_mode == LEARNING) {
|
2013-03-14 21:04:33 -03:00
|
|
|
// do a direct pass through of radio values
|
2013-06-03 06:33:59 -03:00
|
|
|
channel_steer->radio_out = channel_steer->read();
|
|
|
|
channel_throttle->radio_out = channel_throttle->read();
|
2013-03-28 20:25:53 -03:00
|
|
|
if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
|
|
|
|
// suppress throttle if in failsafe and manual
|
2013-06-03 06:33:59 -03:00
|
|
|
channel_throttle->radio_out = channel_throttle->radio_trim;
|
2013-03-28 20:25:53 -03:00
|
|
|
}
|
2015-11-04 23:34:24 -04:00
|
|
|
} else {
|
2013-06-03 06:33:59 -03:00
|
|
|
channel_steer->calc_pwm();
|
2013-11-17 19:58:22 -04:00
|
|
|
if (in_reverse) {
|
2015-11-04 23:34:24 -04:00
|
|
|
channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out,
|
|
|
|
-g.throttle_max,
|
|
|
|
-g.throttle_min);
|
2013-11-17 19:58:22 -04:00
|
|
|
} else {
|
2015-11-04 23:34:24 -04:00
|
|
|
channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out,
|
|
|
|
g.throttle_min.get(),
|
|
|
|
g.throttle_max.get());
|
2013-11-17 19:58:22 -04:00
|
|
|
}
|
2013-03-28 20:25:53 -03:00
|
|
|
|
|
|
|
if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) {
|
|
|
|
// suppress throttle if in failsafe
|
2013-06-03 06:33:59 -03:00
|
|
|
channel_throttle->servo_out = 0;
|
2013-03-28 20:25:53 -03:00
|
|
|
}
|
|
|
|
|
2015-10-30 02:56:41 -03:00
|
|
|
if (!hal.util->get_soft_armed()) {
|
|
|
|
channel_throttle->servo_out = 0;
|
|
|
|
}
|
|
|
|
|
2012-11-27 06:47:30 -04:00
|
|
|
// convert 0 to 100% into PWM
|
2013-06-03 06:33:59 -03:00
|
|
|
channel_throttle->calc_pwm();
|
2012-11-27 21:13:39 -04:00
|
|
|
|
|
|
|
// limit throttle movement speed
|
|
|
|
throttle_slew_limit(last_throttle);
|
2014-10-08 18:59:26 -03:00
|
|
|
}
|
2013-03-14 21:04:33 -03:00
|
|
|
|
2014-10-08 18:59:26 -03:00
|
|
|
// record last throttle before we apply skid steering
|
|
|
|
last_throttle = channel_throttle->radio_out;
|
|
|
|
|
|
|
|
if (g.skid_steer_out) {
|
|
|
|
// convert the two radio_out values to skid steering values
|
|
|
|
/*
|
2015-11-04 23:34:24 -04:00
|
|
|
mixing rule:
|
|
|
|
steering = motor1 - motor2
|
|
|
|
throttle = 0.5*(motor1 + motor2)
|
|
|
|
motor1 = throttle + 0.5*steering
|
|
|
|
motor2 = throttle - 0.5*steering
|
|
|
|
*/
|
2014-10-08 18:59:26 -03:00
|
|
|
float steering_scaled = channel_steer->norm_output();
|
|
|
|
float throttle_scaled = channel_throttle->norm_output();
|
2015-05-02 06:17:06 -03:00
|
|
|
float motor1 = throttle_scaled + 0.5f*steering_scaled;
|
|
|
|
float motor2 = throttle_scaled - 0.5f*steering_scaled;
|
2014-10-08 18:59:26 -03:00
|
|
|
channel_steer->servo_out = 4500*motor1;
|
|
|
|
channel_throttle->servo_out = 100*motor2;
|
|
|
|
channel_steer->calc_pwm();
|
|
|
|
channel_throttle->calc_pwm();
|
2012-11-27 06:47:30 -04:00
|
|
|
}
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2015-10-30 02:56:41 -03:00
|
|
|
if (!arming.is_armed()) {
|
|
|
|
//Some ESCs get noisy (beep error msgs) if PWM == 0.
|
|
|
|
//This little segment aims to avoid this.
|
|
|
|
switch (arming.arming_required()) {
|
|
|
|
case AP_Arming::NO:
|
|
|
|
//keep existing behavior: do nothing to radio_out
|
|
|
|
//(don't disarm throttle channel even if AP_Arming class is)
|
|
|
|
break;
|
|
|
|
|
|
|
|
case AP_Arming::YES_ZERO_PWM:
|
|
|
|
channel_throttle->radio_out = 0;
|
|
|
|
break;
|
|
|
|
|
|
|
|
case AP_Arming::YES_MIN_PWM:
|
|
|
|
default:
|
|
|
|
channel_throttle->radio_out = channel_throttle->radio_trim;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
2012-04-30 04:17:14 -03:00
|
|
|
|
|
|
|
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
|
2015-11-04 23:34:24 -04:00
|
|
|
// send values to the PWM timers for output
|
|
|
|
// ----------------------------------------
|
|
|
|
channel_steer->output();
|
2013-06-03 06:33:59 -03:00
|
|
|
channel_throttle->output();
|
2014-04-02 22:19:25 -03:00
|
|
|
RC_Channel_aux::output_ch_all();
|
2012-04-30 04:17:14 -03:00
|
|
|
#endif
|
|
|
|
}
|
|
|
|
|
2013-09-01 20:46:04 -03:00
|
|
|
|