void Plane::update_loiter(uint16_t radius)
{
if (radius <= 1) {
// if radius is <=1 then use the general loiter radius. if it's small, use default
radius = (abs(g.loiter_radius <= 1)) ? LOITER_RADIUS_DEFAULT : abs(g.loiter_radius);
loiter.direction = (g.loiter_radius < 0) ? -1 : 1;
}
if (loiter.start_time_ms == 0 &&
control_mode == AUTO &&
!auto_state.no_crosstrack &&
get_distance(current_loc, next_WP_loc) > radius*2) {
// if never reached loiter point and using crosstrack and somewhat far away from loiter point
// navigate to it like in auto-mode for normal crosstrack behavior
nav_controller->update_waypoint(prev_WP_loc, next_WP_loc);
} else {
nav_controller->update_loiter(next_WP_loc, radius, loiter.direction);
}
if (loiter.start_time_ms == 0) {
if (nav_controller->reached_loiter_target()) {
// we've reached the target, start the timer
loiter.start_time_ms = millis();
if (control_mode == GUIDED) {
// starting a loiter in GUIDED means we just reached the target point
gcs_send_mission_item_reached_message(0);
}
}
}
}
// verify_command_callback - callback function called from ap-mission at 10hz or higher when a command is being run
// we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
bool Copter::verify_command_callback(const AP_Mission::Mission_Command& cmd)
{
if (control_mode == AUTO) {
bool cmd_complete = verify_command(cmd);
// send message to GCS
if (cmd_complete) {
gcs_send_mission_item_reached_message(cmd.index);
}
return cmd_complete;
}
return false;
}
void Plane::update_loiter(uint16_t radius)
{
if (radius <= 1) {
// if radius is <=1 then use the general loiter radius. if it's small, use default
radius = (abs(g.loiter_radius) <= 1) ? LOITER_RADIUS_DEFAULT : abs(g.loiter_radius);
if (next_WP_loc.flags.loiter_ccw == 1) {
loiter.direction = -1;
} else {
loiter.direction = (g.loiter_radius < 0) ? -1 : 1;
}
}
if (loiter.start_time_ms != 0 &&
quadplane.available() &&
quadplane.guided_mode != 0) {
if (!auto_state.vtol_loiter) {
auto_state.vtol_loiter = true;
// reset loiter start time, so we don't consider the point
// reached till we get much closer
loiter.start_time_ms = 0;
quadplane.guided_start();
}
} else if (loiter.start_time_ms == 0 &&
control_mode == AUTO &&
!auto_state.no_crosstrack &&
get_distance(current_loc, next_WP_loc) > radius*2) {
// if never reached loiter point and using crosstrack and somewhat far away from loiter point
// navigate to it like in auto-mode for normal crosstrack behavior
nav_controller->update_waypoint(prev_WP_loc, next_WP_loc);
} else {
nav_controller->update_loiter(next_WP_loc, radius, loiter.direction);
}
if (loiter.start_time_ms == 0) {
if (reached_loiter_target() ||
auto_state.wp_proportion > 1) {
// we've reached the target, start the timer
loiter.start_time_ms = millis();
if (control_mode == GUIDED) {
// starting a loiter in GUIDED means we just reached the target point
gcs_send_mission_item_reached_message(0);
}
if (quadplane.available() &&
quadplane.guided_mode != 0) {
quadplane.guided_start();
}
}
}
}
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;
}
}
