// set_auto_yaw_roi - sets the yaw to look at roi for auto mode
void Copter::set_auto_yaw_roi(const Location &roi_location)
{
// if location is zero lat, lon and altitude turn off ROI
if (roi_location.alt == 0 && roi_location.lat == 0 && roi_location.lng == 0) {
// set auto yaw mode back to default assuming the active command is a waypoint command. A more sophisticated method is required to ensure we return to the proper yaw control for the active command
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
#if MOUNT == ENABLED
// switch off the camera tracking if enabled
if (camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
camera_mount.set_mode_to_default();
}
#endif // MOUNT == ENABLED
}else{
#if MOUNT == ENABLED
// check if mount type requires us to rotate the quad
if(!camera_mount.has_pan_control()) {
roi_WP = pv_location_to_vector(roi_location);
set_auto_yaw_mode(AUTO_YAW_ROI);
}
// send the command to the camera mount
camera_mount.set_roi_target(roi_location);
// TO-DO: expand handling of the do_nav_roi to support all modes of the MAVLink. Currently we only handle mode 4 (see below)
// 0: do nothing
// 1: point at next waypoint
// 2: point at a waypoint taken from WP# parameter (2nd parameter?)
// 3: point at a location given by alt, lon, lat parameters
// 4: point at a target given a target id (can't be implemented)
#else
// if we have no camera mount aim the quad at the location
roi_WP = pv_location_to_vector(roi_location);
set_auto_yaw_mode(AUTO_YAW_ROI);
#endif // MOUNT == ENABLED
}
}
void Copter::rtl_build_path()
{
// origin point is our stopping point
pos_control.get_stopping_point_xy(rtl_path.origin_point);
pos_control.get_stopping_point_z(rtl_path.origin_point);
// set return target to nearest rally point or home position
#if AC_RALLY == ENABLED
Location rally_point = rally.calc_best_rally_or_home_location(current_loc, 0);
rtl_path.return_target = pv_location_to_vector(rally_point);
#else
rtl_path.return_target = pv_location_to_vector(ahrs.get_home());
#endif
Vector3f return_vector = rtl_path.return_target-rtl_path.origin_point;
float rtl_return_dist = pythagorous2(return_vector.x, return_vector.y);
// compute return altitude
rtl_path.return_target.z = rtl_compute_return_alt_above_origin(rtl_return_dist);
// climb target is above our origin point at the return altitude
rtl_path.climb_target.x = rtl_path.origin_point.x;
rtl_path.climb_target.y = rtl_path.origin_point.y;
rtl_path.climb_target.z = rtl_path.return_target.z;
// descent target is below return target at rtl_alt_final
rtl_path.descent_target.x = rtl_path.return_target.x;
rtl_path.descent_target.y = rtl_path.return_target.y;
rtl_path.descent_target.z = pv_alt_above_origin(g.rtl_alt_final);
// set land flag
rtl_path.land = g.rtl_alt_final <= 0;
}
// verify_circle - check if we have circled the point enough
bool Copter::verify_circle(const AP_Mission::Mission_Command& cmd)
{
// check if we've reached the edge
if (auto_mode == Auto_CircleMoveToEdge) {
if (wp_nav.reached_wp_destination()) {
Vector3f curr_pos = inertial_nav.get_position();
Vector3f circle_center = pv_location_to_vector(cmd.content.location);
// set target altitude if not provided
if (is_zero(circle_center.z)) {
circle_center.z = curr_pos.z;
}
// set lat/lon position if not provided
if (cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
circle_center.x = curr_pos.x;
circle_center.y = curr_pos.y;
}
// start circling
auto_circle_start();
}
return false;
}
// check if we have completed circling
return fabsf(circle_nav.get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1);
}
// do_guided - start guided mode
bool Copter::do_guided(const AP_Mission::Mission_Command& cmd)
{
Vector3f pos_or_vel; // target location or velocity
// only process guided waypoint if we are in guided mode
if (control_mode != GUIDED && !(control_mode == AUTO && auto_mode == Auto_NavGuided)) {
return false;
}
// switch to handle different commands
switch (cmd.id) {
case MAV_CMD_NAV_WAYPOINT:
// set wp_nav's destination
pos_or_vel = pv_location_to_vector(cmd.content.location);
guided_set_destination(pos_or_vel);
return true;
break;
case MAV_CMD_CONDITION_YAW:
do_yaw(cmd);
return true;
break;
default:
// reject unrecognised command
return false;
break;
}
return true;
}
// do_loiter_time - initiate loitering at a point for a given time period
// note: caller should set yaw_mode
void Copter::do_loiter_time(const AP_Mission::Mission_Command& cmd)
{
Vector3f target_pos;
// get current position
Vector3f curr_pos = inertial_nav.get_position();
// default to use position provided
target_pos = pv_location_to_vector(cmd.content.location);
// use current location if not provided
if(cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
wp_nav.get_wp_stopping_point_xy(target_pos);
}
// use current altitude if not provided
if( cmd.content.location.alt == 0 ) {
target_pos.z = curr_pos.z;
}
// start way point navigator and provide it the desired location
auto_wp_start(target_pos);
// setup loiter timer
loiter_time = 0;
loiter_time_max = cmd.p1; // units are (seconds)
}
// fence_check - ask fence library to check for breaches and initiate the response
// called at 1hz
void Sub::fence_check()
{
uint8_t new_breaches; // the type of fence that has been breached
uint8_t orig_breaches = fence.get_breaches();
Vector3f home = pv_location_to_vector(ahrs.get_home());
Vector3f curr = inertial_nav.get_position();
int32_t home_distance = pv_get_horizontal_distance_cm(curr, home);
// give fence library our current distance from home in meters
fence.set_home_distance(home_distance*0.01f);
// check for a breach
new_breaches = fence.check_fence(current_loc.alt/100.0f);
// return immediately if motors are not armed
if (!motors.armed()) {
return;
}
// if there is a new breach take action
if (new_breaches != AC_FENCE_TYPE_NONE) {
// if the user wants some kind of response and motors are armed
if (fence.get_action() != AC_FENCE_ACTION_REPORT_ONLY) {
//
// // disarm immediately if we think we are on the ground or in a manual flight mode with zero throttle
// // don't disarm if the high-altitude fence has been broken because it's likely the user has pulled their throttle to zero to bring it down
// if (ap.land_complete || (mode_has_manual_throttle(control_mode) && ap.throttle_zero && !failsafe.manual_control && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0))){
// init_disarm_motors();
// }else{
// // if we are within 100m of the fence, RTL
// if (fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
// if (!set_mode(RTL, MODE_REASON_FENCE_BREACH)) {
// set_mode(LAND, MODE_REASON_FENCE_BREACH);
// }
// }else{
// // if more than 100m outside the fence just force a land
// set_mode(LAND, MODE_REASON_FENCE_BREACH);
// }
// }
}
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_FENCE, new_breaches);
}
// record clearing of breach
if (orig_breaches != AC_FENCE_TYPE_NONE && fence.get_breaches() == AC_FENCE_TYPE_NONE) {
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_FENCE, ERROR_CODE_ERROR_RESOLVED);
}
}
// rtl_return_start - initialise return to home
void Copter::rtl_return_start()
{
rtl_state = RTL_ReturnHome;
rtl_state_complete = false;
// set target to above home/rally point
#if AC_RALLY == ENABLED
// rally_point will be the nearest rally point or home. uses absolute altitudes
Location rally_point = rally.calc_best_rally_or_home_location(current_loc, rtl_alt+ahrs.get_home().alt);
rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home
rally_point.alt = max(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching home
Vector3f destination = pv_location_to_vector(rally_point);
#else
Vector3f destination = pv_location_to_vector(ahrs.get_home());
destination.z = pv_alt_above_origin(rtl_alt));
#endif
wp_nav.set_wp_destination(destination);
// initialise yaw to point home (maybe)
set_auto_yaw_mode(get_default_auto_yaw_mode(true));
}
// calc_home_distance_and_bearing - calculate distance and bearing to home for reporting and autopilot decisions
void Copter::calc_home_distance_and_bearing()
{
// calculate home distance and bearing
if (position_ok()) {
Vector3f home = pv_location_to_vector(ahrs.get_home());
Vector3f curr = inertial_nav.get_position();
home_distance = pv_get_horizontal_distance_cm(curr, home);
home_bearing = pv_get_bearing_cd(curr,home);
// update super simple bearing (if required) because it relies on home_bearing
update_super_simple_bearing(false);
}
}
// do_land - initiate landing procedure
void Copter::do_land(const AP_Mission::Mission_Command& cmd)
{
// To-Do: check if we have already landed
// 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 fly to location
land_state = LAND_STATE_FLY_TO_LOCATION;
// calculate and set desired location above landing target
Vector3f pos = pv_location_to_vector(cmd.content.location);
pos.z = inertial_nav.get_altitude();
auto_wp_start(pos);
}else{
// set landing state
land_state = LAND_STATE_DESCENDING;
// initialise landing controller
auto_land_start();
}
}
// do_circle - initiate moving in a circle
void Copter::do_circle(const AP_Mission::Mission_Command& cmd)
{
Vector3f curr_pos = inertial_nav.get_position();
Vector3f circle_center = pv_location_to_vector(cmd.content.location);
uint8_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1
bool move_to_edge_required = false;
// set target altitude if not provided
if (cmd.content.location.alt == 0) {
circle_center.z = curr_pos.z;
} else {
move_to_edge_required = true;
}
// set lat/lon position if not provided
// To-Do: use previous command's destination if it was a straight line or spline waypoint command
if (cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
circle_center.x = curr_pos.x;
circle_center.y = curr_pos.y;
} else {
move_to_edge_required = true;
}
// set circle controller's center
circle_nav.set_center(circle_center);
// set circle radius
if (circle_radius_m != 0) {
circle_nav.set_radius((float)circle_radius_m * 100.0f);
}
// check if we need to move to edge of circle
if (move_to_edge_required) {
// move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge
auto_circle_movetoedge_start();
} else {
// start circling
auto_circle_start();
}
}
// do_loiter_unlimited - start loitering with no end conditions
// note: caller should set yaw_mode
void Copter::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
{
Vector3f target_pos;
// get current position
Vector3f curr_pos = inertial_nav.get_position();
// default to use position provided
target_pos = pv_location_to_vector(cmd.content.location);
// use current location if not provided
if(cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
wp_nav.get_wp_stopping_point_xy(target_pos);
}
// use current altitude if not provided
// To-Do: use z-axis stopping point instead of current alt
if( cmd.content.location.alt == 0 ) {
target_pos.z = curr_pos.z;
}
// start way point navigator and provide it the desired location
auto_wp_start(target_pos);
}
// returns distance between a destination and home in cm
float Sub::pv_distance_to_home_cm(const Vector3f &destination)
{
Vector3f home = pv_location_to_vector(ahrs.get_home());
return pv_get_horizontal_distance_cm(home, destination);
}
