3 #if MODE_AUTO_ENABLED == ENABLED 25 if ((
copter.position_ok() &&
copter.mission.num_commands() > 1) || ignore_checks) {
29 if (
motors->armed() &&
ap.land_complete && !
copter.mission.starts_with_takeoff_cmd()) {
30 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"Auto: Missing Takeoff Cmd");
44 copter.mode_guided.limit_clear();
47 copter.mission.start_or_resume();
88 #if NAV_GUIDED == ENABLED 108 if (!
copter.position_ok()) {
132 copter.mode_rtl.init(
true);
153 alt_target =
copter.current_loc.alt + dest.
alt;
157 if (alt_target <
copter.current_loc.alt) {
161 if (alt_target < 100) {
168 copter.failsafe_terrain_on_event();
179 copter.auto_takeoff_set_start_alt();
205 copter.failsafe_terrain_on_event();
256 copter.circle_nav->set_center(circle_center_neu);
260 copter.circle_nav->set_radius(radius_m * 100.0
f);
265 copter.circle_nav->get_closest_point_on_circle(circle_edge_neu);
269 if (dist_to_edge > 300.0
f) {
282 copter.failsafe_terrain_on_event();
287 float dist_to_center =
norm(circle_center_neu.
x - curr_pos.
x, circle_center_neu.
y - curr_pos.
y);
288 if (dist_to_center >
copter.circle_nav->get_radius() && dist_to_center > 500) {
306 copter.circle_nav->init(
copter.circle_nav->get_center());
320 copter.failsafe_terrain_on_event();
331 #if NAV_GUIDED == ENABLED 338 copter.mode_guided.init(
true);
341 copter.mode_guided.limit_init_time_and_pos();
351 return copter.mode_rtl.landing_gear_should_be_deployed();
375 copter.DataFlash.Log_Write_Mission_Cmd(
copter.mission, cmd);
383 case MAV_CMD_NAV_TAKEOFF:
387 case MAV_CMD_NAV_WAYPOINT:
391 case MAV_CMD_NAV_LAND:
395 case MAV_CMD_NAV_PAYLOAD_PLACE:
399 case MAV_CMD_NAV_LOITER_UNLIM:
403 case MAV_CMD_NAV_LOITER_TURNS:
407 case MAV_CMD_NAV_LOITER_TIME:
411 case MAV_CMD_NAV_RETURN_TO_LAUNCH:
415 case MAV_CMD_NAV_SPLINE_WAYPOINT:
419 #if NAV_GUIDED == ENABLED 420 case MAV_CMD_NAV_GUIDED_ENABLE:
425 case MAV_CMD_NAV_DELAY:
432 case MAV_CMD_CONDITION_DELAY:
436 case MAV_CMD_CONDITION_DISTANCE:
440 case MAV_CMD_CONDITION_YAW:
447 case MAV_CMD_DO_CHANGE_SPEED:
451 case MAV_CMD_DO_SET_HOME:
455 case MAV_CMD_DO_SET_SERVO:
459 case MAV_CMD_DO_SET_RELAY:
463 case MAV_CMD_DO_REPEAT_SERVO:
468 case MAV_CMD_DO_REPEAT_RELAY:
473 case MAV_CMD_DO_SET_ROI:
478 case MAV_CMD_DO_MOUNT_CONTROL:
483 case MAV_CMD_DO_FENCE_ENABLE:
484 #if AC_FENCE == ENABLED 486 copter.fence.enable(
false);
489 copter.fence.enable(
true);
492 #endif //AC_FENCE == ENABLED 495 #if CAMERA == ENABLED 496 case MAV_CMD_DO_CONTROL_VIDEO:
499 case MAV_CMD_DO_DIGICAM_CONFIGURE:
503 case MAV_CMD_DO_DIGICAM_CONTROL:
507 case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
512 #if PARACHUTE == ENABLED 513 case MAV_CMD_DO_PARACHUTE:
518 #if GRIPPER_ENABLED == ENABLED 519 case MAV_CMD_DO_GRIPPER:
524 #if NAV_GUIDED == ENABLED 525 case MAV_CMD_DO_GUIDED_LIMITS:
530 #if WINCH_ENABLED == ENABLED 531 case MAV_CMD_DO_WINCH:
568 if(!
ap.land_complete) {
575 copter.init_disarm_motors();
590 case MAV_CMD_NAV_WAYPOINT:
594 return copter.mode_guided.set_destination(dest);
597 case MAV_CMD_CONDITION_YAW:
623 return copter.mode_guided.get_wp(destination);
652 case MAV_CMD_NAV_TAKEOFF:
655 case MAV_CMD_NAV_WAYPOINT:
658 case MAV_CMD_NAV_LAND:
661 case MAV_CMD_NAV_PAYLOAD_PLACE:
664 case MAV_CMD_NAV_LOITER_UNLIM:
667 case MAV_CMD_NAV_LOITER_TURNS:
670 case MAV_CMD_NAV_LOITER_TIME:
673 case MAV_CMD_NAV_RETURN_TO_LAUNCH:
676 case MAV_CMD_NAV_SPLINE_WAYPOINT:
679 #if NAV_GUIDED == ENABLED 680 case MAV_CMD_NAV_GUIDED_ENABLE:
684 case MAV_CMD_NAV_DELAY:
690 case MAV_CMD_CONDITION_DELAY:
693 case MAV_CMD_CONDITION_DISTANCE:
696 case MAV_CMD_CONDITION_YAW:
700 case MAV_CMD_DO_CHANGE_SPEED:
701 case MAV_CMD_DO_SET_HOME:
702 case MAV_CMD_DO_SET_SERVO:
703 case MAV_CMD_DO_SET_RELAY:
704 case MAV_CMD_DO_REPEAT_SERVO:
705 case MAV_CMD_DO_REPEAT_RELAY:
706 case MAV_CMD_DO_SET_ROI:
707 case MAV_CMD_DO_MOUNT_CONTROL:
708 case MAV_CMD_DO_CONTROL_VIDEO:
709 case MAV_CMD_DO_DIGICAM_CONFIGURE:
710 case MAV_CMD_DO_DIGICAM_CONTROL:
711 case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
712 case MAV_CMD_DO_PARACHUTE:
713 case MAV_CMD_DO_GRIPPER:
714 case MAV_CMD_DO_GUIDED_LIMITS:
715 case MAV_CMD_DO_FENCE_ENABLE:
716 case MAV_CMD_DO_WINCH:
721 gcs().
send_text(MAV_SEVERITY_WARNING,
"Skipping invalid cmd #%i",cmd.
id);
732 if (!
motors->armed() || !
ap.auto_armed || !
motors->get_interlock()) {
742 float target_yaw_rate = 0;
743 if (!
copter.failsafe.radio) {
748 #if FRAME_CONFIG == HELI_FRAME 750 if (
motors->rotor_runup_complete()) {
770 copter.auto_takeoff_attitude_run(target_yaw_rate);
778 if (!
motors->armed() || !
ap.auto_armed || !
motors->get_interlock()) {
788 float target_yaw_rate = 0;
789 if (!
copter.failsafe.radio) {
792 if (!
is_zero(target_yaw_rate)) {
821 if (!
motors->armed() || !
ap.auto_armed || !
motors->get_interlock()) {
831 float target_yaw_rate = 0;
832 if (!
copter.failsafe.radio) {
835 if (!
is_zero(target_yaw_rate)) {
864 if (!
motors->armed() || !
ap.auto_armed ||
ap.land_complete || !
motors->get_interlock()) {
883 copter.mode_rtl.run(
false);
891 copter.circle_nav->update();
900 #if NAV_GUIDED == ENABLED 915 if (!
motors->armed() || !
ap.auto_armed ||
ap.land_complete || !
motors->get_interlock()) {
921 float target_yaw_rate = 0;
922 if(!
copter.failsafe.radio) {
994 if (!
ap.auto_armed) {
998 if (
ap.land_complete) {
1002 if (!
motors->get_interlock()) {
1018 const float target_yaw_rate = 0;
1040 int32_t curr_terr_alt_cm, target_terr_alt_cm;
1043 curr_terr_alt_cm = MAX(curr_terr_alt_cm,200);
1071 if (target_loc.
lat == 0 && target_loc.
lng == 0) {
1072 target_loc.
lat = current_loc.
lat;
1073 target_loc.
lng = current_loc.
lng;
1076 if (target_loc.
alt == 0) {
1098 copter.wp_nav->set_fast_waypoint(
true);
1133 if (target_loc.
lat == 0 && target_loc.
lng == 0) {
1136 copter.wp_nav->get_wp_stopping_point_xy(temp_pos);
1138 target_loc.
lat = temp_loc.
lat;
1139 target_loc.
lng = temp_loc.
lng;
1144 if (target_loc.
alt == 0) {
1167 if (circle_center.
lat == 0 && circle_center.
lng == 0) {
1168 circle_center.
lat = current_loc.
lat;
1169 circle_center.
lng = current_loc.
lng;
1173 if (circle_center.
alt == 0) {
1211 if (target_loc.
lat == 0 && target_loc.
lng == 0) {
1212 target_loc.
lat = current_loc.
lat;
1213 target_loc.
lng = current_loc.
lng;
1216 if (target_loc.
alt == 0) {
1233 bool stopped_at_start =
true;
1239 uint16_t prev_cmd_idx =
copter.mission.get_prev_nav_cmd_index();
1241 if (
copter.mission.read_cmd_from_storage(prev_cmd_idx, temp_cmd)) {
1242 if ((temp_cmd.
id == MAV_CMD_NAV_WAYPOINT || temp_cmd.
id == MAV_CMD_NAV_SPLINE_WAYPOINT) && temp_cmd.
p1 == 0) {
1243 stopped_at_start =
false;
1250 if (cmd.
p1 == 0 &&
copter.mission.get_next_nav_cmd(cmd.
index+1, temp_cmd)) {
1253 if (next_loc.
lat == 0 && next_loc.
lng == 0) {
1254 next_loc.
lat = target_loc.
lat;
1255 next_loc.
lng = target_loc.
lng;
1258 if (next_loc.
alt == 0) {
1268 if (temp_cmd.
id == MAV_CMD_NAV_WAYPOINT) {
1270 }
else if (temp_cmd.
id == MAV_CMD_NAV_SPLINE_WAYPOINT) {
1276 spline_start(target_loc, stopped_at_start, seg_end_type, next_loc);
1279 #if NAV_GUIDED == ENABLED 1285 copter.mode_guided.limit_init_time_and_pos();
1295 copter.mode_guided.limit_set(
1301 #endif // NAV_GUIDED 1358 copter.set_home_to_current_location(
false);
1376 #if MOUNT == ENABLED 1377 if(!
copter.camera_mount.has_pan_control()) {
1384 #if CAMERA == ENABLED 1412 #if PARACHUTE == ENABLED 1417 case PARACHUTE_DISABLE:
1418 copter.parachute.enabled(
false);
1421 case PARACHUTE_ENABLE:
1422 copter.parachute.enabled(
true);
1425 case PARACHUTE_RELEASE:
1426 copter.parachute_release();
1435 #if GRIPPER_ENABLED == ENABLED 1441 case GRIPPER_ACTION_RELEASE:
1442 g2.gripper.release();
1445 case GRIPPER_ACTION_GRAB:
1456 #if WINCH_ENABLED == ENABLED 1466 case WINCH_RELATIVE_LENGTH_CONTROL:
1470 case WINCH_RATE_CONTROL:
1516 return copter.wp_nav->reached_wp_destination();
1522 bool retval =
false;
1527 if (
copter.wp_nav->reached_wp_destination()) {
1541 retval =
ap.land_complete;
1555 #define NAV_PAYLOAD_PLACE_DEBUGGING 0 1557 #if NAV_PAYLOAD_PLACE_DEBUGGING 1559 #define debug(fmt, args ...) do {::fprintf(stderr,"%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0) 1561 #define debug(fmt, args ...) 1567 const uint16_t hover_throttle_calibrate_time = 2000;
1568 const uint16_t descend_throttle_calibrate_time = 2000;
1569 const float hover_throttle_placed_fraction = 0.7;
1570 const float descent_throttle_placed_fraction = 0.9;
1571 const uint16_t placed_time = 500;
1573 const float current_throttle_level =
motors->get_throttle();
1577 if (
ap.land_complete) {
1584 gcs().
send_text(MAV_SEVERITY_INFO,
"NAV_PLACE: landed");
1599 if (!
copter.wp_nav->reached_wp_destination()) {
1608 debug(
"Calibrate start");
1613 if (now -
nav_payload_place.hover_start_timestamp < hover_throttle_calibrate_time) {
1620 const float hover_throttle_delta = fabsf(
nav_payload_place.hover_throttle_level -
motors->get_throttle_hover());
1621 gcs().
send_text(MAV_SEVERITY_INFO,
"hover throttle delta: %f", static_cast<double>(hover_throttle_delta));
1637 gcs().
send_text(MAV_SEVERITY_WARNING,
"Reached maximum descent");
1642 now -
nav_payload_place.descend_start_timestamp > descend_throttle_calibrate_time) {
1647 if (current_throttle_level/
nav_payload_place.hover_throttle_level > hover_throttle_placed_fraction &&
1649 current_throttle_level/
nav_payload_place.descend_throttle_level > descent_throttle_placed_fraction)) {
1666 #if GRIPPER_ENABLED == ENABLED 1667 if (
g2.gripper.valid()) {
1668 gcs().
send_text(MAV_SEVERITY_INFO,
"Releasing the gripper");
1669 g2.gripper.release();
1671 gcs().
send_text(MAV_SEVERITY_INFO,
"Gripper not valid");
1676 gcs().
send_text(MAV_SEVERITY_INFO,
"Gripper code disabled");
1681 #if GRIPPER_ENABLED == ENABLED 1682 if (
g2.gripper.valid() && !
g2.gripper.released()) {
1700 if (!
copter.wp_nav->reached_wp_destination()) {
1726 if (!
copter.wp_nav->reached_wp_destination()) {
1785 if( !
copter.wp_nav->reached_wp_destination() ) {
1812 if (
copter.wp_nav->reached_wp_destination()) {
1818 circle_center.
z = curr_pos.
z;
1823 circle_center.
x = curr_pos.
x;
1824 circle_center.
y = curr_pos.
y;
1843 if( !
copter.wp_nav->reached_wp_destination() ) {
1861 #if NAV_GUIDED == ENABLED 1871 return copter.mode_guided.limit_check();
1873 #endif // NAV_GUIDED float norm(const T first, const U second, const Params... parameters)
bool get_vector_from_origin_NEU(Vector3f &vec_neu) const
void zero_throttle_and_relax_ac()
bool set_mode(control_mode_t mode, mode_reason_t reason)
virtual float get_velocity_z() const=0
void spline_start(const Vector3f &destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Vector3f &next_spline_destination)
void wp_start(const Vector3f &destination)
void payload_place_start()
void set_mode_to_default(bool rtl)
void send_mission_item_reached_message(uint16_t mission_index)
virtual const Vector3f & get_position() const=0
Mount_Control mount_control
bool verify_nav_delay(const AP_Mission::Mission_Command &cmd)
Repeat_Servo_Command repeat_servo
int32_t get_pitch() const
bool set_spline_destination(const Location_Class &destination, bool stopped_at_start, spline_segment_end_type seg_end_type, Location_Class next_destination)
void do_mount_control(const AP_Mission::Mission_Command &cmd)
bool do_guided(const AP_Mission::Mission_Command &cmd)
bool start_command(const AP_Mission::Mission_Command &cmd)
void do_digicam_control(const AP_Mission::Mission_Command &cmd)
void takeoff_start(const Location &dest_loc)
void do_winch(const AP_Mission::Mission_Command &cmd)
void update_z_controller()
void get_wp_stopping_point(Vector3f &stopping_point) const
void do_wait_delay(const AP_Mission::Mission_Command &cmd)
void set_fixed_yaw(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle)
void Log_Write_Event(uint8_t id)
bool verify_payload_place()
#define DATA_GRIPPER_RELEASE
bool verify_loiter_unlimited()
Digicam_Control digicam_control
Location_Class terrain_adjusted_location(const AP_Mission::Mission_Command &cmd) const
void do_loiter_unlimited(const AP_Mission::Mission_Command &cmd)
uint32_t get_time_utc(int32_t hour, int32_t min, int32_t sec, int32_t ms) const
void do_guided_limits(const AP_Mission::Mission_Command &cmd)
int32_t wp_bearing() const override
Conditional_Delay_Command delay
Repeat_Relay_Command repeat_relay
void do_nav_delay(const AP_Mission::Mission_Command &cmd)
AC_AttitudeControl_t *& attitude_control
void init_target(const Vector3f &position)
auto wrap_180_cd(const T angle) -> decltype(wrap_180(angle, 100.f))
bool verify_command(const AP_Mission::Mission_Command &cmd)
uint32_t wp_distance() const override
Conditional_Distance_Command distance
void do_digicam_configure(const AP_Mission::Mission_Command &cmd)
Navigation_Delay_Command nav_delay
float get_pilot_desired_yaw_rate(int16_t stick_angle)
int32_t nav_delay_time_max
bool verify_loiter_time()
void land_run_horizontal_control()
bool set_wp_destination(const Location_Class &destination)
Guided_Limits_Command guided_limits
autopilot_yaw_mode mode() const
void land_run_vertical_control(bool pause_descent=false)
void do_gripper(const AP_Mission::Mission_Command &cmd)
bool verify_nav_wp(const AP_Mission::Mission_Command &cmd)
int32_t get_wp_bearing_to_destination() const
#define DATA_PARACHUTE_ENABLED
void do_nav_guided_enable(const AP_Mission::Mission_Command &cmd)
bool verify_circle(const AP_Mission::Mission_Command &cmd)
void do_yaw(const AP_Mission::Mission_Command &cmd)
bool verify_spline_wp(const AP_Mission::Mission_Command &cmd)
void payload_place_run_loiter()
void set_mode(autopilot_yaw_mode new_mode)
void update(float ekfGndSpdLimit, float ekfNavVelGainScaler)
void get_stopping_point_xy(Vector3f &stopping_point) const
int16_t get_control_in() const
#define DATA_WINCH_LENGTH_CONTROL
const Vector3f & get_wp_destination() const
uint32_t mission_complete
ALT_FRAME get_alt_frame() const
void do_payload_place(const AP_Mission::Mission_Command &cmd)
#define DATA_PARACHUTE_DISABLED
bool verify_nav_guided_enable(const AP_Mission::Mission_Command &cmd)
bool is_zero(const T fVal1)
void do_roi(const AP_Mission::Mission_Command &cmd)
bool get_alt_cm(ALT_FRAME desired_frame, int32_t &ret_alt_cm) const
bool landing_gear_should_be_deployed() const override
void set_desired_velocity_z(float vel_z_cms)
void release_length(float length, float rate=0.0f)
#define DATA_WINCH_RELAXED
void do_circle(const AP_Mission::Mission_Command &cmd)
float get_wp_distance_to_destination() const
void do_nav_wp(const AP_Mission::Mission_Command &cmd)
void set_throttle_takeoff(void)
Location_Class current_loc
#define ERROR_CODE_MISSING_TERRAIN_DATA
void shift_wp_origin_to_current_pos()
void wp_and_spline_init()
Change_Speed_Command speed
bool verify_within_distance()
void do_set_home(const AP_Mission::Mission_Command &cmd)
void send_text(MAV_SEVERITY severity, const char *fmt,...)
#define DATA_WINCH_RATE_CONTROL
DESIRED_THROTTLE_UNLIMITED
void do_change_speed(const AP_Mission::Mission_Command &cmd)
AC_PosControl *& pos_control
#define DATA_GRIPPER_GRAB
void do_takeoff(const AP_Mission::Mission_Command &cmd)
void do_loiter_time(const AP_Mission::Mission_Command &cmd)
void do_parachute(const AP_Mission::Mission_Command &cmd)
void run_autopilot() override
Digicam_Configure digicam_configure
bool payload_place_run_should_run()
uint32_t waypoint_complete
void set_alt_cm(int32_t alt_cm, ALT_FRAME frame)
void set_roi(const Location &roi_location)
void set_alt_target(float alt_cm)
#define AP_MISSION_CMD_INDEX_NONE
AP_HAL_MAIN_CALLBACKS & copter
bool verify_command_callback(const AP_Mission::Mission_Command &cmd)
#define ERROR_CODE_FAILED_CIRCLE_INIT
#define debug(fmt, args ...)
Cam_Trigg_Distance cam_trigg_dist
#define ERROR_SUBSYSTEM_TERRAIN
bool init(bool ignore_checks) override
void do_spline_wp(const AP_Mission::Mission_Command &cmd)
float & ekfNavVelGainScaler
AP_InertialNav & inertial_nav
void do_within_distance(const AP_Mission::Mission_Command &cmd)
void payload_place_run_descend()
void set_desired_rate(float rate)
RC_Channel *& channel_yaw
static struct notify_events_type events
virtual float get_altitude() const=0
void do_land(const AP_Mission::Mission_Command &cmd)
struct Copter::ModeAuto::@7 nav_payload_place
#define ERROR_SUBSYSTEM_NAVIGATION
uint32_t nav_delay_time_start
void circle_movetoedge_start(const Location_Class &circle_center, float radius_m)
void set_land_complete(bool b)
bool get_wp(Location_Class &loc) override