Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 20:48:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: OpticalFlow takes care of its own logging

Browse Source
This commit is contained in:
Peter Barker 2018-09-02 22:33:53 +10:00 committed by Andrew Tridgell
parent 2bca18b712
commit 376a422cff
5 changed files with 2 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduSub/ArduSub.cpp
View File

@ -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),

39
ArduSub/Log.cpp
View File

@ -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

2
ArduSub/Sub.h
View File

@ -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();

1
ArduSub/defines.h
View File

@ -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,

31
ArduSub/sensors.cpp
View File

@ -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()) {