Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-27 17:06:31 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

[computer vision] Driver addition for mateksys_PMW3901_l0x (#2655)

Browse Source
This commit is contained in:
Pietro Campolucci
2021-01-30 22:54:08 +01:00
committed by GitHub
parent c6c33572db
commit 9230b506be
9 changed files with 540 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/modules/optical_flow/mateksys_3901_l0x.c
+345
View File
@@ -0,0 +1,345 @@
/*
* Copyright (C) 2020 Paparazzi Team
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
/** @file modules/optical_flow/mateksys_3901_l0x.h
* @brief Driver for the mateksys_3901_l0x sensor via MSP protocol output
*
*/
#include "mateksys_3901_l0x.h"
#include "mcu_periph/uart.h"
#include "subsystems/abi.h"
// State interface for rotation compensation
#include "state.h"
// Messages
#include "pprzlink/messages.h"
#include "subsystems/datalink/downlink.h"
// Define configuration parameters
#ifndef MATEKSYS_3901_L0X_MOTION_THRES
#define MATEKSYS_3901_L0X_MOTION_THRES
#endif
#ifndef MATEKSYS_3901_L0X_DISTANCE_THRES
#define MATEKSYS_3901_L0X_DISTANCE_THRES
#endif
#ifndef USE_MATEKSYS_3901_L0X_AGL
#define USE_MATEKSYS_3901_L0X_AGL
#endif
#ifndef USE_MATEKSYS_3901_L0X_OPTICAL_FLOW
#define USE_MATEKSYS_3901_L0X_OPTICAL_FLOW
#endif
#ifndef MATEKSYS_3901_L0X_COMPENSATE_ROTATION
#define MATEKSYS_3901_L0X_COMPENSATE_ROTATION
#endif
struct Mateksys3901l0X mateksys3901l0x = {
.parse_status = MATEKSYS_3901_L0X_INITIALIZE
};
static void mateksys3901l0x_parse(uint8_t byte);
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
/**
* Downlink message flow and lidar (included velocity estimation, not yet tested)
*/
static void mateksys3901l0x_send_optical_flow(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_OPTICAL_FLOW(trans, dev, AC_ID,
&mateksys3901l0x.time_sec,
&mateksys3901l0x.sensor_id,
&mateksys3901l0x.motionX_clean,
&mateksys3901l0x.motionY_clean,
&mateksys3901l0x.velocityX,
&mateksys3901l0x.velocityY,
&mateksys3901l0x.motion_quality,
&mateksys3901l0x.distance_clean,
&mateksys3901l0x.distancemm_quality);
}
#endif
/**
* Initialization function
*/
void mateksys3901l0x_init(void)
{
mateksys3901l0x.device = &((MATEKSYS_3901_L0X_PORT).device);
mateksys3901l0x.parse_crc = 0;
mateksys3901l0x.motion_quality = 0;
mateksys3901l0x.motionX = 0;
mateksys3901l0x.motionY = 0;
mateksys3901l0x.distancemm_quality = 0;
mateksys3901l0x.distancemm = 0;
mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_OPTICAL_FLOW, mateksys3901l0x_send_optical_flow);
#endif
}
/**
* Receive bytes from the UART port and parse them
*/
void mateksys3901l0x_event(void)
{
while (mateksys3901l0x.parse_status != MATEKSYS_3901_L0X_INITIALIZE && mateksys3901l0x.device->char_available(mateksys3901l0x.device->periph)) {
mateksys3901l0x_parse(mateksys3901l0x.device->get_byte(mateksys3901l0x.device->periph));
}
}
/**
* Parse the sensor MSP output bytes 1 by 1
*/
static void mateksys3901l0x_parse(uint8_t byte)
{
switch (mateksys3901l0x.parse_status) {
case MATEKSYS_3901_L0X_INITIALIZE:
break;
case MATEKSYS_3901_L0X_PARSE_HEAD: // MSP general header $
if (byte == 0x24) {
mateksys3901l0x.parse_status++;
}
break;
case MATEKSYS_3901_L0X_PARSE_HEAD2: // MSPv2 identifier X
if (byte == 0x58) {
mateksys3901l0x.parse_status++;
} else {
mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
}
break;
case MATEKSYS_3901_L0X_PARSE_DIRECTION: // direction <
if (byte == 0x3C) {
mateksys3901l0x.parse_status++;
} else {
mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
}
break;
case MATEKSYS_3901_L0X_PARSE_LENGTH: // set to 0
if (byte == 0x00) {
mateksys3901l0x.parse_crc += byte;
mateksys3901l0x.parse_status++;
} else {
mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
}
break;
case MATEKSYS_3901_L0X_PARSE_FUNCTION_ID_B1: // 0x01 = rangefinder; 0x02 = opticalflow
if (byte == 0x01 || byte == 0x02) {
mateksys3901l0x.sensor_id = byte;
mateksys3901l0x.parse_crc += byte;
mateksys3901l0x.parse_status++;
} else {
mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
}
break;
case MATEKSYS_3901_L0X_PARSE_FUNCTION_ID_B2: // sensor id pointer
if (byte == 0x1F) {
mateksys3901l0x.parse_status++;
mateksys3901l0x.parse_crc += byte;
} else {
mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
}
break;
case MATEKSYS_3901_L0X_PARSE_SIZE: // two fixed sizes are expected if message is
if (byte == 0x05 || byte == 0x09) {
mateksys3901l0x.parse_status++;
mateksys3901l0x.parse_crc += byte;
} else {
mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
}
break;
case MATEKSYS_3901_L0X_PARSE_POINTER: // should be zero, used to redirect to motion parsing or lidar parsing
if (mateksys3901l0x.sensor_id == 0x01) {
mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_DISTANCEQUALITY;
} else if (mateksys3901l0x.sensor_id == 0x02) {
mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_MOTIONQUALITY;
} else {
mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
}
break;
// rangefinder data parsing
case MATEKSYS_3901_L0X_PARSE_DISTANCEQUALITY:
mateksys3901l0x.distancemm_quality = byte;
mateksys3901l0x.parse_crc += byte;
mateksys3901l0x.parse_status++;
break;
case MATEKSYS_3901_L0X_PARSE_DISTANCE_B1:
if (mateksys3901l0x.distancemm_quality <= MATEKSYS_3901_L0X_DISTANCE_THRES) {
mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
} else {
mateksys3901l0x.distancemm = byte;
mateksys3901l0x.parse_crc += byte;
mateksys3901l0x.parse_status++;
}
break;
case MATEKSYS_3901_L0X_PARSE_DISTANCE_B2:
mateksys3901l0x.distancemm |= (byte << 8);
mateksys3901l0x.parse_crc += byte;
mateksys3901l0x.parse_status++;
break;
case MATEKSYS_3901_L0X_PARSE_DISTANCE_B3:
mateksys3901l0x.distancemm |= (byte << 16);
mateksys3901l0x.parse_crc += byte;
mateksys3901l0x.parse_status++;
break;
case MATEKSYS_3901_L0X_PARSE_DISTANCE_B4:
mateksys3901l0x.distancemm |= (byte << 24);
mateksys3901l0x.parse_crc += byte;
mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_CHECKSUM;
break;
// optical flow data parsing
case MATEKSYS_3901_L0X_PARSE_MOTIONQUALITY:
mateksys3901l0x.motion_quality = byte;
mateksys3901l0x.parse_crc += byte;
mateksys3901l0x.parse_status++;
break;
case MATEKSYS_3901_L0X_PARSE_MOTIONY_B1:
if (mateksys3901l0x.motion_quality <= MATEKSYS_3901_L0X_MOTION_THRES) {
mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
} else {
mateksys3901l0x.motionY = byte;
mateksys3901l0x.parse_crc += byte;
mateksys3901l0x.parse_status++;
}
break;
case MATEKSYS_3901_L0X_PARSE_MOTIONY_B2:
mateksys3901l0x.motionY |= (byte << 8);
mateksys3901l0x.parse_crc += byte;
mateksys3901l0x.parse_status++;
break;
case MATEKSYS_3901_L0X_PARSE_MOTIONY_B3:
mateksys3901l0x.motionY |= (byte << 16);
mateksys3901l0x.parse_crc += byte;
mateksys3901l0x.parse_status++;
break;
case MATEKSYS_3901_L0X_PARSE_MOTIONY_B4:
mateksys3901l0x.motionY |= (byte << 24);
mateksys3901l0x.parse_crc += byte;
mateksys3901l0x.parse_status++;
break;
case MATEKSYS_3901_L0X_PARSE_MOTIONX_B1:
mateksys3901l0x.motionX = byte;
mateksys3901l0x.parse_crc += byte;
mateksys3901l0x.parse_status++;
break;
case MATEKSYS_3901_L0X_PARSE_MOTIONX_B2:
mateksys3901l0x.motionX |= (byte << 8);
mateksys3901l0x.parse_crc += byte;
mateksys3901l0x.parse_status++;
break;
case MATEKSYS_3901_L0X_PARSE_MOTIONX_B3:
mateksys3901l0x.motionX |= (byte << 16);
mateksys3901l0x.parse_crc += byte;
mateksys3901l0x.parse_status++;
break;
case MATEKSYS_3901_L0X_PARSE_MOTIONX_B4:
mateksys3901l0x.motionX |= (byte << 24);
mateksys3901l0x.parse_crc += byte;
mateksys3901l0x.parse_status++;
break;
case MATEKSYS_3901_L0X_PARSE_CHECKSUM:
// When the distance and motion info are valid (max values based on sensor specifications)...
if (mateksys3901l0x.distancemm > 0 && mateksys3901l0x.distancemm <= 3000 && abs(mateksys3901l0x.motionX) <= 300 && abs(mateksys3901l0x.motionY) <= 300) {
// ... compensate AGL measurement for body rotation
if (MATEKSYS_3901_L0X_COMPENSATE_ROTATION) {
float phi = stateGetNedToBodyEulers_f()->phi;
float theta = stateGetNedToBodyEulers_f()->theta;
float gain = (float)fabs((double)(cosf(phi) * cosf(theta)));
mateksys3901l0x.distancemm = mateksys3901l0x.distancemm * gain;
}
// send messages with no error measurements
mateksys3901l0x.distance_clean = mateksys3901l0x.distancemm/1000.f;
mateksys3901l0x.motionX_clean = mateksys3901l0x.motionX;
mateksys3901l0x.motionY_clean = mateksys3901l0x.motionY;
// estimate velocity and send it to telemetry
mateksys3901l0x.velocityX = mateksys3901l0x.distance_clean * sin(RadOfDeg(mateksys3901l0x.motionX_clean));
mateksys3901l0x.velocityY = mateksys3901l0x.distance_clean * sin(RadOfDeg(mateksys3901l0x.motionY_clean));
// get ticks
uint32_t now_ts = get_sys_time_usec();
mateksys3901l0x.time_sec = now_ts*1e-6;
// send AGL (if requested)
if (USE_MATEKSYS_3901_L0X_AGL) {
AbiSendMsgAGL(AGL_LIDAR_MATEKSYS_3901_L0X_ID,
now_ts,
mateksys3901l0x.distance_clean);
}
// send optical flow (if requested)
if (USE_MATEKSYS_3901_L0X_OPTICAL_FLOW) {
AbiSendMsgOPTICAL_FLOW(FLOW_OPTICFLOW_MATEKSYS_3901_L0X_ID,
now_ts,
mateksys3901l0x.motionX_clean,
mateksys3901l0x.motionY_clean,
0,
0,
mateksys3901l0x.motion_quality,
0.0);
}
}
// Start reading again
mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
break;
default:
// Error, return to start
mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
break;
}
}
sw/airborne/modules/optical_flow/mateksys_3901_l0x.h
+99
View File
@@ -0,0 +1,99 @@
/*
* Copyright (C) 2020 Paparazzi Team
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
/** @file modules/optical_flow/mateksys_3901_l0x.h
* @brief Driver for the mateksys_3901_l0x sensor via MSPx protocol output
*
*/
/*
Since the is no official MSP2 nor MSP1 message definition we choose for time being that
the IDs to be compatible with INAV as see here
https://github.com/iNavFlight/inav/blob/master/src/main/msp/msp_protocol_v2_sensor.h
https://github.com/iNavFlight/inav/wiki/MSP-V2
*/
#ifndef MATEKSYS_3901_L0X_H
#define MATEKSYS_3901_L0X_H
#include "std.h"
#include "stdbool.h"
//#include "filters/median_filter.h" //Who knows we need it ;)
#define MSP2_IS_SENSOR_MESSAGE(x) ((x) >= 0x1F00 && (x) <= 0x1FFF)
#define MSP2_SENSOR_RANGEFINDER 0x1F01
#define MSP2_SENSOR_OPTIC_FLOW 0x1F02
enum Mateksys3901l0XParseStatus {
MATEKSYS_3901_L0X_INITIALIZE, // initialization
MATEKSYS_3901_L0X_PARSE_HEAD,
MATEKSYS_3901_L0X_PARSE_HEAD2,
MATEKSYS_3901_L0X_PARSE_DIRECTION,
MATEKSYS_3901_L0X_PARSE_LENGTH,
MATEKSYS_3901_L0X_PARSE_FUNCTION_ID_B1,
MATEKSYS_3901_L0X_PARSE_FUNCTION_ID_B2,
MATEKSYS_3901_L0X_PARSE_SIZE,
MATEKSYS_3901_L0X_PARSE_POINTER, // ??
MATEKSYS_3901_L0X_PARSE_DISTANCEQUALITY, // used if lidar message
MATEKSYS_3901_L0X_PARSE_DISTANCE_B1,
MATEKSYS_3901_L0X_PARSE_DISTANCE_B2,
MATEKSYS_3901_L0X_PARSE_DISTANCE_B3,
MATEKSYS_3901_L0X_PARSE_DISTANCE_B4,
MATEKSYS_3901_L0X_PARSE_MOTIONQUALITY, // used if flow message
MATEKSYS_3901_L0X_PARSE_MOTIONY_B1,
MATEKSYS_3901_L0X_PARSE_MOTIONY_B2,
MATEKSYS_3901_L0X_PARSE_MOTIONY_B3,
MATEKSYS_3901_L0X_PARSE_MOTIONY_B4,
MATEKSYS_3901_L0X_PARSE_MOTIONX_B1,
MATEKSYS_3901_L0X_PARSE_MOTIONX_B2,
MATEKSYS_3901_L0X_PARSE_MOTIONX_B3,
MATEKSYS_3901_L0X_PARSE_MOTIONX_B4,
MATEKSYS_3901_L0X_PARSE_CHECKSUM,
};
struct Mateksys3901l0X {
struct link_device *device;
enum Mateksys3901l0XParseStatus parse_status;
float time_sec;
uint8_t sensor_id;
uint8_t motion_quality;
int32_t motionX;
int32_t motionY;
int32_t motionX_clean;
int32_t motionY_clean;
uint8_t distancemm_quality;
int32_t distancemm;
float distance_clean;
float velocityX;
float velocityY;
uint8_t parse_crc;
};
extern struct Mateksys3901l0X mateksys3901l0x;
extern void mateksys3901l0x_init(void);
extern void mateksys3901l0x_event(void);
extern void mateksys3901l0x_downlink(void);
#endif /* MATEKSYS_3901_L0X_H */
sw/airborne/modules/optical_flow/px4flow.c
+11 -9
View File
@@ -162,15 +162,17 @@ void px4flow_init(void)
void px4flow_downlink(void)
{
static float timestamp = 0;
static float distance_quality = 0;
timestamp = ((float)optical_flow.time_usec) * 0.000001;
DOWNLINK_SEND_PX4FLOW(DefaultChannel, DefaultDevice,
&timestamp,
&optical_flow.sensor_id,
&optical_flow.flow_x,
&optical_flow.flow_y,
&optical_flow.flow_comp_m_x,
&optical_flow.flow_comp_m_y,
&optical_flow.quality,
&optical_flow.ground_distance);
DOWNLINK_SEND_OPTICAL_FLOW(DefaultChannel, DefaultDevice,
&timestamp,
&optical_flow.sensor_id,
&optical_flow.flow_x,
&optical_flow.flow_y,
&optical_flow.flow_comp_m_x,
&optical_flow.flow_comp_m_y,
&optical_flow.quality,
&optical_flow.ground_distance,
&distance_quality);
}
sw/airborne/modules/optical_flow/px4flow.h
+2 -2
View File
@@ -47,8 +47,8 @@ struct mavlink_optical_flow {
float flow_comp_m_x; ///< Flow in meters in x-sensor direction, angular-speed compensated [meters/sec]
float flow_comp_m_y; ///< Flow in meters in y-sensor direction, angular-speed compensated [meters/sec]
float ground_distance; ///< Ground distance in meters. Positive value: distance known. Negative value: Unknown distance
int16_t flow_x; ///< Flow in pixels in x-sensor direction
int16_t flow_y; ///< Flow in pixels in y-sensor direction
int32_t flow_x; ///< Flow in pixels in x-sensor direction
int32_t flow_y; ///< Flow in pixels in y-sensor direction
uint8_t sensor_id; ///< Sensor ID
uint8_t quality; ///< Optical flow quality / confidence. 0: bad, 255: maximum quality
};
sw/airborne/modules/optical_flow/px4flow_i2c.c
+15 -13
View File
@@ -267,10 +267,11 @@ void px4flow_i2c_downlink(void)
uint8_t id = PX4FLOW_I2C_ID;
float timestamp = get_sys_time_float();
static float distance_quality = 0;
#if REQUEST_INT_FRAME
int16_t flow_x = px4flow.i2c_int_frame.pixel_flow_x_integral;
int16_t flow_y = px4flow.i2c_int_frame.pixel_flow_y_integral;
int32_t flow_x = px4flow.i2c_int_frame.pixel_flow_x_integral;
int32_t flow_y = px4flow.i2c_int_frame.pixel_flow_y_integral;
float flow_comp_m_x = 0.0;
float flow_comp_m_y = 0.0;
@@ -278,8 +279,8 @@ void px4flow_i2c_downlink(void)
uint8_t quality = px4flow.i2c_int_frame.qual;
float ground_distance = ((float)px4flow.i2c_int_frame.ground_distance) / 1000.0;
#else
int16_t flow_x = px4flow.i2c_frame.pixel_flow_x_sum;
int16_t flow_y = px4flow.i2c_frame.pixel_flow_y_sum;
int32_t flow_x = px4flow.i2c_frame.pixel_flow_x_sum;
int32_t flow_y = px4flow.i2c_frame.pixel_flow_y_sum;
float flow_comp_m_x = ((float)px4flow.i2c_frame.flow_comp_m_x) / 1000.0;
float flow_comp_m_y = ((float)px4flow.i2c_frame.flow_comp_m_y) / 1000.0;
@@ -288,13 +289,14 @@ void px4flow_i2c_downlink(void)
float ground_distance = ((float)px4flow.i2c_frame.ground_distance) / 1000.0;
#endif
DOWNLINK_SEND_PX4FLOW(DefaultChannel, DefaultDevice,
&timestamp,
&id,
&flow_x,
&flow_y,
&flow_comp_m_x,
&flow_comp_m_y,
&quality,
&ground_distance);
DOWNLINK_SEND_OPTICAL_FLOW(DefaultChannel, DefaultDevice,
&timestamp,
&id,
&flow_x,
&flow_y,
&flow_comp_m_x,
&flow_comp_m_y,
&quality,
&ground_distance,
&distance_quality);
}
sw/airborne/subsystems/abi_sender_ids.h
+8
View File
@@ -181,6 +181,10 @@
#define AGL_LIDAR_TFMINI_I2C_ID 14
#endif
#ifndef AGL_LIDAR_MATEKSYS_3901_L0X_ID
#define AGL_LIDAR_MATEKSYS_3901_L0X_ID 15
#endif
/*
* IDs of magnetometer sensors (including IMUs with mag)
*/
@@ -367,6 +371,10 @@
#define FLOW_OPTICFLOW_PMW3901_ID 3
#endif
#ifndef FLOW_OPTICFLOW_MATEKSYS_3901_L0X_ID
#define FLOW_OPTICFLOW_MATEKSYS_3901_L0X_ID 4
#endif
/*
* IDs of VELOCITY estimates (message 12)
*/
sw/ext/pprzlink
+1 -1
Submodule sw/ext/pprzlink updated: 46c4c210ac...da37238464