Browse Source

AP_DAL: add wheelencoder_enabled

c415-sdk
Randy Mackay 4 years ago
parent
commit
7d319f8059
  1. 2
      libraries/AP_DAL/AP_DAL.cpp
  2. 4
      libraries/AP_DAL/AP_DAL.h
  3. 1
      libraries/AP_DAL/LogStructure.h

2
libraries/AP_DAL/AP_DAL.cpp

@ -5,6 +5,7 @@ @@ -5,6 +5,7 @@
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_OpticalFlow/AP_OpticalFlow.h>
#include <AP_WheelEncoder/AP_WheelEncoder.h>
#if APM_BUILD_TYPE(APM_BUILD_Replay)
#include <AP_NavEKF2/AP_NavEKF2.h>
@ -65,6 +66,7 @@ void AP_DAL::start_frame(AP_DAL::FrameType frametype) @@ -65,6 +66,7 @@ void AP_DAL::start_frame(AP_DAL::FrameType frametype)
_RFRN.available_memory = hal.util->available_memory();
_RFRN.ahrs_trim = ahrs.get_trim();
_RFRN.opticalflow_enabled = AP::opticalflow() && AP::opticalflow()->enabled();
_RFRN.wheelencoder_enabled = AP::wheelencoder() && (AP::wheelencoder()->num_sensors() > 0);
WRITE_REPLAY_BLOCK_IFCHANGED(RFRN, _RFRN, old);
// update body conversion

4
libraries/AP_DAL/AP_DAL.h

@ -192,6 +192,10 @@ public: @@ -192,6 +192,10 @@ public:
return _RFRN.opticalflow_enabled;
}
bool wheelencoder_enabled(void) const {
return _RFRN.wheelencoder_enabled;
}
// log optical flow data
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset);

1
libraries/AP_DAL/LogStructure.h

@ -66,6 +66,7 @@ struct log_RFRN { @@ -66,6 +66,7 @@ struct log_RFRN {
uint8_t fly_forward:1;
uint8_t ahrs_airspeed_sensor_enabled:1;
uint8_t opticalflow_enabled:1;
uint8_t wheelencoder_enabled:1;
uint8_t _end;
};

Loading…
Cancel
Save