mirror of https://github.com/ArduPilot/ardupilot
Copter: add option to disable AUTO mode
Saves ~12k of flash
This commit is contained in:
parent
eb9bbddcb7
commit
b9ad2bc8db
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
|
|
@ -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) {
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue