void Copter::rtl_build_path(bool terrain_following_allowed) { // origin point is our stopping point Vector3f stopping_point; pos_control.get_stopping_point_xy(stopping_point); pos_control.get_stopping_point_z(stopping_point); rtl_path.origin_point = Location_Class(stopping_point); rtl_path.origin_point.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_HOME); // set return target to nearest rally point or home position #if AC_RALLY == ENABLED rtl_path.return_target = rally.calc_best_rally_or_home_location(current_loc, ahrs.get_home().alt); #else rtl_path.return_target = ahrs.get_home(); #endif // compute return altitude rtl_compute_return_alt(rtl_path.origin_point, rtl_path.return_target, terrain_following_allowed); // climb target is above our origin point at the return altitude rtl_path.climb_target = Location_Class(rtl_path.origin_point.lat, rtl_path.origin_point.lng, rtl_path.return_target.alt, rtl_path.return_target.get_alt_frame()); // descent target is below return target at rtl_alt_final rtl_path.descent_target = Location_Class(rtl_path.return_target.lat, rtl_path.return_target.lng, g.rtl_alt_final, Location_Class::ALT_FRAME_ABOVE_HOME); // set land flag rtl_path.land = g.rtl_alt_final <= 0; }
// do_surface - initiate surface procedure void Sub::do_surface(const AP_Mission::Mission_Command& cmd) { Location_Class target_location; // if location provided we fly to that location at current altitude if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) { // set state to go to location auto_surface_state = AUTO_SURFACE_STATE_GO_TO_LOCATION; // calculate and set desired location below surface target // convert to location class target_location = Location_Class(cmd.content.location); // decide if we will use terrain following int32_t curr_terr_alt_cm, target_terr_alt_cm; if (current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_terr_alt_cm) && target_location.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, target_terr_alt_cm)) { // if using terrain, set target altitude to current altitude above terrain target_location.set_alt_cm(curr_terr_alt_cm, Location_Class::ALT_FRAME_ABOVE_TERRAIN); } else { // set target altitude to current altitude above home target_location.set_alt_cm(current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME); } } else { // set surface state to ascend auto_surface_state = AUTO_SURFACE_STATE_ASCEND; // Set waypoint destination to current location at zero depth target_location = Location_Class(current_loc.lat, current_loc.lng, 0, Location_Class::ALT_FRAME_ABOVE_HOME); } // Go to wp location auto_wp_start(target_location); }
/* * Update the vehicle list. If the vehicle is already in the * list then it will update it, otherwise it will be added. */ void AP_ADSB::update_vehicle(const mavlink_message_t* packet) { if (in_state.vehicle_list == nullptr) { // We are only null when disabled. Updating is inhibited. return; } uint16_t index = in_state.list_size + 1; // initialize with invalid index adsb_vehicle_t vehicle {}; mavlink_msg_adsb_vehicle_decode(packet, &vehicle.info); Location_Class vehicle_loc = Location_Class(AP_ADSB::get_location(vehicle)); bool my_loc_is_zero = _my_loc.is_zero(); float my_loc_distance_to_vehicle = _my_loc.get_distance(vehicle_loc); bool out_of_range = in_state.list_radius > 0 && !my_loc_is_zero && my_loc_distance_to_vehicle > in_state.list_radius; bool is_tracked_in_list = find_index(vehicle, &index); if (vehicle_loc.is_zero() || out_of_range) { // vehicle is out of range or invalid lat/lng. If we're tracking it, delete from list. Otherwise ignore it. if (is_tracked_in_list) { delete_vehicle(index); } return; } else if (is_tracked_in_list) { // found, update it set_vehicle(index, vehicle); } else if (in_state.vehicle_count < in_state.list_size) { // not found and there's room, add it to the end of the list set_vehicle(in_state.vehicle_count, vehicle); in_state.vehicle_count++; } else if (!my_loc_is_zero && !is_zero(avoid_state.lowest_threat_distance) && my_loc_distance_to_vehicle < avoid_state.lowest_threat_distance) { // is closer than the furthest // buffer is full, replace the vehicle with lowest threat as long as it's not further away // overwrite the lowest_threat/furthest index = avoid_state.lowest_threat_index; set_vehicle(index, vehicle); // this is now invalid because the vehicle was overwritten, need // to run perform_threat_detection() to determine new one because // we aren't keeping track of the second-furthest vehicle. avoid_state.lowest_threat_distance = 0; // is it the nearest? Then it's the highest threat. That's an easy check // that we don't need to run perform_threat_detection() to determine if (avoid_state.highest_threat_distance > my_loc_distance_to_vehicle) { avoid_state.highest_threat_distance = my_loc_distance_to_vehicle; avoid_state.highest_threat_index = index; } } // if buffer full vehicle.last_update_ms = AP_HAL::millis() - (vehicle.info.tslc * 1000); push_sample(vehicle); // note that set_vehicle modifies vehicle }
/* * Convert/Extract a Location from a vehicle */ Location_Class AP_ADSB::get_location(const adsb_vehicle_t &vehicle) const { Location_Class loc = Location_Class( vehicle.info.lat, vehicle.info.lon, vehicle.info.altitude * 0.1f, Location_Class::ALT_FRAME_ABSOLUTE); return loc; }
void Copter::ModeRTL::build_path(bool terrain_following_allowed) { // origin point is our stopping point Vector3f stopping_point; pos_control->get_stopping_point_xy(stopping_point); pos_control->get_stopping_point_z(stopping_point); rtl_path.origin_point = Location_Class(stopping_point); rtl_path.origin_point.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_HOME); // compute return target compute_return_target(terrain_following_allowed); // climb target is above our origin point at the return altitude rtl_path.climb_target = Location_Class(rtl_path.origin_point.lat, rtl_path.origin_point.lng, rtl_path.return_target.alt, rtl_path.return_target.get_alt_frame()); // descent target is below return target at rtl_alt_final rtl_path.descent_target = Location_Class(rtl_path.return_target.lat, rtl_path.return_target.lng, g.rtl_alt_final, Location_Class::ALT_FRAME_ABOVE_HOME); // set land flag rtl_path.land = g.rtl_alt_final <= 0; }
// do_nav_wp - initiate move to next waypoint void Copter::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; // Set wp navigation target auto_wp_start(Location_Class(cmd.content.location)); // if no delay set the waypoint as "fast" if (loiter_time_max == 0 ) { wp_nav.set_fast_waypoint(true); } }
/* * Update the vehicle list. If the vehicle is already in the * list then it will update it, otherwise it will be added. */ void AP_ADSB::handle_vehicle(const mavlink_message_t* packet) { if (in_state.vehicle_list == nullptr) { // We are only null when disabled. Updating is inhibited. return; } uint16_t index = in_state.list_size + 1; // initialize with invalid index adsb_vehicle_t vehicle {}; mavlink_msg_adsb_vehicle_decode(packet, &vehicle.info); Location_Class vehicle_loc = Location_Class(AP_ADSB::get_location(vehicle)); bool my_loc_is_zero = _my_loc.is_zero(); float my_loc_distance_to_vehicle = _my_loc.get_distance(vehicle_loc); bool out_of_range = in_state.list_radius > 0 && !my_loc_is_zero && my_loc_distance_to_vehicle > in_state.list_radius; bool is_tracked_in_list = find_index(vehicle, &index); uint32_t now = AP_HAL::millis(); // note the last time the receiver got a packet from the aircraft vehicle.last_update_ms = now - (vehicle.info.tslc * 1000); const uint16_t required_flags_position = ADSB_FLAGS_VALID_COORDS | ADSB_FLAGS_VALID_ALTITUDE; const bool detected_ourself = (out_state.cfg.ICAO_id != 0) && ((uint32_t)out_state.cfg.ICAO_id == vehicle.info.ICAO_address); if (vehicle_loc.is_zero() || out_of_range || detected_ourself || (vehicle.info.ICAO_address > 0x00FFFFFF) || // ICAO address is 24bits, so ignore higher values. !(vehicle.info.flags & required_flags_position) || now - vehicle.last_update_ms > VEHICLE_TIMEOUT_MS) { // vehicle is out of range or invalid lat/lng. If we're tracking it, delete from list. Otherwise ignore it. if (is_tracked_in_list) { delete_vehicle(index); } return; } else if (is_tracked_in_list) { // found, update it set_vehicle(index, vehicle); } else if (in_state.vehicle_count < in_state.list_size) { // not found and there's room, add it to the end of the list set_vehicle(in_state.vehicle_count, vehicle); in_state.vehicle_count++; } else { // buffer is full. if new vehicle is closer than furthest, replace furthest with new if (my_loc_is_zero) { // nothing else to do furthest_vehicle_distance = 0; furthest_vehicle_index = 0; } else { if (furthest_vehicle_distance <= 0) { // ensure this is populated determine_furthest_aircraft(); } if (my_loc_distance_to_vehicle < furthest_vehicle_distance) { // is closer than the furthest // replace with the furthest vehicle set_vehicle(furthest_vehicle_index, vehicle); // furthest_vehicle_index is now invalid because the vehicle was overwritten, need // to run determine_furthest_aircraft() to determine a new one next time furthest_vehicle_distance = 0; furthest_vehicle_index = 0; } } } // if buffer full const uint16_t required_flags_avoidance = ADSB_FLAGS_VALID_COORDS | ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_HEADING | ADSB_FLAGS_VALID_VELOCITY; if (vehicle.info.flags & required_flags_avoidance) { push_sample(vehicle); // note that set_vehicle modifies vehicle } }