Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-22 20:36:06 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

[lidar] Added Parallax SF11 laser rangefinder

Browse Source
This commit is contained in:
Michal Podhradsky
2017-01-06 14:39:32 +01:00
parent f7eb7dc3e8
commit 853081b832
9 changed files with 321 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/AGGIEAIR/aggieair_conf.xml
+1 -1
View File
@@ -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"
/>
<aircraft
conf/airframes/AGGIEAIR/aggieair_iris_indi.xml
+8 -2
View File
@@ -127,17 +127,23 @@
<!-- optical flow camera over i2c-->
<module name="px4flow_i2c">
<configure name="USE_PX4FLOW_AGL" value="0"/> <!-- send AGL measurements -->
<configure name="USE_PX4FLOW_AGL" value="0"/> <!-- don't send AGL measurements -->
<configure name="PX4FLOW_COMPENSATE_ROTATION" value="1"/> <!-- compensate AGL for body rotation -->
<configure name="PX4FLOW_NOISE_STDDEV" value="10.0"/> <!-- standard deviation of the optical flow measurements-->
</module>
<!-- laser range finder for precise AGL measurement -->
<module name="lidar_lite">
<configure name="USE_LIDAR_LITE_AGL" value="1"/> <!-- dont send AGL measurements -->
<configure name="USE_LIDAR_LITE_AGL" value="1"/> <!-- send AGL measurements -->
<configure name="LIDAR_LITE_COMPENSATE_ROTATION" value="1" /> <!-- compensate AGL for body rotation -->
</module>
<!-- laser range finder for precise AGL measurement -->
<module name="lidar_sf11">
<configure name="USE_LIDAR_SF11_AGL" value="0"/> <!-- dont send AGL measurements -->
<configure name="LIDAR_SF11_COMPENSATE_ROTATION" value="1" /> <!-- compensate AGL for body rotation -->
</module>
<!-- external mag for better heading estimate -->
<module name="mag" type="hmc58xx">
<configure name="MAG_HMC58XX_I2C_DEV" value="i2c1"/>
conf/modules/lidar_lite.xml
+6 -6
View File
@@ -4,20 +4,20 @@
<doc>
<description>
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)
</description>
<configure name="LIDAR_LITE_I2C_DEV" value="i2c2" description="I2C device to use for Lidar-lite"/>
<configure name="LIDAR_LITE_I2C_ADDR" value="0x62" description="default slave address of Lidar-lite"/>
<configure name="USE_LIDAR_LITE_AGL" value="1" description="use Lidar-lite for AGL measurements. On by deafult"/>
<configure name="LIDAR_LITE_I2C_DEV" value="i2c2" description="I2C device to use for lidar"/>
<configure name="LIDAR_LITE_I2C_ADDR" value="0x62" description="default slave address of lidar"/>
<configure name="USE_LIDAR_LITE_AGL" value="1" description="use this lidar for AGL measurements. On by deafult"/>
<configure name="LIDAR_LITE_COMPENSATE_ROTATION" value="1" description="compensate AGL measurements for body rotation"/>
</doc>
<settings>
<dl_settings NAME="Lidar lite">
<dl_settings NAME="Lidar">
<dl_setting MAX="1" MIN="0" STEP="1" VAR="lidar.compensate_rotation" shortname="derotate_agl"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="lidar.update_agl" shortname="update_agl"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="lidar_lite.compensate_rotation" shortname="derotate_agl"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="lidar_lite.update_agl" shortname="update_agl"/>
</dl_settings>
</dl_settings>
</settings>
conf/modules/lidar_sf11.xml
+48
View File
@@ -0,0 +1,48 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="lidar_lite" dir="lidar">
<doc>
<description>
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.
</description>
<configure name="LIDAR_SF11_I2C_DEV" value="i2c2" description="I2C device to use for lidar"/>
<configure name="LIDAR_SF11_I2C_ADDR" value="0x50" description="default slave address of lidar"/>
<configure name="USE_LIDAR_SF11_AGL" value="1" description="use this lidar for AGL measurements. On by deafult"/>
<configure name="LIDAR_SF11_COMPENSATE_ROTATION" value="1" description="compensate AGL measurements for body rotation"/>
</doc>
<settings>
<dl_settings NAME="Lidar SF11">
<dl_settings NAME="Lidar">
<dl_setting MAX="1" MIN="0" STEP="1" VAR="lidar_sf11.compensate_rotation" shortname="derotate_agl"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="lidar_sf11.update_agl" shortname="update_agl"/>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="lidar_sf11.h"/>
</header>
<init fun="lidar_sf11_init()"/>
<periodic fun="lidar_sf11_downlink()" freq="4" autorun="FALSE"/> <!-- for debug -->
<periodic fun="lidar_sf11_periodic()" freq="32"/> <!-- poll data, since we have 2 states in the state machine, divide the frequency
by 2 to get the actual number of readings per second. For example 32Hz = 16 range readings, which is the maximum (per the datasheet) -->
<event fun="lidar_sf11_event()"/>
<makefile>
<configure name="LIDAR_SF11_I2C_DEV" default="i2c1" case="lower|upper"/>
<configure name="LIDAR_SF11_I2C_ADDR" default="0x50"/>
<configure name="USE_LIDAR_SF11_AGL" default="1"/>
<configure name="LIDAR_SF11_COMPENSATE_ROTATION" default="1"/>
<define name="LIDAR_SF11_I2C_DEV" value="$(LIDAR_SF11_I2C_DEV_LOWER)"/>
<define name="USE_$(LIDAR_SF11_I2C_DEV_UPPER)"/>
<define name="LIDAR_SF11_I2C_ADDR" value="$(LIDAR_SF11_I2C_ADDR)"/>
<define name="USE_LIDAR_SF11_AGL" value="$(USE_LIDAR_SF11_AGL)"/>
<define name="LIDAR_SF11_COMPENSATE_ROTATION" value="$(LIDAR_SF11_COMPENSATE_ROTATION)"/>
<file name="lidar_sf11.c"/>
</makefile>
</module>
sw/airborne/modules/lidar/lidar_lite.c
+49 -45
View File
@@ -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);
}
sw/airborne/modules/lidar/lidar_lite.h
+11 -9
View File
@@ -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);
sw/airborne/modules/lidar/lidar_sf11.c
+139
View File
@@ -0,0 +1,139 @@
/*
* Copyright (C) 2017 Michal Podhradsky <michal.podhradsky@aggiemail.usu.edu>
*
* 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);
}
sw/airborne/modules/lidar/lidar_sf11.h
+55
View File
@@ -0,0 +1,55 @@
/*
* Copyright (C) 2017 Michal Podhradsky <michal.podhradsky@aggiemail.usu.edu>
*
* 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 */
sw/airborne/subsystems/abi_sender_ids.h
+4
View File
@@ -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)
*/