2015-05-13 00:16:45 -03:00
# include "Rover.h"
2017-08-02 22:33:33 -03:00
// update mission including starting or stopping commands. called by scheduler at 10Hz
void Rover : : update_mission ( void )
{
if ( control_mode = = & mode_auto ) {
2018-12-11 20:10:20 -04:00
if ( ahrs . home_is_set ( ) & & mode_auto . mission . num_commands ( ) > 1 ) {
mode_auto . mission . update ( ) ;
2017-08-02 22:33:33 -03:00
}
}
}
2012-04-30 04:17:14 -03:00
/********************************************************************************/
// Command Event Handlers
/********************************************************************************/
2018-12-11 20:10:20 -04:00
bool ModeAuto : : start_command ( const AP_Mission : : Mission_Command & cmd )
2012-04-30 04:17:14 -03:00
{
2014-03-18 23:42:02 -03:00
// log when new commands start
2018-12-11 20:10:20 -04:00
if ( rover . should_log ( MASK_LOG_CMD ) ) {
rover . DataFlash . Log_Write_Mission_Cmd ( mission , cmd ) ;
2014-03-18 23:42:02 -03:00
}
2017-11-26 18:24:21 -04:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Executing %s(ID=%i) " ,
cmd . type ( ) , cmd . id ) ;
2012-04-30 04:17:14 -03:00
2016-10-30 06:21:03 -03:00
switch ( cmd . id ) {
2016-12-20 09:32:19 -04:00
case MAV_CMD_NAV_WAYPOINT : // Navigate to Waypoint
2017-08-02 03:56:31 -03:00
do_nav_wp ( cmd , false ) ;
2016-10-30 06:21:03 -03:00
break ;
case MAV_CMD_NAV_RETURN_TO_LAUNCH :
do_RTL ( ) ;
break ;
2017-12-06 22:42:34 -04:00
case MAV_CMD_NAV_LOITER_UNLIM : // Loiter indefinitely
case MAV_CMD_NAV_LOITER_TIME : // Loiter for specified time
do_nav_wp ( cmd , true ) ;
2016-10-30 07:42:46 -03:00
break ;
2017-08-02 02:31:26 -03:00
case MAV_CMD_NAV_SET_YAW_SPEED :
do_nav_set_yaw_speed ( cmd ) ;
break ;
2016-10-30 06:21:03 -03:00
// Conditional commands
case MAV_CMD_CONDITION_DELAY :
do_wait_delay ( cmd ) ;
break ;
case MAV_CMD_CONDITION_DISTANCE :
do_within_distance ( cmd ) ;
break ;
// Do commands
case MAV_CMD_DO_CHANGE_SPEED :
do_change_speed ( cmd ) ;
break ;
case MAV_CMD_DO_SET_HOME :
do_set_home ( cmd ) ;
break ;
2012-04-30 04:17:14 -03:00
# if MOUNT == ENABLED
2016-10-30 06:21:03 -03:00
// Sets the region of interest (ROI) for a sensor set or the
// vehicle itself. This can then be used by the vehicles control
// system to control the vehicle attitude and the attitude of various
// devices such as cameras.
// |Region of interest mode. (see MAV_ROI enum)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple cameras etc.)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|
case MAV_CMD_DO_SET_ROI :
if ( cmd . content . location . alt = = 0 & & cmd . content . location . lat = = 0 & & cmd . content . location . lng = = 0 ) {
// switch off the camera tracking if enabled
2018-12-11 20:10:20 -04:00
if ( rover . camera_mount . get_mode ( ) = = MAV_MOUNT_MODE_GPS_POINT ) {
rover . camera_mount . set_mode_to_default ( ) ;
2014-07-04 03:43:08 -03:00
}
2016-10-30 06:21:03 -03:00
} else {
// send the command to the camera mount
2018-12-11 20:10:20 -04:00
rover . camera_mount . set_roi_target ( cmd . content . location ) ;
2016-10-30 06:21:03 -03:00
}
break ;
2012-04-30 04:17:14 -03:00
# endif
2014-03-10 05:45:26 -03:00
2016-10-30 06:21:03 -03:00
case MAV_CMD_DO_SET_REVERSE :
do_set_reverse ( cmd ) ;
break ;
2016-07-14 04:50:39 -03:00
2017-08-16 07:02:56 -03:00
case MAV_CMD_DO_FENCE_ENABLE :
if ( cmd . p1 = = 0 ) { //disable
g2 . fence . enable ( false ) ;
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Fence Disabled " ) ;
} else { //enable fence
g2 . fence . enable ( true ) ;
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Fence Enabled " ) ;
}
break ;
2016-10-30 06:21:03 -03:00
default :
// return false for unhandled commands
return false ;
}
2014-03-10 05:45:26 -03:00
2016-10-30 06:21:03 -03:00
// if we got this far we must have been successful
return true ;
2012-04-30 04:17:14 -03:00
}
2014-03-17 04:08:10 -03:00
// exit_mission - callback function called from ap-mission when the mission has completed
2018-12-11 20:10:20 -04:00
void ModeAuto : : exit_mission ( )
2014-03-10 05:45:26 -03:00
{
2018-11-08 12:14:45 -04:00
// play a tone
AP_Notify : : events . mission_complete = 1 ;
// send message
2017-08-02 03:56:31 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_NOTICE , " Mission Complete " ) ;
2018-11-08 12:14:45 -04:00
2018-12-11 20:10:20 -04:00
if ( g2 . mis_done_behave = = MIS_DONE_BEHAVE_LOITER & & rover . set_mode ( rover . mode_loiter , MODE_REASON_MISSION_END ) ) {
2018-11-08 12:14:45 -04:00
return ;
}
2018-12-11 20:10:20 -04:00
rover . set_mode ( rover . mode_hold , MODE_REASON_MISSION_END ) ;
2012-04-30 04:17:14 -03:00
}
2015-07-24 05:20:49 -03:00
// verify_command_callback - callback function called from ap-mission at 10hz or higher when a command is being run
// we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
2018-12-11 20:10:20 -04:00
bool ModeAuto : : verify_command_callback ( const AP_Mission : : Mission_Command & cmd )
2015-07-24 05:20:49 -03:00
{
2017-07-19 06:57:10 -03:00
const bool cmd_complete = verify_command ( cmd ) ;
2015-07-24 04:31:30 -03:00
2017-07-19 06:57:10 -03:00
// send message to GCS
if ( cmd_complete ) {
gcs ( ) . send_mission_item_reached_message ( cmd . index ) ;
2015-07-24 05:20:49 -03:00
}
2017-07-19 06:57:10 -03:00
return cmd_complete ;
2015-07-24 05:20:49 -03:00
}
2016-08-22 06:00:12 -03:00
/*******************************************************************************
Verify command Handlers
Each type of mission element has a " verify " operation . The verify
operation returns true when the mission element has completed and we
should move onto the next mission element .
Return true if we do not recognize the command so that we move on to the next command
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
2012-04-30 04:17:14 -03:00
2018-12-11 20:10:20 -04:00
bool ModeAuto : : verify_command ( const AP_Mission : : Mission_Command & cmd )
2012-04-30 04:17:14 -03:00
{
2016-10-30 06:21:03 -03:00
switch ( cmd . id ) {
case MAV_CMD_NAV_WAYPOINT :
return verify_nav_wp ( cmd ) ;
case MAV_CMD_NAV_RETURN_TO_LAUNCH :
return verify_RTL ( ) ;
case MAV_CMD_NAV_LOITER_UNLIM :
2016-11-11 22:55:37 -04:00
return verify_loiter_unlimited ( cmd ) ;
2016-10-30 06:21:03 -03:00
2016-10-30 07:42:46 -03:00
case MAV_CMD_NAV_LOITER_TIME :
return verify_loiter_time ( cmd ) ;
2016-10-30 06:21:03 -03:00
case MAV_CMD_CONDITION_DELAY :
return verify_wait_delay ( ) ;
case MAV_CMD_CONDITION_DISTANCE :
return verify_within_distance ( ) ;
2017-08-02 02:31:26 -03:00
case MAV_CMD_NAV_SET_YAW_SPEED :
return verify_nav_set_yaw_speed ( ) ;
2017-01-18 13:34:42 -04:00
2016-10-30 06:21:03 -03:00
// do commands (always return true)
case MAV_CMD_DO_CHANGE_SPEED :
case MAV_CMD_DO_SET_HOME :
case MAV_CMD_DO_CONTROL_VIDEO :
case MAV_CMD_DO_SET_CAM_TRIGG_DIST :
case MAV_CMD_DO_SET_ROI :
case MAV_CMD_DO_SET_REVERSE :
2017-08-16 07:02:56 -03:00
case MAV_CMD_DO_FENCE_ENABLE :
2016-10-30 06:21:03 -03:00
return true ;
default :
// error message
2017-07-08 22:40:59 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_WARNING , " Skipping invalid cmd #%i " , cmd . id ) ;
2016-10-30 06:21:03 -03:00
// return true if we do not recognize the command so that we move on to the next command
return true ;
}
2012-04-30 04:17:14 -03:00
}
/********************************************************************************/
// Nav (Must) commands
/********************************************************************************/
2018-12-11 20:10:20 -04:00
void ModeAuto : : do_RTL ( void )
2012-04-30 04:17:14 -03:00
{
2017-11-22 08:38:57 -04:00
// start rtl in auto mode
2018-12-11 20:10:20 -04:00
start_RTL ( ) ;
2012-04-30 04:17:14 -03:00
}
2018-12-11 20:10:20 -04:00
void ModeAuto : : do_nav_wp ( const AP_Mission : : Mission_Command & cmd , bool always_stop_at_destination )
2012-04-30 04:17:14 -03:00
{
2016-10-30 07:42:46 -03:00
// just starting so we haven't previously reached the waypoint
previously_reached_wp = false ;
2015-07-27 09:10:37 -03:00
// this will be used to remember the time in millis after we reach or pass the WP.
2016-10-30 07:42:46 -03:00
loiter_start_time = 0 ;
2015-07-27 09:10:37 -03:00
// this is the delay, stored in seconds
2016-10-30 07:42:46 -03:00
loiter_duration = cmd . p1 ;
2015-07-27 09:10:37 -03:00
2017-08-14 05:14:14 -03:00
// get heading to following waypoint (auto mode reduces speed to allow corning without large overshoot)
2017-08-09 22:30:35 -03:00
// in case of non-zero loiter duration, we provide heading-unknown to signal we should stop at the point
float next_leg_bearing_cd = MODE_NEXT_HEADING_UNKNOWN ;
2017-12-06 22:42:34 -04:00
if ( ! always_stop_at_destination & & loiter_duration = = 0 ) {
2017-08-16 04:58:14 -03:00
next_leg_bearing_cd = mission . get_next_ground_course_cd ( MODE_NEXT_HEADING_UNKNOWN ) ;
2017-08-09 22:30:35 -03:00
}
// retrieve and sanitize target location
2016-11-11 22:55:37 -04:00
Location cmdloc = cmd . content . location ;
2018-12-11 20:10:20 -04:00
location_sanitize ( rover . current_loc , cmdloc ) ;
set_desired_location ( cmdloc , next_leg_bearing_cd ) ;
2016-10-30 07:42:46 -03:00
}
2017-08-02 02:31:26 -03:00
// do_set_yaw_speed - turn to a specified heading and achieve and given speed
2018-12-11 20:10:20 -04:00
void ModeAuto : : do_nav_set_yaw_speed ( const AP_Mission : : Mission_Command & cmd )
2017-08-02 02:31:26 -03:00
{
float desired_heading_cd ;
// get final angle, 1 = Relative, 0 = Absolute
if ( cmd . content . set_yaw_speed . relative_angle > 0 ) {
// relative angle
desired_heading_cd = wrap_180_cd ( ahrs . yaw_sensor + cmd . content . set_yaw_speed . angle_deg * 100.0f ) ;
} else {
// absolute angle
desired_heading_cd = cmd . content . set_yaw_speed . angle_deg * 100.0f ;
}
// set auto target
2018-12-11 20:10:20 -04:00
const float speed_max = get_speed_default ( ) ;
set_desired_heading_and_speed ( desired_heading_cd , constrain_float ( cmd . content . set_yaw_speed . speed , - speed_max , speed_max ) ) ;
2017-08-02 02:31:26 -03:00
}
2012-04-30 04:17:14 -03:00
/********************************************************************************/
// Verify Nav (Must) commands
/********************************************************************************/
2018-12-11 20:10:20 -04:00
bool ModeAuto : : verify_nav_wp ( const AP_Mission : : Mission_Command & cmd )
2012-04-30 04:17:14 -03:00
{
2017-08-02 03:56:31 -03:00
// exit immediately if we haven't reached the destination
2018-12-11 20:10:20 -04:00
if ( ! reached_destination ( ) ) {
2017-08-02 03:56:31 -03:00
return false ;
2012-11-18 01:07:50 -04:00
}
2017-08-02 03:56:31 -03:00
// Check if this is the first time we have noticed reaching the waypoint
if ( ! previously_reached_wp ) {
previously_reached_wp = true ;
// check if we are loitering at this waypoint - the message sent to the GCS is different
if ( loiter_duration > 0 ) {
// send message including loiter time
2017-07-08 22:40:59 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Reached waypoint #%u. Loiter for %u seconds " ,
2017-03-30 11:07:24 -03:00
static_cast < uint32_t > ( cmd . index ) ,
static_cast < uint32_t > ( loiter_duration ) ) ;
2016-10-30 07:42:46 -03:00
// record the current time i.e. start timer
loiter_start_time = millis ( ) ;
2017-08-02 03:56:31 -03:00
} else {
// send simpler message to GCS
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Reached waypoint #%u " , static_cast < uint32_t > ( cmd . index ) ) ;
2015-07-27 09:10:37 -03:00
}
2017-08-02 03:56:31 -03:00
}
2016-10-30 07:42:46 -03:00
2017-08-02 03:56:31 -03:00
// Check if we have loitered long enough
if ( loiter_duration = = 0 ) {
2012-11-18 01:07:50 -04:00
return true ;
2017-08-02 03:56:31 -03:00
} else {
return ( ( ( millis ( ) - loiter_start_time ) / 1000 ) > = loiter_duration ) ;
2012-11-18 01:07:50 -04:00
}
2012-04-30 04:17:14 -03:00
}
2018-12-11 20:10:20 -04:00
bool ModeAuto : : verify_RTL ( )
2012-04-30 04:17:14 -03:00
{
2018-12-11 20:10:20 -04:00
return reached_destination ( ) ;
2012-04-30 04:17:14 -03:00
}
2018-12-11 20:10:20 -04:00
bool ModeAuto : : verify_loiter_unlimited ( const AP_Mission : : Mission_Command & cmd )
2016-07-13 22:34:22 -03:00
{
2016-11-11 22:55:37 -04:00
verify_nav_wp ( cmd ) ;
2016-07-13 22:34:22 -03:00
return false ;
}
2016-10-30 07:42:46 -03:00
// verify_loiter_time - check if we have loitered long enough
2018-12-11 20:10:20 -04:00
bool ModeAuto : : verify_loiter_time ( const AP_Mission : : Mission_Command & cmd )
2016-10-30 07:42:46 -03:00
{
2017-01-31 06:12:26 -04:00
const bool result = verify_nav_wp ( cmd ) ;
2016-10-30 07:42:46 -03:00
if ( result ) {
2018-02-01 08:57:36 -04:00
gcs ( ) . send_text ( MAV_SEVERITY_WARNING , " Finished active loiter " ) ;
2016-10-30 07:42:46 -03:00
}
return result ;
}
2017-08-02 02:31:26 -03:00
// verify_yaw - return true if we have reached the desired heading
2018-12-11 20:10:20 -04:00
bool ModeAuto : : verify_nav_set_yaw_speed ( )
2017-08-02 02:31:26 -03:00
{
2018-12-11 20:10:20 -04:00
return reached_heading ( ) ;
2017-08-02 02:31:26 -03:00
}
2012-04-30 04:17:14 -03:00
/********************************************************************************/
// Condition (May) commands
/********************************************************************************/
2018-12-11 20:10:20 -04:00
void ModeAuto : : do_wait_delay ( const AP_Mission : : Mission_Command & cmd )
2012-04-30 04:17:14 -03:00
{
2016-10-30 06:21:03 -03:00
condition_start = millis ( ) ;
2017-01-18 13:34:42 -04:00
condition_value = static_cast < int32_t > ( cmd . content . delay . seconds * 1000 ) ; // convert seconds to milliseconds
2012-04-30 04:17:14 -03:00
}
2018-12-11 20:10:20 -04:00
void ModeAuto : : do_within_distance ( const AP_Mission : : Mission_Command & cmd )
2012-04-30 04:17:14 -03:00
{
2017-01-31 05:46:32 -04:00
condition_value = cmd . content . distance . meters ;
2012-04-30 04:17:14 -03:00
}
/********************************************************************************/
// Verify Condition (May) commands
/********************************************************************************/
2018-12-11 20:10:20 -04:00
bool ModeAuto : : verify_wait_delay ( )
2012-04-30 04:17:14 -03:00
{
2017-03-30 11:07:24 -03:00
if ( static_cast < uint32_t > ( millis ( ) - condition_start ) > static_cast < uint32_t > ( condition_value ) ) {
2016-12-20 09:32:19 -04:00
condition_value = 0 ;
2016-10-30 06:21:03 -03:00
return true ;
}
return false ;
2012-04-30 04:17:14 -03:00
}
2018-12-11 20:10:20 -04:00
bool ModeAuto : : verify_within_distance ( )
2012-04-30 04:17:14 -03:00
{
2018-12-11 20:10:20 -04:00
if ( get_distance_to_destination ( ) < condition_value ) {
2016-10-30 06:21:03 -03:00
condition_value = 0 ;
return true ;
}
return false ;
2012-04-30 04:17:14 -03:00
}
2017-01-18 13:34:42 -04:00
2012-04-30 04:17:14 -03:00
/********************************************************************************/
// Do (Now) commands
/********************************************************************************/
2018-12-11 20:10:20 -04:00
void ModeAuto : : do_change_speed ( const AP_Mission : : Mission_Command & cmd )
2012-04-30 04:17:14 -03:00
{
2017-12-05 21:41:28 -04:00
// set speed for active mode
2018-12-11 20:10:20 -04:00
if ( set_desired_speed ( cmd . content . speed . target_ms ) ) {
2017-12-05 21:41:28 -04:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " speed: %.1f m/s " , static_cast < double > ( cmd . content . speed . target_ms ) ) ;
2013-05-26 22:08:25 -03:00
}
2012-04-30 04:17:14 -03:00
}
2018-12-11 20:10:20 -04:00
void ModeAuto : : do_set_home ( const AP_Mission : : Mission_Command & cmd )
2012-04-30 04:17:14 -03:00
{
2018-12-11 20:10:20 -04:00
if ( cmd . p1 = = 1 & & rover . have_position ) {
rover . set_home_to_current_location ( false ) ;
2016-10-30 06:21:03 -03:00
} else {
2018-12-11 20:10:20 -04:00
rover . set_home ( cmd . content . location , false ) ;
2016-10-30 06:21:03 -03:00
}
2012-04-30 04:17:14 -03:00
}
2018-12-11 20:10:20 -04:00
void ModeAuto : : do_set_reverse ( const AP_Mission : : Mission_Command & cmd )
2016-07-14 04:50:39 -03:00
{
2018-12-11 20:10:20 -04:00
set_reversed ( cmd . p1 = = 1 ) ;
2016-07-14 04:50:39 -03:00
}