Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: OpticalFlow takes care of its own logging

This commit is contained in:
Peter Barker 2018-09-02 22:25:52 +10:00 committed by Andrew Tridgell
parent e53a009a35
commit 33ca577653
5 changed files with 2 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/ArduCopter.cpp
View File

@ -87,7 +87,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
SCHED_TASK(throttle_loop, 50, 75),
SCHED_TASK(update_GPS, 50, 200),
#if OPTFLOW == ENABLED
SCHED_TASK(update_optical_flow, 200, 160),
SCHED_TASK_CLASS(OpticalFlow, &copter.optflow, update, 200, 160),
#endif
SCHED_TASK(update_batt_compass, 10, 120),
SCHED_TASK(read_aux_all, 10, 50),

1
ArduCopter/Copter.h
View File

@ -757,7 +757,6 @@ private:
void landinggear_update();
// Log.cpp
void Log_Write_Optflow();
void Log_Write_Control_Tuning();
void Log_Write_Performance();
void Log_Write_Attitude();

41
ArduCopter/Log.cpp
View File

@ -59,39 +59,6 @@ void Copter::ModeAutoTune::Log_Write_AutoTuneDetails(float angle_cd, float rate_
}
#endif
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 Copter::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;
@ -507,10 +474,6 @@ const struct LogStructure Copter::log_structure[] = {
#endif
{ LOG_PARAMTUNE_MSG, sizeof(log_ParameterTuning),
"PTUN", "QBfHHH", "TimeUS,Param,TunVal,CtrlIn,TunLo,TunHi", "s-----", "F-----" },
#if OPTFLOW == ENABLED
{ LOG_OPTFLOW_MSG, sizeof(log_Optflow),
"OF", "QBffff", "TimeUS,Qual,flowX,flowY,bodyX,bodyY", "s-EEEE", "F-0000" },
#endif
{ LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),
"CTUN", "Qffffffefcfhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00B0BBBB" },
{ LOG_MOTBATT_MSG, sizeof(log_MotBatt),
@ -583,10 +546,6 @@ void Copter::Log_Write_Vehicle_Startup_Messages() {}
void Copter::Log_Write_Heli() {}
#endif
#if OPTFLOW == ENABLED
void Copter::Log_Write_Optflow() {}
#endif
void Copter::log_init(void) {}
#endif // LOGGING_ENABLED

1
ArduCopter/defines.h
View File

@ -240,7 +240,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
ArduCopter/sensors.cpp
View File

@ -121,39 +121,10 @@ void Copter::init_optflow()
{
#if OPTFLOW == ENABLED
// initialise optical flow sensor
optflow.init();
optflow.init(MASK_LOG_OPTFLOW);
#endif // OPTFLOW == ENABLED
}
// called at 200hz
#if OPTFLOW == ENABLED
void Copter::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 Copter::compass_cal_update()
{
static uint32_t compass_cal_stick_gesture_begin = 0;