// 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)
{
// convert back to location
Location_Class target_loc(cmd.content.location);
// use current location if not provided
if (target_loc.lat == 0 && target_loc.lng == 0) {
// To-Do: make this simpler
Vector3f temp_pos;
wp_nav.get_wp_stopping_point_xy(temp_pos);
Location_Class temp_loc(temp_pos);
target_loc.lat = temp_loc.lat;
target_loc.lng = temp_loc.lng;
}
// use current altitude if not provided
// To-Do: use z-axis stopping point instead of current alt
if (target_loc.alt == 0) {
// set to current altitude but in command's alt frame
int32_t curr_alt;
if (current_loc.get_alt_cm(target_loc.get_alt_frame(),curr_alt)) {
target_loc.set_alt_cm(curr_alt, target_loc.get_alt_frame());
} else {
// default to current altitude as alt-above-home
target_loc.set_alt_cm(current_loc.alt, current_loc.get_alt_frame());
}
}
// start way point navigator and provide it the desired location
auto_wp_start(target_loc);
}
// 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)
}
// do_nav_wp - initiate move to next waypoint
void Copter::do_nav_wp(const AP_Mission::Mission_Command& cmd)
{
Location_Class target_loc(cmd.content.location);
// use current lat, lon if zero
if (target_loc.lat == 0 && target_loc.lng == 0) {
target_loc.lat = current_loc.lat;
target_loc.lng = current_loc.lng;
}
// use current altitude if not provided
if (target_loc.alt == 0) {
// set to current altitude but in command's alt frame
int32_t curr_alt;
if (current_loc.get_alt_cm(target_loc.get_alt_frame(),curr_alt)) {
target_loc.set_alt_cm(curr_alt, target_loc.get_alt_frame());
} else {
// default to current altitude as alt-above-home
target_loc.set_alt_cm(current_loc.alt, current_loc.get_alt_frame());
}
}
// 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(target_loc);
// if no delay set the waypoint as "fast"
if (loiter_time_max == 0 ) {
wp_nav.set_fast_waypoint(true);
}
}
// 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;
// convert to location class
Location_Class target_loc(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_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, target_terr_alt_cm)) {
curr_terr_alt_cm = MAX(curr_terr_alt_cm,200);
// if using terrain, set target altitude to current altitude above terrain
target_loc.set_alt_cm(curr_terr_alt_cm, Location_Class::ALT_FRAME_ABOVE_TERRAIN);
} else {
// set target altitude to current altitude above home
target_loc.set_alt_cm(current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME);
}
auto_wp_start(target_loc);
}else{
// set landing state
land_state = LAND_STATE_DESCENDING;
// initialise landing controller
auto_land_start();
}
}
// verify_surface - returns true if surface procedure has been completed
bool Sub::verify_surface(const AP_Mission::Mission_Command& cmd)
{
bool retval = false;
switch (auto_surface_state) {
case AUTO_SURFACE_STATE_GO_TO_LOCATION:
// check if we've reached the location
if (wp_nav.reached_wp_destination()) {
// Set target to current xy and zero depth
// TODO get xy target from current wp destination, because current location may be acceptance-radius away from original destination
Location_Class target_location(cmd.content.location.lat, cmd.content.location.lng, 0, Location_Class::ALT_FRAME_ABOVE_HOME);
auto_wp_start(target_location);
// advance to next state
auto_surface_state = AUTO_SURFACE_STATE_ASCEND;
}
break;
case AUTO_SURFACE_STATE_ASCEND:
if (wp_nav.reached_wp_destination()) {
retval = true;
}
break;
default:
// this should never happen
// TO-DO: log an error
retval = true;
break;
}
// true is returned if we've successfully surfaced
return retval;
}
// 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);
}
// 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);
}
}
// do_nav_wp - initiate move to next waypoint
void Copter::do_nav_wp(const AP_Mission::Mission_Command& cmd)
{
const Vector3f &curr_pos = inertial_nav.get_position();
const Vector3f local_pos = pv_location_to_vector_with_default(cmd.content.location, curr_pos);
// 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 = abs(cmd.p1);
// Set wp navigation target
auto_wp_start(local_pos);
// if no delay set the waypoint as "fast"
if (loiter_time_max == 0 ) {
wp_nav.set_fast_waypoint(true);
}
}
// do_payload_place - initiate placing procedure
void Copter::do_payload_place(const AP_Mission::Mission_Command& cmd)
{
// 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
nav_payload_place.state = PayloadPlaceStateType_FlyToLocation;
Location_Class target_loc = terrain_adjusted_location(cmd);
auto_wp_start(target_loc);
} else {
nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start;
// initialise placing controller
auto_payload_place_start();
}
nav_payload_place.descend_max = cmd.p1;
}
// 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 = LandStateType_FlyToLocation;
Location_Class target_loc = terrain_adjusted_location(cmd);
auto_wp_start(target_loc);
}else{
// set landing state
land_state = LandStateType_Descending;
// initialise landing controller
auto_land_start();
}
}
// 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_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);
}
// do_loiter_unlimited - start loitering with no end conditions
// note: caller should set yaw_mode
void Sub::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
{
// convert back to location
Location_Class target_loc(cmd.content.location);
// use current location if not provided
if (target_loc.lat == 0 && target_loc.lng == 0) {
// To-Do: make this simpler
Vector3f temp_pos;
wp_nav.get_wp_stopping_point_xy(temp_pos);
Location_Class temp_loc(temp_pos);
target_loc.lat = temp_loc.lat;
target_loc.lng = temp_loc.lng;
}
// In mavproxy misseditor: Abs = 0 = ALT_FRAME_ABSOLUTE
// Rel = 1 = ALT_FRAME_ABOVE_HOME
// AGL = 3 = ALT_FRAME_ABOVE_TERRAIN
// 2 = ALT_FRAME_ABOVE_ORIGIN, not an option in mavproxy misseditor
// use current altitude if not provided
// To-Do: use z-axis stopping point instead of current alt
if (target_loc.alt == 0) {
// set to current altitude but in command's alt frame
int32_t curr_alt;
if (current_loc.get_alt_cm(target_loc.get_alt_frame(),curr_alt)) {
target_loc.set_alt_cm(curr_alt, target_loc.get_alt_frame());
} else {
// default to current altitude as alt-above-home
target_loc.set_alt_cm(current_loc.alt, current_loc.get_alt_frame());
}
}
// start way point navigator and provide it the desired location
auto_wp_start(target_loc);
}
// verify_payload_place - returns true if placing has been completed
bool Copter::verify_payload_place()
{
const uint16_t hover_throttle_calibrate_time = 2000; // milliseconds
const uint16_t descend_throttle_calibrate_time = 2000; // milliseconds
const float hover_throttle_placed_fraction = 0.8; // i.e. if throttle is less than 80% of hover we have placed
const float descent_throttle_placed_fraction = 0.9; // i.e. if throttle is less than 90% of descent throttle we have placed
const uint16_t placed_time = 500; // how long we have to be below a throttle threshold before considering placed
const float current_throttle_level = motors.get_throttle();
const uint32_t now = AP_HAL::millis();
// if we discover we've landed then immediately release the load:
if (ap.land_complete) {
switch (nav_payload_place.state) {
case PayloadPlaceStateType_FlyToLocation:
case PayloadPlaceStateType_Calibrating_Hover_Start:
case PayloadPlaceStateType_Calibrating_Hover:
case PayloadPlaceStateType_Descending_Start:
case PayloadPlaceStateType_Descending:
gcs_send_text_fmt(MAV_SEVERITY_INFO, "NAV_PLACE: landed");
nav_payload_place.state = PayloadPlaceStateType_Releasing_Start;
break;
case PayloadPlaceStateType_Releasing_Start:
case PayloadPlaceStateType_Releasing:
case PayloadPlaceStateType_Released:
case PayloadPlaceStateType_Ascending_Start:
case PayloadPlaceStateType_Ascending:
case PayloadPlaceStateType_Done:
break;
}
}
switch (nav_payload_place.state) {
case PayloadPlaceStateType_FlyToLocation:
if (!wp_nav.reached_wp_destination()) {
return false;
}
// we're there; set loiter target
auto_payload_place_start(wp_nav.get_wp_destination());
nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start;
// no break
case PayloadPlaceStateType_Calibrating_Hover_Start:
// hover for 1 second to get an idea of what our hover
// throttle looks like
debug("Calibrate start");
nav_payload_place.hover_start_timestamp = now;
nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover;
// no break
case PayloadPlaceStateType_Calibrating_Hover: {
if (now - nav_payload_place.hover_start_timestamp < hover_throttle_calibrate_time) {
// still calibrating...
debug("Calibrate Timer: %d", now - nav_payload_place.hover_start_timestamp);
return false;
}
// we have a valid calibration. Hopefully.
nav_payload_place.hover_throttle_level = current_throttle_level;
const float hover_throttle_delta = fabsf(nav_payload_place.hover_throttle_level - motors.get_throttle_hover());
gcs_send_text_fmt(MAV_SEVERITY_INFO, "hover throttle delta: %f", static_cast<double>(hover_throttle_delta));
nav_payload_place.state = PayloadPlaceStateType_Descending_Start;
}
// no break
case PayloadPlaceStateType_Descending_Start:
nav_payload_place.descend_start_timestamp = now;
nav_payload_place.descend_start_altitude = inertial_nav.get_altitude();
nav_payload_place.descend_throttle_level = 0;
nav_payload_place.state = PayloadPlaceStateType_Descending;
// no break
case PayloadPlaceStateType_Descending:
// make sure we don't descend too far:
debug("descended: %f cm (%f cm max)", (nav_payload_place.descend_start_altitude - inertial_nav.get_altitude()), nav_payload_place.descend_max);
if (!is_zero(nav_payload_place.descend_max) &&
nav_payload_place.descend_start_altitude - inertial_nav.get_altitude() > nav_payload_place.descend_max) {
nav_payload_place.state = PayloadPlaceStateType_Ascending;
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Reached maximum descent");
return false; // we'll do any cleanups required next time through the loop
}
// see if we've been descending long enough to calibrate a descend-throttle-level:
if (is_zero(nav_payload_place.descend_throttle_level) &&
now - nav_payload_place.descend_start_timestamp > descend_throttle_calibrate_time) {
nav_payload_place.descend_throttle_level = current_throttle_level;
}
// watch the throttle to determine whether the load has been placed
// debug("hover ratio: %f descend ratio: %f\n", current_throttle_level/nav_payload_place.hover_throttle_level, ((nav_payload_place.descend_throttle_level == 0) ? -1.0f : current_throttle_level/nav_payload_place.descend_throttle_level));
if (current_throttle_level/nav_payload_place.hover_throttle_level > hover_throttle_placed_fraction &&
(is_zero(nav_payload_place.descend_throttle_level) ||
current_throttle_level/nav_payload_place.descend_throttle_level > descent_throttle_placed_fraction)) {
// throttle is above both threshold ratios (or above hover threshold ration and descent threshold ratio not yet valid)
nav_payload_place.place_start_timestamp = 0;
return false;
}
if (nav_payload_place.place_start_timestamp == 0) {
// we've only just now hit the correct throttle level
nav_payload_place.place_start_timestamp = now;
return false;
} else if (now - nav_payload_place.place_start_timestamp < placed_time) {
// keep going down....
debug("Place Timer: %d", now - nav_payload_place.place_start_timestamp);
return false;
}
nav_payload_place.state = PayloadPlaceStateType_Releasing_Start;
// no break
case PayloadPlaceStateType_Releasing_Start:
if (g2.gripper.valid()) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Releasing the gripper");
g2.gripper.release();
} else {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Gripper not valid");
}
nav_payload_place.state = PayloadPlaceStateType_Releasing;
// no break
case PayloadPlaceStateType_Releasing:
if (g2.gripper.valid() && !g2.gripper.released()) {
return false;
}
nav_payload_place.state = PayloadPlaceStateType_Released;
// no break
case PayloadPlaceStateType_Released: {
nav_payload_place.state = PayloadPlaceStateType_Ascending_Start;
}
// no break
case PayloadPlaceStateType_Ascending_Start: {
Location_Class target_loc = inertial_nav.get_position();
target_loc.alt = nav_payload_place.descend_start_altitude;
auto_wp_start(target_loc);
nav_payload_place.state = PayloadPlaceStateType_Ascending;
}
// no break
case PayloadPlaceStateType_Ascending:
if (!wp_nav.reached_wp_destination()) {
return false;
}
nav_payload_place.state = PayloadPlaceStateType_Done;
// no break
case PayloadPlaceStateType_Done:
return true;
default:
// this should never happen
// TO-DO: log an error
return true;
}
// should never get here
return true;
}
void Sub::do_RTL()
{
auto_wp_start(ahrs.get_home());
}
