void Rover::set_mode(enum mode mode) { if(control_mode == mode){ // don't switch modes if we are already in the correct mode. return; } // If we are changing out of AUTO mode reset the loiter timer if (control_mode == AUTO) loiter_time = 0; control_mode = mode; throttle_last = 0; throttle = 500; set_reverse(false); g.pidSpeedThrottle.reset_I(); if (control_mode != AUTO) { auto_triggered = false; } switch(control_mode) { case MANUAL: case HOLD: case LEARNING: case STEERING: break; case AUTO: rtl_complete = false; restart_nav(); break; case RTL: do_RTL(); break; case GUIDED: rtl_complete = false; /* when entering guided mode we set the target as the current location. This matches the behaviour of the copter code. */ guided_WP = current_loc; set_guided_WP(); break; default: do_RTL(); break; } if (should_log(MASK_LOG_MODE)) { DataFlash.Log_Write_Mode(control_mode); } }
void Rover::set_mode(enum mode mode) { if(control_mode == mode){ // don't switch modes if we are already in the correct mode. return; } control_mode = mode; throttle_last = 0; throttle = 500; set_reverse(false); g.pidSpeedThrottle.reset_I(); if (control_mode != AUTO) { auto_triggered = false; } switch(control_mode) { case MANUAL: case HOLD: case LEARNING: case STEERING: break; case AUTO: rtl_complete = false; restart_nav(); break; case RTL: do_RTL(); break; case GUIDED: rtl_complete = false; set_guided_WP(); break; default: do_RTL(); break; } if (should_log(MASK_LOG_MODE)) { DataFlash.Log_Write_Mode(control_mode); } }
void Plane::set_mode(enum FlightMode mode) { if(control_mode == mode) { // don't switch modes if we are already in the correct mode. return; } if(g.auto_trim > 0 && control_mode == MANUAL) trim_control_surfaces(); // perform any cleanup required for prev flight mode exit_mode(control_mode); // cancel inverted flight auto_state.inverted_flight = false; // don't cross-track when starting a mission auto_state.next_wp_no_crosstrack = true; // reset landing check auto_state.checked_for_autoland = false; // reset go around command auto_state.commanded_go_around = false; // zero locked course steer_state.locked_course_err = 0; // set mode previous_mode = control_mode; control_mode = mode; if (previous_mode == AUTOTUNE && control_mode != AUTOTUNE) { // restore last gains autotune_restore(); } // zero initial pitch and highest airspeed on mode change auto_state.highest_airspeed = 0; auto_state.initial_pitch_cd = ahrs.pitch_sensor; // disable taildrag takeoff on mode change auto_state.fbwa_tdrag_takeoff_mode = false; switch(control_mode) { case INITIALISING: auto_throttle_mode = true; break; case MANUAL: case STABILIZE: case TRAINING: case FLY_BY_WIRE_A: auto_throttle_mode = false; break; case AUTOTUNE: auto_throttle_mode = false; autotune_start(); break; case ACRO: auto_throttle_mode = false; acro_state.locked_roll = false; acro_state.locked_pitch = false; break; case CRUISE: auto_throttle_mode = true; cruise_state.locked_heading = false; cruise_state.lock_timer_ms = 0; set_target_altitude_current(); break; case FLY_BY_WIRE_B: auto_throttle_mode = true; set_target_altitude_current(); break; case CIRCLE: // the altitude to circle at is taken from the current altitude auto_throttle_mode = true; next_WP_loc.alt = current_loc.alt; break; case AUTO: auto_throttle_mode = true; next_WP_loc = prev_WP_loc = current_loc; // start or resume the mission, based on MIS_AUTORESET mission.start_or_resume(); break; case RTL: auto_throttle_mode = true; prev_WP_loc = current_loc; do_RTL(); break; case LOITER: auto_throttle_mode = true; do_loiter_at_location(); break; case GUIDED: auto_throttle_mode = true; guided_throttle_passthru = false; /* when entering guided mode we set the target as the current location. This matches the behaviour of the copter code */ guided_WP_loc = current_loc; set_guided_WP(); break; } // start with throttle suppressed in auto_throttle modes throttle_suppressed = auto_throttle_mode; if (should_log(MASK_LOG_MODE)) DataFlash.Log_Write_Mode(control_mode); // reset attitude integrators on mode change rollController.reset_I(); pitchController.reset_I(); yawController.reset_I(); steerController.reset_I(); }
// start_command - this function will be called when the ap_mission lib wishes to start a new command bool Copter::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); } switch(cmd.id) { /// /// navigation commands /// case MAV_CMD_NAV_TAKEOFF: // 22 do_takeoff(cmd); break; case MAV_CMD_NAV_WAYPOINT: // 16 Navigate to Waypoint do_nav_wp(cmd); break; case MAV_CMD_NAV_LAND: // 21 LAND to Waypoint do_land(cmd); break; case MAV_CMD_NAV_PAYLOAD_PLACE: // 94 place at Waypoint do_payload_place(cmd); 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_RETURN_TO_LAUNCH: //20 do_RTL(); 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 copter 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 PARACHUTE == ENABLED case MAV_CMD_DO_PARACHUTE: // Mission command to configure or release parachute do_parachute(cmd); 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; }
void Plane::set_mode(enum FlightMode mode) { if(control_mode == mode) { // don't switch modes if we are already in the correct mode. return; } if(g.auto_trim > 0 && control_mode == MANUAL) trim_control_surfaces(); // perform any cleanup required for prev flight mode exit_mode(control_mode); // cancel inverted flight auto_state.inverted_flight = false; // don't cross-track when starting a mission auto_state.next_wp_no_crosstrack = true; // reset landing check auto_state.checked_for_autoland = false; // reset go around command auto_state.commanded_go_around = false; // not in pre-flare auto_state.land_pre_flare = false; // zero locked course steer_state.locked_course_err = 0; // reset crash detection crash_state.is_crashed = false; crash_state.impact_detected = false; // always reset this because we don't know who called set_mode. In evasion // behavior you should set this flag after set_mode so you know the evasion // logic is controlling the mode. This allows manual override of the mode // to exit evasion behavior automatically but if the mode is manually switched // then we won't resume AUTO after an evasion adsb_state.is_evading = false; // set mode previous_mode = control_mode; control_mode = mode; if (previous_mode == AUTOTUNE && control_mode != AUTOTUNE) { // restore last gains autotune_restore(); } // zero initial pitch and highest airspeed on mode change auto_state.highest_airspeed = 0; auto_state.initial_pitch_cd = ahrs.pitch_sensor; // disable taildrag takeoff on mode change auto_state.fbwa_tdrag_takeoff_mode = false; // start with previous WP at current location prev_WP_loc = current_loc; // new mode means new loiter loiter.start_time_ms = 0; // assume non-VTOL mode auto_state.vtol_mode = false; switch(control_mode) { case INITIALISING: auto_throttle_mode = true; break; case MANUAL: case STABILIZE: case TRAINING: case FLY_BY_WIRE_A: auto_throttle_mode = false; break; case AUTOTUNE: auto_throttle_mode = false; autotune_start(); break; case ACRO: auto_throttle_mode = false; acro_state.locked_roll = false; acro_state.locked_pitch = false; break; case CRUISE: auto_throttle_mode = true; cruise_state.locked_heading = false; cruise_state.lock_timer_ms = 0; set_target_altitude_current(); break; case FLY_BY_WIRE_B: auto_throttle_mode = true; set_target_altitude_current(); break; case CIRCLE: // the altitude to circle at is taken from the current altitude auto_throttle_mode = true; next_WP_loc.alt = current_loc.alt; break; case AUTO: auto_throttle_mode = true; auto_state.vtol_mode = false; next_WP_loc = prev_WP_loc = current_loc; // start or resume the mission, based on MIS_AUTORESET mission.start_or_resume(); break; case RTL: auto_throttle_mode = true; prev_WP_loc = current_loc; do_RTL(); break; case LOITER: auto_throttle_mode = true; do_loiter_at_location(); break; case GUIDED: auto_throttle_mode = true; guided_throttle_passthru = false; /* when entering guided mode we set the target as the current location. This matches the behaviour of the copter code */ guided_WP_loc = current_loc; set_guided_WP(); break; case QSTABILIZE: case QHOVER: case QLOITER: case QLAND: if (!quadplane.init_mode()) { control_mode = previous_mode; } else { auto_throttle_mode = false; auto_state.vtol_mode = true; } break; } // start with throttle suppressed in auto_throttle modes throttle_suppressed = auto_throttle_mode; if (should_log(MASK_LOG_MODE)) DataFlash.Log_Write_Mode(control_mode); // reset attitude integrators on mode change rollController.reset_I(); pitchController.reset_I(); yawController.reset_I(); steerController.reset_I(); }
bool Rover::start_command(const AP_Mission::Mission_Command& cmd) { // log when new commands start if (should_log(MASK_LOG_CMD)) { DataFlash.Log_Write_Mission_Cmd(mission, cmd); } // exit immediately if not in AUTO mode if (control_mode != AUTO) { return false; } gcs_send_text_fmt(MAV_SEVERITY_INFO, "Executing command ID #%i",cmd.id); // remember the course of our next navigation leg next_navigation_leg_cd = mission.get_next_ground_course_cd(0); switch(cmd.id){ case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint do_nav_wp(cmd); break; case MAV_CMD_NAV_RETURN_TO_LAUNCH: do_RTL(); break; case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely do_loiter_unlimited(cmd); break; // 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; 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; #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 MOUNT == ENABLED // 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 if (camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { camera_mount.set_mode_to_default(); } } else { // send the command to the camera mount camera_mount.set_roi_target(cmd.content.location); } break; #endif case MAV_CMD_DO_SET_REVERSE: do_set_reverse(cmd); break; default: // return false for unhandled commands return false; } // if we got this far we must have been successful return true; }
void Plane::set_mode(enum FlightMode mode, mode_reason_t reason) { if(control_mode == mode) { // don't switch modes if we are already in the correct mode. return; } if(g.auto_trim > 0 && control_mode == MANUAL) trim_control_surfaces(); // perform any cleanup required for prev flight mode exit_mode(control_mode); // cancel inverted flight auto_state.inverted_flight = false; // don't cross-track when starting a mission auto_state.next_wp_no_crosstrack = true; // reset landing check auto_state.checked_for_autoland = false; // reset go around command auto_state.commanded_go_around = false; // not in pre-flare auto_state.land_pre_flare = false; // zero locked course steer_state.locked_course_err = 0; // reset crash detection crash_state.is_crashed = false; crash_state.impact_detected = false; // reset external attitude guidance guided_state.last_forced_rpy_ms.zero(); guided_state.last_forced_throttle_ms = 0; // set mode previous_mode = control_mode; control_mode = mode; previous_mode_reason = control_mode_reason; control_mode_reason = reason; #if FRSKY_TELEM_ENABLED == ENABLED frsky_telemetry.update_control_mode(control_mode); #endif if (previous_mode == AUTOTUNE && control_mode != AUTOTUNE) { // restore last gains autotune_restore(); } // zero initial pitch and highest airspeed on mode change auto_state.highest_airspeed = 0; auto_state.initial_pitch_cd = ahrs.pitch_sensor; // disable taildrag takeoff on mode change auto_state.fbwa_tdrag_takeoff_mode = false; // start with previous WP at current location prev_WP_loc = current_loc; // new mode means new loiter loiter.start_time_ms = 0; // assume non-VTOL mode auto_state.vtol_mode = false; auto_state.vtol_loiter = false; switch(control_mode) { case INITIALISING: auto_throttle_mode = true; auto_navigation_mode = false; break; case MANUAL: case STABILIZE: case TRAINING: case FLY_BY_WIRE_A: auto_throttle_mode = false; auto_navigation_mode = false; break; case AUTOTUNE: auto_throttle_mode = false; auto_navigation_mode = false; autotune_start(); break; case ACRO: auto_throttle_mode = false; auto_navigation_mode = false; acro_state.locked_roll = false; acro_state.locked_pitch = false; break; case CRUISE: auto_throttle_mode = true; auto_navigation_mode = false; cruise_state.locked_heading = false; cruise_state.lock_timer_ms = 0; set_target_altitude_current(); break; case FLY_BY_WIRE_B: auto_throttle_mode = true; auto_navigation_mode = false; set_target_altitude_current(); break; case CIRCLE: // the altitude to circle at is taken from the current altitude auto_throttle_mode = true; auto_navigation_mode = true; next_WP_loc.alt = current_loc.alt; break; case AUTO: auto_throttle_mode = true; auto_navigation_mode = true; if (quadplane.available() && quadplane.enable == 2) { auto_state.vtol_mode = true; } else { auto_state.vtol_mode = false; } next_WP_loc = prev_WP_loc = current_loc; // start or resume the mission, based on MIS_AUTORESET mission.start_or_resume(); break; case RTL: auto_throttle_mode = true; auto_navigation_mode = true; prev_WP_loc = current_loc; do_RTL(get_RTL_altitude()); break; case LOITER: auto_throttle_mode = true; auto_navigation_mode = true; do_loiter_at_location(); break; case AVOID_ADSB: case GUIDED: auto_throttle_mode = true; auto_navigation_mode = true; guided_throttle_passthru = false; /* when entering guided mode we set the target as the current location. This matches the behaviour of the copter code */ guided_WP_loc = current_loc; set_guided_WP(); break; case QSTABILIZE: case QHOVER: case QLOITER: case QLAND: case QRTL: auto_navigation_mode = false; if (!quadplane.init_mode()) { control_mode = previous_mode; } else { auto_throttle_mode = false; auto_state.vtol_mode = true; } break; } // start with throttle suppressed in auto_throttle modes throttle_suppressed = auto_throttle_mode; adsb.set_is_auto_mode(auto_navigation_mode); if (should_log(MASK_LOG_MODE)) DataFlash.Log_Write_Mode(control_mode); // reset attitude integrators on mode change rollController.reset_I(); pitchController.reset_I(); yawController.reset_I(); steerController.reset_I(); }
// 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)) { logger.Write_Mission_Cmd(mission, cmd); } const Location &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::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; }
bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) { // log when new commands start if (rover.should_log(MASK_LOG_CMD)) { rover.logger.Write_Mission_Cmd(mission, cmd); } gcs().send_text(MAV_SEVERITY_INFO, "Executing %s(ID=%i)", cmd.type(), cmd.id); switch (cmd.id) { case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint do_nav_wp(cmd, false); break; case MAV_CMD_NAV_RETURN_TO_LAUNCH: do_RTL(); break; case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely case MAV_CMD_NAV_LOITER_TIME: // Loiter for specified time do_nav_wp(cmd, true); break; case MAV_CMD_NAV_SET_YAW_SPEED: do_nav_set_yaw_speed(cmd); break; // 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; #if MOUNT == ENABLED // 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 if (rover.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { rover.camera_mount.set_mode_to_default(); } } else { // send the command to the camera mount rover.camera_mount.set_roi_target(cmd.content.location); } break; #endif case MAV_CMD_DO_SET_REVERSE: do_set_reverse(cmd); break; 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; default: // return false for unhandled commands return false; } // if we got this far we must have been successful return true; }