Browse Source

Copter: add option to disable AUTO mode

Saves ~12k of flash
mission-4.1.18
Peter Barker 7 years ago committed by Randy Mackay
parent
commit
b9ad2bc8db
  1. 1
      ArduCopter/APM_Config.h
  2. 4
      ArduCopter/Copter.h
  3. 10
      ArduCopter/GCS_Mavlink.cpp
  4. 2
      ArduCopter/Parameters.cpp
  5. 2
      ArduCopter/commands.cpp
  6. 4
      ArduCopter/commands_logic.cpp
  7. 6
      ArduCopter/config.h
  8. 4
      ArduCopter/mode.cpp
  9. 8
      ArduCopter/mode_auto.cpp
  10. 2
      ArduCopter/motors.cpp
  11. 14
      ArduCopter/switches.cpp
  12. 4
      ArduCopter/system.cpp

1
ArduCopter/APM_Config.h

@ -23,6 +23,7 @@ @@ -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

4
ArduCopter/Copter.h

@ -241,6 +241,7 @@ private: @@ -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: @@ -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: @@ -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

10
ArduCopter/GCS_Mavlink.cpp

@ -669,7 +669,11 @@ GCS_MAVLINK_Copter::data_stream_send(void) @@ -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) @@ -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) @@ -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 @@ -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

2
ArduCopter/Parameters.cpp

@ -725,9 +725,11 @@ const AP_Param::Info Copter::var_info[] = { @@ -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

2
ArduCopter/commands.cpp

@ -85,6 +85,7 @@ bool Copter::set_home(const Location& loc, bool lock) @@ -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) @@ -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

4
ArduCopter/commands_logic.cpp

@ -1,5 +1,7 @@ @@ -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) @@ -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

6
ArduCopter/config.h

@ -267,6 +267,12 @@ @@ -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
//////////////////////////////////////////////////////////////////////////////

4
ArduCopter/mode.cpp

@ -50,9 +50,11 @@ Copter::Mode *Copter::mode_from_mode_num(const uint8_t mode) @@ -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, @@ -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, @@ -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) {

8
ArduCopter/mode_auto.cpp

@ -1,5 +1,7 @@ @@ -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() @@ -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) @@ -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() @@ -868,3 +874,5 @@ void Copter::ModeAuto::payload_place_run_descend()
copter.land_run_horizontal_control();
copter.land_run_vertical_control();
}
#endif

2
ArduCopter/motors.cpp

@ -272,8 +272,10 @@ void Copter::init_disarm_motors() @@ -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);

14
ArduCopter/switches.cpp

@ -249,6 +249,7 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) @@ -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) @@ -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) @@ -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);

4
ArduCopter/system.cpp

@ -231,14 +231,18 @@ void Copter::init_ardupilot() @@ -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…
Cancel
Save