From 376a422cff1392ff9cd2633f2456f5dfcc96b965 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 2 Sep 2018 22:33:53 +1000 Subject: [PATCH] Sub: OpticalFlow takes care of its own logging --- ArduSub/ArduSub.cpp | 2 +- ArduSub/Log.cpp | 39 --------------------------------------- ArduSub/Sub.h | 2 -- ArduSub/defines.h | 1 - ArduSub/sensors.cpp | 31 +------------------------------ 5 files changed, 2 insertions(+), 73 deletions(-) diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index 43ae91c633..0b2a503797 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -28,7 +28,7 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = { SCHED_TASK(fifty_hz_loop, 50, 75), SCHED_TASK(update_GPS, 50, 200), #if OPTFLOW == ENABLED - SCHED_TASK(update_optical_flow, 200, 160), + SCHED_TASK_CLASS(OpticalFlow, &sub.optflow, update, 200, 160), #endif SCHED_TASK(update_batt_compass, 10, 120), SCHED_TASK(read_rangefinder, 20, 100), diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index c48963d334..e9bf239e24 100644 --- a/ArduSub/Log.cpp +++ b/ArduSub/Log.cpp @@ -5,39 +5,6 @@ // Code to Write and Read packets from DataFlash log memory // Code to interact with the user to dump or erase logs -struct PACKED log_Optflow { - LOG_PACKET_HEADER; - uint64_t time_us; - uint8_t surface_quality; - float flow_x; - float flow_y; - float body_x; - float body_y; -}; - -// Write an optical flow packet -void Sub::Log_Write_Optflow() -{ -#if OPTFLOW == ENABLED - // exit immediately if not enabled - if (!optflow.enabled()) { - return; - } - const Vector2f &flowRate = optflow.flowRate(); - const Vector2f &bodyRate = optflow.bodyRate(); - struct log_Optflow pkt = { - LOG_PACKET_HEADER_INIT(LOG_OPTFLOW_MSG), - time_us : AP_HAL::micros64(), - surface_quality : optflow.quality(), - flow_x : flowRate.x, - flow_y : flowRate.y, - body_x : bodyRate.x, - body_y : bodyRate.y - }; - DataFlash.WriteBlock(&pkt, sizeof(pkt)); -#endif // OPTFLOW == ENABLED -} - struct PACKED log_Control_Tuning { LOG_PACKET_HEADER; uint64_t time_us; @@ -317,8 +284,6 @@ void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target // units and "Format characters" for field type information const struct LogStructure Sub::log_structure[] = { LOG_COMMON_STRUCTURES, - { LOG_OPTFLOW_MSG, sizeof(log_Optflow), - "OF", "QBffff", "TimeUS,Qual,flowX,flowY,bodyX,bodyY", "s-EEEE", "F-0000" }, { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning), "CTUN", "Qfffffffccfhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00BBBBBB" }, { LOG_MOTBATT_MSG, sizeof(log_MotBatt), @@ -372,10 +337,6 @@ void Sub::Log_Sensor_Health() {} void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {} void Sub::Log_Write_Vehicle_Startup_Messages() {} -#if OPTFLOW == ENABLED -void Sub::Log_Write_Optflow() {} -#endif - void Sub::log_init(void) {} #endif // LOGGING_ENABLED diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 390d95f74d..f1ab07897a 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -485,7 +485,6 @@ private: void send_pid_tuning(mavlink_channel_t chan); void gcs_data_stream_send(void); void gcs_update(void); - void Log_Write_Optflow(); void Log_Write_Control_Tuning(); void Log_Write_Performance(); void Log_Write_Attitude(); @@ -617,7 +616,6 @@ private: void init_compass(); #if OPTFLOW == ENABLED void init_optflow(); - void update_optical_flow(void); #endif void terrain_update(); void terrain_logging(); diff --git a/ArduSub/defines.h b/ArduSub/defines.h index 63e46248ef..0a95fba0b8 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -105,7 +105,6 @@ enum LoggingParameters { TYPE_AIRSTART_MSG, TYPE_GROUNDSTART_MSG, LOG_CONTROL_TUNING_MSG, - LOG_OPTFLOW_MSG, LOG_EVENT_MSG, LOG_ERROR_MSG, LOG_DATA_INT16_MSG, diff --git a/ArduSub/sensors.cpp b/ArduSub/sensors.cpp index 75543e54fe..f6cc83b96b 100644 --- a/ArduSub/sensors.cpp +++ b/ArduSub/sensors.cpp @@ -112,39 +112,10 @@ void Sub::init_compass_location() void Sub::init_optflow() { // initialise optical flow sensor - optflow.init(); + optflow.init(MASK_LOG_OPTFLOW); } #endif // OPTFLOW == ENABLED -// called at 200hz -#if OPTFLOW == ENABLED -void Sub::update_optical_flow(void) -{ - static uint32_t last_of_update = 0; - - // exit immediately if not enabled - if (!optflow.enabled()) { - return; - } - - // read from sensor - optflow.update(); - - // write to log and send to EKF if new data has arrived - if (optflow.last_update() != last_of_update) { - last_of_update = optflow.last_update(); - uint8_t flowQuality = optflow.quality(); - Vector2f flowRate = optflow.flowRate(); - Vector2f bodyRate = optflow.bodyRate(); - const Vector3f &posOffset = optflow.get_pos_offset(); - ahrs.writeOptFlowMeas(flowQuality, flowRate, bodyRate, last_of_update, posOffset); - if (g.log_bitmask & MASK_LOG_OPTFLOW) { - Log_Write_Optflow(); - } - } -} -#endif // OPTFLOW == ENABLED - void Sub::compass_cal_update() { if (!hal.util->get_soft_armed()) {