// 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_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_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();
}
}
// terrain_adjusted_location: returns a Location with lat/lon from cmd
// and altitude from our current altitude adjusted for location
Location_Class Copter::terrain_adjusted_location(const AP_Mission::Mission_Command& cmd) const
{
// 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);
}
return target_loc;
}
// 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);
}
// do_spline_wp - initiate move to next waypoint
void Copter::do_spline_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;
// determine segment start and end type
bool stopped_at_start = true;
AC_WPNav::spline_segment_end_type seg_end_type = AC_WPNav::SEGMENT_END_STOP;
AP_Mission::Mission_Command temp_cmd;
// if previous command was a wp_nav command with no delay set stopped_at_start to false
// To-Do: move processing of delay into wp-nav controller to allow it to determine the stopped_at_start value itself?
uint16_t prev_cmd_idx = mission.get_prev_nav_cmd_index();
if (prev_cmd_idx != AP_MISSION_CMD_INDEX_NONE) {
if (mission.read_cmd_from_storage(prev_cmd_idx, temp_cmd)) {
if ((temp_cmd.id == MAV_CMD_NAV_WAYPOINT || temp_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT) && temp_cmd.p1 == 0) {
stopped_at_start = false;
}
}
}
// if there is no delay at the end of this segment get next nav command
Location_Class next_loc;
if (cmd.p1 == 0 && mission.get_next_nav_cmd(cmd.index+1, temp_cmd)) {
next_loc = temp_cmd.content.location;
// default lat, lon to first waypoint's lat, lon
if (next_loc.lat == 0 && next_loc.lng == 0) {
next_loc.lat = target_loc.lat;
next_loc.lng = target_loc.lng;
}
// default alt to first waypoint's alt but in next waypoint's alt frame
if (next_loc.alt == 0) {
int32_t next_alt;
if (target_loc.get_alt_cm(next_loc.get_alt_frame(), next_alt)) {
next_loc.set_alt_cm(next_alt, next_loc.get_alt_frame());
} else {
// default to first waypoints altitude
next_loc.set_alt_cm(target_loc.alt, target_loc.get_alt_frame());
}
}
// if the next nav command is a waypoint set end type to spline or straight
if (temp_cmd.id == MAV_CMD_NAV_WAYPOINT) {
seg_end_type = AC_WPNav::SEGMENT_END_STRAIGHT;
}else if (temp_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT) {
seg_end_type = AC_WPNav::SEGMENT_END_SPLINE;
}
}
// set spline navigation target
auto_spline_start(target_loc, stopped_at_start, seg_end_type, next_loc);
}
// start_command - this function will be called when the ap_mission lib wishes to start a new command
bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
{
// To-Do: logging when new commands start/end
if (should_log(MASK_LOG_CMD)) {
DataFlash.Log_Write_Mission_Cmd(mission, cmd);
}
Location_Class target_loc(cmd.content.location);
// target alt must be negative (underwater)
if (target_loc.alt > 0.0f) {
gcs().send_text(MAV_SEVERITY_WARNING, "BAD NAV ALT %0.2f", (double)target_loc.alt);
return true;
}
// only tested/supported alt frame so far is ALT_FRAME_ABOVE_HOME, where Home alt is always water's surface ie zero depth
if (target_loc.get_alt_frame() != Location_Class::ALT_FRAME_ABOVE_HOME) {
gcs().send_text(MAV_SEVERITY_WARNING, "BAD NAV ALT_FRAME %d", (int8_t)target_loc.get_alt_frame());
return true;
}
switch (cmd.id) {
///
/// navigation commands
///
case MAV_CMD_NAV_WAYPOINT: // 16 Navigate to Waypoint
do_nav_wp(cmd);
break;
case MAV_CMD_NAV_LAND: // 21 LAND to Waypoint
do_surface(cmd);
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
do_RTL();
break;
case MAV_CMD_NAV_LOITER_UNLIM: // 17 Loiter indefinitely
do_loiter_unlimited(cmd);
break;
case MAV_CMD_NAV_LOITER_TURNS: //18 Loiter N Times
do_circle(cmd);
break;
case MAV_CMD_NAV_LOITER_TIME: // 19
do_loiter_time(cmd);
break;
case MAV_CMD_NAV_SPLINE_WAYPOINT: // 82 Navigate to Waypoint using spline
do_spline_wp(cmd);
break;
#if NAV_GUIDED == ENABLED
case MAV_CMD_NAV_GUIDED_ENABLE: // 92 accept navigation commands from external nav computer
do_nav_guided_enable(cmd);
break;
#endif
case MAV_CMD_NAV_DELAY: // 94 Delay the next navigation command
do_nav_delay(cmd);
break;
//
// conditional commands
//
case MAV_CMD_CONDITION_DELAY: // 112
do_wait_delay(cmd);
break;
case MAV_CMD_CONDITION_DISTANCE: // 114
do_within_distance(cmd);
break;
case MAV_CMD_CONDITION_YAW: // 115
do_yaw(cmd);
break;
///
/// do commands
///
case MAV_CMD_DO_CHANGE_SPEED: // 178
do_change_speed(cmd);
break;
case MAV_CMD_DO_SET_HOME: // 179
do_set_home(cmd);
break;
case MAV_CMD_DO_SET_SERVO:
ServoRelayEvents.do_set_servo(cmd.content.servo.channel, cmd.content.servo.pwm);
break;
case MAV_CMD_DO_SET_RELAY:
ServoRelayEvents.do_set_relay(cmd.content.relay.num, cmd.content.relay.state);
break;
case MAV_CMD_DO_REPEAT_SERVO:
ServoRelayEvents.do_repeat_servo(cmd.content.repeat_servo.channel, cmd.content.repeat_servo.pwm,
cmd.content.repeat_servo.repeat_count, cmd.content.repeat_servo.cycle_time * 1000.0f);
break;
case MAV_CMD_DO_REPEAT_RELAY:
ServoRelayEvents.do_repeat_relay(cmd.content.repeat_relay.num, cmd.content.repeat_relay.repeat_count,
cmd.content.repeat_relay.cycle_time * 1000.0f);
break;
case MAV_CMD_DO_SET_ROI: // 201
// point the vehicle and camera at a region of interest (ROI)
do_roi(cmd);
break;
case MAV_CMD_DO_MOUNT_CONTROL: // 205
// point the camera to a specified angle
do_mount_control(cmd);
break;
#if CAMERA == ENABLED
case MAV_CMD_DO_CONTROL_VIDEO: // Control on-board camera capturing. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|
break;
case MAV_CMD_DO_DIGICAM_CONFIGURE: // Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)|
do_digicam_configure(cmd);
break;
case MAV_CMD_DO_DIGICAM_CONTROL: // Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty|
do_digicam_control(cmd);
break;
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
camera.set_trigger_distance(cmd.content.cam_trigg_dist.meters);
break;
#endif
#if GRIPPER_ENABLED == ENABLED
case MAV_CMD_DO_GRIPPER: // Mission command to control gripper
do_gripper(cmd);
break;
#endif
#if NAV_GUIDED == ENABLED
case MAV_CMD_DO_GUIDED_LIMITS: // 220 accept guided mode limits
do_guided_limits(cmd);
break;
#endif
default:
// do nothing with unrecognized MAVLink messages
break;
}
// always return success
return true;
}
// start_command - this function will be called when the ap_mission lib wishes to start a new command
bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
{
// To-Do: logging when new commands start/end
if (should_log(MASK_LOG_CMD)) {
DataFlash.Log_Write_Mission_Cmd(mission, cmd);
}
Location_Class target_loc(cmd.content.location);
// target alt must be negative (underwater)
if (target_loc.alt > 0.0f) {
gcs().send_text(MAV_SEVERITY_WARNING, "BAD NAV ALT %0.2f", (double)target_loc.alt);
return false;
}
// only tested/supported alt frame so far is ALT_FRAME_ABOVE_HOME, where Home alt is always water's surface ie zero depth
if (target_loc.get_alt_frame() != Location_Class::ALT_FRAME_ABOVE_HOME) {
gcs().send_text(MAV_SEVERITY_WARNING, "BAD NAV ALT_FRAME %d", (int8_t)target_loc.get_alt_frame());
return false;
}
switch (cmd.id) {
///
/// navigation commands
///
case MAV_CMD_NAV_WAYPOINT: // 16 Navigate to Waypoint
do_nav_wp(cmd);
break;
case MAV_CMD_NAV_LAND: // 21 LAND to Waypoint
do_surface(cmd);
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
do_RTL();
break;
case MAV_CMD_NAV_LOITER_UNLIM: // 17 Loiter indefinitely
do_loiter_unlimited(cmd);
break;
case MAV_CMD_NAV_LOITER_TURNS: //18 Loiter N Times
do_circle(cmd);
break;
case MAV_CMD_NAV_LOITER_TIME: // 19
do_loiter_time(cmd);
break;
case MAV_CMD_NAV_SPLINE_WAYPOINT: // 82 Navigate to Waypoint using spline
do_spline_wp(cmd);
break;
#if NAV_GUIDED == ENABLED
case MAV_CMD_NAV_GUIDED_ENABLE: // 92 accept navigation commands from external nav computer
do_nav_guided_enable(cmd);
break;
#endif
case MAV_CMD_NAV_DELAY: // 93 Delay the next navigation command
do_nav_delay(cmd);
break;
//
// conditional commands
//
case MAV_CMD_CONDITION_DELAY: // 112
do_wait_delay(cmd);
break;
case MAV_CMD_CONDITION_DISTANCE: // 114
do_within_distance(cmd);
break;
case MAV_CMD_CONDITION_YAW: // 115
do_yaw(cmd);
break;
///
/// do commands
///
case MAV_CMD_DO_CHANGE_SPEED: // 178
do_change_speed(cmd);
break;
case MAV_CMD_DO_SET_HOME: // 179
do_set_home(cmd);
break;
case MAV_CMD_DO_SET_ROI: // 201
// point the vehicle and camera at a region of interest (ROI)
do_roi(cmd);
break;
case MAV_CMD_DO_MOUNT_CONTROL: // 205
// point the camera to a specified angle
do_mount_control(cmd);
break;
#if NAV_GUIDED == ENABLED
case MAV_CMD_DO_GUIDED_LIMITS: // 222 accept guided mode limits
do_guided_limits(cmd);
break;
#endif
default:
// unable to use the command, allow the vehicle to try the next command
return false;
}
// always return success
return true;
}
