diff --git a/conf/airframes/AGGIEAIR/aggieair_conf.xml b/conf/airframes/AGGIEAIR/aggieair_conf.xml index cdd1e0db15..d153731935 100644 --- a/conf/airframes/AGGIEAIR/aggieair_conf.xml +++ b/conf/airframes/AGGIEAIR/aggieair_conf.xml @@ -18,7 +18,7 @@ telemetry="telemetry/AGGIEAIR/aggieair_iris.xml" flight_plan="flight_plans/AGGIEAIR/rotorcraft_opticlow_test.xml" settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml [settings/control/stabilization_att_int_quat.xml] settings/control/stabilization_indi.xml [settings/test_actuators_pwm.xml]" - settings_modules="modules/air_data.xml modules/lidar_lite.xml modules/px4flow_i2c.xml modules/ins_extended.xml modules/ahrs_float_mlkf.xml modules/ahrs_int_cmpl_quat.xml modules/imu_common.xml" + settings_modules="modules/air_data.xml modules/lidar_sf11.xml modules/lidar_lite.xml modules/px4flow_i2c.xml modules/ins_extended.xml modules/ahrs_float_mlkf.xml modules/ahrs_int_cmpl_quat.xml modules/imu_common.xml" gui_color="#337e387bffff" /> - + - + + + + + + + diff --git a/conf/modules/lidar_lite.xml b/conf/modules/lidar_lite.xml index 3aa26f2a4c..eaef282c30 100644 --- a/conf/modules/lidar_lite.xml +++ b/conf/modules/lidar_lite.xml @@ -4,20 +4,20 @@ Lidar-Lite v1-3 (all labels) from PulsedLight3D (now Garmin) connected over i2c bus. - With the default settings the ranging takes ~3ms so don't poll teh lidar faster than 300Hz + With the default settings the ranging takes ~3ms so don't poll the lidar faster than 300Hz (in practice lidar_lite_periodic() can run at the PERIODIC_FREQUENCY and still be OK) - - - + + + - - + + diff --git a/conf/modules/lidar_sf11.xml b/conf/modules/lidar_sf11.xml new file mode 100644 index 0000000000..51ce26da9f --- /dev/null +++ b/conf/modules/lidar_sf11.xml @@ -0,0 +1,48 @@ + + + + + + Parallax SF11-A/B/C Laser Rangefinder connected over i2c bus. + The update rate is 16 readings per second, so don't poll the lidar faster than 32Hz. + In practice lidar_sf11_periodic() can run at most at freq=32. + The i2c address can be reconfigured. + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + diff --git a/sw/airborne/modules/lidar/lidar_lite.c b/sw/airborne/modules/lidar/lidar_lite.c index 6c565768ba..fb29bd4271 100644 --- a/sw/airborne/modules/lidar/lidar_lite.c +++ b/sw/airborne/modules/lidar/lidar_lite.c @@ -23,11 +23,13 @@ /** @file modules/lidar/lidar_lite.c * @brief driver for the Lidar-Lite i2c lidar version 1 (silver label) * - * Note that the lidar seems to generate unexpected events on the i2c bus, - * such as misplaced start or stop or word reset (see I2C_ERRORS message). + * Note that the version 1 (silver label) seems to generate unexpected events + * on the i2c bus, such as misplaced start or stop or word reset (see I2C_ERRORS message). * It seems to have no effect on other i2c devices (especially the IMU), but * use with caution. * + * The newer versions function correctly. + * */ #include "modules/lidar/lidar_lite.h" #include "subsystems/abi.h" @@ -40,8 +42,8 @@ #include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" -struct lidar_lite lidar; -struct MedianFilterInt lidar_filter; +struct LidarLite lidar_lite; +struct MedianFilterInt lidar_lite_filter; #define LIDAR_LITE_REG_ADDR 0x00 #define LIDAR_LITE_REG_VAL 0x04 @@ -53,16 +55,16 @@ struct MedianFilterInt lidar_filter; */ void lidar_lite_init(void) { - lidar.trans.status = I2CTransDone; - lidar.addr = LIDAR_LITE_I2C_ADDR; - lidar.status = LIDAR_INIT_RANGING; - lidar.update_agl = USE_LIDAR_LITE_AGL; - lidar.compensate_rotation = LIDAR_LITE_COMPENSATE_ROTATION; + lidar_lite.trans.status = I2CTransDone; + lidar_lite.addr = LIDAR_LITE_I2C_ADDR; + lidar_lite.status = LIDAR_LITE_INIT_RANGING; + lidar_lite.update_agl = USE_LIDAR_LITE_AGL; + lidar_lite.compensate_rotation = LIDAR_LITE_COMPENSATE_ROTATION; - lidar.distance = 0; - lidar.distance_raw = 0; + lidar_lite.distance = 0; + lidar_lite.distance_raw = 0; - init_median_filter(&lidar_filter); + init_median_filter(&lidar_lite_filter); } /** @@ -73,20 +75,20 @@ void lidar_lite_init(void) */ void lidar_lite_event(void) { - switch (lidar.trans.status) { + switch (lidar_lite.trans.status) { case I2CTransPending: - //wait and do nothing + // wait and do nothing break; case I2CTransRunning: // wait and do nothing break; case I2CTransSuccess: // set to done - lidar.trans.status = I2CTransDone; + lidar_lite.trans.status = I2CTransDone; break; case I2CTransFailed: // set to done - lidar.trans.status = I2CTransDone; + lidar_lite.trans.status = I2CTransDone; break; case I2CTransDone: // do nothing @@ -103,58 +105,60 @@ void lidar_lite_event(void) */ void lidar_lite_periodic(void) { - switch (lidar.status) { - case LIDAR_INIT_RANGING: - if (lidar.trans.status == I2CTransDone) { + switch (lidar_lite.status) { + case LIDAR_LITE_INIT_RANGING: + if (lidar_lite.trans.status == I2CTransDone) { // ask for i2c frame - lidar.trans.buf[0] = LIDAR_LITE_REG_ADDR; // sets register pointer to (0x00) - lidar.trans.buf[1] = LIDAR_LITE_REG_VAL; // take acquisition & correlation processing with DC correction - if (i2c_transmit(&LIDAR_LITE_I2C_DEV, &lidar.trans, lidar.addr, 2)){ + lidar_lite.trans.buf[0] = LIDAR_LITE_REG_ADDR; // sets register pointer to (0x00) + lidar_lite.trans.buf[1] = LIDAR_LITE_REG_VAL; // take acquisition & correlation processing with DC correction + if (i2c_transmit(&LIDAR_LITE_I2C_DEV, &lidar_lite.trans, lidar_lite.addr, 2)){ // transaction OK, increment status - lidar.status = LIDAR_REQ_READ; + lidar_lite.status = LIDAR_LITE_REQ_READ; } } break; - case LIDAR_REQ_READ: - if (lidar.trans.status == I2CTransDone) { - lidar.trans.buf[0] = LIDAR_LITE_READ_ADDR; // sets register pointer to results register - if (i2c_transmit(&LIDAR_LITE_I2C_DEV, &lidar.trans, lidar.addr, 1)){ + case LIDAR_LITE_REQ_READ: + if (lidar_lite.trans.status == I2CTransDone) { + lidar_lite.trans.buf[0] = LIDAR_LITE_READ_ADDR; // sets register pointer to results register + if (i2c_transmit(&LIDAR_LITE_I2C_DEV, &lidar_lite.trans, lidar_lite.addr, 1)){ // transaction OK, increment status - lidar.status = LIDAR_READ_DISTANCE; + lidar_lite.status = LIDAR_LITE_READ_DISTANCE; } } break; - case LIDAR_READ_DISTANCE: - if (lidar.trans.status == I2CTransDone) { + case LIDAR_LITE_READ_DISTANCE: + if (lidar_lite.trans.status == I2CTransDone) { // clear buffer - lidar.trans.buf[0] = 0; - lidar.trans.buf[1] = 0; - if (i2c_receive(&LIDAR_LITE_I2C_DEV, &lidar.trans, lidar.addr, 2)){ + lidar_lite.trans.buf[0] = 0; + lidar_lite.trans.buf[1] = 0; + if (i2c_receive(&LIDAR_LITE_I2C_DEV, &lidar_lite.trans, lidar_lite.addr, 2)){ // transaction OK, increment status - lidar.status = LIDAR_PARSE; + lidar_lite.status = LIDAR_LITE_PARSE; } } break; - case LIDAR_PARSE: + case LIDAR_LITE_PARSE: // filter data - lidar.distance_raw = update_median_filter(&lidar_filter, (uint32_t)((lidar.trans.buf[0] << 8) | lidar.trans.buf[1])); - lidar.distance = ((float)lidar.distance_raw)/100.0; + lidar_lite.distance_raw = update_median_filter( + &lidar_lite_filter, + (uint32_t)((lidar_lite.trans.buf[0] << 8) | lidar_lite.trans.buf[1])); + lidar_lite.distance = ((float)lidar_lite.distance_raw)/100.0; // compensate AGL measurement for body rotation - if (lidar.compensate_rotation) { + if (lidar_lite.compensate_rotation) { float phi = stateGetNedToBodyEulers_f()->phi; float theta = stateGetNedToBodyEulers_f()->theta; float gain = (float)fabs( (double) (cosf(phi) * cosf(theta))); - lidar.distance = lidar.distance / gain; + lidar_lite.distance = lidar_lite.distance / gain; } // send message (if requested) - if (lidar.update_agl) { - AbiSendMsgAGL(AGL_LIDAR_LITE_ID, lidar.distance); + if (lidar_lite.update_agl) { + AbiSendMsgAGL(AGL_LIDAR_LITE_ID, lidar_lite.distance); } // increment status - lidar.status = LIDAR_INIT_RANGING; + lidar_lite.status = LIDAR_LITE_INIT_RANGING; break; default: break; @@ -166,9 +170,9 @@ void lidar_lite_periodic(void) */ void lidar_lite_downlink(void) { - uint8_t trans = lidar.trans.status; + uint8_t trans = lidar_lite.trans.status; DOWNLINK_SEND_LIDAR(DefaultChannel, DefaultDevice, - &lidar.distance, - &lidar.status, + &lidar_lite.distance, + &lidar_lite.status, &trans); } diff --git a/sw/airborne/modules/lidar/lidar_lite.h b/sw/airborne/modules/lidar/lidar_lite.h index 254bdfb780..8999f994db 100644 --- a/sw/airborne/modules/lidar/lidar_lite.h +++ b/sw/airborne/modules/lidar/lidar_lite.h @@ -21,12 +21,14 @@ */ /** @file modules/lidar/lidar_lite.h - * @brief driver for the Lidar-Lite i2c lidar version 1 (silver label) + * @brief driver for the Lidar-Lite i2c lidar * - * Note that the lidar seems to generate unexpected events on the i2c bus, - * such as misplaced start or stop or word reset (see I2C_ERRORS message). + * Note that the version 1 (silver label) seems to generate unexpected events + * on the i2c bus, such as misplaced start or stop or word reset (see I2C_ERRORS message). * It seems to have no effect on other i2c devices (especially the IMU), but * use with caution. + * + * The newer versions function correctly. */ #ifndef LIDAR_LITE_I2C_H #define LIDAR_LITE_I2C_H @@ -35,13 +37,13 @@ #include "mcu_periph/i2c.h" enum LidarLiteStatus { - LIDAR_INIT_RANGING, - LIDAR_REQ_READ, - LIDAR_READ_DISTANCE, - LIDAR_PARSE + LIDAR_LITE_INIT_RANGING, + LIDAR_LITE_REQ_READ, + LIDAR_LITE_READ_DISTANCE, + LIDAR_LITE_PARSE }; -struct lidar_lite +struct LidarLite { struct i2c_transaction trans; uint8_t addr; @@ -52,7 +54,7 @@ struct lidar_lite bool compensate_rotation; }; -extern struct lidar_lite lidar; +extern struct LidarLite lidar_lite; extern void lidar_lite_init(void); extern void lidar_lite_event(void); diff --git a/sw/airborne/modules/lidar/lidar_sf11.c b/sw/airborne/modules/lidar/lidar_sf11.c new file mode 100644 index 0000000000..199642b52f --- /dev/null +++ b/sw/airborne/modules/lidar/lidar_sf11.c @@ -0,0 +1,139 @@ +/* + * Copyright (C) 2017 Michal Podhradsky + * + * 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/lidar/lidar_sf11.h + * @brief driver for the Parallax SF11-A/B/C Laser Rangefinder connected over i2c bus. + */ +#include "modules/lidar/lidar_sf11.h" +#include "subsystems/abi.h" +#include "filters/median_filter.h" + +// State interface for rotation compensation +#include "state.h" + +// Messages +#include "pprzlink/messages.h" +#include "subsystems/datalink/downlink.h" + +struct LidarSF11 lidar_sf11; +struct MedianFilterInt lidar_sf11_filter; + + +/** + * Initialization function + */ +void lidar_sf11_init(void) +{ + lidar_sf11.trans.status = I2CTransDone; + lidar_sf11.addr = LIDAR_SF11_I2C_ADDR; + lidar_sf11.status = LIDAR_LITE_REQ_READ; + lidar_sf11.update_agl = USE_LIDAR_SF11_AGL; + lidar_sf11.compensate_rotation = LIDAR_SF11_COMPENSATE_ROTATION; + + lidar_sf11.distance = 0; + lidar_sf11.distance_raw = 0; + + init_median_filter(&lidar_sf11_filter); +} + +/** + * Lidar event function + * Check if the transaction succeded before reading the result + */ +void lidar_sf11_event(void) +{ + switch (lidar_sf11.trans.status) { + case I2CTransPending: + // wait and do nothing + break; + case I2CTransRunning: + // wait and do nothing + break; + case I2CTransSuccess: + // set to done + lidar_sf11.trans.status = I2CTransDone; + // increment status + lidar_sf11.status = LIDAR_SF11_READ_OK; + break; + case I2CTransFailed: + // set to done + lidar_sf11.trans.status = I2CTransDone; + break; + case I2CTransDone: + // do nothing + break; + default: + break; + } +} + +/** + * Poll lidar for data + */ +void lidar_sf11_periodic(void) +{ + switch (lidar_sf11.status) { + case LIDAR_SF11_REQ_READ: + // read two bytes + lidar_sf11.trans.buf[0] = 0; // set tx to zero + i2c_transceive(&LIDAR_SF11_I2C_DEV, &lidar_sf11.trans, lidar_sf11.addr, 1, 2); + break; + case LIDAR_SF11_READ_OK: + // process results + // filter data + lidar_sf11.distance_raw = update_median_filter( + &lidar_sf11_filter, + (uint32_t)((lidar_sf11.trans.buf[0] << 8) | lidar_sf11.trans.buf[1])); + lidar_sf11.distance = ((float)lidar_sf11.distance_raw)/100.0; + + // compensate AGL measurement for body rotation + if (lidar_sf11.compensate_rotation) { + float phi = stateGetNedToBodyEulers_f()->phi; + float theta = stateGetNedToBodyEulers_f()->theta; + float gain = (float)fabs( (double) (cosf(phi) * cosf(theta))); + lidar_sf11.distance = lidar_sf11.distance / gain; + } + + // send message (if requested) + if (lidar_sf11.update_agl) { + AbiSendMsgAGL(AGL_LIDAR_SF11_ID, lidar_sf11.distance); + } + + // reset status + lidar_sf11.status = LIDAR_SF11_REQ_READ; + break; + default: + break; + } +} + +/** + * Downlink message for debug + */ +void lidar_sf11_downlink(void) +{ + uint8_t trans = lidar_sf11.trans.status; + DOWNLINK_SEND_LIDAR(DefaultChannel, DefaultDevice, + &lidar_sf11.distance, + &lidar_sf11.status, + &trans); +} diff --git a/sw/airborne/modules/lidar/lidar_sf11.h b/sw/airborne/modules/lidar/lidar_sf11.h new file mode 100644 index 0000000000..704767f174 --- /dev/null +++ b/sw/airborne/modules/lidar/lidar_sf11.h @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2017 Michal Podhradsky + * + * 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/lidar/lidar_sf11.h + * @brief driver for the Parallax SF11-A/B/C Laser Rangefinder connected over i2c bus. + */ +#ifndef LIDAR_SF11_I2C_H +#define LIDAR_SF11_I2C_H + +#include "std.h" +#include "mcu_periph/i2c.h" + +enum LidarSF11Status { + LIDAR_SF11_REQ_READ, + LIDAR_SF11_READ_OK +}; + +struct LidarSF11 +{ + struct i2c_transaction trans; + uint8_t addr; + uint32_t distance_raw; // [cm] + float distance; // [m] + enum LidarSF11Status status; + bool update_agl; + bool compensate_rotation; +}; + +extern struct LidarSF11 lidar_sf11; + +extern void lidar_sf11_init(void); +extern void lidar_sf11_event(void); +extern void lidar_sf11_periodic(void); +extern void lidar_sf11_downlink(void); + +#endif /* LIDAR_SF11_I2C_H */ diff --git a/sw/airborne/subsystems/abi_sender_ids.h b/sw/airborne/subsystems/abi_sender_ids.h index 71a33cd3e0..ac6ba9d9ce 100644 --- a/sw/airborne/subsystems/abi_sender_ids.h +++ b/sw/airborne/subsystems/abi_sender_ids.h @@ -119,6 +119,10 @@ #define AGL_PX4FLOW_ID 7 #endif +#ifndef AGL_LIDAR_SF11_ID +#define AGL_LIDAR_SF11_ID 8 +#endif + /* * IDs of magnetometer sensors (including IMUs with mag) */