void Navigator::set_rtl_item() { reset_reached(); switch (_rtl_state) { case RTL_STATE_CLIMB: { memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); float climb_alt = _home_pos.alt + _parameters.rtl_alt; if (_vstatus.condition_landed) { climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); } _mission_item_valid = true; _mission_item.lat = _global_pos.lat; _mission_item.lon = _global_pos.lon; _mission_item.altitude_is_relative = false; _mission_item.altitude = climb_alt; _mission_item.yaw = NAN; _mission_item.loiter_radius = _parameters.loiter_radius; _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_TAKEOFF; _mission_item.acceptance_radius = _parameters.acceptance_radius; _mission_item.time_inside = 0.0f; _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); _pos_sp_triplet.next.valid = false; mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %.1fm above home", (double)(climb_alt - _home_pos.alt)); break; } case RTL_STATE_RETURN: { memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); _mission_item_valid = true; _mission_item.lat = _home_pos.lat; _mission_item.lon = _home_pos.lon; // don't change altitude if (_pos_sp_triplet.previous.valid) { /* if previous setpoint is valid then use it to calculate heading to home */ _mission_item.yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, _mission_item.lat, _mission_item.lon); } else { /* else use current position */ _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon); } _mission_item.loiter_radius = _parameters.loiter_radius; _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.acceptance_radius = _parameters.acceptance_radius; _mission_item.time_inside = 0.0f; _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); _pos_sp_triplet.next.valid = false; mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt)); break; } case RTL_STATE_DESCEND: { memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); _mission_item_valid = true; _mission_item.lat = _home_pos.lat; _mission_item.lon = _home_pos.lon; _mission_item.altitude_is_relative = false; _mission_item.altitude = _home_pos.alt + _parameters.land_alt; _mission_item.yaw = NAN; _mission_item.loiter_radius = _parameters.loiter_radius; _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.acceptance_radius = _parameters.acceptance_radius; _mission_item.time_inside = _parameters.rtl_land_delay < 0.0f ? 0.0f : _parameters.rtl_land_delay; _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = _parameters.rtl_land_delay > -0.001f; _mission_item.origin = ORIGIN_ONBOARD; position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); _pos_sp_triplet.next.valid = false; mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt)); break; } default: { mavlink_log_critical(_mavlink_fd, "#audio: [navigator] error: unknown RTL state: %d", _rtl_state); start_loiter(); break; } } _pos_sp_triplet_updated = true; }
void build_gps_response(uint8_t *buffer, size_t *size) { /* get a local copy of the current sensor values */ struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw); /* get a local copy of the battery data */ struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps); struct gps_module_msg msg; *size = sizeof(msg); memset(&msg, 0, *size); msg.start = START_BYTE; msg.sensor_id = GPS_SENSOR_ID; msg.sensor_text_id = GPS_SENSOR_TEXT_ID; msg.gps_num_sat = gps.satellites_used; /* The GPS fix type: 0 = none, 2 = 2D, 3 = 3D */ msg.gps_fix_char = (uint8_t)(gps.fix_type + 48); msg.gps_fix = (uint8_t)(gps.fix_type + 48); /* No point collecting more data if we don't have a 3D fix yet */ if (gps.fix_type > 2) { /* Current flight direction */ msg.flight_direction = (uint8_t)(gps.cog_rad * M_RAD_TO_DEG_F); /* GPS speed */ uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6f); msg.gps_speed_L = (uint8_t)speed & 0xff; msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff; /* Get latitude in degrees, minutes and seconds */ double lat = ((double)(gps.lat)) * 1e-7d; /* Set the N or S specifier */ msg.latitude_ns = 0; if (lat < 0) { msg.latitude_ns = 1; lat = abs(lat); } int deg; int min; int sec; convert_to_degrees_minutes_seconds(lat, °, &min, &sec); uint16_t lat_min = (uint16_t)(deg * 100 + min); msg.latitude_min_L = (uint8_t)lat_min & 0xff; msg.latitude_min_H = (uint8_t)(lat_min >> 8) & 0xff; uint16_t lat_sec = (uint16_t)(sec); msg.latitude_sec_L = (uint8_t)lat_sec & 0xff; msg.latitude_sec_H = (uint8_t)(lat_sec >> 8) & 0xff; /* Get longitude in degrees, minutes and seconds */ double lon = ((double)(gps.lon)) * 1e-7d; /* Set the E or W specifier */ msg.longitude_ew = 0; if (lon < 0) { msg.longitude_ew = 1; lon = abs(lon); } convert_to_degrees_minutes_seconds(lon, °, &min, &sec); uint16_t lon_min = (uint16_t)(deg * 100 + min); msg.longitude_min_L = (uint8_t)lon_min & 0xff; msg.longitude_min_H = (uint8_t)(lon_min >> 8) & 0xff; uint16_t lon_sec = (uint16_t)(sec); msg.longitude_sec_L = (uint8_t)lon_sec & 0xff; msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff; /* Altitude */ uint16_t alt = (uint16_t)(gps.alt * 1e-3f + 500.0f); msg.altitude_L = (uint8_t)alt & 0xff; msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; /* Get any (and probably only ever one) _home_sub position report */ bool updated; orb_check(_home_sub, &updated); if (updated) { /* get a local copy of the home position data */ struct home_position_s home; memset(&home, 0, sizeof(home)); orb_copy(ORB_ID(home_position), _home_sub, &home); _home_lat = home.lat; _home_lon = home.lon; _home_position_set = true; } /* Distance from home */ if (_home_position_set) { uint16_t dist = (uint16_t)get_distance_to_next_waypoint(_home_lat, _home_lon, lat, lon); msg.distance_L = (uint8_t)dist & 0xff; msg.distance_H = (uint8_t)(dist >> 8) & 0xff; /* Direction back to home */ uint16_t bearing = (uint16_t)(get_bearing_to_next_waypoint(_home_lat, _home_lon, lat, lon) * M_RAD_TO_DEG_F); msg.home_direction = (uint8_t)bearing >> 1; } }
bool FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { bool setpoint = true; calculate_gndspeed_undershoot(current_position, ground_speed, pos_sp_triplet); float eas2tas = 1.0f; // XXX calculate actual number based on current measurements // XXX re-visit float baro_altitude = _global_pos.alt; /* filter speed and altitude for controller */ math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); math::Vector<3> accel_earth = _R_nb * accel_body; _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; /* no throttle limit as default */ float throttle_max = 1.0f; /* AUTONOMOUS FLIGHT */ // XXX this should only execute if auto AND safety off (actuators active), // else integrators should be constantly reset. if (_control_mode.flag_control_position_enabled) { /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); /* restore speed weight, in case changed intermittently (e.g. in landing handling) */ _tecs.set_speed_weight(_parameters.speed_weight); /* current waypoint (the one currently heading for) */ math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon); /* current waypoint (the one currently heading for) */ math::Vector<2> curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon); /* previous waypoint */ math::Vector<2> prev_wp; if (pos_sp_triplet.previous.valid) { prev_wp(0) = (float)pos_sp_triplet.previous.lat; prev_wp(1) = (float)pos_sp_triplet.previous.lon; } else { /* * No valid previous waypoint, go for the current wp. * This is automatically handled by the L1 library. */ prev_wp(0) = (float)pos_sp_triplet.current.lat; prev_wp(1) = (float)pos_sp_triplet.current.lon; } if (pos_sp_triplet.current.type == SETPOINT_TYPE_NORMAL) { /* waypoint is a plain navigation waypoint */ _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) { /* waypoint is a loiter waypoint */ _l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius, pos_sp_triplet.current.loiter_direction, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); /* Horizontal landing control */ /* switch to heading hold for the last meters, continue heading hold after */ float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) { /* heading hold, along the line connecting this and the last waypoint */ if (!land_noreturn_horizontal) {//set target_bearing in first occurrence if (pos_sp_triplet.previous.valid) { target_bearing = bearing_lastwp_currwp; } else { target_bearing = _att.yaw; } mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold"); } // warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); land_noreturn_horizontal = true; } else { /* normal navigation */ _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); } _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); /* Vertical landing control */ //xxx: using the tecs altitude controller for slope control for now // /* do not go down too early */ // if (wp_distance > 50.0f) { // altitude_error = (_global_triplet.current.alt + 25.0f) - _global_pos.alt; // } /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(pos_sp_triplet.current.param1) float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); float L_altitude_rel = landingslope.getLandingSlopeRelativeAltitude(L_wp_distance); float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); float relative_alt = get_relative_landingalt(_pos_sp_triplet.current.alt, _global_pos.alt, _range_finder, _parameters.range_finder_rel_alt); if ( (relative_alt < landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ // /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ // _tecs.set_speed_weight(2.0f); /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; if (relative_alt < landingslope.motor_lim_relative_alt() || land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); if (!land_motor_lim) { land_motor_lim = true; mavlink_log_info(_mavlink_fd, "#audio: Landing, limiting throttle"); } } float flare_curve_alt_rel = landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); /* avoid climbout */ if ((flare_curve_alt_rel_last < flare_curve_alt_rel && land_noreturn_vertical) || land_stayonground) { flare_curve_alt_rel = 0.0f; // stay on ground land_stayonground = true; } _tecs.update_pitch_throttle(_R_nb, _att.pitch, _pos_sp_triplet.current.alt + relative_alt, _pos_sp_triplet.current.alt + flare_curve_alt_rel, calculate_target_airspeed(airspeed_land), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_pitch_angle_rad, 0.0f, throttle_max, throttle_land, flare_pitch_angle_rad, math::radians(15.0f)); if (!land_noreturn_vertical) { mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring"); land_noreturn_vertical = true; } //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); flare_curve_alt_rel_last = flare_curve_alt_rel; } else { /* intersect glide slope: * minimize speed to approach speed * if current position is higher or within 10m of slope follow the glide slope * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope * */ float altitude_desired_rel = relative_alt; if (relative_alt > landing_slope_alt_rel_desired - 10.0f) { /* stay on slope */ altitude_desired_rel = landing_slope_alt_rel_desired; if (!land_onslope) { mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope"); land_onslope = true; } } else { /* continue horizontally */ altitude_desired_rel = math::max(relative_alt, L_altitude_rel); } _tecs.update_pitch_throttle(_R_nb, _att.pitch, _pos_sp_triplet.current.alt + relative_alt, _pos_sp_triplet.current.alt + altitude_desired_rel, calculate_target_airspeed(airspeed_approach), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); } } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) { /* Perform launch detection */ // warnx("Launch detection running"); if(!launch_detected) { //do not do further checks once a launch was detected if (launchDetector.launchDetectionEnabled()) { static hrt_abstime last_sent = 0; if(hrt_absolute_time() - last_sent > 4e6) { // warnx("Launch detection running"); mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); last_sent = hrt_absolute_time(); } launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); if (launchDetector.getLaunchDetected()) { launch_detected = true; mavlink_log_info(_mavlink_fd, "#audio: Takeoff"); } } else { /* no takeoff detection --> fly */ launch_detected = true; warnx("launchdetection off"); } } _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); if (launch_detected) { usePreTakeoffThrust = false; /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ if (altitude_error > 15.0f) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min), _airspeed.indicated_airspeed_m_s, eas2tas, true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); /* limit roll motion to ensure enough lift */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); } else { _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); } } else { usePreTakeoffThrust = true; } } // warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(), // (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing()); // warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp(0), (double)prev_wp(1), // (double)next_wp(0), (double)next_wp(1), (pos_sp_triplet.previous_valid) ? "valid" : "invalid"); // XXX at this point we always want no loiter hold if a // mission is active _loiter_hold = false; /* reset landing state */ if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) { reset_landing_state(); } /* reset takeoff/launch state */ if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) { reset_takeoff_state(); } if (was_circle_mode && !_l1_control.circle_mode()) { /* just kicked out of loiter, reset roll integrals */ _att_sp.roll_reset_integral = true; } } else if (0/* posctrl mode enabled */) { /** POSCTRL FLIGHT **/ if (0/* switched from another mode to posctrl */) { _altctrl_hold_heading = _att.yaw; } if (0/* posctrl on and manual control yaw non-zero */) { _altctrl_hold_heading = _att.yaw + _manual.r; } //XXX not used /* climb out control */ // bool climb_out = false; // // /* user wants to climb out */ // if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { // climb_out = true; // } /* if in altctrl mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here float altctrl_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.z; _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, false, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, _parameters.pitch_limit_min, _parameters.pitch_limit_max); } else if (0/* altctrl mode enabled */) { /** ALTCTRL FLIGHT **/ if (0/* switched from another mode to altctrl */) { _altctrl_hold_heading = _att.yaw; } if (0/* altctrl on and manual control yaw non-zero */) { _altctrl_hold_heading = _att.yaw + _manual.r; } /* if in altctrl mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here float altctrl_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.z; /* user switched off throttle */ if (_manual.z < 0.1f) { throttle_max = 0.0f; /* switch to pure pitch based altitude control, give up speed */ _tecs.set_speed_weight(0.0f); } /* climb out control */ bool climb_out = false; /* user wants to climb out */ if (_manual.x > 0.3f && _manual.z > 0.8f) { climb_out = true; } _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _manual.y; _att_sp.yaw_body = _manual.r; _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, climb_out, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, _parameters.pitch_limit_min, _parameters.pitch_limit_max); } else { /** MANUAL FLIGHT **/ /* no flight mode applies, do not publish an attitude setpoint */ setpoint = false; /* reset landing and takeoff state */ if (!last_manual) { reset_landing_state(); reset_takeoff_state(); } } if (usePreTakeoffThrust) { _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } else { _att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max); } _att_sp.pitch_body = _tecs.get_pitch_demand(); if (_control_mode.flag_control_position_enabled) { last_manual = false; } else { last_manual = true; } return setpoint; }
void FollowTarget::on_active() { struct map_projection_reference_s target_ref; math::Vector<3> target_reported_velocity(0, 0, 0); follow_target_s target_motion_with_offset = {}; uint64_t current_time = hrt_absolute_time(); bool _radius_entered = false; bool _radius_exited = false; bool updated = false; float dt_ms = 0; orb_check(_follow_target_sub, &updated); if (updated) { follow_target_s target_motion; _target_updates++; // save last known motion topic _previous_target_motion = _current_target_motion; orb_copy(ORB_ID(follow_target), _follow_target_sub, &target_motion); if (_current_target_motion.timestamp == 0) { _current_target_motion = target_motion; } _current_target_motion.timestamp = target_motion.timestamp; _current_target_motion.lat = (_current_target_motion.lat * (double)_responsiveness) + target_motion.lat * (double)( 1 - _responsiveness); _current_target_motion.lon = (_current_target_motion.lon * (double)_responsiveness) + target_motion.lon * (double)( 1 - _responsiveness); target_reported_velocity(0) = _current_target_motion.vx; target_reported_velocity(1) = _current_target_motion.vy; } else if (((current_time - _current_target_motion.timestamp) / 1000) > TARGET_TIMEOUT_MS && target_velocity_valid()) { reset_target_validity(); } // update distance to target if (target_position_valid()) { // get distance to target map_projection_init(&target_ref, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &_target_distance(0), &_target_distance(1)); } // update target velocity if (target_velocity_valid() && updated) { dt_ms = ((_current_target_motion.timestamp - _previous_target_motion.timestamp) / 1000); // ignore a small dt if (dt_ms > 10.0F) { // get last gps known reference for target map_projection_init(&target_ref, _previous_target_motion.lat, _previous_target_motion.lon); // calculate distance the target has moved map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &(_target_position_delta(0)), &(_target_position_delta(1))); // update the average velocity of the target based on the position _est_target_vel = _target_position_delta / (dt_ms / 1000.0f); // if the target is moving add an offset and rotation if (_est_target_vel.length() > .5F) { _target_position_offset = _rot_matrix * _est_target_vel.normalized() * _follow_offset; } // are we within the target acceptance radius? // give a buffer to exit/enter the radius to give the velocity controller // a chance to catch up _radius_exited = ((_target_position_offset + _target_distance).length() > (float) TARGET_ACCEPTANCE_RADIUS_M * 1.5f); _radius_entered = ((_target_position_offset + _target_distance).length() < (float) TARGET_ACCEPTANCE_RADIUS_M); // to keep the velocity increase/decrease smooth // calculate how many velocity increments/decrements // it will take to reach the targets velocity // with the given amount of steps also add a feed forward input that adjusts the // velocity as the position gap increases since // just traveling at the exact velocity of the target will not // get any closer or farther from the target _step_vel = (_est_target_vel - _current_vel) + (_target_position_offset + _target_distance) * FF_K; _step_vel /= (dt_ms / 1000.0F * (float) INTERPOLATION_PNTS); _step_time_in_ms = (dt_ms / (float) INTERPOLATION_PNTS); // if we are less than 1 meter from the target don't worry about trying to yaw // lock the yaw until we are at a distance that makes sense if ((_target_distance).length() > 1.0F) { // yaw rate smoothing // this really needs to control the yaw rate directly in the attitude pid controller // but seems to work ok for now since the yaw rate cannot be controlled directly in auto mode _yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon, _current_target_motion.lat, _current_target_motion.lon); _yaw_rate = (_yaw_angle - _navigator->get_global_position()->yaw) / (dt_ms / 1000.0F); _yaw_rate = _wrap_pi(_yaw_rate); _yaw_rate = math::constrain(_yaw_rate, -1.0F * _yaw_auto_max, _yaw_auto_max); } else { _yaw_angle = _yaw_rate = NAN; } } // warnx(" _step_vel x %3.6f y %3.6f cur vel %3.6f %3.6f tar vel %3.6f %3.6f dist = %3.6f (%3.6f) mode = %d con ratio = %3.6f yaw rate = %3.6f", // (double) _step_vel(0), // (double) _step_vel(1), // (double) _current_vel(0), // (double) _current_vel(1), // (double) _est_target_vel(0), // (double) _est_target_vel(1), // (double) (_target_distance).length(), // (double) (_target_position_offset + _target_distance).length(), // _follow_target_state, // (double)_avg_cos_ratio, (double) _yaw_rate); } if (target_position_valid()) { // get the target position using the calculated offset map_projection_init(&target_ref, _current_target_motion.lat, _current_target_motion.lon); map_projection_reproject(&target_ref, _target_position_offset(0), _target_position_offset(1), &target_motion_with_offset.lat, &target_motion_with_offset.lon); } // clamp yaw rate smoothing if we are with in // 3 degrees of facing target if (PX4_ISFINITE(_yaw_rate)) { if (fabsf(fabsf(_yaw_angle) - fabsf(_navigator->get_global_position()->yaw)) < math::radians(3.0F)) { _yaw_rate = NAN; } } // update state machine switch (_follow_target_state) { case TRACK_POSITION: { if (_radius_entered == true) { _follow_target_state = TRACK_VELOCITY; } else if (target_velocity_valid()) { set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion_with_offset, _yaw_angle); // keep the current velocity updated with the target velocity for when it's needed _current_vel = _est_target_vel; update_position_sp(true, true, _yaw_rate); } else { _follow_target_state = SET_WAIT_FOR_TARGET_POSITION; } break; } case TRACK_VELOCITY: { if (_radius_exited == true) { _follow_target_state = TRACK_POSITION; } else if (target_velocity_valid()) { if ((float)(current_time - _last_update_time) / 1000.0f >= _step_time_in_ms) { _current_vel += _step_vel; _last_update_time = current_time; } set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion_with_offset, _yaw_angle); update_position_sp(true, false, _yaw_rate); } else { _follow_target_state = SET_WAIT_FOR_TARGET_POSITION; } break; } case SET_WAIT_FOR_TARGET_POSITION: { // Climb to the minimum altitude // and wait until a position is received follow_target_s target = {}; // for now set the target at the minimum height above the uav target.lat = _navigator->get_global_position()->lat; target.lon = _navigator->get_global_position()->lon; target.alt = 0.0F; set_follow_target_item(&_mission_item, _param_min_alt.get(), target, _yaw_angle); update_position_sp(false, false, _yaw_rate); _follow_target_state = WAIT_FOR_TARGET_POSITION; } /* FALLTHROUGH */ case WAIT_FOR_TARGET_POSITION: { if (is_mission_item_reached() && target_velocity_valid()) { _target_position_offset(0) = _follow_offset; _follow_target_state = TRACK_POSITION; } break; } } }
__EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, float radius, float arc_start_bearing, float arc_sweep) { // This function returns the distance to the nearest point on the track arc. Distance is positive if current // position is right of the arc and negative if left of the arc as seen from the closest point on the arc and // headed towards the end point. // Determine if the current position is inside or outside the sector between the line from the center // to the arc start and the line from the center to the arc end float bearing_sector_start; float bearing_sector_end; float bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); bool in_sector; int return_value = ERROR; // Set error flag, cleared when valid result calculated. crosstrack_error->past_end = false; crosstrack_error->distance = 0.0f; crosstrack_error->bearing = 0.0f; // Return error if arguments are bad if (radius < 0.1f) { return return_value; } if (arc_sweep >= 0.0f) { bearing_sector_start = arc_start_bearing; bearing_sector_end = arc_start_bearing + arc_sweep; if (bearing_sector_end > 2.0f * M_PI_F) { bearing_sector_end -= M_TWOPI_F; } } else { bearing_sector_end = arc_start_bearing; bearing_sector_start = arc_start_bearing - arc_sweep; if (bearing_sector_start < 0.0f) { bearing_sector_start += M_TWOPI_F; } } in_sector = false; // Case where sector does not span zero if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) { in_sector = true; } // Case where sector does span zero if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) { in_sector = true; } // If in the sector then calculate distance and bearing to closest point if (in_sector) { crosstrack_error->past_end = false; float dist_to_center = get_distance_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); if (dist_to_center <= radius) { crosstrack_error->distance = radius - dist_to_center; crosstrack_error->bearing = bearing_now + M_PI_F; } else { crosstrack_error->distance = dist_to_center - radius; crosstrack_error->bearing = bearing_now; } // If out of the sector then calculate dist and bearing to start or end point } else { // Use the approximation that 111,111 meters in the y direction is 1 degree (of latitude) // and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to // calculate the position of the start and end points. We should not be doing this often // as this function generally will not be called repeatedly when we are out of the sector. double start_disp_x = (double)radius * sin(arc_start_bearing); double start_disp_y = (double)radius * cos(arc_start_bearing); double end_disp_x = (double)radius * sin(_wrap_pi((double)(arc_start_bearing + arc_sweep))); double end_disp_y = (double)radius * cos(_wrap_pi((double)(arc_start_bearing + arc_sweep))); double lon_start = lon_now + start_disp_x / 111111.0; double lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0; double lon_end = lon_now + end_disp_x / 111111.0; double lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0; double dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); double dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); if (dist_to_start < dist_to_end) { crosstrack_error->distance = dist_to_start; crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); } else { crosstrack_error->past_end = true; crosstrack_error->distance = dist_to_end; crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); } } crosstrack_error->bearing = _wrap_pi((double)crosstrack_error->bearing); return_value = OK; return return_value; }
void Mission::heading_sp_update() { /* we don't want to be yawing during takeoff, landing or aligning for a transition */ if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION || _mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND || _work_item_type == WORK_ITEM_TYPE_ALIGN) { return; } struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); /* Don't change setpoint if last and current waypoint are not valid */ if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid) { return; } /* set yaw angle for the waypoint if a loiter time has been specified */ if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) { // XXX: should actually be param4 from mission item // at the moment it will just keep the heading it has //_mission_item.yaw = _on_arrival_yaw; //pos_sp_triplet->current.yaw = _mission_item.yaw; } else { /* Calculate direction the vehicle should point to. */ double point_from_latlon[2]; double point_to_latlon[2]; point_from_latlon[0] = _navigator->get_global_position()->lat; point_from_latlon[1] = _navigator->get_global_position()->lon; /* target location is home */ if ((_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME || _param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) // need to be rotary wing for this but not in a transition // in VTOL mode this will prevent updating yaw during FW flight // (which would result in a wrong yaw setpoint spike during back transition) && _navigator->get_vstatus()->is_rotary_wing && !(_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION || _navigator->get_vstatus()->in_transition_mode)) { point_to_latlon[0] = _navigator->get_home_position()->lat; point_to_latlon[1] = _navigator->get_home_position()->lon; /* target location is next (current) waypoint */ } else { point_to_latlon[0] = pos_sp_triplet->current.lat; point_to_latlon[1] = pos_sp_triplet->current.lon; } float d_current = get_distance_to_next_waypoint( point_from_latlon[0], point_from_latlon[1], point_to_latlon[0], point_to_latlon[1]); /* stop if positions are close together to prevent excessive yawing */ if (d_current > _navigator->get_acceptance_radius()) { float yaw = get_bearing_to_next_waypoint( point_from_latlon[0], point_from_latlon[1], point_to_latlon[0], point_to_latlon[1]); /* always keep the back of the rotary wing pointing towards home */ if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) { _mission_item.yaw = _wrap_pi(yaw + M_PI_F); pos_sp_triplet->current.yaw = _mission_item.yaw; } else { _mission_item.yaw = yaw; pos_sp_triplet->current.yaw = _mission_item.yaw; } } } // we set yaw directly so we can run this in parallel to the FOH update _navigator->set_position_setpoint_triplet_updated(); }
void Mission::set_mission_items() { /* make sure param is up to date */ updateParams(); /* reset the altitude foh logic, if altitude foh is enabled (param) a new foh element starts now */ altitude_sp_foh_reset(); struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); /* the home dist check provides user feedback, so we initialize it to this */ bool user_feedback_done = false; /* mission item that comes after current if available */ struct mission_item_s mission_item_next_position; bool has_next_position_item = false; work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT; /* copy information about the previous mission item */ if (item_contains_position(&_mission_item) && pos_sp_triplet->current.valid) { /* Copy previous mission item altitude */ _mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item); } /* try setting onboard mission item */ if (_param_onboard_enabled.get() && prepare_mission_items(true, &_mission_item, &mission_item_next_position, &has_next_position_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_ONBOARD) { mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running"); user_feedback_done = true; } _mission_type = MISSION_TYPE_ONBOARD; /* try setting offboard mission item */ } else if (prepare_mission_items(false, &_mission_item, &mission_item_next_position, &has_next_position_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running"); user_feedback_done = true; } _mission_type = MISSION_TYPE_OFFBOARD; } else { /* no mission available or mission finished, switch to loiter */ if (_mission_type != MISSION_TYPE_NONE) { /* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */ mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering"); user_feedback_done = true; /* use last setpoint for loiter */ _navigator->set_can_loiter_at_sp(true); } _mission_type = MISSION_TYPE_NONE; /* set loiter mission item and ensure that there is a minimum clearance from home */ set_loiter_item(&_mission_item, _param_takeoff_alt.get()); /* update position setpoint triplet */ pos_sp_triplet->previous.valid = false; mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; /* reuse setpoint for LOITER only if it's not IDLE */ _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); set_mission_finished(); if (!user_feedback_done) { /* only tell users that we got no mission if there has not been any * better, more specific feedback yet * https://en.wikipedia.org/wiki/Loiter_(aeronautics) */ if (_navigator->get_vstatus()->condition_landed) { /* landed, refusing to take off without a mission */ mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, refusing takeoff"); } else { mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, loitering"); } user_feedback_done = true; } _navigator->set_position_setpoint_triplet_updated(); return; } /*********************************** handle mission item *********************************************/ /* handle position mission items */ if (item_contains_position(&_mission_item)) { /* we have a new position item so set previous position setpoint to current */ set_previous_pos_setpoint(); /* do takeoff before going to setpoint if needed and not already in takeoff */ if (do_need_takeoff() && _work_item_type != WORK_ITEM_TYPE_TAKEOFF) { new_work_item_type = WORK_ITEM_TYPE_TAKEOFF; /* use current mission item as next position item */ memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s)); mission_item_next_position.nav_cmd = NAV_CMD_WAYPOINT; has_next_position_item = true; float takeoff_alt = calculate_takeoff_altitude(&_mission_item); mavlink_log_info(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); _mission_item.nav_cmd = NAV_CMD_TAKEOFF; _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; /* ignore yaw for takeoff items */ _mission_item.yaw = NAN; _mission_item.altitude = takeoff_alt; _mission_item.altitude_is_relative = false; _mission_item.autocontinue = true; _mission_item.time_inside = 0; } /* if we just did a takeoff navigate to the actual waypoint now */ if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) { new_work_item_type = WORK_ITEM_TYPE_DEFAULT; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ _mission_item.yaw = NAN; } /* move to landing waypoint before descent if necessary */ if (do_need_move_to_land() && _work_item_type != WORK_ITEM_TYPE_MOVE_TO_LAND) { new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND; /* use current mission item as next position item */ memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s)); has_next_position_item = true; /* * Ignoring waypoint altitude: * Set altitude to the same as we have now to prevent descending too fast into * the ground. Actual landing will descend anyway until it touches down. * XXX: We might want to change that at some point if it is clear to the user * what the altitude means on this waypoint type. */ float altitude = _navigator->get_global_position()->alt; if (pos_sp_triplet->current.valid) { altitude = pos_sp_triplet->current.alt; } _mission_item.altitude = altitude; _mission_item.altitude_is_relative = false; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.autocontinue = true; _mission_item.time_inside = 0; } /* we just moved to the landing waypoint, now descend */ if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND) { new_work_item_type = WORK_ITEM_TYPE_DEFAULT; } /* ignore yaw for landing items */ /* XXX: if specified heading for landing is desired we could add another step before the descent * that aligns the vehicle first */ if (_mission_item.nav_cmd == NAV_CMD_LAND) { _mission_item.yaw = NAN; } /* handle non-position mission items such as commands */ } else { /* turn towards next waypoint before MC to FW transition */ if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION && _work_item_type != WORK_ITEM_TYPE_ALIGN && _navigator->get_vstatus()->is_rotary_wing && !_navigator->get_vstatus()->condition_landed && has_next_position_item) { new_work_item_type = WORK_ITEM_TYPE_ALIGN; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; _mission_item.altitude = _navigator->get_global_position()->alt; _mission_item.altitude_is_relative = false; _mission_item.autocontinue = true; _mission_item.time_inside = 0; _mission_item.yaw = get_bearing_to_next_waypoint( _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, mission_item_next_position.lat, mission_item_next_position.lon); } /* yaw is aligned now */ if (_work_item_type == WORK_ITEM_TYPE_ALIGN) { new_work_item_type = WORK_ITEM_TYPE_DEFAULT; } /* don't advance mission after FW to MC command */ if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION && _work_item_type != WORK_ITEM_TYPE_CMD_BEFORE_MOVE && !_navigator->get_vstatus()->is_rotary_wing && !_navigator->get_vstatus()->condition_landed && pos_sp_triplet->current.valid) { new_work_item_type = WORK_ITEM_TYPE_CMD_BEFORE_MOVE; } /* after FW to MC transition finish moving to the waypoint */ if (_work_item_type == WORK_ITEM_TYPE_CMD_BEFORE_MOVE && pos_sp_triplet->current.valid) { new_work_item_type = WORK_ITEM_TYPE_DEFAULT; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.lat = pos_sp_triplet->current.lat; _mission_item.lon = pos_sp_triplet->current.lon; _mission_item.altitude = pos_sp_triplet->current.alt; _mission_item.altitude_is_relative = false; _mission_item.autocontinue = true; _mission_item.time_inside = 0; } } /*********************************** set setpoints and check next *********************************************/ /* set current position setpoint from mission item (is protected agains non-position items) */ mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); /* issue command if ready (will do nothing for position mission items) */ issue_command(&_mission_item); /* set current work item type */ _work_item_type = new_work_item_type; /* require takeoff after landing or idle */ if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _need_takeoff = true; } _navigator->set_can_loiter_at_sp(false); reset_mission_item_reached(); if (_mission_type == MISSION_TYPE_OFFBOARD) { set_current_offboard_mission_item(); } // TODO: report onboard mission item somehow if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) { /* try to process next mission item */ if (has_next_position_item) { /* got next mission item, update setpoint triplet */ mission_item_to_position_setpoint(&mission_item_next_position, &pos_sp_triplet->next); } else { /* next mission item is not available */ pos_sp_triplet->next.valid = false; } } else { /* vehicle will be paused on current waypoint, don't set next item */ pos_sp_triplet->next.valid = false; } /* Save the distance between the current sp and the previous one */ if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) { _distance_current_previous = get_distance_to_next_waypoint( pos_sp_triplet->current.lat, pos_sp_triplet->current.lon, pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon); } _navigator->set_position_setpoint_triplet_updated(); }
void RTL::set_rtl_item() { struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); /* make sure we have the latest params */ updateParams(); if (!_rtl_start_lock) { set_previous_pos_setpoint(); } _navigator->set_can_loiter_at_sp(false); switch (_rtl_state) { case RTL_STATE_CLIMB: { float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get(); _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; _mission_item.altitude_is_relative = false; _mission_item.altitude = climb_alt; _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)", (int)(climb_alt), (int)(climb_alt - _navigator->get_home_position()->alt)); break; } case RTL_STATE_RETURN: { _mission_item.lat = _navigator->get_home_position()->lat; _mission_item.lon = _navigator->get_home_position()->lon; // don't change altitude // use home yaw if close to home /* check if we are pretty close to home already */ float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat, _navigator->get_home_position()->lon, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); if (home_dist < _param_rtl_min_dist.get()) { _mission_item.yaw = _navigator->get_home_position()->yaw; } else { if (pos_sp_triplet->previous.valid) { /* if previous setpoint is valid then use it to calculate heading to home */ _mission_item.yaw = get_bearing_to_next_waypoint( pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon, _mission_item.lat, _mission_item.lon); } else { /* else use current position */ _mission_item.yaw = get_bearing_to_next_waypoint( _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, _mission_item.lat, _mission_item.lon); } } _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)", (int)(_mission_item.altitude), (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); _rtl_start_lock = true; break; } case RTL_STATE_TRANSITION_TO_MC: { _mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION; _mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; break; } case RTL_STATE_DESCEND: { _mission_item.lat = _navigator->get_home_position()->lat; _mission_item.lon = _navigator->get_home_position()->lon; _mission_item.altitude_is_relative = false; _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); // check if we are already lower - then we will just stay there if (_mission_item.altitude > _navigator->get_global_position()->alt) { _mission_item.altitude = _navigator->get_global_position()->alt; } _mission_item.yaw = _navigator->get_home_position()->yaw; // except for vtol which might be still off here and should point towards this location float d_current = get_distance_to_next_waypoint( _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, _mission_item.lat, _mission_item.lon); if (_navigator->get_vstatus()->is_vtol && d_current > _navigator->get_acceptance_radius()) { _mission_item.yaw = get_bearing_to_next_waypoint( _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, _mission_item.lat, _mission_item.lon); } _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; _mission_item.autocontinue = false; _mission_item.origin = ORIGIN_ONBOARD; /* disable previous setpoint to prevent drift */ pos_sp_triplet->previous.valid = false; mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)", (int)(_mission_item.altitude), (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); break; } case RTL_STATE_LOITER: { bool autoland = _param_land_delay.get() > -DELAY_SIGMA; _mission_item.lat = _navigator->get_home_position()->lat; _mission_item.lon = _navigator->get_home_position()->lon; // don't change altitude _mission_item.yaw = _navigator->get_home_position()->yaw; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get(); _mission_item.autocontinue = autoland; _mission_item.origin = ORIGIN_ONBOARD; _navigator->set_can_loiter_at_sp(true); if (autoland && (Navigator::get_time_inside(_mission_item) > FLT_EPSILON)) { mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs", (double)Navigator::get_time_inside(_mission_item)); } else { mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter"); } break; } case RTL_STATE_LAND: { _mission_item.yaw = _navigator->get_home_position()->yaw; set_land_item(&_mission_item, false); mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at home"); break; } case RTL_STATE_LANDED: { set_idle_item(&_mission_item); mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, landed"); break; } default: break; } reset_mission_item_reached(); /* execute command if set */ if (!item_contains_position(&_mission_item)) { issue_command(&_mission_item); } /* convert mission item to current position setpoint and make it valid */ mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; _navigator->set_position_setpoint_triplet_updated(); }
void RTL::set_rtl_item() { struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); /* make sure we have the latest params */ updateParams(); if (!_rtl_start_lock) { set_previous_pos_setpoint(); } _navigator->set_can_loiter_at_sp(false); switch (_rtl_state) { case RTL_STATE_CLIMB: { float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get(); _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; _mission_item.altitude_is_relative = false; _mission_item.altitude = climb_alt; _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)", (int)(climb_alt), (int)(climb_alt - _navigator->get_home_position()->alt)); break; } case RTL_STATE_RETURN: { _mission_item.lat = _navigator->get_home_position()->lat; _mission_item.lon = _navigator->get_home_position()->lon; // don't change altitude if (pos_sp_triplet->previous.valid) { /* if previous setpoint is valid then use it to calculate heading to home */ _mission_item.yaw = get_bearing_to_next_waypoint( pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon, _mission_item.lat, _mission_item.lon); } else { /* else use current position */ _mission_item.yaw = get_bearing_to_next_waypoint( _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, _mission_item.lat, _mission_item.lon); } _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)", (int)(_mission_item.altitude), (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); _rtl_start_lock = true; break; } case RTL_STATE_DESCEND: { _mission_item.lat = _navigator->get_home_position()->lat; _mission_item.lon = _navigator->get_home_position()->lon; _mission_item.altitude_is_relative = false; _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); _mission_item.yaw = _navigator->get_home_position()->yaw; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = false; _mission_item.origin = ORIGIN_ONBOARD; mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)", (int)(_mission_item.altitude), (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); break; } case RTL_STATE_LOITER: { bool autoland = _param_land_delay.get() > -DELAY_SIGMA; _mission_item.lat = _navigator->get_home_position()->lat; _mission_item.lon = _navigator->get_home_position()->lon; _mission_item.altitude_is_relative = false; _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); _mission_item.yaw = _navigator->get_home_position()->yaw; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get(); _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = autoland; _mission_item.origin = ORIGIN_ONBOARD; _navigator->set_can_loiter_at_sp(true); if (autoland) { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs", (double)_mission_item.time_inside); } else { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter"); } break; } case RTL_STATE_LAND: { set_land_item(&_mission_item, false); mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: land at home"); break; } case RTL_STATE_LANDED: { set_idle_item(&_mission_item); mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: completed, landed"); break; } default: break; } reset_mission_item_reached(); /* convert mission item to current position setpoint and make it valid */ mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; _navigator->set_position_setpoint_triplet_updated(); }
bool MissionBlock::is_mission_item_reached() { /* handle non-navigation or indefinite waypoints */ switch (_mission_item.nav_cmd) { case NAV_CMD_DO_SET_SERVO: return true; case NAV_CMD_LAND: /* fall through */ case NAV_CMD_VTOL_LAND: return _navigator->get_land_detected()->landed; case NAV_CMD_IDLE: /* fall through */ case NAV_CMD_LOITER_UNLIMITED: return false; case NAV_CMD_DO_LAND_START: case NAV_CMD_DO_TRIGGER_CONTROL: case NAV_CMD_DO_DIGICAM_CONTROL: case NAV_CMD_IMAGE_START_CAPTURE: case NAV_CMD_IMAGE_STOP_CAPTURE: case NAV_CMD_VIDEO_START_CAPTURE: case NAV_CMD_VIDEO_STOP_CAPTURE: case NAV_CMD_DO_MOUNT_CONFIGURE: case NAV_CMD_DO_MOUNT_CONTROL: case NAV_CMD_DO_SET_ROI: case NAV_CMD_DO_SET_CAM_TRIGG_DIST: case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL: case NAV_CMD_SET_CAMERA_MODE: return true; case NAV_CMD_DO_VTOL_TRANSITION: /* * We wait half a second to give the transition command time to propagate. * Then monitor the transition status for completion. */ // TODO: check desired transition state achieved and drop _action_start if (hrt_absolute_time() - _action_start > 500000 && !_navigator->get_vstatus()->in_transition_mode) { _action_start = 0; return true; } else { return false; } case NAV_CMD_DO_CHANGE_SPEED: return true; default: /* do nothing, this is a 3D waypoint */ break; } hrt_abstime now = hrt_absolute_time(); if (!_navigator->get_land_detected()->landed && !_waypoint_position_reached) { float dist = -1.0f; float dist_xy = -1.0f; float dist_z = -1.0f; float altitude_amsl = _mission_item.altitude_is_relative ? _mission_item.altitude + _navigator->get_home_position()->alt : _mission_item.altitude; dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, _navigator->get_global_position()->alt, &dist_xy, &dist_z); /* FW special case for NAV_CMD_WAYPOINT to achieve altitude via loiter */ if (!_navigator->get_vstatus()->is_rotary_wing && (_mission_item.nav_cmd == NAV_CMD_WAYPOINT)) { struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current; /* close to waypoint, but altitude error greater than twice acceptance */ if ((dist >= 0.0f) && (dist_z > 2 * _navigator->get_altitude_acceptance_radius()) && (dist_xy < 2 * _navigator->get_loiter_radius())) { /* SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER */ if (curr_sp->type == position_setpoint_s::SETPOINT_TYPE_POSITION) { curr_sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER; curr_sp->loiter_radius = _navigator->get_loiter_radius(); curr_sp->loiter_direction = 1; _navigator->set_position_setpoint_triplet_updated(); } } else { /* restore SETPOINT_TYPE_POSITION */ if (curr_sp->type == position_setpoint_s::SETPOINT_TYPE_LOITER) { /* loiter acceptance criteria required to revert back to SETPOINT_TYPE_POSITION */ if ((dist >= 0.0f) && (dist_z < _navigator->get_loiter_radius()) && (dist_xy <= _navigator->get_loiter_radius() * 1.2f)) { curr_sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION; _navigator->set_position_setpoint_triplet_updated(); } } } } if ((_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) && _navigator->get_vstatus()->is_rotary_wing) { /* We want to avoid the edge case where the acceptance radius is bigger or equal than * the altitude of the takeoff waypoint above home. Otherwise, we do not really follow * take-off procedures like leaving the landing gear down. */ float takeoff_alt = _mission_item.altitude_is_relative ? _mission_item.altitude : (_mission_item.altitude - _navigator->get_home_position()->alt); float altitude_acceptance_radius = _navigator->get_altitude_acceptance_radius(); /* It should be safe to just use half of the takoeff_alt as an acceptance radius. */ if (takeoff_alt > 0 && takeoff_alt < altitude_acceptance_radius) { altitude_acceptance_radius = takeoff_alt / 2.0f; } /* require only altitude for takeoff for multicopter */ if (_navigator->get_global_position()->alt > altitude_amsl - altitude_acceptance_radius) { _waypoint_position_reached = true; } } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { /* for takeoff mission items use the parameter for the takeoff acceptance radius */ if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius() && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; } } else if (!_navigator->get_vstatus()->is_rotary_wing && (_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT)) { /* Loiter mission item on a non rotary wing: the aircraft is going to circle the * coordinates with a radius equal to the loiter_radius field. It is not flying * through the waypoint center. * Therefore the item is marked as reached once the system reaches the loiter * radius (+ some margin). Time inside and turn count is handled elsewhere. */ if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f) && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; } else { _time_first_inside_orbit = 0; } } else if (!_navigator->get_vstatus()->is_rotary_wing && (_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) { // NAV_CMD_LOITER_TO_ALT only uses mission item altitude once it's in the loiter // first check if the altitude setpoint is the mission setpoint struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current; if (fabsf(curr_sp->alt - altitude_amsl) >= FLT_EPSILON) { // check if the initial loiter has been accepted dist_xy = -1.0f; dist_z = -1.0f; dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, curr_sp->alt, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, _navigator->get_global_position()->alt, &dist_xy, &dist_z); if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f) && dist_z <= _navigator->get_altitude_acceptance_radius()) { // now set the loiter to the final altitude in the NAV_CMD_LOITER_TO_ALT mission item curr_sp->alt = altitude_amsl; _navigator->set_position_setpoint_triplet_updated(); } } else { if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f) && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; // set required yaw from bearing to the next mission item if (_mission_item.force_heading) { struct position_setpoint_s next_sp = _navigator->get_position_setpoint_triplet()->next; if (next_sp.valid) { _mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon, next_sp.lat, next_sp.lon); _waypoint_yaw_reached = false; } else { _waypoint_yaw_reached = true; } } } } } else if (_mission_item.nav_cmd == NAV_CMD_DELAY) { _waypoint_position_reached = true; _waypoint_yaw_reached = true; _time_wp_reached = now; } else { /* for normal mission items used their acceptance radius */ float mission_acceptance_radius = _navigator->get_acceptance_radius(_mission_item.acceptance_radius); /* if set to zero use the default instead */ if (mission_acceptance_radius < NAV_EPSILON_POSITION) { mission_acceptance_radius = _navigator->get_acceptance_radius(); } /* for vtol back transition calculate acceptance radius based on time and ground speed */ if (_mission_item.vtol_back_transition) { float velocity = sqrtf(_navigator->get_local_position()->vx * _navigator->get_local_position()->vx + _navigator->get_local_position()->vy * _navigator->get_local_position()->vy); if (_param_back_trans_dec_mss.get() > FLT_EPSILON && velocity > FLT_EPSILON) { mission_acceptance_radius = ((velocity / _param_back_trans_dec_mss.get() / 2) * velocity) + _param_reverse_delay.get() * velocity; } } if (dist >= 0.0f && dist <= mission_acceptance_radius && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; } } if (_waypoint_position_reached) { // reached just now _time_wp_reached = now; } } /* Check if the waypoint and the requested yaw setpoint. */ if (_waypoint_position_reached && !_waypoint_yaw_reached) { if ((_navigator->get_vstatus()->is_rotary_wing || (_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT && _mission_item.force_heading)) && PX4_ISFINITE(_mission_item.yaw)) { /* check course if defined only for rotary wing except takeoff */ float cog = _navigator->get_vstatus()->is_rotary_wing ? _navigator->get_global_position()->yaw : atan2f( _navigator->get_global_position()->vel_e, _navigator->get_global_position()->vel_n); float yaw_err = _wrap_pi(_mission_item.yaw - cog); /* accept yaw if reached or if timeout is set in which case we ignore not forced headings */ if (fabsf(yaw_err) < math::radians(_param_yaw_err.get()) || (_param_yaw_timeout.get() >= FLT_EPSILON && !_mission_item.force_heading)) { _waypoint_yaw_reached = true; } /* if heading needs to be reached, the timeout is enabled and we don't make it, abort mission */ if (!_waypoint_yaw_reached && _mission_item.force_heading && (_param_yaw_timeout.get() >= FLT_EPSILON) && (now - _time_wp_reached >= (hrt_abstime)_param_yaw_timeout.get() * 1e6f)) { _navigator->set_mission_failure("unable to reach heading within timeout"); } } else { _waypoint_yaw_reached = true; } } /* Once the waypoint and yaw setpoint have been reached we can start the loiter time countdown */ if (_waypoint_position_reached && _waypoint_yaw_reached) { if (_time_first_inside_orbit == 0) { _time_first_inside_orbit = now; } /* check if the MAV was long enough inside the waypoint orbit */ if ((get_time_inside(_mission_item) < FLT_EPSILON) || (now - _time_first_inside_orbit >= (hrt_abstime)(get_time_inside(_mission_item) * 1e6f))) { position_setpoint_s &curr_sp = _navigator->get_position_setpoint_triplet()->current; const position_setpoint_s &next_sp = _navigator->get_position_setpoint_triplet()->next; const float range = get_distance_to_next_waypoint(curr_sp.lat, curr_sp.lon, next_sp.lat, next_sp.lon); // exit xtrack location // reset lat/lon of loiter waypoint so vehicle follows a tangent if (_mission_item.loiter_exit_xtrack && next_sp.valid && PX4_ISFINITE(range) && (_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) { float bearing = get_bearing_to_next_waypoint(curr_sp.lat, curr_sp.lon, next_sp.lat, next_sp.lon); float inner_angle = M_PI_2_F - asinf(_mission_item.loiter_radius / range); // Compute "ideal" tangent origin if (curr_sp.loiter_direction > 0) { bearing -= inner_angle; } else { bearing += inner_angle; } // Replace current setpoint lat/lon with tangent coordinate waypoint_from_heading_and_distance(curr_sp.lat, curr_sp.lon, bearing, curr_sp.loiter_radius, &curr_sp.lat, &curr_sp.lon); } return true; } } // all acceptance criteria must be met in the same iteration _waypoint_position_reached = false; _waypoint_yaw_reached = false; return false; }
void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, const math::Vector2f &ground_speed_vector) { /* this follows the logic presented in [1] */ float eta; float xtrack_vel; float ltrack_vel; /* get the direction between the last (visited) and next waypoint */ _target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_B.getX(), vector_B.getY()); /* enforce a minimum ground speed of 0.1 m/s to avoid singularities */ float ground_speed = math::max(ground_speed_vector.length(), 0.1f); /* calculate the L1 length required for the desired period */ _L1_distance = _L1_ratio * ground_speed; /* calculate vector from A to B */ math::Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B); /* * check if waypoints are on top of each other. If yes, * skip A and directly continue to B */ if (vector_AB.length() < 1.0e-6f) { vector_AB = get_local_planar_vector(vector_curr_position, vector_B); } vector_AB.normalize(); /* calculate the vector from waypoint A to the aircraft */ math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); /* calculate crosstrack error (output only) */ _crosstrack_error = vector_AB % vector_A_to_airplane; /* * If the current position is in a +-135 degree angle behind waypoint A * and further away from A than the L1 distance, then A becomes the L1 point. * If the aircraft is already between A and B normal L1 logic is applied. */ float distance_A_to_airplane = vector_A_to_airplane.length(); float alongTrackDist = vector_A_to_airplane * vector_AB; /* estimate airplane position WRT to B */ math::Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); /* calculate angle of airplane position vector relative to line) */ // XXX this could probably also be based solely on the dot product float AB_to_BP_bearing = atan2f(vector_B_to_P_unit % vector_AB, vector_B_to_P_unit * vector_AB); /* extension from [2], fly directly to A */ if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane , 1.0f) < -0.7071f) { /* calculate eta to fly to waypoint A */ /* unit vector from waypoint A to current position */ math::Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); /* velocity across / orthogonal to line */ xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit); /* velocity along line */ ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit); eta = atan2f(xtrack_vel, ltrack_vel); /* bearing from current position to L1 point */ _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); /* * If the AB vector and the vector from B to airplane point in the same * direction, we have missed the waypoint. At +- 90 degrees we are just passing it. */ } else if (fabsf(AB_to_BP_bearing) < math::radians(100.0f)) { /* * Extension, fly back to waypoint. * * This corner case is possible if the system was following * the AB line from waypoint A to waypoint B, then is * switched to manual mode (or otherwise misses the waypoint) * and behind the waypoint continues to follow the AB line. */ /* calculate eta to fly to waypoint B */ /* velocity across / orthogonal to line */ xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit); /* velocity along line */ ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit); eta = atan2f(xtrack_vel, ltrack_vel); /* bearing from current position to L1 point */ _nav_bearing = atan2f(-vector_B_to_P_unit.getY() , -vector_B_to_P_unit.getX()); } else { /* calculate eta to fly along the line between A and B */ /* velocity across / orthogonal to line */ xtrack_vel = ground_speed_vector % vector_AB; /* velocity along line */ ltrack_vel = ground_speed_vector * vector_AB; /* calculate eta2 (angle of velocity vector relative to line) */ float eta2 = atan2f(xtrack_vel, ltrack_vel); /* calculate eta1 (angle to L1 point) */ float xtrackErr = vector_A_to_airplane % vector_AB; float sine_eta1 = xtrackErr / math::max(_L1_distance , 0.1f); /* limit output to 45 degrees */ sine_eta1 = math::constrain(sine_eta1, -0.7071f, 0.7071f); //sin(pi/4) = 0.7071 float eta1 = asinf(sine_eta1); eta = eta1 + eta2; /* bearing from current position to L1 point */ _nav_bearing = atan2f(vector_AB.getY(), vector_AB.getX()) + eta1; } /* limit angle to +-90 degrees */ eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f); _lateral_accel = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta); /* flying to waypoints, not circling them */ _circle_mode = false; /* the bearing angle, in NED frame */ _bearing_error = eta; }
void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction, const math::Vector2f &ground_speed_vector) { /* the complete guidance logic in this section was proposed by [2] */ /* calculate the gains for the PD loop (circle tracking) */ float omega = (2.0f * M_PI_F / _L1_period); float K_crosstrack = omega * omega; float K_velocity = 2.0f * _L1_damping * omega; /* update bearing to next waypoint */ _target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_A.getX(), vector_A.getY()); /* ground speed, enforce minimum of 0.1 m/s to avoid singularities */ float ground_speed = math::max(ground_speed_vector.length() , 0.1f); /* calculate the L1 length required for the desired period */ _L1_distance = _L1_ratio * ground_speed; /* calculate the vector from waypoint A to current position */ math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); /* store the normalized vector from waypoint A to current position */ math::Vector2f vector_A_to_airplane_unit = (vector_A_to_airplane).normalized(); /* calculate eta angle towards the loiter center */ /* velocity across / orthogonal to line from waypoint to current position */ float xtrack_vel_center = vector_A_to_airplane_unit % ground_speed_vector; /* velocity along line from waypoint to current position */ float ltrack_vel_center = - (ground_speed_vector * vector_A_to_airplane_unit); float eta = atan2f(xtrack_vel_center, ltrack_vel_center); /* limit eta to 90 degrees */ eta = math::constrain(eta, -M_PI_F / 2.0f, +M_PI_F / 2.0f); /* calculate the lateral acceleration to capture the center point */ float lateral_accel_sp_center = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta); /* for PD control: Calculate radial position and velocity errors */ /* radial velocity error */ float xtrack_vel_circle = -ltrack_vel_center; /* radial distance from the loiter circle (not center) */ float xtrack_err_circle = vector_A_to_airplane.length() - radius; /* cross track error for feedback */ _crosstrack_error = xtrack_err_circle; /* calculate PD update to circle waypoint */ float lateral_accel_sp_circle_pd = (xtrack_err_circle * K_crosstrack + xtrack_vel_circle * K_velocity); /* calculate velocity on circle / along tangent */ float tangent_vel = xtrack_vel_center * loiter_direction; /* prevent PD output from turning the wrong way */ if (tangent_vel < 0.0f) { lateral_accel_sp_circle_pd = math::max(lateral_accel_sp_circle_pd , 0.0f); } /* calculate centripetal acceleration setpoint */ float lateral_accel_sp_circle_centripetal = tangent_vel * tangent_vel / math::max((0.5f * radius) , (radius + xtrack_err_circle)); /* add PD control on circle and centripetal acceleration for total circle command */ float lateral_accel_sp_circle = loiter_direction * (lateral_accel_sp_circle_pd + lateral_accel_sp_circle_centripetal); /* * Switch between circle (loiter) and capture (towards waypoint center) mode when * the commands switch over. Only fly towards waypoint if outside the circle. */ // XXX check switch over if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) | (lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction < 0 && xtrack_err_circle > 0.0f)) { _lateral_accel = lateral_accel_sp_center; _circle_mode = false; /* angle between requested and current velocity vector */ _bearing_error = eta; /* bearing from current position to L1 point */ _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); } else { _lateral_accel = lateral_accel_sp_circle; _circle_mode = true; _bearing_error = 0.0f; /* bearing from current position to L1 point */ _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); } }
bool MissionBlock::is_mission_item_reached() { /* handle non-navigation or indefinite waypoints */ switch (_mission_item.nav_cmd) { case NAV_CMD_DO_SET_SERVO: return true; case NAV_CMD_LAND: /* fall through */ case NAV_CMD_VTOL_LAND: return _navigator->get_land_detected()->landed; case NAV_CMD_IDLE: /* fall through */ case NAV_CMD_LOITER_UNLIMITED: return false; case NAV_CMD_DO_DIGICAM_CONTROL: case NAV_CMD_IMAGE_START_CAPTURE: case NAV_CMD_IMAGE_STOP_CAPTURE: case NAV_CMD_VIDEO_START_CAPTURE: case NAV_CMD_VIDEO_STOP_CAPTURE: case NAV_CMD_DO_MOUNT_CONFIGURE: case NAV_CMD_DO_MOUNT_CONTROL: case NAV_CMD_DO_SET_ROI: case NAV_CMD_ROI: case NAV_CMD_DO_SET_CAM_TRIGG_DIST: return true; case NAV_CMD_DO_VTOL_TRANSITION: /* * We wait half a second to give the transition command time to propagate. * Then monitor the transition status for completion. */ if (hrt_absolute_time() - _action_start > 500000 && !_navigator->get_vstatus()->in_transition_mode) { _action_start = 0; return true; } else { return false; } case NAV_CMD_DO_CHANGE_SPEED: // XXX not differentiating ground and airspeed yet if (_mission_item.params[1] > 0.0f) { _navigator->set_cruising_speed(_mission_item.params[1]); } else { _navigator->set_cruising_speed(); /* if no speed target was given try to set throttle */ if (_mission_item.params[2] > 0.0f) { _navigator->set_cruising_throttle(_mission_item.params[2] / 100); } else { _navigator->set_cruising_throttle(); } } return true; default: /* do nothing, this is a 3D waypoint */ break; } hrt_abstime now = hrt_absolute_time(); if ((_navigator->get_land_detected()->landed == false) && !_waypoint_position_reached) { float dist = -1.0f; float dist_xy = -1.0f; float dist_z = -1.0f; float altitude_amsl = _mission_item.altitude_is_relative ? _mission_item.altitude + _navigator->get_home_position()->alt : _mission_item.altitude; dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, _navigator->get_global_position()->alt, &dist_xy, &dist_z); if ((_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) && _navigator->get_vstatus()->is_rotary_wing) { /* We want to avoid the edge case where the acceptance radius is bigger or equal than * the altitude of the takeoff waypoint above home. Otherwise, we do not really follow * take-off procedures like leaving the landing gear down. */ float takeoff_alt = _mission_item.altitude_is_relative ? _mission_item.altitude : (_mission_item.altitude - _navigator->get_home_position()->alt); float altitude_acceptance_radius = _navigator->get_altitude_acceptance_radius(); /* It should be safe to just use half of the takoeff_alt as an acceptance radius. */ if (takeoff_alt > 0 && takeoff_alt < altitude_acceptance_radius) { altitude_acceptance_radius = takeoff_alt / 2.0f; } /* require only altitude for takeoff for multicopter */ if (_navigator->get_global_position()->alt > altitude_amsl - altitude_acceptance_radius) { _waypoint_position_reached = true; } } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { /* for takeoff mission items use the parameter for the takeoff acceptance radius */ if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius() && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; } } else if (!_navigator->get_vstatus()->is_rotary_wing && (_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT)) { /* Loiter mission item on a non rotary wing: the aircraft is going to circle the * coordinates with a radius equal to the loiter_radius field. It is not flying * through the waypoint center. * Therefore the item is marked as reached once the system reaches the loiter * radius (+ some margin). Time inside and turn count is handled elsewhere. */ if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f) && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; } else { _time_first_inside_orbit = 0; } } else if (!_navigator->get_vstatus()->is_rotary_wing && (_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) { // NAV_CMD_LOITER_TO_ALT only uses mission item altitude once it's in the loiter // first check if the altitude setpoint is the mission setpoint struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current; if (fabsf(curr_sp->alt - altitude_amsl) >= FLT_EPSILON) { // check if the initial loiter has been accepted dist = -1.0f; dist_xy = -1.0f; dist_z = -1.0f; dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, curr_sp->alt, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, _navigator->get_global_position()->alt, &dist_xy, &dist_z); if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f) && dist_z <= _navigator->get_altitude_acceptance_radius()) { // now set the loiter to the final altitude in the NAV_CMD_LOITER_TO_ALT mission item curr_sp->alt = altitude_amsl; _navigator->set_position_setpoint_triplet_updated(); } } else { if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f) && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; // set required yaw from bearing to the next mission item if (_mission_item.force_heading) { struct position_setpoint_s next_sp = _navigator->get_position_setpoint_triplet()->next; if (next_sp.valid) { _mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon, next_sp.lat, next_sp.lon); _waypoint_yaw_reached = false; } else { _waypoint_yaw_reached = true; } } } } } else { /* for normal mission items used their acceptance radius */ float mission_acceptance_radius = _navigator->get_acceptance_radius(_mission_item.acceptance_radius); /* if set to zero use the default instead */ if (mission_acceptance_radius < NAV_EPSILON_POSITION) { mission_acceptance_radius = _navigator->get_acceptance_radius(); } if (dist >= 0.0f && dist <= mission_acceptance_radius && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; } } if (_waypoint_position_reached) { // reached just now _time_wp_reached = now; } } /* Check if the waypoint and the requested yaw setpoint. */ if (_waypoint_position_reached && !_waypoint_yaw_reached) { if ((_navigator->get_vstatus()->is_rotary_wing || (_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT && _mission_item.force_heading)) && PX4_ISFINITE(_mission_item.yaw)) { /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw); /* accept yaw if reached or if timeout is set in which case we ignore not forced headings */ if (fabsf(yaw_err) < math::radians(_param_yaw_err.get()) || (_param_yaw_timeout.get() >= FLT_EPSILON && !_mission_item.force_heading)) { _waypoint_yaw_reached = true; } /* if heading needs to be reached, the timeout is enabled and we don't make it, abort mission */ if (!_waypoint_yaw_reached && _mission_item.force_heading && (_param_yaw_timeout.get() >= FLT_EPSILON) && (now - _time_wp_reached >= (hrt_abstime)_param_yaw_timeout.get() * 1e6f)) { _navigator->set_mission_failure("unable to reach heading within timeout"); } } else { _waypoint_yaw_reached = true; } } /* Once the waypoint and yaw setpoint have been reached we can start the loiter time countdown */ if (_waypoint_position_reached && _waypoint_yaw_reached) { if (_time_first_inside_orbit == 0) { _time_first_inside_orbit = now; } /* check if the MAV was long enough inside the waypoint orbit */ if ((Navigator::get_time_inside(_mission_item) < FLT_EPSILON) || (now - _time_first_inside_orbit >= (hrt_abstime)(Navigator::get_time_inside(_mission_item) * 1e6f))) { // exit xtrack location if (_mission_item.loiter_exit_xtrack && (_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) { // reset lat/lon of loiter waypoint so vehicle exits on a tangent struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current; curr_sp->lat = _navigator->get_global_position()->lat; curr_sp->lon = _navigator->get_global_position()->lon; } return true; } } // all acceptance criteria must be met in the same iteration _waypoint_position_reached = false; _waypoint_yaw_reached = false; return false; }