fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 01:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

optical flow sensor pipeline overhaul

Browse Source 
- all sources of optical flow publish sensor_optical_flow
 - sensor_optical_flow is aggregated by the sensors module, aligned with integrated gyro, and published as vehicle_optical_flow

Co-authored-by: alexklimaj <alex@arkelectron.com>
This commit is contained in:
Daniel Agar
2022-06-20 20:56:56 -04:00
co-authored by alexklimaj
parent 32544452f0
commit d5839e2dd5
49 changed files with 1415 additions and 634 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/optical_flow/thoneflow/thoneflow.cpp
+40 -87
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -50,10 +50,10 @@
#include <drivers/device/device.h>
#include <drivers/drv_hrt.h>
#include <lib/parameters/param.h>
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/uORB.h>
#include <lib/perf/perf_counter.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_optical_flow.h>
#include "thoneflow_parser.h"
@@ -71,21 +71,20 @@ public:
void print_info();
private:
char _port[20];
Rotation _rotation;
int _cycle_interval;
int _fd;
char _linebuf[5];
unsigned _linebuf_index;
THONEFLOW_PARSE_STATE _parse_state;
char _port[20] {};
Rotation _rotation{ROTATION_NONE};
int _cycle_interval{10526};
int _fd{-1};
char _linebuf[5] {};
unsigned _linebuf_index{0};
THONEFLOW_PARSE_STATE _parse_state{THONEFLOW_PARSE_STATE0_UNSYNC};
hrt_abstime _last_read;
hrt_abstime _last_read{0};
optical_flow_s _report;
orb_advert_t _optical_flow_pub;
uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")};
/**
* Initialise the automatic measurement state machine and start it.
@@ -106,23 +105,8 @@ private:
};
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int thoneflow_main(int argc, char *argv[]);
Thoneflow::Thoneflow(const char *port) :
ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
_rotation(Rotation(0)),
_cycle_interval(10526),
_fd(-1),
_linebuf_index(0),
_parse_state(THONEFLOW_PARSE_STATE0_UNSYNC),
_last_read(0),
_report(),
_optical_flow_pub(nullptr),
_sample_perf(perf_alloc(PC_ELAPSED, "thoneflow_read")),
_comms_errors(perf_alloc(PC_COUNT, "thoneflow_com_err"))
ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port))
{
/* store port name */
strncpy(_port, port, sizeof(_port) - 1);
@@ -206,41 +190,6 @@ Thoneflow::init()
ret = PX4_ERROR;
break;
}
/* get yaw rotation from sensor frame to body frame */
param_t rot = param_find("SENS_FLOW_ROT");
if (rot != PARAM_INVALID) {
int32_t val = 0;
param_get(rot, &val);
_rotation = Rotation(val);
}
/* Initialise report structure */
/* No gyro on this board */
_report.gyro_x_rate_integral = NAN;
_report.gyro_y_rate_integral = NAN;
_report.gyro_z_rate_integral = NAN;
/* Conservative specs according to datasheet */
_report.max_flow_rate = 5.0f; // Datasheet: 7.4 rad/s
_report.min_ground_distance = 0.1f; // Datasheet: 80mm
_report.max_ground_distance = 30.0f; // Datasheet: infinity
/* Integrated flow is sent at 66fps */
_report.frame_count_since_last_readout = 1;
_report.integration_timespan = 10526; // microseconds
/* Get a publish handle on the optical flow topic */
_optical_flow_pub = orb_advertise(ORB_ID(optical_flow), &_report);
if (_optical_flow_pub == nullptr) {
PX4_ERR("Failed to create optical_flow object");
ret = PX4_ERROR;
break;
}
} while (0);
/* Close the fd */
@@ -299,20 +248,33 @@ Thoneflow::collect()
_last_read = hrt_absolute_time();
// publish sensor_optical_flow
sensor_optical_flow_s report{};
report.timestamp_sample = hrt_absolute_time();
/* Parse each byte of read buffer */
for (int i = 0; i < ret; i++) {
valid |= thoneflow_parse(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &_report);
valid |= thoneflow_parse(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &report);
}
/* Publish most recent valid measurement */
if (valid) {
_report.timestamp = hrt_absolute_time();
report.device_id = 0; // TODO get_device_id();
report.integration_timespan_us = 10526; // microseconds
/* Rotate measurements from sensor frame to body frame */
float zeroval = 0.0f;
rotate_3f(_rotation, _report.pixel_flow_x_integral, _report.pixel_flow_y_integral, zeroval);
rotate_3f(_rotation, report.pixel_flow[0], report.pixel_flow[1], zeroval);
orb_publish(ORB_ID(optical_flow), _optical_flow_pub, &_report);
// Conservative specs according to datasheet
report.max_flow_rate = 7.4f; // Datasheet: 7.4 rad/s
report.min_ground_distance = 0.08f; // Datasheet: 80mm
report.max_ground_distance = INFINITY; // Datasheet: infinity
report.timestamp = hrt_absolute_time();
_sensor_optical_flow_pub.publish(report);
}
/* Bytes left to parse */
@@ -478,16 +440,15 @@ $ thoneflow stop
PRINT_MODULE_USAGE_NAME("thoneflow", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("optical_flow");
PRINT_MODULE_USAGE_COMMAND_DESCR("start","Start driver");
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver");
PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false);
PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver");
PRINT_MODULE_USAGE_COMMAND_DESCR("info","Print driver information");
PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver");
PRINT_MODULE_USAGE_COMMAND_DESCR("info", "Print driver information");
}
} // namespace
int
thoneflow_main(int argc, char *argv[])
extern "C" __EXPORT int thoneflow_main(int argc, char *argv[])
{
int ch;
const char *device_path = "";
@@ -522,19 +483,11 @@ thoneflow_main(int argc, char *argv[])
thoneflow::usage();
return -1;
}
}
/*
* Stop the driver
*/
if (!strcmp(argv[myoptind], "stop")) {
} else if (!strcmp(argv[myoptind], "stop")) {
return thoneflow::stop();
}
/*
* Print driver information.
*/
if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
} else if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
thoneflow::info();
return 0;
}
src/drivers/optical_flow/thoneflow/thoneflow_parser.cpp
+3 -3
View File
@@ -61,7 +61,7 @@ const char *parser_state[] = {
#endif
bool thoneflow_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum THONEFLOW_PARSE_STATE *state,
optical_flow_s *flow)
sensor_optical_flow_s *flow)
{
bool parsed_packet = false;
@@ -133,8 +133,8 @@ bool thoneflow_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum TH
// Checksum valid, populate sensor report
int16_t delta_x = uint16_t(parserbuf[1]) << 8 | parserbuf[0];
int16_t delta_y = uint16_t(parserbuf[3]) << 8 | parserbuf[2];
flow->pixel_flow_x_integral = static_cast<float>(delta_x) * (3.52e-3f);
flow->pixel_flow_y_integral = static_cast<float>(delta_y) * (3.52e-3f);
flow->pixel_flow[0] = static_cast<float>(delta_x) * (3.52e-3f);
flow->pixel_flow[1] = static_cast<float>(delta_y) * (3.52e-3f);
*state = THONEFLOW_PARSE_STATE7_CHECKSUM;
} else {
src/drivers/optical_flow/thoneflow/thoneflow_parser.h
+2 -2
View File
@@ -33,7 +33,7 @@
#pragma once
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/sensor_optical_flow.h>
// Data Format for ThoneFlow 3901U
// ===============================
@@ -62,4 +62,4 @@ enum THONEFLOW_PARSE_STATE {
};
bool thoneflow_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum THONEFLOW_PARSE_STATE *state,
optical_flow_s *report);
sensor_optical_flow_s *report);