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; } }