diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 30e0ea40aa..8f419a7295 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -38,6 +38,7 @@ //#define MODE_RTL_ENABLED DISABLED // disable rtl mode support //#define MODE_SMARTRTL_ENABLED DISABLED // disable smartrtl mode support //#define MODE_SPORT_ENABLED DISABLED // disable sport mode support +//#define MODE_THROW_ENABLED DISABLED // disable throw mode support // features below are disabled by default on all boards diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 434d8a568f..8c82ff0130 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -1011,7 +1011,9 @@ private: #if ADSB_ENABLED == ENABLED ModeAvoidADSB mode_avoid_adsb; #endif +#if MODE_THROW_ENABLED == ENABLED ModeThrow mode_throw; +#endif #if MODE_GUIDED_NOGPS_ENABLED == ENABLED ModeGuidedNoGPS mode_guided_nogps; #endif diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index 7292e5f236..e5eccc9842 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -537,7 +537,8 @@ void Copter::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_tar DataFlash.WriteBlock(&pkt, sizeof(pkt)); } -// precision landing logging +#if MODE_THROW_ENABLED == ENABLED +// throw flight mode logging struct PACKED log_Throw { LOG_PACKET_HEADER; uint64_t time_us; @@ -570,6 +571,7 @@ void Copter::Log_Write_Throw(ThrowModeStage stage, float velocity, float velocit }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); } +#endif // type and unit information can be found in @@ -613,8 +615,10 @@ const struct LogStructure Copter::log_structure[] = { "PL", "QBBffff", "TimeUS,Heal,TAcq,pX,pY,vX,vY", "s--ddmm","F--00BB" }, { LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget), "GUID", "QBffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ", "s-mmmnnn", "F-000000" }, +#if MODE_THROW_ENABLED == ENABLED { LOG_THROW_MSG, sizeof(log_Throw), "THRO", "QBffffbbbb", "TimeUS,Stage,Vel,VelZ,Acc,AccEfZ,Throw,AttOk,HgtOk,PosOk", "s-nnoo----", "F-0000----" }, +#endif }; void Copter::Log_Write_Vehicle_Startup_Messages() diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index d472aabc2c..58551f1737 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -801,12 +801,14 @@ const AP_Param::Info Copter::var_info[] = { // @Path: ../libraries/AP_Notify/AP_Notify.cpp GOBJECT(notify, "NTF_", AP_Notify), +#if MODE_THROW_ENABLED == ENABLED // @Param: THROW_MOT_START // @DisplayName: Start motors before throwing is detected // @Description: Used by THROW mode. Controls whether motors will run at the speed set by THR_MIN or will be stopped when armed and waiting for the throw. // @Values: 0:Stopped,1:Running // @User: Standard GSCALAR(throw_motor_start, "THROW_MOT_START", 0), +#endif // @Param: TERRAIN_FOLLOW // @DisplayName: Terrain Following use control @@ -838,6 +840,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Path: ../libraries/AP_Button/AP_Button.cpp AP_SUBGROUPINFO(button, "BTN_", 2, ParametersG2, AP_Button), +#if MODE_THROW_ENABLED == ENABLED // @Param: THROW_NEXTMODE // @DisplayName: Throw mode's follow up mode // @Description: Vehicle will switch to this mode after the throw is successfully completed. Default is to stay in throw mode (18) @@ -851,6 +854,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Values: 0:Upward Throw,1:Drop // @User: Standard AP_GROUPINFO("THROW_TYPE", 4, ParametersG2, throw_type, ThrowType_Upward), +#endif // @Param: GND_EFFECT_COMP // @DisplayName: Ground Effect Compensation Enable/Disable diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 7a217cad21..8333dc9bcb 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -457,7 +457,9 @@ public: AP_Float fs_ekf_thresh; AP_Int16 gcs_pid_mask; +#if MODE_THROW_ENABLED == ENABLED AP_Int8 throw_motor_start; +#endif AP_Int8 terrain_follow; AP_Int16 rc_speed; // speed of fast RC Channels in Hz @@ -507,9 +509,11 @@ public: AP_Gripper gripper; #endif +#if MODE_THROW_ENABLED == ENABLED // Throw mode parameters AP_Int8 throw_nextmode; AP_Int8 throw_type; +#endif // ground effect compensation enable/disable AP_Int8 gndeffect_comp_enabled; diff --git a/ArduCopter/avoidance_adsb.cpp b/ArduCopter/avoidance_adsb.cpp index dad7e42f0d..b623e68480 100644 --- a/ArduCopter/avoidance_adsb.cpp +++ b/ArduCopter/avoidance_adsb.cpp @@ -25,7 +25,9 @@ MAV_COLLISION_ACTION AP_Avoidance_Copter::handle_avoidance(const AP_Avoidance::O // take no action in some flight modes if (copter.control_mode == LAND || +#if MODE_THROW_ENABLED == ENABLED copter.control_mode == THROW || +#endif copter.control_mode == FLIP) { actual_action = MAV_COLLISION_ACTION_NONE; } diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 4d2ec1406c..3681ea26ff 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -345,6 +345,12 @@ # define MODE_SPORT_ENABLED ENABLED #endif +////////////////////////////////////////////////////////////////////////////// +// Throw - fly vehicle after throwing it in the air +#ifndef MODE_THROW_ENABLED +# define MODE_THROW_ENABLED ENABLED +#endif + ////////////////////////////////////////////////////////////////////////////// // Beacon support - support for local positioning systems #ifndef BEACON_ENABLED diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index ea11b5bd25..2b1b9b21b4 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -120,9 +120,11 @@ Copter::Mode *Copter::mode_from_mode_num(const uint8_t mode) break; #endif +#if MODE_THROW_ENABLED == ENABLED case THROW: ret = &mode_throw; break; +#endif #if ADSB_ENABLED == ENABLED case AVOID_ADSB: diff --git a/ArduCopter/mode_throw.cpp b/ArduCopter/mode_throw.cpp index 9d13845e0d..3902103f74 100644 --- a/ArduCopter/mode_throw.cpp +++ b/ArduCopter/mode_throw.cpp @@ -1,5 +1,6 @@ #include "Copter.h" +#if MODE_THROW_ENABLED == ENABLED // throw_init - initialise throw controller bool Copter::ModeThrow::init(bool ignore_checks) @@ -267,3 +268,4 @@ bool Copter::ModeThrow::throw_position_good() // check that our horizontal position error is within 50cm return (pos_control->get_horizontal_error() < 50.0f); } +#endif diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index 586d7d2a61..93834553d1 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -546,6 +546,7 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) break; case AUXSW_THROW: +#if MODE_THROW_ENABLED == ENABLED // throw flight mode if (ch_flag == AUX_SWITCH_HIGH) { set_mode(THROW, MODE_REASON_TX_COMMAND); @@ -555,6 +556,7 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) reset_control_switch(); } } +#endif break; case AUXSW_AVOID_ADSB: diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index d3600bea3d..0db2a52c63 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -425,7 +425,7 @@ void Copter::update_auto_armed() } #else // if motors are armed and throttle is above zero auto_armed should be true - // if motors are armed and we are in throw mode, then auto_ermed should be true + // if motors are armed and we are in throw mode, then auto_armed should be true if(motors->armed() && (!ap.throttle_zero || control_mode == THROW)) { set_auto_armed(true); } diff --git a/ArduCopter/toy_mode.cpp b/ArduCopter/toy_mode.cpp index 5bf2d67ef0..e4e969fd9a 100644 --- a/ArduCopter/toy_mode.cpp +++ b/ArduCopter/toy_mode.cpp @@ -489,10 +489,12 @@ void ToyMode::update() send_named_int("VIDEOTOG", 1); break; +#if MODE_THROW_ENABLED == ENABLED case ACTION_MODE_ACRO: new_mode = ACRO; break; - +#endif + case ACTION_MODE_ALTHOLD: new_mode = ALT_HOLD; break; @@ -537,9 +539,11 @@ void ToyMode::update() new_mode = BRAKE; break; +#if MODE_THROW_ENABLED == ENABLED case ACTION_MODE_THROW: new_mode = THROW; break; +#endif case ACTION_MODE_FLIP: new_mode = FLIP;