diff --git a/conf/airframes/ENAC/quadrotor/crazyflie_2.1.xml b/conf/airframes/ENAC/quadrotor/crazyflie_2.1.xml index 65d2d68046..2d87f7d4a0 100644 --- a/conf/airframes/ENAC/quadrotor/crazyflie_2.1.xml +++ b/conf/airframes/ENAC/quadrotor/crazyflie_2.1.xml @@ -48,8 +48,14 @@ - - + + + + + + + + diff --git a/conf/modules/opticflow_pmw3901.xml b/conf/modules/opticflow_pmw3901.xml new file mode 100644 index 0000000000..d91994a0ad --- /dev/null +++ b/conf/modules/opticflow_pmw3901.xml @@ -0,0 +1,49 @@ + + + + + Driver for PMW3901 optical flow sensor + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + include $(CFG_SHARED)/spi_master.makefile + + + + diff --git a/sw/airborne/arch/chibios/mcu_periph/spi_arch.c b/sw/airborne/arch/chibios/mcu_periph/spi_arch.c index 27bdbfae14..b671a034bc 100644 --- a/sw/airborne/arch/chibios/mcu_periph/spi_arch.c +++ b/sw/airborne/arch/chibios/mcu_periph/spi_arch.c @@ -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); } + } /** diff --git a/sw/airborne/mcu_periph/spi.h b/sw/airborne/mcu_periph/spi.h index 4e6305f449..abd3c7d02b 100644 --- a/sw/airborne/mcu_periph/spi.h +++ b/sw/airborne/mcu_periph/spi.h @@ -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 */ diff --git a/sw/airborne/modules/sensors/opticflow_pmw3901.c b/sw/airborne/modules/sensors/opticflow_pmw3901.c new file mode 100644 index 0000000000..28ee54d5a5 --- /dev/null +++ b/sw/airborne/modules/sensors/opticflow_pmw3901.c @@ -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 + * . + */ +/** + * @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 + + +#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()); + } +} + + diff --git a/sw/airborne/modules/sensors/opticflow_pmw3901.h b/sw/airborne/modules/sensors/opticflow_pmw3901.h new file mode 100644 index 0000000000..3412c36766 --- /dev/null +++ b/sw/airborne/modules/sensors/opticflow_pmw3901.h @@ -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 + * . + */ +/** + * @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 + diff --git a/sw/airborne/peripherals/pmw3901.c b/sw/airborne/peripherals/pmw3901.c new file mode 100644 index 0000000000..4e2504feb8 --- /dev/null +++ b/sw/airborne/peripherals/pmw3901.c @@ -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 + * . + */ +/** + * @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; +} diff --git a/sw/airborne/peripherals/pmw3901.h b/sw/airborne/peripherals/pmw3901.h new file mode 100644 index 0000000000..1629c2c0f4 --- /dev/null +++ b/sw/airborne/peripherals/pmw3901.h @@ -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 + * . + */ +/** + * @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 +#include + + +#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_ diff --git a/sw/airborne/subsystems/abi_sender_ids.h b/sw/airborne/subsystems/abi_sender_ids.h index efae34f12b..bf0e1c0c87 100644 --- a/sw/airborne/subsystems/abi_sender_ids.h +++ b/sw/airborne/subsystems/abi_sender_ids.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) */