diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index b93fd47759..c7c4eb9e19 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -23,6 +23,7 @@ //#define PRECISION_LANDING DISABLED // disable precision landing using companion computer or IRLock sensor //#define SPRAYER DISABLED // disable the crop sprayer feature (two ESC controlled pumps the speed of which depends upon the vehicle's horizontal velocity) //#define WINCH_ENABLED DISABLED // disable winch support +//#define MODE_AUTO_ENABLED DISABLED // disable auto mode support // features below are disabled by default on all boards //#define CAL_ALWAYS_REBOOT // flight controller will reboot after compass or accelerometer calibration completes diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index bc64b9e4f3..66b909012f 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -241,6 +241,7 @@ private: #endif // Mission library +#if MODE_AUTO_ENABLED == ENABLED AP_Mission mission{ahrs, FUNCTOR_BIND_MEMBER(&Copter::start_command, bool, const AP_Mission::Mission_Command &), FUNCTOR_BIND_MEMBER(&Copter::verify_command_callback, bool, const AP_Mission::Mission_Command &), @@ -255,6 +256,7 @@ private: void exit_mission() { mode_auto.exit_mission(); } +#endif // Arming/Disarming mangement class AP_Arming_Copter arming{ahrs, barometer, compass, battery, inertial_nav, ins}; @@ -954,7 +956,9 @@ private: ModeAcro mode_acro; #endif ModeAltHold mode_althold; +#if MODE_AUTO_ENABLED == ENABLED ModeAuto mode_auto; +#endif #if AUTOTUNE_ENABLED == ENABLED ModeAutoTune mode_autotune; #endif diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index c165f3676a..b6d14f64ce 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -669,7 +669,11 @@ GCS_MAVLINK_Copter::data_stream_send(void) bool GCS_MAVLINK_Copter::handle_guided_request(AP_Mission::Mission_Command &cmd) { +#if MODE_AUTO_ENABLED == ENABLED return copter.mode_auto.do_guided(cmd); +#else + return false; +#endif } void GCS_MAVLINK_Copter::handle_change_alt_request(AP_Mission::Mission_Command &cmd) @@ -1025,6 +1029,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) #endif break; +#if MODE_AUTO_ENABLED == ENABLED case MAV_CMD_MISSION_START: if (copter.motors->armed() && copter.set_mode(AUTO, MODE_REASON_GCS_COMMAND)) { copter.set_auto_armed(true); @@ -1034,6 +1039,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) result = MAV_RESULT_ACCEPTED; } break; +#endif case MAV_CMD_PREFLIGHT_CALIBRATION: // exit immediately if armed @@ -1793,7 +1799,11 @@ bool GCS_MAVLINK_Copter::accept_packet(const mavlink_status_t &status, mavlink_m AP_Mission *GCS_MAVLINK_Copter::get_mission() { +#if MODE_AUTO_ENABLED == ENABLED return &copter.mission; +#else + return nullptr; +#endif } Compass *GCS_MAVLINK_Copter::get_compass() const diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 13d027af11..4fb8d5699b 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -725,9 +725,11 @@ const AP_Param::Info Copter::var_info[] = { // @Path: ../libraries/AP_NavEKF3/AP_NavEKF3.cpp GOBJECTN(EKF3, NavEKF3, "EK3_", NavEKF3), +#if MODE_AUTO_ENABLED == ENABLED // @Group: MIS_ // @Path: ../libraries/AP_Mission/AP_Mission.cpp GOBJECT(mission, "MIS_", AP_Mission), +#endif // @Group: RSSI_ // @Path: ../libraries/AP_RSSI/AP_RSSI.cpp diff --git a/ArduCopter/commands.cpp b/ArduCopter/commands.cpp index c55f5c6433..db3e19df94 100644 --- a/ArduCopter/commands.cpp +++ b/ArduCopter/commands.cpp @@ -85,6 +85,7 @@ bool Copter::set_home(const Location& loc, bool lock) // record home is set set_home_state(HOME_SET_NOT_LOCKED); +#if MODE_AUTO_ENABLED == ENABLED // log new home position which mission library will pull from ahrs if (should_log(MASK_LOG_CMD)) { AP_Mission::Mission_Command temp_cmd; @@ -92,6 +93,7 @@ bool Copter::set_home(const Location& loc, bool lock) DataFlash.Log_Write_Mission_Cmd(mission, temp_cmd); } } +#endif } // lock home position diff --git a/ArduCopter/commands_logic.cpp b/ArduCopter/commands_logic.cpp index 14efd1a3e5..04215878bc 100644 --- a/ArduCopter/commands_logic.cpp +++ b/ArduCopter/commands_logic.cpp @@ -1,5 +1,7 @@ #include "Copter.h" +#if MODE_AUTO_ENABLED == ENABLED + // start_command - this function will be called when the ap_mission lib wishes to start a new command bool Copter::ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) { @@ -1192,3 +1194,5 @@ void Copter::ModeAuto::do_mount_control(const AP_Mission::Mission_Command& cmd) copter.camera_mount.set_angle_targets(cmd.content.mount_control.roll, cmd.content.mount_control.pitch, cmd.content.mount_control.yaw); #endif } + +#endif diff --git a/ArduCopter/config.h b/ArduCopter/config.h index d04525b12e..0562ed97ea 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -267,6 +267,12 @@ # define NAV_GUIDED ENABLED #endif +////////////////////////////////////////////////////////////////////////////// +// Auto mode - allows vehicle to trace waypoints and perform automated actions +#ifndef MODE_AUTO_ENABLED +# define MODE_AUTO_ENABLED ENABLED +#endif + ////////////////////////////////////////////////////////////////////////////// // RADIO CONFIGURATION ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 9c57cdac87..a97621ac14 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -50,9 +50,11 @@ Copter::Mode *Copter::mode_from_mode_num(const uint8_t mode) ret = &mode_althold; break; +#if MODE_AUTO_ENABLED == ENABLED case AUTO: ret = &mode_auto; break; +#endif case CIRCLE: ret = &mode_circle; @@ -225,6 +227,7 @@ void Copter::exit_mode(Copter::Mode *&old_flightmode, #endif // stop mission when we leave auto mode +#if MODE_AUTO_ENABLED == ENABLED if (old_flightmode == &mode_auto) { if (mission.state() == AP_Mission::MISSION_RUNNING) { mission.stop(); @@ -233,6 +236,7 @@ void Copter::exit_mode(Copter::Mode *&old_flightmode, camera_mount.set_mode_to_default(); #endif // MOUNT == ENABLED } +#endif // smooth throttle transition when switching from manual to automatic flight modes if (old_flightmode->has_manual_throttle() && !new_flightmode->has_manual_throttle() && motors->armed() && !ap.land_complete) { diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 95bb25f5e1..8fea9060fb 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -1,5 +1,7 @@ #include "Copter.h" +#if MODE_AUTO_ENABLED == ENABLED + /* * Init and run calls for auto flight mode * @@ -577,6 +579,8 @@ void Copter::ModeAuto::loiter_run() attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain()); } +#endif + // get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter // set rtl parameter to true if this is during an RTL uint8_t Copter::get_default_auto_yaw_mode(bool rtl) @@ -762,6 +766,8 @@ float Copter::get_auto_yaw_rate_cds(void) return 0.0f; } +#if MODE_AUTO_ENABLED == ENABLED + // auto_payload_place_start - initialises controller to implement a placing void Copter::ModeAuto::payload_place_start() { @@ -868,3 +874,5 @@ void Copter::ModeAuto::payload_place_run_descend() copter.land_run_horizontal_control(); copter.land_run_vertical_control(); } + +#endif diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 677601fff1..c4b2ca2ab6 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -272,8 +272,10 @@ void Copter::init_disarm_motors() // send disarm command to motors motors->armed(false); +#if MODE_AUTO_ENABLED == ENABLED // reset the mission mission.reset(); +#endif DataFlash_Class::instance()->set_vehicle_armed(false); diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index 90cb6e6723..1a87ab8c63 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -249,6 +249,7 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) } break; +#if MODE_AUTO_ENABLED == ENABLED case AUXSW_SAVE_WP: // save waypoint when switch is brought high if (ch_flag == AUX_SWITCH_HIGH) { @@ -303,6 +304,13 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) } break; + case AUXSW_MISSION_RESET: + if (ch_flag == AUX_SWITCH_HIGH) { + mission.reset(); + } + break; +#endif + case AUXSW_CAMERA_TRIGGER: #if CAMERA == ENABLED if (ch_flag == AUX_SWITCH_HIGH) { @@ -451,12 +459,6 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) #endif break; - case AUXSW_MISSION_RESET: - if (ch_flag == AUX_SWITCH_HIGH) { - mission.reset(); - } - break; - case AUXSW_ATTCON_FEEDFWD: // enable or disable feed forward attitude_control->bf_feedforward(ch_flag == AUX_SWITCH_HIGH); diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 6a739acdf8..4d882e66e7 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -231,14 +231,18 @@ void Copter::init_ardupilot() // initialise AP_RPM library rpm_sensor.init(); +#if MODE_AUTO_ENABLED == ENABLED // initialise mission library mission.init(); +#endif // initialize SmartRTL g2.smart_rtl.init(); // initialise DataFlash library +#if MODE_AUTO_ENABLED == ENABLED DataFlash.set_mission(&mission); +#endif DataFlash.setVehicle_Startup_Log_Writer(FUNCTOR_BIND(&copter, &Copter::Log_Write_Vehicle_Startup_Messages, void)); // initialise the flight mode and aux switch