Paparazzi UAS
v5.15_devel-230-gc96ce27
Paparazzi is a free software Unmanned Aircraft System.
|
#include "modules/decawave/dw1000_arduino.h"
#include "std.h"
#include "mcu_periph/uart.h"
#include "pprzlink/messages.h"
#include "subsystems/datalink/downlink.h"
#include "subsystems/abi.h"
#include "modules/decawave/trilateration.h"
#include "subsystems/gps.h"
#include "state.h"
#include "generated/flight_plan.h"
#include "generated/airframe.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
Go to the source code of this file.
Data Structures | |
struct | DW1000 |
DW1000 positionning system structure. More... | |
Macros | |
#define | DW1000_USE_AS_LOCAL_POS TRUE |
TRUE if dw1000 are used as local position estimate. More... | |
#define | DW1000_USE_AS_GPS FALSE |
TRUE if dw1000 are used as GPS. More... | |
#define | DW1000_USE_EKF TRUE |
TRUE if EKF range filter is use. More... | |
#define | DW1000_NB_ANCHORS 3 |
Number of anchors. More... | |
#define | DW1000_OFFSET { 0.f, 0.f, 0.f } |
default offset, applied to individual distances More... | |
#define | DW1000_SCALE { 1.f, 1.f, 1.f } |
default scale factor, applied to individual distances More... | |
#define | DW1000_INITIAL_HEADING 0.f |
default initial heading correction between anchors frame and global frame More... | |
#define | DW1000_TIMEOUT 500 |
default timeout (in ms) More... | |
#define | DW1000_NOISE_X 0.1f |
DW1000 Noise. More... | |
#define | DW1000_NOISE_Y 0.1f |
#define | DW1000_NOISE_Z 0.1f |
#define | DW1000_VEL_NOISE_X 0.1f |
#define | DW1000_VEL_NOISE_Y 0.1f |
#define | DW1000_VEL_NOISE_Z 0.1f |
#define | DW_STX 0xFE |
waypoints to use as anchors in simulation More... | |
#define | DW_WAIT_STX 0 |
Parsing states. More... | |
#define | DW_GET_DATA 1 |
#define | DW_GET_CK 2 |
#define | DW_NB_DATA 6 |
Functions | |
static float | float_from_buf (uint8_t *b) |
Utility function to get float from buffer. More... | |
static uint16_t | uint16_from_buf (uint8_t *b) |
Utility function to get uint16_t from buffer. More... | |
static void | fill_anchor (struct DW1000 *dw) |
Utility function to fill anchor from buffer. More... | |
static void | dw1000_arduino_parse (struct DW1000 *dw, uint8_t c) |
Data parsing function. More... | |
static bool | check_anchor_timeout (struct DW1000 *dw, float timeout) |
check timeout for each anchor More... | |
static bool | check_and_compute_data (struct DW1000 *dw) |
check new data and compute with the proper algorithm More... | |
static void | process_data (struct DW1000 *dw) |
void | dw1000_reset_heading_ref (void) |
Reset reference heading to current heading AHRS/INS should be aligned before calling this function. More... | |
void | dw1000_arduino_init (void) |
void | dw1000_arduino_periodic (void) |
void | dw1000_arduino_report (void) |
void | dw1000_arduino_event (void) |
void | dw1000_arduino_update_ekf_q (float v) |
settings handler More... | |
void | dw1000_arduino_update_ekf_r_dist (float v) |
void | dw1000_arduino_update_ekf_r_speed (float v) |
Variables | |
static struct DW1000 | dw1000 |
bool | dw1000_use_ekf |
enable EKF filtering More... | |
float | dw1000_ekf_q |
process and measurements noise More... | |
float | dw1000_ekf_r_dist |
float | dw1000_ekf_r_speed |
static const uint16_t | ids [] = DW1000_ANCHORS_IDS |
init arrays from airframe file More... | |
static const float | pos_x [] = DW1000_ANCHORS_POS_X |
static const float | pos_y [] = DW1000_ANCHORS_POS_Y |
static const float | pos_z [] = DW1000_ANCHORS_POS_Z |
static const float | offset [] = DW1000_OFFSET |
static const float | scale [] = DW1000_SCALE |
Definition in file dw1000_arduino.c.
struct DW1000 |
DW1000 positionning system structure.
Definition at line 166 of file dw1000_arduino.c.
Data Fields | ||
---|---|---|
struct Anchor | anchors[DW1000_NB_ANCHORS] | anchors data |
uint8_t | buf[DW_NB_DATA] | incoming data buffer |
uint8_t | ck | checksum |
struct EKFRange | ekf_range | EKF filter. |
bool | ekf_running | EKF logic status. |
struct GpsState | gps_dw1000 | "fake" gps structure |
uint8_t | idx | buffer index |
float | initial_heading | initial heading correction |
struct LtpDef_i | ltp_def | ltp reference |
struct MedianFilterFloat | mf[DW1000_NB_ANCHORS] | median filter for EKF input data |
struct EnuCoor_f | pos | local pos in anchors frame |
float | raw_dist[DW1000_NB_ANCHORS] | raw distance from anchors |
struct EnuCoor_f | speed | local speed in anchors frame |
uint8_t | state | parser state |
bool | updated | new anchor data available |
#define DW1000_INITIAL_HEADING 0.f |
default initial heading correction between anchors frame and global frame
Definition at line 79 of file dw1000_arduino.c.
Referenced by dw1000_arduino_init().
#define DW1000_NB_ANCHORS 3 |
Number of anchors.
using standard trilateration algorithm, only 3 anchors are required/supported at the moment. More advanced multilateration algorithms might allow more anchors in the future
Definition at line 64 of file dw1000_arduino.c.
Referenced by check_anchor_timeout(), check_and_compute_data(), dw1000_arduino_init(), and fill_anchor().
#define DW1000_NOISE_X 0.1f |
DW1000 Noise.
Definition at line 89 of file dw1000_arduino.c.
#define DW1000_NOISE_Y 0.1f |
Definition at line 93 of file dw1000_arduino.c.
#define DW1000_NOISE_Z 0.1f |
Definition at line 97 of file dw1000_arduino.c.
#define DW1000_OFFSET { 0.f, 0.f, 0.f } |
default offset, applied to individual distances
Definition at line 69 of file dw1000_arduino.c.
#define DW1000_SCALE { 1.f, 1.f, 1.f } |
default scale factor, applied to individual distances
Definition at line 74 of file dw1000_arduino.c.
#define DW1000_TIMEOUT 500 |
default timeout (in ms)
Definition at line 84 of file dw1000_arduino.c.
Referenced by check_and_compute_data().
#define DW1000_USE_AS_GPS FALSE |
TRUE if dw1000 are used as GPS.
Definition at line 49 of file dw1000_arduino.c.
#define DW1000_USE_AS_LOCAL_POS TRUE |
TRUE if dw1000 are used as local position estimate.
Definition at line 44 of file dw1000_arduino.c.
#define DW1000_USE_EKF TRUE |
TRUE if EKF range filter is use.
Definition at line 54 of file dw1000_arduino.c.
Referenced by dw1000_arduino_init().
#define DW1000_VEL_NOISE_X 0.1f |
Definition at line 101 of file dw1000_arduino.c.
#define DW1000_VEL_NOISE_Y 0.1f |
Definition at line 105 of file dw1000_arduino.c.
#define DW1000_VEL_NOISE_Z 0.1f |
Definition at line 109 of file dw1000_arduino.c.
#define DW_GET_CK 2 |
Definition at line 162 of file dw1000_arduino.c.
Referenced by dw1000_arduino_parse().
#define DW_GET_DATA 1 |
Definition at line 161 of file dw1000_arduino.c.
Referenced by dw1000_arduino_parse().
#define DW_NB_DATA 6 |
Definition at line 163 of file dw1000_arduino.c.
Referenced by dw1000_arduino_parse().
#define DW_STX 0xFE |
waypoints to use as anchors in simulation
ChibiOS SD logger frame sync byte
Definition at line 157 of file dw1000_arduino.c.
Referenced by dw1000_arduino_parse().
#define DW_WAIT_STX 0 |
Parsing states.
Definition at line 160 of file dw1000_arduino.c.
Referenced by dw1000_arduino_init(), and dw1000_arduino_parse().
|
static |
check timeout for each anchor
dw | DW1000 struct |
timeout | timeout in seconds |
Definition at line 353 of file dw1000_arduino.c.
References DW1000::anchors, DW1000_NB_ANCHORS, get_sys_time_float(), and Anchor::time.
Referenced by check_and_compute_data().
|
inlinestatic |
check new data and compute with the proper algorithm
Definition at line 367 of file dw1000_arduino.c.
References DW1000::anchors, check_anchor_timeout(), Anchor::distance, DW1000_NB_ANCHORS, DW1000_TIMEOUT, dw1000_use_ekf, DW1000::ekf_range, ekf_range_get_pos(), ekf_range_get_speed(), ekf_range_set_state(), ekf_range_update_dist(), DW1000::ekf_running, Anchor::pos, DW1000::pos, DW1000::speed, timeout, trilateration_compute(), and Anchor::updated.
Referenced by process_data().
void dw1000_arduino_event | ( | void | ) |
Definition at line 617 of file dw1000_arduino.c.
References dw1000, dw1000_arduino_parse(), process_data(), uart_char_available(), and uart_getch().
void dw1000_arduino_init | ( | void | ) |
Definition at line 513 of file dw1000_arduino.c.
References LlaCoor_i::alt, DW1000::anchors, GpsState::cacc, DW1000::ck, GpsState::comp_id, Anchor::distance, dw1000, dw1000_ekf_q, dw1000_ekf_r_dist, dw1000_ekf_r_speed, DW1000_INITIAL_HEADING, DW1000_NB_ANCHORS, DW1000_USE_EKF, dw1000_use_ekf, DW_WAIT_STX, DW1000::ekf_range, ekf_range_init(), DW1000::ekf_running, GpsState::fix, DW1000::gps_dw1000, GPS_DW1000_ID, GPS_FIX_NONE, Anchor::id, ids, DW1000::idx, init_median_filter_f(), DW1000::initial_heading, LlaCoor_i::lat, LlaCoor_i::lon, DW1000::ltp_def, ltp_def_from_lla_i(), DW1000::mf, GpsState::pacc, GpsState::pdop, Anchor::pos, DW1000::pos, pos_x, pos_y, pos_z, DW1000::raw_dist, GpsState::sacc, DW1000::speed, DW1000::state, Anchor::time, trilateration_init(), Anchor::updated, DW1000::updated, EnuCoor_f::x, EnuCoor_f::y, and EnuCoor_f::z.
Data parsing function.
Definition at line 236 of file dw1000_arduino.c.
References DW1000::buf, DW1000::ck, DW_GET_CK, DW_GET_DATA, DW_NB_DATA, DW_STX, DW_WAIT_STX, fill_anchor(), DW1000::idx, and DW1000::state.
Referenced by dw1000_arduino_event().
void dw1000_arduino_periodic | ( | void | ) |
Definition at line 568 of file dw1000_arduino.c.
References dw1000, dw1000_use_ekf, DW1000::ekf_range, ekf_range_get_pos(), ekf_range_get_speed(), ekf_range_predict(), DW1000::ekf_running, DW1000::gps_dw1000, gps_periodic_check(), log_started, DW1000::pos, pprzLogFile, process_data(), and DW1000::speed.
void dw1000_arduino_report | ( | void | ) |
Definition at line 599 of file dw1000_arduino.c.
References DW1000::anchors, DefaultChannel, DefaultDevice, Anchor::distance, dw1000, DW1000::pos, DW1000::raw_dist, DW1000::speed, EnuCoor_f::x, EnuCoor_f::y, and EnuCoor_f::z.
void dw1000_arduino_update_ekf_q | ( | float | v | ) |
settings handler
Definition at line 630 of file dw1000_arduino.c.
References dw1000, dw1000_ekf_q, dw1000_ekf_r_dist, dw1000_ekf_r_speed, DW1000::ekf_range, and ekf_range_update_noise().
void dw1000_arduino_update_ekf_r_dist | ( | float | v | ) |
Definition at line 636 of file dw1000_arduino.c.
References dw1000, dw1000_ekf_q, dw1000_ekf_r_dist, dw1000_ekf_r_speed, DW1000::ekf_range, and ekf_range_update_noise().
void dw1000_arduino_update_ekf_r_speed | ( | float | v | ) |
Definition at line 642 of file dw1000_arduino.c.
References dw1000, dw1000_ekf_q, dw1000_ekf_r_dist, dw1000_ekf_r_speed, DW1000::ekf_range, and ekf_range_update_noise().
void dw1000_reset_heading_ref | ( | void | ) |
Reset reference heading to current heading AHRS/INS should be aligned before calling this function.
Definition at line 463 of file dw1000_arduino.c.
References dw1000, DW1000::initial_heading, FloatEulers::psi, and stateGetNedToBodyEulers_f().
|
static |
Utility function to fill anchor from buffer.
Definition at line 218 of file dw1000_arduino.c.
References DW1000::anchors, DW1000::buf, Anchor::distance, DW1000_NB_ANCHORS, float_from_buf(), get_sys_time_float(), Anchor::id, DW1000::mf, offset, DW1000::raw_dist, scale, Anchor::time, uint16_from_buf(), update_median_filter_f(), Anchor::updated, and DW1000::updated.
Referenced by dw1000_arduino_parse().
|
inlinestatic |
Utility function to get float from buffer.
Definition at line 204 of file dw1000_arduino.c.
Referenced by fill_anchor().
|
static |
Definition at line 417 of file dw1000_arduino.c.
References DW1000::anchors, check_and_compute_data(), Anchor::distance, dw1000, log_started, MAT33_ELMT, FloatRates::p, DW1000::pos, pprzLogFile, FloatRates::q, FloatRates::r, stateGetBodyRates_f(), stateGetNedToBodyRMat_f(), stateGetPositionEnu_f(), stateGetSpeedEnu_f(), Anchor::time, DW1000::updated, EnuCoor_f::x, EnuCoor_f::y, and EnuCoor_f::z.
Referenced by dw1000_arduino_event(), and dw1000_arduino_periodic().
Utility function to get uint16_t from buffer.
Definition at line 211 of file dw1000_arduino.c.
Referenced by fill_anchor().
|
static |
Definition at line 187 of file dw1000_arduino.c.
Referenced by dw1000_arduino_event(), dw1000_arduino_init(), dw1000_arduino_periodic(), dw1000_arduino_report(), dw1000_arduino_update_ekf_q(), dw1000_arduino_update_ekf_r_dist(), dw1000_arduino_update_ekf_r_speed(), dw1000_reset_heading_ref(), and process_data().
float dw1000_ekf_q |
process and measurements noise
Definition at line 190 of file dw1000_arduino.c.
Referenced by dw1000_arduino_init(), dw1000_arduino_update_ekf_q(), dw1000_arduino_update_ekf_r_dist(), and dw1000_arduino_update_ekf_r_speed().
float dw1000_ekf_r_dist |
Definition at line 191 of file dw1000_arduino.c.
Referenced by dw1000_arduino_init(), dw1000_arduino_update_ekf_q(), dw1000_arduino_update_ekf_r_dist(), and dw1000_arduino_update_ekf_r_speed().
float dw1000_ekf_r_speed |
Definition at line 192 of file dw1000_arduino.c.
Referenced by dw1000_arduino_init(), dw1000_arduino_update_ekf_q(), dw1000_arduino_update_ekf_r_dist(), and dw1000_arduino_update_ekf_r_speed().
bool dw1000_use_ekf |
enable EKF filtering
Definition at line 189 of file dw1000_arduino.c.
Referenced by check_and_compute_data(), dw1000_arduino_init(), and dw1000_arduino_periodic().
|
static |
init arrays from airframe file
Definition at line 195 of file dw1000_arduino.c.
Referenced by dw1000_arduino_init().
|
static |
Definition at line 199 of file dw1000_arduino.c.
Referenced by e_identification_fr_periodic(), fill_anchor(), offset_commands(), parse_xsens700_msg(), parse_xsens_msg(), put_lat_lon_home(), rtp_frame_send(), and w5100_send().
|
static |
Definition at line 196 of file dw1000_arduino.c.
Referenced by dw1000_arduino_init().
|
static |
Definition at line 197 of file dw1000_arduino.c.
Referenced by dw1000_arduino_init().
|
static |
Definition at line 198 of file dw1000_arduino.c.
Referenced by dw1000_arduino_init().
|
static |
Definition at line 200 of file dw1000_arduino.c.
Referenced by detection_cb(), dnrm2(), fill_anchor(), h(), lla_of_utm_d(), lla_of_utm_f(), xnrm2(), and xnrm2_f().