From a07ecfe2b310d3ba3fe004058bc9d23deb84840c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 16 Aug 2016 21:09:17 +0900 Subject: [PATCH] Copter: log proximity sensor at 10hz This is current in the Copter vehicle code but we can move to the dataflash library when other vehicles start using this type of sensor. Until then adding it to common will just increase the dependencies unnecessarily for other vehicles. --- ArduCopter/ArduCopter.cpp | 1 + ArduCopter/Copter.h | 1 + ArduCopter/Log.cpp | 48 +++++++++++++++++++++++++++++++++++++++ ArduCopter/defines.h | 1 + 4 files changed, 51 insertions(+) diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index ee82ad78eb..2f751bdaea 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -397,6 +397,7 @@ void Copter::ten_hz_logging_loop() } if (should_log(MASK_LOG_CTUN)) { attitude_control.control_monitor_log(); + Log_Write_Proximity(); } #if FRAME_CONFIG == HELI_FRAME Log_Write_Heli(); diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 96b532a8ff..80729eb01e 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -752,6 +752,7 @@ private: 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_Proximity(); void Log_Write_Vehicle_Startup_Messages(); void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page); void start_logging() ; diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index 900c61da20..c44bbc213e 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -750,6 +750,52 @@ void Copter::Log_Write_Throw(ThrowModeStage stage, float velocity, float velocit DataFlash.WriteBlock(&pkt, sizeof(pkt)); } +// proximity sensor logging +struct PACKED log_Proximity { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t health; + float dist0; + float dist45; + float dist90; + float dist135; + float dist180; + float dist225; + float dist270; + float dist315; +}; + +// Write proximity sensor distances +void Copter::Log_Write_Proximity() +{ +#if PROXIMITY_ENABLED == ENABLED + float sector_distance[8] = {0,0,0,0,0,0,0,0}; + proximity.get_horizontal_distance(0, sector_distance[0]); + proximity.get_horizontal_distance(45, sector_distance[1]); + proximity.get_horizontal_distance(90, sector_distance[2]); + proximity.get_horizontal_distance(135, sector_distance[3]); + proximity.get_horizontal_distance(180, sector_distance[4]); + proximity.get_horizontal_distance(225, sector_distance[5]); + proximity.get_horizontal_distance(270, sector_distance[6]); + proximity.get_horizontal_distance(315, sector_distance[7]); + + struct log_Proximity pkt = { + LOG_PACKET_HEADER_INIT(LOG_PROXIMITY_MSG), + time_us : AP_HAL::micros64(), + health : (uint8_t)proximity.get_status(), + dist0 : sector_distance[0], + dist45 : sector_distance[1], + dist90 : sector_distance[2], + dist135 : sector_distance[3], + dist180 : sector_distance[4], + dist225 : sector_distance[5], + dist270 : sector_distance[6], + dist315 : sector_distance[7] + }; + DataFlash.WriteBlock(&pkt, sizeof(pkt)); +#endif +} + const struct LogStructure Copter::log_structure[] = { LOG_COMMON_STRUCTURES, #if AUTOTUNE_ENABLED == ENABLED @@ -792,6 +838,8 @@ const struct LogStructure Copter::log_structure[] = { "GUID", "QBffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ" }, { LOG_THROW_MSG, sizeof(log_Throw), "THRO", "QBffffbbbb", "TimeUS,Stage,Vel,VelZ,Acc,AccEfZ,Throw,AttOk,HgtOk,PosOk" }, + { LOG_PROXIMITY_MSG, sizeof(log_Proximity), + "PRX", "QBffffffff", "TimeUS,Health,D0,D45,D90,D135,D180,D225,D270,D315" }, }; #if CLI_ENABLED == ENABLED diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 06be55d065..6a86ee82ce 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -307,6 +307,7 @@ enum DevOptions { #define LOG_PRECLAND_MSG 0x21 #define LOG_GUIDEDTARGET_MSG 0x22 #define LOG_THROW_MSG 0x23 +#define LOG_PROXIMITY_MSG 0x24 #define MASK_LOG_ATTITUDE_FAST (1<<0) #define MASK_LOG_ATTITUDE_MED (1<<1)