void Rover::update_navigation()
{
switch (control_mode) {
case MANUAL:
case HOLD:
case LEARNING:
case STEERING:
case INITIALISING:
break;
case AUTO:
mission.update();
if (do_auto_rotation) {
do_yaw_rotation();
}
break;
case RTL:
// no loitering around the wp with the rover, goes direct to the wp position
calc_lateral_acceleration();
calc_nav_steer();
if (verify_RTL()) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
set_mode(HOLD);
}
break;
case GUIDED:
switch (guided_mode) {
case Guided_Angle:
nav_set_yaw_speed();
break;
case Guided_WP:
// no loitering around the wp with the rover, goes direct to the wp position
calc_lateral_acceleration();
calc_nav_steer();
if (rtl_complete || verify_RTL()) {
// we have reached destination so stop where we are
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
lateral_acceleration = 0;
}
break;
default:
gcs_send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
break;
}
break;
}
}
void Rover::update_navigation()
{
switch (control_mode) {
case MANUAL:
case HOLD:
case LEARNING:
case STEERING:
case INITIALISING:
break;
case AUTO:
mission.update();
break;
case RTL:
// no loitering around the wp with the rover, goes direct to the wp position
calc_lateral_acceleration();
calc_nav_steer();
if (verify_RTL()) {
channel_throttle->servo_out = g.throttle_min.get();
set_mode(HOLD);
}
break;
case GUIDED:
// no loitering around the wp with the rover, goes direct to the wp position
calc_lateral_acceleration();
calc_nav_steer();
if (!rtl_complete) {
if (verify_RTL()) {
// we have reached destination so stop where we are
channel_throttle->servo_out = g.throttle_min.get();
channel_steer->servo_out = 0;
lateral_acceleration = 0;
}
}
break;
}
}
void ModeSteering::update()
{
// convert pilot throttle input to desired speed (up to twice the cruise speed)
float target_speed = channel_throttle->get_control_in() * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);
// get speed forward
float speed;
if (!attitude_control.get_forward_speed(speed)) {
// no valid speed so stop
g2.motors.set_throttle(0.0f);
g2.motors.set_steering(0.0f);
lateral_acceleration = 0.0f;
return;
}
// in steering mode we control lateral acceleration directly. We first calculate the maximum lateral
// acceleration at full steering lock for this speed. That is V^2/R where R is the radius of turn.
float max_g_force = speed * speed / MAX(g2.turn_radius, 0.1f);
// constrain to user set TURN_MAX_G
max_g_force = constrain_float(max_g_force, 0.1f, g.turn_max_g * GRAVITY_MSS);
// convert pilot steering input to desired lateral acceleration
lateral_acceleration = max_g_force * (channel_steer->get_control_in() / 4500.0f);
// reverse target lateral acceleration if backing up
bool reversed = false;
if (is_negative(target_speed)) {
reversed = true;
lateral_acceleration = -lateral_acceleration;
}
// mark us as in_reverse when using a negative throttle
rover.set_reverse(reversed);
// run speed to throttle output controller
if (is_zero(target_speed)) {
stop_vehicle();
} else {
// run steering controller
calc_nav_steer(reversed);
calc_throttle(target_speed, false);
}
}
void Rover::update_current_mode(void)
{
switch (control_mode){
case AUTO:
case RTL:
set_reverse(false);
calc_lateral_acceleration();
calc_nav_steer();
calc_throttle(g.speed_cruise);
break;
case GUIDED:
set_reverse(false);
if (rtl_complete || verify_RTL()) {
// we have reached destination so stop where we are
if (channel_throttle->servo_out != g.throttle_min.get()) {
gcs_send_mission_item_reached_message(0);
}
channel_throttle->servo_out = g.throttle_min.get();
channel_steer->servo_out = 0;
lateral_acceleration = 0;
} else {
calc_lateral_acceleration();
calc_nav_steer();
calc_throttle(g.speed_cruise);
}
break;
case STEERING: {
/*
in steering mode we control lateral acceleration
directly. We first calculate the maximum lateral
acceleration at full steering lock for this speed. That is
V^2/R where R is the radius of turn. We get the radius of
turn from half the STEER2SRV_P.
*/
float max_g_force = ground_speed * ground_speed / steerController.get_turn_radius();
// constrain to user set TURN_MAX_G
max_g_force = constrain_float(max_g_force, 0.1f, g.turn_max_g * GRAVITY_MSS);
lateral_acceleration = max_g_force * (channel_steer->pwm_to_angle()/4500.0f);
calc_nav_steer();
// and throttle gives speed in proportion to cruise speed, up
// to 50% throttle, then uses nudging above that.
float target_speed = channel_throttle->pwm_to_angle() * 0.01f * 2 * g.speed_cruise;
set_reverse(target_speed < 0);
if (in_reverse) {
target_speed = constrain_float(target_speed, -g.speed_cruise, 0);
} else {
target_speed = constrain_float(target_speed, 0, g.speed_cruise);
}
calc_throttle(target_speed);
break;
}
case LEARNING:
case MANUAL:
/*
in both MANUAL and LEARNING we pass through the
controls. Setting servo_out here actually doesn't matter, as
we set the exact value in set_servos(), but it helps for
logging
*/
channel_throttle->servo_out = channel_throttle->control_in;
channel_steer->servo_out = channel_steer->pwm_to_angle();
// mark us as in_reverse when using a negative throttle to
// stop AHRS getting off
set_reverse(channel_throttle->servo_out < 0);
break;
case HOLD:
// hold position - stop motors and center steering
channel_throttle->servo_out = 0;
channel_steer->servo_out = 0;
set_reverse(false);
break;
case INITIALISING:
break;
}
}
void Rover::update_current_mode(void)
{
switch (control_mode) {
case AUTO:
case RTL:
if (!in_auto_reverse) {
set_reverse(false);
}
if (!do_auto_rotation) {
calc_lateral_acceleration();
calc_nav_steer();
calc_throttle(g.speed_cruise);
} else {
do_yaw_rotation();
}
break;
case GUIDED: {
if (!in_auto_reverse) {
set_reverse(false);
}
switch (guided_mode) {
case Guided_Angle:
nav_set_yaw_speed();
break;
case Guided_WP:
if (rtl_complete || verify_RTL()) {
// we have reached destination so stop where we are
if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) != g.throttle_min.get()) {
gcs_send_mission_item_reached_message(0);
}
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
lateral_acceleration = 0;
} else {
calc_lateral_acceleration();
calc_nav_steer();
calc_throttle(g.speed_cruise);
Log_Write_GuidedTarget(guided_mode, Vector3f(guided_WP.lat, guided_WP.lng, guided_WP.alt),
Vector3f(g.speed_cruise, SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), 0));
}
break;
default:
gcs_send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
break;
}
break;
}
case STEERING: {
/*
in steering mode we control lateral acceleration
directly. We first calculate the maximum lateral
acceleration at full steering lock for this speed. That is
V^2/R where R is the radius of turn. We get the radius of
turn from half the STEER2SRV_P.
*/
float max_g_force = ground_speed * ground_speed / steerController.get_turn_radius();
// constrain to user set TURN_MAX_G
max_g_force = constrain_float(max_g_force, 0.1f, g.turn_max_g * GRAVITY_MSS);
lateral_acceleration = max_g_force * (channel_steer->get_control_in()/4500.0f);
calc_nav_steer();
// and throttle gives speed in proportion to cruise speed, up
// to 50% throttle, then uses nudging above that.
float target_speed = channel_throttle->get_control_in() * 0.01f * 2 * g.speed_cruise;
set_reverse(target_speed < 0);
if (in_reverse) {
target_speed = constrain_float(target_speed, -g.speed_cruise, 0);
} else {
target_speed = constrain_float(target_speed, 0, g.speed_cruise);
}
calc_throttle(target_speed);
break;
}
case LEARNING:
case MANUAL:
/*
in both MANUAL and LEARNING we pass through the
controls. Setting servo_out here actually doesn't matter, as
we set the exact value in set_servos(), but it helps for
logging
*/
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in());
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, channel_steer->get_control_in());
// mark us as in_reverse when using a negative throttle to
// stop AHRS getting off
set_reverse(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < 0);
break;
case HOLD:
// hold position - stop motors and center steering
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
if (!in_auto_reverse) {
set_reverse(false);
}
break;
case INITIALISING:
break;
}
}
