From bb73d31e0be943bf96599612867b996acddc88de Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Wed, 15 Dec 2021 19:09:29 +0000 Subject: [PATCH] Rover: add video stab log bit and log in ahrs_update --- Rover/Parameters.cpp | 2 +- Rover/Rover.cpp | 4 ++++ Rover/defines.h | 2 ++ 3 files changed, 7 insertions(+), 1 deletion(-) diff --git a/Rover/Parameters.cpp b/Rover/Parameters.cpp index 88207d8c92..e06ef57e9f 100644 --- a/Rover/Parameters.cpp +++ b/Rover/Parameters.cpp @@ -19,7 +19,7 @@ const AP_Param::Info Rover::var_info[] = { // @Param: LOG_BITMASK // @DisplayName: Log bitmask // @Description: Bitmap of what log types to enable in on-board logger. This value is made up of the sum of each of the log types you want to be saved. On boards supporting microSD cards or other large block-storage devices it is usually best just to enable all log types by setting this to 65535. The individual bits are ATTITUDE_FAST=1, ATTITUDE_MEDIUM=2, GPS=4, PerformanceMonitoring=8, ControlTuning=16, NavigationTuning=32, Mode=64, IMU=128, Commands=256, Battery=512, Compass=1024, TECS=2048, Camera=4096, RCandServo=8192, Rangefinder=16384, Arming=32768, FullLogs=65535 - // @Bitmask: 0:ATTITUDE_FAST,1:ATTITUDE_MED,2:GPS,3:PM,4:THR,5:NTUN,7:IMU,8:CMD,9:CURRENT,10:RANGEFINDER,11:COMPASS,12:CAMERA,13:STEERING,14:RC,15:ARM/DISARM,19:IMU_RAW + // @Bitmask: 0:ATTITUDE_FAST,1:ATTITUDE_MED,2:GPS,3:PM,4:THR,5:NTUN,7:IMU,8:CMD,9:CURRENT,10:RANGEFINDER,11:COMPASS,12:CAMERA,13:STEERING,14:RC,15:ARM/DISARM,19:IMU_RAW,20:VideoStabilization // @User: Advanced GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK), diff --git a/Rover/Rover.cpp b/Rover/Rover.cpp index 4413311124..b80f1eb8bf 100644 --- a/Rover/Rover.cpp +++ b/Rover/Rover.cpp @@ -289,6 +289,10 @@ void Rover::ahrs_update() if (should_log(MASK_LOG_IMU)) { AP::ins().Write_IMU(); } + + if (should_log(MASK_LOG_VIDEO_STABILISATION)) { + ahrs.write_video_stabilisation(); + } } /* diff --git a/Rover/defines.h b/Rover/defines.h index 20d3f86af1..1546f865de 100644 --- a/Rover/defines.h +++ b/Rover/defines.h @@ -45,6 +45,8 @@ enum LoggingParameters { #define MASK_LOG_RC (1<<14) // #define MASK_LOG_ARM_DISARM (1<<15) #define MASK_LOG_IMU_RAW (1UL<<19) +#define MASK_LOG_VIDEO_STABILISATION (1UL<<20) + // for mavlink SET_POSITION_TARGET messages #define MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE ((1<<0) | (1<<1))