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