mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-10 06:59:54 +08:00
[driver] add pmw3901mb driver
correspond to the flow2 deck from bitcraze update chibios SPI driver tested on crazyflie and crazybee merge and close #2529
This commit is contained in:
Gautier Hattenberger
@@ -48,8 +48,14 @@
|
||||
<module name="range_forcefield">
|
||||
<define name="RANGE_FORCEFIELD_MAX_VEL" value="0.1"/>
|
||||
</module>
|
||||
<module name="sonar" type="vl53l1x"/>
|
||||
<define name="SENSOR_SYNC_SEND_SONAR"/>
|
||||
<module name="sonar" type="vl53l1x">
|
||||
<define name="SENSOR_SYNC_SEND_SONAR"/>
|
||||
</module>
|
||||
<module name="opticflow" type="pmw3901">
|
||||
<configure name="OPTICFLOW_PMW3901_SPI_DEV" value="spi1"/>
|
||||
<define name="SENSOR_SYNC_SEND_OPTICFLOW_PMW3901"/>
|
||||
<define name="PMW3901_SPI_CDIV" value="SPIDiv32"/>
|
||||
</module>
|
||||
<define name="NO_GPS_NEEDED_FOR_NAV" value="TRUE"/>
|
||||
|
||||
</firmware>
|
||||
|
||||
@@ -0,0 +1,49 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="opticflow_pmw3901" dir="sensors">
|
||||
<doc>
|
||||
<description>Driver for PMW3901 optical flow sensor</description>
|
||||
<configure name="OPTICFLOW_PMW3901_SPI_DEV" value="SPIx" description="SPI device"/>
|
||||
<configure name="OPTICFLOW_PMW3901_SPI_SLAVE_IDX" value="2" description="SPI slave index"/>
|
||||
<define name="OPTICFLOW_PMW3901_SENSOR_ANGLE" value="90" description="Sensor mounting angle in degrees"/>
|
||||
<define name="OPTICFLOW_PMW3901_SUBPIXEL_FACTOR" value="100" description="Subpixel factor"/>
|
||||
<define name="OPTICFLOW_PMW3901_STD_PX" value="50" description="Standard deviation of flow measurement in px"/>
|
||||
<define name="OPTICFLOW_PMW3901_AGL_ID" value="ABI_BROADCAST" description="AGL source to listen to for velocity estimation"/>
|
||||
<define name="OPTICFLOW_PMW3901_AGL_TIMEOUT_US" value="500000" description="Lifetime of AGL measurements"/>
|
||||
<define name="PMW3901_RAD_PER_PX" value="0.002443389" description="Rad-per-px for ALL PMW3901 sensors"/>
|
||||
<define name="PMW3901_SPI_CDIV" value="SPIDiv256" description="SPI Clock divisor to adjust according to PCLK, don't exceeed 2MHz"/>
|
||||
<define name="OPTICFLOW_PMW3901_RAD_PER_PX" value="(undefined)" description="Override rad-per-px only for the PMW3901 used by this module"/>
|
||||
<define name="SENSOR_SYNC_SEND_OPTICFLOW_PMW3901" value="FALSE" description="Send all new measurements over telemetry"/>
|
||||
</doc>
|
||||
<settings>
|
||||
<dl_settings>
|
||||
<dl_settings name="OF_PMW3901">
|
||||
<dl_setting shortname="sensor_angle" var="of_pmw.sensor_angle" min="0" step="1" max="360"/>
|
||||
<dl_setting shortname="subpixel_factor" var="of_pmw.subpixel_factor" min="0" step="1" max="500"/>
|
||||
<dl_setting shortname="std_px" var="of_pmw.std_px" min="0.0" max="100.0" step="0.1"/>
|
||||
<dl_setting shortname="agl_timeout" var="of_pmw.agl_timeout" min="0" step="1000" max="1000000"/>
|
||||
<dl_setting shortname="rad_per_px" var="of_pmw.pmw.rad_per_px" min="0.000" max="0.010" step="0.001"/>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
<header>
|
||||
<file name="opticflow_pmw3901.h"/>
|
||||
</header>
|
||||
<init fun="opticflow_pmw3901_init()"/>
|
||||
<periodic fun="opticflow_pmw3901_periodic()" freq="100"/>
|
||||
<event fun="opticflow_pmw3901_event()"/>
|
||||
<makefile>
|
||||
<configure name="OPTICFLOW_PMW3901_SPI_DEV" default="SPI2" case="upper|lower"/>
|
||||
<configure name="OPTICFLOW_PMW3901_SPI_SLAVE_IDX" default="1"/>
|
||||
<define name="USE_$(OPTICFLOW_PMW3901_SPI_DEV_UPPER)"/>
|
||||
<define name="USE_SPI_SLAVE$(OPTICFLOW_PMW3901_SPI_SLAVE_IDX)"/>
|
||||
<define name="OPTICFLOW_PMW3901_SPI_DEV" value="$(OPTICFLOW_PMW3901_SPI_DEV_LOWER)"/>
|
||||
<define name="OPTICFLOW_PMW3901_SPI_SLAVE_IDX" value="$(OPTICFLOW_PMW3901_SPI_SLAVE_IDX)"/>
|
||||
<file name="opticflow_pmw3901.c"/>
|
||||
<file name="pmw3901.c" dir="peripherals"/>
|
||||
<raw>
|
||||
include $(CFG_SHARED)/spi_master.makefile
|
||||
</raw>
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
@@ -281,7 +281,10 @@ static void handle_spi_thd(struct spi_periph *p)
|
||||
|
||||
// Configure SPI bus with the current slave select pin
|
||||
spiStart((SPIDriver *)p->reg_addr, &spi_cfg);
|
||||
spiSelect((SPIDriver *)p->reg_addr);
|
||||
// Select the slave after reconfiguration of the peripheral
|
||||
if (t->select == SPISelectUnselect || t->select == SPISelect) {
|
||||
spiSelect((SPIDriver *)p->reg_addr);
|
||||
}
|
||||
|
||||
// Run the callback after selecting the slave
|
||||
// FIXME warning: done in spi thread
|
||||
@@ -300,7 +303,9 @@ static void handle_spi_thd(struct spi_periph *p)
|
||||
#endif
|
||||
|
||||
// Unselect the slave
|
||||
spiUnselect((SPIDriver *)p->reg_addr);
|
||||
if (t->select == SPISelectUnselect || t->select == SPIUnselect) {
|
||||
spiUnselect((SPIDriver *)p->reg_addr);
|
||||
}
|
||||
|
||||
chSysLock();
|
||||
// end of transaction, handle fifo
|
||||
@@ -322,6 +327,7 @@ static void handle_spi_thd(struct spi_periph *p)
|
||||
if (t->after_cb != 0) {
|
||||
t->after_cb(t);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -34,6 +34,12 @@
|
||||
#include "std.h"
|
||||
|
||||
#include "mcu_periph/spi_arch.h"
|
||||
#include "mcu_periph/sys_time.h"
|
||||
|
||||
#ifndef SPI_BLOCKING_TIMEOUT
|
||||
#define SPI_BLOCKING_TIMEOUT 1.f
|
||||
#endif
|
||||
|
||||
|
||||
/**
|
||||
* @addtogroup mcu_periph
|
||||
@@ -270,6 +276,25 @@ extern void spi_init_slaves(void);
|
||||
*/
|
||||
extern bool spi_submit(struct spi_periph *p, struct spi_transaction *t);
|
||||
|
||||
/** Perform a spi transaction (blocking).
|
||||
* @param p spi peripheral to be used
|
||||
* @param t spi transaction
|
||||
* @return TRUE if transaction completed (success or failure)
|
||||
*/
|
||||
static inline bool spi_blocking_transceive(struct spi_periph *p, struct spi_transaction *t) {
|
||||
if (!spi_submit(p, t)) {
|
||||
return false;
|
||||
}
|
||||
// Wait for transaction to complete
|
||||
float start_t = get_sys_time_float();
|
||||
while (t->status == SPITransPending || t->status == SPITransRunning) {
|
||||
if (get_sys_time_float() - start_t > SPI_BLOCKING_TIMEOUT) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/** Select a slave.
|
||||
* @param slave slave id
|
||||
*/
|
||||
|
||||
@@ -0,0 +1,193 @@
|
||||
/*
|
||||
* Copyright (C) Tom van Dijk
|
||||
*
|
||||
* 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, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/**
|
||||
* @file "modules/sensors/opticflow_pmw3901.c"
|
||||
* @author Tom van Dijk
|
||||
* Driver for PMW3901 optical flow sensor
|
||||
*/
|
||||
|
||||
#include "modules/sensors/opticflow_pmw3901.h"
|
||||
|
||||
#include "subsystems/abi.h"
|
||||
#include "subsystems/datalink/downlink.h"
|
||||
#include "generated/modules.h"
|
||||
|
||||
#include "state.h"
|
||||
|
||||
#include "std.h"
|
||||
|
||||
#include <math.h>
|
||||
|
||||
|
||||
#ifndef OPTICFLOW_PMW3901_SENSOR_ANGLE
|
||||
#define OPTICFLOW_PMW3901_SENSOR_ANGLE 270 // [deg] Sensor rotation around body z axis (down). 0 rad = sensor x forward, y right.
|
||||
#endif
|
||||
|
||||
#ifndef OPTICFLOW_PMW3901_SUBPIXEL_FACTOR
|
||||
#define OPTICFLOW_PMW3901_SUBPIXEL_FACTOR 100
|
||||
#endif
|
||||
|
||||
#ifndef OPTICFLOW_PMW3901_STD_PX
|
||||
#define OPTICFLOW_PMW3901_STD_PX 50 // [px] standard deviation of flow measurement
|
||||
#endif
|
||||
|
||||
#ifndef OPTICFLOW_PMW3901_AGL_ID
|
||||
#define OPTICFLOW_PMW3901_AGL_ID ABI_BROADCAST
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(OPTICFLOW_PMW3901_AGL_ID)
|
||||
|
||||
#ifndef OPTICFLOW_PMW3901_AGL_TIMEOUT_US
|
||||
#define OPTICFLOW_PMW3901_AGL_TIMEOUT_US 500000
|
||||
#endif
|
||||
|
||||
|
||||
struct opticflow_pmw3901_t of_pmw;
|
||||
|
||||
abi_event agl_ev;
|
||||
static float agl_dist;
|
||||
static uint32_t agl_ts;
|
||||
|
||||
|
||||
static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance) {
|
||||
(void) sender_id;
|
||||
agl_dist = distance;
|
||||
agl_ts = stamp;
|
||||
}
|
||||
|
||||
static bool agl_valid(uint32_t at_ts) {
|
||||
return \
|
||||
((at_ts - agl_ts) < of_pmw.agl_timeout) &&
|
||||
(agl_dist > 0.080);
|
||||
}
|
||||
|
||||
|
||||
static void opticflow_pmw3901_publish(int16_t delta_x, int16_t delta_y, uint32_t ts_usec) {
|
||||
/* Prepare message variables */
|
||||
// Time
|
||||
static uint32_t prev_ts_usec = 0;
|
||||
float dt = (ts_usec - prev_ts_usec) / 1.0e6;
|
||||
if (prev_ts_usec == 0) {
|
||||
dt = OPTICFLOW_PMW3901_PERIODIC_PERIOD;
|
||||
}
|
||||
prev_ts_usec = ts_usec;
|
||||
// Sensor orientation
|
||||
float c = cosf(RadOfDeg(of_pmw.sensor_angle));
|
||||
float s = sinf(RadOfDeg(of_pmw.sensor_angle));
|
||||
// Flow [px/s] (body-frame)
|
||||
float flow_x = (c * delta_x - s * delta_y) / dt;
|
||||
float flow_y = (s * delta_x + c * delta_y) / dt;
|
||||
int16_t flow_x_subpix = (int16_t)(of_pmw.subpixel_factor * flow_x);
|
||||
int16_t flow_y_subpix = (int16_t)(of_pmw.subpixel_factor * flow_y);
|
||||
// Derotated flow [px/s] (body-frame)
|
||||
struct FloatRates *rates = stateGetBodyRates_f();
|
||||
float flow_dy_p = rates->p / of_pmw.pmw.rad_per_px;
|
||||
float flow_dx_q = -rates->q / of_pmw.pmw.rad_per_px;
|
||||
float flow_der_x = flow_x - flow_dx_q;
|
||||
float flow_der_y = flow_y - flow_dy_p;
|
||||
int16_t flow_der_x_subpix = (int16_t)(of_pmw.subpixel_factor * flow_der_x);
|
||||
int16_t flow_der_y_subpix = (int16_t)(of_pmw.subpixel_factor * flow_der_y);
|
||||
// Velocity
|
||||
static float vel_x = 0; // static: keep last measurement for telemetry if agl not valid
|
||||
static float vel_y = 0;
|
||||
static float noise = 0;
|
||||
if (agl_valid(ts_usec)) {
|
||||
vel_x = -flow_der_x * of_pmw.pmw.rad_per_px * agl_dist;
|
||||
vel_y = -flow_der_y * of_pmw.pmw.rad_per_px * agl_dist;
|
||||
noise = of_pmw.std_px * of_pmw.pmw.rad_per_px * agl_dist;
|
||||
}
|
||||
|
||||
/* Send ABI messages */
|
||||
// Note: INS only subscribes to VELOCITY_ESTIMATE. OPTICAL_FLOW is only used
|
||||
// for niche applications(?) and therefore only uses (sub)pixels without any
|
||||
// camera intrinsics?? On the bright side, the sensor datasheet does not
|
||||
// provide any intrinsics either.....
|
||||
AbiSendMsgOPTICAL_FLOW(FLOW_OPTICFLOW_PMW3901_ID,
|
||||
ts_usec, /* stamp [us] */
|
||||
flow_x_subpix, /* flow_x [subpixels] */
|
||||
flow_y_subpix, /* flow_y [subpixels] */
|
||||
flow_der_x_subpix, /* flow_der_x [subpixels] */
|
||||
flow_der_y_subpix, /* flow_der_y [subpixels] */
|
||||
0.f, /* quality [???] */
|
||||
0.f /* size_divergence [1/s] */
|
||||
);
|
||||
if (agl_valid(ts_usec)) {
|
||||
AbiSendMsgVELOCITY_ESTIMATE(VEL_OPTICFLOW_PMW3901_ID,
|
||||
ts_usec, /* stamp [us] */
|
||||
vel_x, /* x [m/s] */
|
||||
vel_y, /* y [m/s] */
|
||||
0.f, /* z [m/s] */
|
||||
noise, /* noise_x [m/s] */
|
||||
noise, /* noise_y [m/s] */
|
||||
-1.f /* noise_z [disabled] */
|
||||
);
|
||||
}
|
||||
|
||||
/* Send telemetry */
|
||||
#if SENSOR_SYNC_SEND_OPTICFLOW_PMW3901
|
||||
float dummy_f = 0.f;
|
||||
uint16_t dummy_u16 = 0;
|
||||
float fps = 1.f / dt;
|
||||
DOWNLINK_SEND_OPTIC_FLOW_EST(DefaultChannel, DefaultDevice,
|
||||
&fps, /* fps */
|
||||
&dummy_u16, /* corner_cnt */
|
||||
&dummy_u16, /* tracked_cnt */
|
||||
&flow_x_subpix, /* flow_x */
|
||||
&flow_y_subpix, /* flow_y */
|
||||
&flow_der_x_subpix, /* flow_der_x */
|
||||
&flow_der_y_subpix, /* flow_der_y */
|
||||
&vel_x, /* vel_x */
|
||||
&vel_y, /* vel_y */
|
||||
&dummy_f, /* vel_z */
|
||||
&dummy_f, /* div_size */
|
||||
&dummy_f, /* surface_roughness */
|
||||
&dummy_f /* divergence */
|
||||
);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
void opticflow_pmw3901_init(void) {
|
||||
pmw3901_init(&of_pmw.pmw, &OPTICFLOW_PMW3901_SPI_DEV, OPTICFLOW_PMW3901_SPI_SLAVE_IDX);
|
||||
AbiBindMsgAGL(OPTICFLOW_PMW3901_AGL_ID, &agl_ev, agl_cb);
|
||||
of_pmw.sensor_angle = OPTICFLOW_PMW3901_SENSOR_ANGLE;
|
||||
of_pmw.subpixel_factor = OPTICFLOW_PMW3901_SUBPIXEL_FACTOR;
|
||||
of_pmw.std_px = OPTICFLOW_PMW3901_STD_PX;
|
||||
of_pmw.agl_timeout = OPTICFLOW_PMW3901_AGL_TIMEOUT_US;
|
||||
#ifdef OPTICFLOW_PMW3901_RAD_PER_PX
|
||||
of_pmw.pmw.rad_per_px = OPTICFLOW_PMW3901_RAD_PER_PX;
|
||||
#endif
|
||||
}
|
||||
|
||||
void opticflow_pmw3901_periodic(void) {
|
||||
if (pmw3901_is_idle(&of_pmw.pmw)) {
|
||||
pmw3901_start_read(&of_pmw.pmw);
|
||||
}
|
||||
}
|
||||
|
||||
void opticflow_pmw3901_event(void) {
|
||||
pmw3901_event(&of_pmw.pmw);
|
||||
if (pmw3901_data_available(&of_pmw.pmw)) {
|
||||
int16_t delta_x, delta_y;
|
||||
pmw3901_get_data(&of_pmw.pmw, &delta_x, &delta_y);
|
||||
opticflow_pmw3901_publish(delta_x, delta_y, get_sys_time_usec());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,45 @@
|
||||
/*
|
||||
* Copyright (C) Tom van Dijk
|
||||
*
|
||||
* 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, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/**
|
||||
* @file "modules/sensors/opticflow_pmw3901.h"
|
||||
* @author Tom van Dijk
|
||||
* Driver for PMW3901 optical flow sensor
|
||||
*/
|
||||
|
||||
#ifndef OPTICFLOW_PMW3901_H
|
||||
#define OPTICFLOW_PMW3901_H
|
||||
|
||||
#include "peripherals/pmw3901.h"
|
||||
|
||||
struct opticflow_pmw3901_t {
|
||||
struct pmw3901_t pmw;
|
||||
float sensor_angle; // [deg!]
|
||||
int16_t subpixel_factor;
|
||||
float std_px;
|
||||
uint32_t agl_timeout;
|
||||
};
|
||||
extern struct opticflow_pmw3901_t of_pmw;
|
||||
|
||||
extern void opticflow_pmw3901_init(void);
|
||||
extern void opticflow_pmw3901_periodic(void);
|
||||
extern void opticflow_pmw3901_event(void);
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,332 @@
|
||||
/*
|
||||
* Copyright (C) Tom van Dijk
|
||||
*
|
||||
* 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, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/**
|
||||
* @file "modules/peripherals/pmw3901.c"
|
||||
* @author Tom van Dijk
|
||||
* Low-level driver for PMW3901 optical flow sensor
|
||||
*
|
||||
* Code based on the PixArt reference firmware
|
||||
* https://os.mbed.com/teams/PixArt/code/3901_referenceFirmware//file/10365086d44e/commHeaders/SPIcommFunctions.h/
|
||||
*
|
||||
*/
|
||||
|
||||
#include "pmw3901.h"
|
||||
|
||||
#include "mcu_periph/sys_time.h"
|
||||
|
||||
|
||||
// Based on crazyflie-firmware
|
||||
#ifndef PMW3901_RAD_PER_PX
|
||||
#define PMW3901_RAD_PER_PX 0.002443389
|
||||
#endif
|
||||
|
||||
// SPI divisor, to adjust the clock speed according to the PCLK
|
||||
// Don't exceed 2MHz
|
||||
#ifndef PMW3901_SPI_CDIV
|
||||
#define PMW3901_SPI_CDIV SPIDiv256
|
||||
#endif
|
||||
|
||||
#define PMW3901_REG_MOTION 0x02
|
||||
#define PMW3901_REG_DELTA_X_L 0x03
|
||||
#define PMW3901_REG_DELTA_X_H 0x04
|
||||
#define PMW3901_REG_DELTA_Y_L 0x05
|
||||
#define PMW3901_REG_DELTA_Y_H 0x06
|
||||
|
||||
|
||||
// Non-blocking read function
|
||||
// returns true upon completion
|
||||
static bool readRegister_nonblocking(struct pmw3901_t *pmw, uint8_t addr, uint8_t *value) {
|
||||
switch (pmw->readwrite_state) {
|
||||
case 0:
|
||||
if (get_sys_time_usec() < pmw->readwrite_timeout) return false;
|
||||
pmw->trans.output_buf[0] = addr & 0x7F; // MSB 0 => read
|
||||
pmw->trans.output_length = 1;
|
||||
pmw->trans.input_length = 0;
|
||||
pmw->trans.select = SPISelect;
|
||||
spi_submit(pmw->periph, &pmw->trans);
|
||||
pmw->readwrite_state++;
|
||||
/* Falls through. */
|
||||
case 1:
|
||||
if (pmw->trans.status == SPITransPending || pmw->trans.status == SPITransRunning) return false;
|
||||
// Write addr complete
|
||||
pmw->readwrite_timeout = get_sys_time_usec() + 35;
|
||||
pmw->readwrite_state++;
|
||||
/* Falls through. */
|
||||
case 2:
|
||||
if (get_sys_time_usec() < pmw->readwrite_timeout) return false;
|
||||
// Addr-read delay passed
|
||||
pmw->trans.output_length = 0;
|
||||
pmw->trans.input_length = 1;
|
||||
pmw->trans.select = SPIUnselect;
|
||||
spi_submit(pmw->periph, &pmw->trans);
|
||||
pmw->readwrite_state++;
|
||||
/* Falls through. */
|
||||
case 3:
|
||||
if (pmw->trans.status == SPITransPending || pmw->trans.status == SPITransRunning) return false;
|
||||
// Read complete
|
||||
pmw->trans.select = SPISelectUnselect;
|
||||
*value = pmw->trans.input_buf[0];
|
||||
pmw->readwrite_timeout = get_sys_time_usec() + 20;
|
||||
pmw->readwrite_state = 0;
|
||||
return true;
|
||||
default: return false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Blocking read/write functions
|
||||
static uint8_t readRegister_blocking(struct pmw3901_t *pmw, uint8_t addr) {
|
||||
pmw->trans.output_buf[0] = addr & 0x7F; // MSB 0 => read
|
||||
pmw->trans.output_length = 1;
|
||||
pmw->trans.input_length = 0;
|
||||
pmw->trans.select = SPISelect;
|
||||
spi_blocking_transceive(pmw->periph, &pmw->trans);
|
||||
sys_time_usleep(35); // See ref firmware and datasheet
|
||||
pmw->trans.output_length = 0;
|
||||
pmw->trans.input_length = 1;
|
||||
pmw->trans.select = SPIUnselect;
|
||||
spi_blocking_transceive(pmw->periph, &pmw->trans);
|
||||
pmw->trans.select = SPISelectUnselect;
|
||||
return pmw->trans.input_buf[0];
|
||||
}
|
||||
|
||||
static void writeRegister_blocking(struct pmw3901_t *pmw, uint8_t addr, uint8_t data) {
|
||||
pmw->trans.output_buf[0] = addr | 0x80; // MSB 1 => write
|
||||
pmw->trans.output_buf[1] = data;
|
||||
pmw->trans.output_length = 2;
|
||||
pmw->trans.input_length = 0;
|
||||
spi_blocking_transceive(pmw->periph, &pmw->trans);
|
||||
}
|
||||
|
||||
// For PixArt firmware compatibility:
|
||||
#define writeRegister(_addr, _data) writeRegister_blocking(pmw, (_addr), (_data))
|
||||
#define readRegister(_addr) readRegister_blocking(pmw, (_addr))
|
||||
#define wait_ms(_ms) sys_time_usleep((_ms) * 1000)
|
||||
|
||||
|
||||
static void initializeSensor(struct pmw3901_t *pmw) {
|
||||
// Try to detect sensor before initializing
|
||||
int tries = 0;
|
||||
while (readRegister(0x00) != 0x49 && tries < 100) {
|
||||
sys_time_usleep(50);
|
||||
tries++;
|
||||
}
|
||||
|
||||
// From reference firmware
|
||||
writeRegister(0x7F, 0x00);
|
||||
writeRegister(0x55, 0x01);
|
||||
writeRegister(0x50, 0x07);
|
||||
writeRegister(0x7F, 0x0E);
|
||||
writeRegister(0x43, 0x10);
|
||||
|
||||
if (readRegister(0x67) & 0x40)
|
||||
writeRegister(0x48, 0x04);
|
||||
|
||||
else
|
||||
writeRegister(0x48, 0x02);
|
||||
|
||||
writeRegister(0x7F, 0x00);
|
||||
writeRegister(0x51, 0x7B);
|
||||
writeRegister(0x50, 0x00);
|
||||
writeRegister(0x55, 0x00);
|
||||
writeRegister(0x7F, 0x0E);
|
||||
|
||||
if (readRegister(0x73) == 0x00) {
|
||||
writeRegister(0x7F, 0x00);
|
||||
writeRegister(0x61, 0xAD);
|
||||
writeRegister(0x51, 0x70);
|
||||
writeRegister(0x7F, 0x0E);
|
||||
|
||||
if (readRegister(0x70) <= 28)
|
||||
writeRegister(0x70, readRegister(0x70) + 14);
|
||||
|
||||
else
|
||||
writeRegister(0x70, readRegister(0x70) + 11);
|
||||
|
||||
writeRegister(0x71, readRegister(0x71) * 45/100);
|
||||
}
|
||||
|
||||
writeRegister(0x7F, 0x00);
|
||||
writeRegister(0x61, 0xAD);
|
||||
writeRegister(0x7F, 0x03);
|
||||
writeRegister(0x40, 0x00);
|
||||
writeRegister(0x7F, 0x05);
|
||||
writeRegister(0x41, 0xB3);
|
||||
writeRegister(0x43, 0xF1);
|
||||
writeRegister(0x45, 0x14);
|
||||
writeRegister(0x5B, 0x32);
|
||||
writeRegister(0x5F, 0x34);
|
||||
writeRegister(0x7B, 0x08);
|
||||
writeRegister(0x7F, 0x06);
|
||||
writeRegister(0x44, 0x1B);
|
||||
writeRegister(0x40, 0xBF);
|
||||
writeRegister(0x4E, 0x3F);
|
||||
writeRegister(0x7F, 0x06);
|
||||
writeRegister(0x44, 0x1B);
|
||||
writeRegister(0x40, 0xBF);
|
||||
writeRegister(0x4E, 0x3F);
|
||||
writeRegister(0x7F, 0x08);
|
||||
writeRegister(0x65, 0x20);
|
||||
writeRegister(0x6A, 0x18);
|
||||
writeRegister(0x7F, 0x09);
|
||||
writeRegister(0x4F, 0xAF);
|
||||
writeRegister(0x5F, 0x40);
|
||||
writeRegister(0x48, 0x80);
|
||||
writeRegister(0x49, 0x80);
|
||||
writeRegister(0x57, 0x77);
|
||||
writeRegister(0x60, 0x78);
|
||||
writeRegister(0x61, 0x78);
|
||||
writeRegister(0x62, 0x08);
|
||||
writeRegister(0x63, 0x50);
|
||||
writeRegister(0x7F, 0x0A);
|
||||
writeRegister(0x45, 0x60);
|
||||
writeRegister(0x7F, 0x00);
|
||||
writeRegister(0x4D, 0x11);
|
||||
writeRegister(0x55, 0x80);
|
||||
writeRegister(0x74, 0x21);
|
||||
writeRegister(0x75, 0x1F);
|
||||
writeRegister(0x4A, 0x78);
|
||||
writeRegister(0x4B, 0x78);
|
||||
writeRegister(0x44, 0x08);
|
||||
writeRegister(0x45, 0x50);
|
||||
writeRegister(0x64, 0xFF);
|
||||
writeRegister(0x65, 0x1F);
|
||||
writeRegister(0x7F, 0x14);
|
||||
writeRegister(0x65, 0x67);
|
||||
writeRegister(0x66, 0x08);
|
||||
writeRegister(0x63, 0x70);
|
||||
writeRegister(0x7F, 0x15);
|
||||
writeRegister(0x48, 0x48);
|
||||
writeRegister(0x7F, 0x07);
|
||||
writeRegister(0x41, 0x0D);
|
||||
writeRegister(0x43, 0x14);
|
||||
writeRegister(0x4B, 0x0E);
|
||||
writeRegister(0x45, 0x0F);
|
||||
writeRegister(0x44, 0x42);
|
||||
writeRegister(0x4C, 0x80);
|
||||
writeRegister(0x7F, 0x10);
|
||||
writeRegister(0x5B, 0x02);
|
||||
writeRegister(0x7F, 0x07);
|
||||
writeRegister(0x40, 0x41);
|
||||
writeRegister(0x70, 0x00);
|
||||
|
||||
wait_ms(10);
|
||||
|
||||
writeRegister(0x32, 0x44);
|
||||
writeRegister(0x7F, 0x07);
|
||||
writeRegister(0x40, 0x40);
|
||||
writeRegister(0x7F, 0x06);
|
||||
writeRegister(0x62, 0xF0);
|
||||
writeRegister(0x63, 0x00);
|
||||
writeRegister(0x7F, 0x0D);
|
||||
writeRegister(0x48, 0xC0);
|
||||
writeRegister(0x6F, 0xD5);
|
||||
writeRegister(0x7F, 0x00);
|
||||
writeRegister(0x5B, 0xA0);
|
||||
writeRegister(0x4E, 0xA8);
|
||||
writeRegister(0x5A, 0x50);
|
||||
writeRegister(0x40, 0x80);
|
||||
}
|
||||
|
||||
|
||||
void pmw3901_init(struct pmw3901_t *pmw, struct spi_periph *periph, uint8_t slave_idx) {
|
||||
// Set up SPI peripheral and transaction
|
||||
pmw->periph = periph;
|
||||
pmw->trans.input_buf = pmw->spi_input_buf;
|
||||
pmw->trans.output_buf = pmw->spi_output_buf;
|
||||
pmw->trans.slave_idx = slave_idx;
|
||||
pmw->trans.select = SPISelectUnselect;
|
||||
pmw->trans.cpol = SPICpolIdleLow;
|
||||
pmw->trans.cpha = SPICphaEdge1;
|
||||
pmw->trans.dss = SPIDss8bit;
|
||||
pmw->trans.bitorder = SPIMSBFirst;
|
||||
pmw->trans.cdiv = PMW3901_SPI_CDIV;
|
||||
pmw->trans.before_cb = NULL;
|
||||
pmw->trans.after_cb = NULL;
|
||||
pmw->trans.status = SPITransDone;
|
||||
// Initialize sensor registers
|
||||
initializeSensor(pmw);
|
||||
// Set up remaining fields
|
||||
pmw->state = PMW3901_IDLE;
|
||||
pmw->delta_x = 0;
|
||||
pmw->delta_y = 0;
|
||||
pmw->data_available = false;
|
||||
pmw->rad_per_px = PMW3901_RAD_PER_PX;
|
||||
}
|
||||
|
||||
void pmw3901_event(struct pmw3901_t *pmw) {
|
||||
uint8_t temp;
|
||||
switch (pmw->state) {
|
||||
case PMW3901_IDLE:
|
||||
/* Do nothing */
|
||||
return;
|
||||
case PMW3901_READ_MOTION:
|
||||
if (!readRegister_nonblocking(pmw, PMW3901_REG_MOTION, &temp)) return;
|
||||
if (!(temp & 0x80)) return;
|
||||
pmw->delta_x = 0;
|
||||
pmw->delta_y = 0;
|
||||
pmw->state++;
|
||||
/* Falls through. */
|
||||
case PMW3901_READ_DELTAXLOW:
|
||||
if (!readRegister_nonblocking(pmw, PMW3901_REG_DELTA_X_L, &temp)) return;
|
||||
pmw->delta_x |= temp;
|
||||
pmw->state++;
|
||||
/* Falls through. */
|
||||
case PMW3901_READ_DELTAXHIGH:
|
||||
if (!readRegister_nonblocking(pmw, PMW3901_REG_DELTA_X_H, &temp)) return;
|
||||
pmw->delta_x |= (temp << 8) & 0xFF00;
|
||||
pmw->state++;
|
||||
/* Falls through. */
|
||||
case PMW3901_READ_DELTAYLOW:
|
||||
if (!readRegister_nonblocking(pmw, PMW3901_REG_DELTA_Y_L, &temp)) return;
|
||||
pmw->delta_y |= temp;
|
||||
pmw->state++;
|
||||
/* Falls through. */
|
||||
case PMW3901_READ_DELTAYHIGH:
|
||||
if (!readRegister_nonblocking(pmw, PMW3901_REG_DELTA_Y_H, &temp)) return;
|
||||
pmw->delta_y |= (temp << 8) & 0xFF00;
|
||||
pmw->data_available = true;
|
||||
pmw->state = PMW3901_IDLE;
|
||||
return;
|
||||
default: return;
|
||||
}
|
||||
}
|
||||
|
||||
bool pmw3901_is_idle(struct pmw3901_t *pmw) {
|
||||
return pmw->state == PMW3901_IDLE;
|
||||
}
|
||||
|
||||
void pmw3901_start_read(struct pmw3901_t *pmw) {
|
||||
if (pmw3901_is_idle(pmw)) {
|
||||
pmw->state = PMW3901_READ_MOTION;
|
||||
}
|
||||
}
|
||||
|
||||
bool pmw3901_data_available(struct pmw3901_t *pmw) {
|
||||
return pmw->data_available;
|
||||
}
|
||||
|
||||
bool pmw3901_get_data(struct pmw3901_t *pmw, int16_t *delta_x, int16_t *delta_y) {
|
||||
if (!pmw->data_available) return false;
|
||||
*delta_x = pmw->delta_x;
|
||||
*delta_y = pmw->delta_y;
|
||||
pmw->data_available = false;
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,85 @@
|
||||
/*
|
||||
* Copyright (C) Tom van Dijk
|
||||
*
|
||||
* 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, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/**
|
||||
* @file "modules/peripherals/pmw3901.h"
|
||||
* @author Tom van Dijk
|
||||
* Low-level driver for PMW3901 optical flow sensor
|
||||
*
|
||||
* Some notes about the sensor:
|
||||
* - The sensor is extremely poorly documented:
|
||||
* - There are no units or delta_t's specified for the delta_x and _y registers.
|
||||
* - The datasheet does not specify whether delta_x and _y are rates [something/s]
|
||||
* or pixel counts. In the latter case, the datasheet does not specify when
|
||||
* delta_x and _y are reset to 0.
|
||||
* - The squal quality(?) register is not described.
|
||||
*
|
||||
* Based on crazyflie-firmware::kalman_core.c::512
|
||||
* - delta_x, _y are pixel counts, not rates.
|
||||
* - crazyflie uses scaling factor of thetapix/Npix = 0,002443389 rad/px,
|
||||
* corresponding to a focal length of ~409 px.
|
||||
*/
|
||||
|
||||
#ifndef PMW3901_H_
|
||||
#define PMW3901_H_
|
||||
|
||||
#include "mcu_periph/spi.h"
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <math.h>
|
||||
|
||||
|
||||
#define SPI_BUFFER_SIZE 8
|
||||
|
||||
|
||||
enum pmw3901_state {
|
||||
PMW3901_IDLE,
|
||||
PMW3901_READ_MOTION,
|
||||
PMW3901_READ_DELTAXLOW,
|
||||
PMW3901_READ_DELTAXHIGH,
|
||||
PMW3901_READ_DELTAYLOW,
|
||||
PMW3901_READ_DELTAYHIGH,
|
||||
};
|
||||
|
||||
struct pmw3901_t {
|
||||
struct spi_periph *periph;
|
||||
struct spi_transaction trans;
|
||||
volatile uint8_t spi_input_buf[SPI_BUFFER_SIZE];
|
||||
volatile uint8_t spi_output_buf[SPI_BUFFER_SIZE];
|
||||
enum pmw3901_state state;
|
||||
uint8_t readwrite_state;
|
||||
uint32_t readwrite_timeout;
|
||||
int16_t delta_x;
|
||||
int16_t delta_y;
|
||||
bool data_available;
|
||||
float rad_per_px;
|
||||
};
|
||||
|
||||
void pmw3901_init(struct pmw3901_t *pmw, struct spi_periph *periph, uint8_t slave_idx);
|
||||
|
||||
void pmw3901_event(struct pmw3901_t *pmw);
|
||||
|
||||
bool pmw3901_is_idle(struct pmw3901_t *pmw);
|
||||
void pmw3901_start_read(struct pmw3901_t *pmw);
|
||||
bool pmw3901_data_available(struct pmw3901_t *pmw);
|
||||
bool pmw3901_get_data(struct pmw3901_t *pmw, int16_t *delta_x, int16_t *delta_y);
|
||||
|
||||
|
||||
|
||||
#endif // PMW3901_H_
|
||||
@@ -347,6 +347,10 @@
|
||||
#define FLOW_OPTICFLOW_ID 1
|
||||
#endif
|
||||
|
||||
#ifndef FLOW_OPTICFLOW_PMW3901_ID
|
||||
#define FLOW_OPTICFLOW_PMW3901_ID 2
|
||||
#endif
|
||||
|
||||
/*
|
||||
* IDs of VELOCITY estimates (message 12)
|
||||
*/
|
||||
@@ -366,6 +370,10 @@
|
||||
#define VEL_STEREOCAM_ID 4
|
||||
#endif
|
||||
|
||||
#ifndef VEL_OPTICFLOW_PMW3901_ID
|
||||
#define VEL_OPTICFLOW_PMW3901_ID 5
|
||||
#endif
|
||||
|
||||
/*
|
||||
* IDs of RSSI measurements (message 13)
|
||||
*/
|
||||
|
||||
Reference in New Issue
Block a user