diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 518f7a7784..b169504c48 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -836,7 +836,6 @@ private: #endif void Log_Write_Precland(); void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target); - void Log_Write_Throw(ThrowModeStage stage, float velocity, float velocity_z, float accel, float ef_accel_z, bool throw_detect, bool attitude_ok, bool height_ok, bool position_ok); void Log_Write_Vehicle_Startup_Messages(); void log_init(void); diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index bf48e9dc51..60e5681f87 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -489,43 +489,6 @@ void Copter::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_tar DataFlash.WriteBlock(&pkt, sizeof(pkt)); } -#if MODE_THROW_ENABLED == ENABLED -// throw flight mode logging -struct PACKED log_Throw { - LOG_PACKET_HEADER; - uint64_t time_us; - uint8_t stage; - float velocity; - float velocity_z; - float accel; - float ef_accel_z; - uint8_t throw_detect; - uint8_t attitude_ok; - uint8_t height_ok; - uint8_t pos_ok; -}; - -// Write a Throw mode details -void Copter::Log_Write_Throw(ThrowModeStage stage, float velocity, float velocity_z, float accel, float ef_accel_z, bool throw_detect, bool attitude_ok, bool height_ok, bool pos_ok) -{ - struct log_Throw pkt = { - LOG_PACKET_HEADER_INIT(LOG_THROW_MSG), - time_us : AP_HAL::micros64(), - stage : (uint8_t)stage, - velocity : velocity, - velocity_z : velocity_z, - accel : accel, - ef_accel_z : ef_accel_z, - throw_detect : throw_detect, - attitude_ok : attitude_ok, - height_ok : height_ok, - pos_ok : pos_ok - }; - DataFlash.WriteBlock(&pkt, sizeof(pkt)); -} -#endif - - // type and unit information can be found in // libraries/DataFlash/Logstructure.h; search for "log_Units" for // units and "Format characters" for field type information @@ -571,10 +534,6 @@ const struct LogStructure Copter::log_structure[] = { #endif { 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() @@ -614,7 +573,6 @@ void Copter::Log_Write_Home_And_Origin() {} void Copter::Log_Sensor_Health() {} void Copter::Log_Write_Precland() {} void Copter::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {} -void Copter::Log_Write_Throw(ThrowModeStage stage, float velocity, float velocity_z, float accel, float ef_accel_z, bool throw_detect, bool attitude_ok, bool height_ok, bool pos_ok) {} void Copter::Log_Write_Proximity() {} void Copter::Log_Write_Vehicle_Startup_Messages() {} diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 7e7f8b1b85..2bd5071c9f 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -839,7 +839,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Description: Used by THROW mode. Specifies whether Copter is thrown upward or dropped. // @Values: 0:Upward Throw,1:Drop // @User: Standard - AP_GROUPINFO("THROW_TYPE", 4, ParametersG2, throw_type, ThrowType_Upward), + AP_GROUPINFO("THROW_TYPE", 4, ParametersG2, throw_type, Copter::ModeThrow::ThrowType_Upward), #endif // @Param: GND_EFFECT_COMP diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index f9fa22e0f9..388c2ef600 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -265,21 +265,6 @@ enum FlipState { Flip_Abandon }; -// Throw stages -enum ThrowModeStage { - Throw_Disarmed, - Throw_Detecting, - Throw_Uprighting, - Throw_HgtStabilise, - Throw_PosHold -}; - -// Throw types -enum ThrowModeType { - ThrowType_Upward = 0, - ThrowType_Drop = 1 -}; - enum LandStateType { LandStateType_FlyToLocation = 0, LandStateType_Descending = 1 @@ -324,7 +309,6 @@ enum LoggingParameters { LOG_HELI_MSG, LOG_PRECLAND_MSG, LOG_GUIDEDTARGET_MSG, - LOG_THROW_MSG, }; #define MASK_LOG_ATTITUDE_FAST (1<<0) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 9c23b4b28d..f3f8b39b35 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1057,6 +1057,11 @@ public: bool allows_arming(bool from_gcs) const override { return true; }; bool is_autopilot() const override { return false; } + // Throw types + enum ThrowModeType { + ThrowType_Upward = 0, + ThrowType_Drop = 1 + }; protected: @@ -1070,13 +1075,21 @@ private: bool throw_height_good(); bool throw_attitude_good(); + // Throw stages + enum ThrowModeStage { + Throw_Disarmed, + Throw_Detecting, + Throw_Uprighting, + Throw_HgtStabilise, + Throw_PosHold + }; + ThrowModeStage stage = Throw_Disarmed; ThrowModeStage prev_stage = Throw_Disarmed; uint32_t last_log_ms; bool nextmode_attempted; uint32_t free_fall_start_ms; // system time free fall was detected float free_fall_start_velz; // vertical velocity when free fall was detected - }; // modes below rely on Guided mode so must be declared at the end (instead of in alphabetical order) diff --git a/ArduCopter/mode_throw.cpp b/ArduCopter/mode_throw.cpp index c5f7850ee3..63e4e59a3c 100644 --- a/ArduCopter/mode_throw.cpp +++ b/ArduCopter/mode_throw.cpp @@ -193,15 +193,22 @@ void Copter::ModeThrow::run() bool attitude_ok = (stage > Throw_Uprighting) || throw_attitude_good(); bool height_ok = (stage > Throw_HgtStabilise) || throw_height_good(); bool pos_ok = (stage > Throw_PosHold) || throw_position_good(); - copter.Log_Write_Throw(stage, - velocity, - velocity_z, - accel, - ef_accel_z, - throw_detect, - attitude_ok, - height_ok, - pos_ok); + DataFlash_Class::instance()->Log_Write( + "THRO", + "TimeUS,Stage,Vel,VelZ,Acc,AccEfZ,Throw,AttOk,HgtOk,PosOk", + "s-nnoo----", + "F-0000----", + "QBffffbbbb", + AP_HAL::micros64(), + (uint8_t)stage, + velocity, + velocity_z, + accel, + ef_accel_z, + throw_detect, + attitude_ok, + height_ok, + pos_ok); } }