|
Paparazzi UAS
v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
|
Go to the documentation of this file.
30 #include "pprzlink/messages.h"
36 #include "generated/flight_plan.h"
37 #include "generated/airframe.h"
43 #ifndef DW1000_USE_AS_LOCAL_POS
44 #define DW1000_USE_AS_LOCAL_POS TRUE
48 #ifndef DW1000_USE_AS_GPS
49 #define DW1000_USE_AS_GPS FALSE
53 #ifndef DW1000_USE_EKF
54 #define DW1000_USE_EKF TRUE
63 #ifndef DW1000_NB_ANCHORS
64 #define DW1000_NB_ANCHORS 3
69 #define DW1000_OFFSET { 0.f, 0.f, 0.f }
74 #define DW1000_SCALE { 1.f, 1.f, 1.f }
78 #ifndef DW1000_INITIAL_HEADING
79 #define DW1000_INITIAL_HEADING 0.f
83 #ifndef DW1000_TIMEOUT
84 #define DW1000_TIMEOUT 500
88 #ifndef DW1000_NOISE_X
89 #define DW1000_NOISE_X 0.1f
92 #ifndef DW1000_NOISE_Y
93 #define DW1000_NOISE_Y 0.1f
96 #ifndef DW1000_NOISE_Z
97 #define DW1000_NOISE_Z 0.1f
100 #ifndef DW1000_VEL_NOISE_X
101 #define DW1000_VEL_NOISE_X 0.1f
104 #ifndef DW1000_VEL_NOISE_Y
105 #define DW1000_VEL_NOISE_Y 0.1f
108 #ifndef DW1000_VEL_NOISE_Z
109 #define DW1000_VEL_NOISE_Z 0.1f
116 #define DW1000_EKF_UNINIT 0
117 #define DW1000_EKF_POS_INIT 1
118 #define DW1000_EKF_RUNNING 2
120 #ifndef DW1000_EKF_P0_POS
121 #define DW1000_EKF_P0_POS 1.0f
124 #ifndef DW1000_EKF_P0_SPEED
125 #define DW1000_EKF_P0_SPEED 1.0f
129 #define DW1000_EKF_Q 4.0f
132 #ifndef DW1000_EKF_R_DIST
133 #define DW1000_EKF_R_DIST 0.1f
136 #ifndef DW1000_EKF_R_SPEED
137 #define DW1000_EKF_R_SPEED 0.1f
145 #ifndef DW1000_ANCHOR_SIM_WP
146 #define DW1000_ANCHOR_SIM_WP { WP_ANCHOR_1, WP_ANCHOR_2, WP_ANCHOR_3 }
160 #define DW_WAIT_STX 0
161 #define DW_GET_DATA 1
196 static const float pos_x[] = DW1000_ANCHORS_POS_X;
197 static const float pos_y[] = DW1000_ANCHORS_POS_Y;
198 static const float pos_z[] = DW1000_ANCHORS_POS_Z;
206 memcpy((
uint8_t*)(&
f),
b,
sizeof(
float));
272 #if DW1000_USE_AS_GPS
273 static void send_gps_dw1000_small(
struct DW1000 *dw)
309 #if defined(SECONDARY_GPS) && AHRS_USE_GPS_HEADING
331 #if DW1000_USE_AS_LOCAL_POS
332 static void send_pos_estimate(
struct DW1000 *dw)
399 struct EnuCoor_f speed = { 0.f, 0.f, 0.f };
422 #if DW1000_USE_AS_GPS
424 send_gps_dw1000_small(dw);
426 #if DW1000_USE_AS_LOCAL_POS
428 send_pos_estimate(dw);
437 float omega_z = -rates->
p *
MAT33_ELMT(*ned_to_body, 2, 0)
440 sdLogWriteLog(
pprzLogFile,
"%.3f %.3f %.3f %3.f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",
467 dw1000_arduino_dw1000_reset_heading_ref_status = MODULES_STOP;
471 static const uint8_t wp_ids[] = DW1000_ANCHOR_SIM_WP;
473 #define DW1000_SITL_SYNC FALSE
475 static void compute_anchors_dist_from_wp(
struct DW1000 *dw)
477 #if !DW1000_SITL_SYNC
547 llh_nav0.
lat = NAV_LAT0;
548 llh_nav0.
lon = NAV_LON0;
550 llh_nav0.
alt = NAV_ALT0 + NAV_MSL0;
562 DW1000_EKF_Q, DW1000_EKF_R_DIST, DW1000_EKF_R_SPEED, 0.1f);
581 compute_anchors_dist_from_wp(&
dw1000);
584 #if DW1000_USE_AS_GPS
592 "d1 t1 d2 t2 d3 t3 x y z gps_x gps_y gps_z vx vy vz omega\n");
VIC slots used for the LPC2148 define name e g gps UART1_VIC_SLOT e g modem SPI1_VIC_SLOT SPI1 in mcu_periph spi_arch c or spi_slave_hs_arch c(and some others not using the SPI peripheral yet..) I2C0_VIC_SLOT 8 mcu_periph/i2c_arch.c I2C1_VIC_SLOT 9 mcu_periph/i2c_arch.c USB_VIC_SLOT 10 usb
int32_t lon
in degrees*1e7
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
void dw1000_arduino_periodic(void)
#define VECT3_NED_OF_ENU(_o, _i)
#define DW_STX
waypoints to use as anchors in simulation
int32_t alt
in millimeters above WGS84 reference ellipsoid
static bool check_anchor_timeout(struct DW1000 *dw, float timeout)
check timeout for each anchor
uint8_t uart_getch(struct uart_periph *p)
void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in)
Convert a ECEF to LLA.
static const float scale[]
uint32_t tow
GPS time of week in ms.
#define DW1000_VEL_NOISE_Z
definition of the local (flat earth) coordinate system
static float get_sys_time_float(void)
Get the time in seconds since startup.
static const uint16_t ids[]
init arrays from airframe file
data structure for GPS information
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
void dw1000_arduino_update_ekf_q(float v)
settings handler
struct MedianFilterFloat mf[DW1000_NB_ANCHORS]
median filter for EKF input data
uint32_t pacc
position accuracy in cm
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
int uart_char_available(struct uart_periph *p)
Check UART for available chars in receive buffer.
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
static void dw1000_arduino_parse(struct DW1000 *dw, uint8_t c)
Data parsing function.
static struct DW1000 dw1000
bool updated
new data available
uint32_t sacc
speed accuracy in cm/s
#define VECT3_DIFF(_c, _a, _b)
void dw1000_reset_heading_ref(void)
Reset reference heading to current heading AHRS/INS should be aligned before calling this function.
void ecef_of_enu_vect_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu)
Rotate a vector from ENU to ECEF.
#define FLOAT_VECT3_NORM(_v)
struct EnuCoor_f ekf_range_get_speed(struct EKFRange *ekf_range)
Get current speed.
#define MAT33_ELMT(_m, _row, _col)
struct EnuCoor_f ekf_range_get_pos(struct EKFRange *ekf_range)
Get current pos.
struct LtpDef_i ltp_def
ltp reference
struct NedCoor_i ned_vel
speed NED in cm/s
struct EKFRange ekf_range
EKF filter.
uint16_t u16
Unsigned 16-bit integer.
void dw1000_arduino_report(void)
#define DW1000_SCALE
default scale factor, applied to individual distances
#define GPS_VALID_POS_LLA_BIT
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
static float float_from_buf(uint8_t *b)
Utility function to get float from buffer.
int32_t lat
in degrees*1e7
void dw1000_arduino_event(void)
#define DW1000_INITIAL_HEADING
default initial heading correction between anchors frame and global frame
uint32_t last_msg_ticks
cpu time ticks at last received GPS message
#define DW_WAIT_STX
Parsing states.
float initial_heading
initial heading correction
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
DW1000 positionning system structure.
static float float_vect3_norm(struct FloatVect3 *v)
struct EnuCoor_f speed
local speed in anchors frame
struct EnuCoor_f pos
local pos in anchors frame
Device independent GPS code (interface)
static const float pos_z[]
float raw_dist[DW1000_NB_ANCHORS]
raw distance from anchors
float dw1000_ekf_q
process and measurements noise
void dw1000_arduino_init(void)
bool dw1000_use_ekf
enable EKF filtering
void dw1000_arduino_update_ekf_r_dist(float v)
uint16_t speed_3d
norm of 3d speed in cm/s
void ekf_range_update_noise(struct EKFRange *ekf_range, float Q_sigma2, float R_dist, float R_speed)
Update process and measurement noises.
#define DW1000_VEL_NOISE_X
uint8_t state
parser state
#define FLOAT_VECT2_NORM(_v)
#define DW1000_VEL_NOISE_Y
int trilateration_init(struct Anchor *anchors)
Init internal trilateration structures.
struct GpsState gps_dw1000
"fake" gps structure
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
uint8_t buf[DW_NB_DATA]
incoming data buffer
uint16_t gspeed
norm of 2d ground speed in cm/s
#define DW1000_USE_EKF
TRUE if EKF range filter is use.
vector in East North Up coordinates Units: meters
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
void gps_periodic_check(struct GpsState *gps_s)
Periodic GPS check.
uint32_t last_3dfix_ticks
cpu time ticks at last valid 3D fix
static uint16_t uint16_from_buf(uint8_t *b)
Utility function to get uint16_t from buffer.
uint8_t comp_id
id of current gps
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
static void process_data(struct DW1000 *dw)
Common code for AP and FBW telemetry.
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
struct EnuCoor_f pos
position of the anchor
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
void ekf_range_set_state(struct EKFRange *ekf_range, struct EnuCoor_f pos, struct EnuCoor_f speed)
Set initial state vector.
#define DW1000_NB_ANCHORS
Number of anchors.
#define DW1000_TIMEOUT
default timeout (in ms)
#define DW1000_OFFSET
default offset, applied to individual distances
uint32_t last_msg_time
cpu time in sec at last received GPS message
struct EcefCoor_i ecef_pos
position in ECEF in cm
#define DW1000_NOISE_X
DW1000 Noise.
void dw1000_arduino_update_ekf_r_speed(float v)
#define WaypointAlt(_wp)
waypoint altitude in m above MSL
static const float offset[]
void ekf_range_update_dist(struct EKFRange *ekf_range, float dist, struct EnuCoor_f anchor)
correction step
static const float pos_x[]
static const float pos_y[]
bool ekf_running
EKF logic status.
#define GPS_VALID_VEL_NED_BIT
vector in Latitude, Longitude and Altitude
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
int32_t hmsl
Height above mean sea level in mm.
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
#define GPS_VALID_VEL_ECEF_BIT
uint8_t num_sv
number of sat in fix
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
int32_t hmsl
height above mean sea level (MSL) in mm
struct GpsState gps_datalink
int trilateration_compute(struct Anchor *anchors, struct EnuCoor_f *pos)
Compute trilateration based on the latest measurments.
uint16_t pdop
position dilution of precision scaled by 100
void ecef_of_enu_point_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu)
Convert a point in local ENU to ECEF.
float time
time of the last received data
struct Anchor anchors[DW1000_NB_ANCHORS]
anchors data
#define GPS_VALID_POS_ECEF_BIT
uint32_t cacc
course accuracy in rad*1e7
float distance
last measured distance
volatile uint32_t nb_sec
full seconds since startup
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
static void fill_anchor(struct DW1000 *dw)
Utility function to fill anchor from buffer.
#define GPS_FIX_NONE
No GPS fix.
#define GPS_FIX_3D
3D GPS fix
void ekf_range_init(struct EKFRange *ekf_range, float P0_pos, float P0_speed, float Q_sigma2, float R_dist, float R_speed, float dt)
Init EKF_range internal struct.
void ekf_range_predict(struct EKFRange *ekf_range)
propagate dynamic model
vector in East North Up coordinates
#define GPS_VALID_HMSL_BIT
static bool check_and_compute_data(struct DW1000 *dw)
check new data and compute with the proper algorithm
bool updated
new anchor data available