void Plane::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) { Location cmdloc = cmd.content.location; location_sanitize(current_loc, cmdloc); set_next_WP(cmdloc); loiter_set_direction_wp(cmd); }
void Plane::do_loiter_time(const AP_Mission::Mission_Command& cmd) { set_next_WP(cmd.content.location); // we set start_time_ms when we reach the waypoint loiter.time_max_ms = cmd.p1 * (uint32_t)1000; // units are seconds loiter_set_direction_wp(cmd); }
void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd) { //set target alt next_WP_loc.alt = cmd.content.location.alt; // convert relative alt to absolute alt if (cmd.content.location.flags.relative_alt) { next_WP_loc.flags.relative_alt = false; next_WP_loc.alt += home.alt; } //I know I'm storing this twice -- I'm doing that on purpose -- //see verify_loiter_alt() function condition_value = next_WP_loc.alt; //are lat and lon 0? if so, don't change the current wp lat/lon if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) { set_next_WP(cmd.content.location); } //set loiter direction loiter_set_direction_wp(cmd); //must the plane be heading towards the next waypoint before breaking? condition_value2 = LOWBYTE(cmd.p1); }
void Rover::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) { Location cmdloc = cmd.content.location; location_sanitize(current_loc, cmdloc); set_next_WP(cmdloc); loiter_time_max = 100; // an arbitrary large loiter time distance_past_wp = 0; }
void Plane::do_loiter_turns(const AP_Mission::Mission_Command& cmd) { Location cmdloc = cmd.content.location; location_sanitize(current_loc, cmdloc); set_next_WP(cmdloc); loiter_set_direction_wp(cmd); loiter.total_cd = (uint32_t)(LOWBYTE(cmd.p1)) * 36000UL; condition_value = 1; // used to signify primary turns goal not yet met }
void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd) { //set target alt Location loc = cmd.content.location; location_sanitize(current_loc, loc); set_next_WP(loc); loiter_set_direction_wp(cmd); // init to 0, set to 1 when altitude is reached condition_value = 0; }
void Plane::do_loiter_time(const AP_Mission::Mission_Command& cmd) { Location cmdloc = cmd.content.location; location_sanitize(current_loc, cmdloc); set_next_WP(cmdloc); loiter_set_direction_wp(cmd); // we set start_time_ms when we reach the waypoint loiter.time_max_ms = cmd.p1 * (uint32_t)1000; // convert sec to ms condition_value = 1; // used to signify primary time goal not yet met }
void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd) { // this will be used to remember the time in millis after we reach or pass the WP. loiter_time = 0; // this is the delay, stored in seconds loiter_time_max = cmd.p1; // this is the distance we travel past the waypoint - not there yet so 0 initially distance_past_wp = 0; set_next_WP(cmd.content.location); }
void Plane::do_takeoff(const AP_Mission::Mission_Command& cmd) { prev_WP_loc = current_loc; set_next_WP(cmd.content.location); // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 auto_state.takeoff_pitch_cd = (int16_t)cmd.p1 * 100; auto_state.takeoff_altitude_rel_cm = next_WP_loc.alt - home.alt; next_WP_loc.lat = home.lat + 10; next_WP_loc.lng = home.lng + 10; auto_state.takeoff_speed_time_ms = 0; auto_state.takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction // Flag also used to override "on the ground" throttle disable // zero locked course steer_state.locked_course_err = 0; }
void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd) { // just starting so we haven't previously reached the waypoint previously_reached_wp = false; // this will be used to remember the time in millis after we reach or pass the WP. loiter_start_time = 0; // this is the delay, stored in seconds loiter_duration = cmd.p1; // this is the distance we travel past the waypoint - not there yet so 0 initially distance_past_wp = 0; Location cmdloc = cmd.content.location; location_sanitize(current_loc, cmdloc); set_next_WP(cmdloc); }
void Plane::do_land(const AP_Mission::Mission_Command& cmd) { set_next_WP(cmd.content.location); // configure abort altitude and pitch // if NAV_LAND has an abort altitude then use it, else use last takeoff, else use 50m if (cmd.p1 > 0) { auto_state.takeoff_altitude_rel_cm = (int16_t)cmd.p1 * 100; } else if (auto_state.takeoff_altitude_rel_cm <= 0) { auto_state.takeoff_altitude_rel_cm = 3000; } if (auto_state.takeoff_pitch_cd <= 0) { // If no takeoff command has ever been used, default to a conservative 10deg auto_state.takeoff_pitch_cd = 1000; } // zero rangefinder state, start to accumulate good samples now memset(&rangefinder_state, 0, sizeof(rangefinder_state)); landing.do_land(cmd, relative_altitude); if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { // if we were in an abort we need to explicitly move out of the abort state, as it's sticky set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND); } #if GEOFENCE_ENABLED == ENABLED if (g.fence_autoenable == 1) { if (! geofence_set_enabled(false, AUTO_TOGGLED)) { gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence failed (autodisable)"); } else { gcs_send_text(MAV_SEVERITY_NOTICE, "Fence disabled (autodisable)"); } } else if (g.fence_autoenable == 2) { if (! geofence_set_floor_enabled(false)) { gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence floor failed (autodisable)"); } else { gcs_send_text(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)"); } } #endif }
void Plane::do_land(const AP_Mission::Mission_Command& cmd) { auto_state.commanded_go_around = false; set_next_WP(cmd.content.location); // configure abort altitude and pitch // if NAV_LAND has an abort altitude then use it, else use last takeoff, else use 50m if (cmd.p1 > 0) { auto_state.takeoff_altitude_rel_cm = (int16_t)cmd.p1 * 100; } else if (auto_state.takeoff_altitude_rel_cm <= 0) { auto_state.takeoff_altitude_rel_cm = 3000; } if (auto_state.takeoff_pitch_cd <= 0) { // If no takeoff command has ever been used, default to a conservative 10deg auto_state.takeoff_pitch_cd = 1000; } #if RANGEFINDER_ENABLED == ENABLED // zero rangefinder state, start to accumulate good samples now memset(&rangefinder_state, 0, sizeof(rangefinder_state)); #endif }
void Plane::do_takeoff(const AP_Mission::Mission_Command& cmd) { prev_WP_loc = current_loc; set_next_WP(cmd.content.location); // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 auto_state.takeoff_pitch_cd = (int16_t)cmd.p1 * 100; if (auto_state.takeoff_pitch_cd <= 0) { // if the mission doesn't specify a pitch use 4 degrees auto_state.takeoff_pitch_cd = 400; } auto_state.takeoff_altitude_rel_cm = next_WP_loc.alt - home.alt; next_WP_loc.lat = home.lat + 10; next_WP_loc.lng = home.lng + 10; auto_state.takeoff_speed_time_ms = 0; auto_state.takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction auto_state.height_below_takeoff_to_level_off_cm = 0; // Flag also used to override "on the ground" throttle disable // zero locked course steer_state.locked_course_err = 0; steer_state.hold_course_cd = -1; auto_state.baro_takeoff_alt = barometer.get_altitude(); }
void Plane::do_loiter_turns(const AP_Mission::Mission_Command& cmd) { set_next_WP(cmd.content.location); loiter.total_cd = (uint32_t)(LOWBYTE(cmd.p1)) * 36000UL; loiter_set_direction_wp(cmd); }
void Plane::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) { set_next_WP(cmd.content.location); loiter_set_direction_wp(cmd); }
void Plane::do_land(const AP_Mission::Mission_Command& cmd) { auto_state.commanded_go_around = false; set_next_WP(cmd.content.location); }
void Plane::do_nav_wp(const AP_Mission::Mission_Command& cmd) { set_next_WP(cmd.content.location); }