37static void send_ins(
struct transport_tx *trans,
struct link_device *
dev)
39 pprz_msg_send_INS(trans,
dev, AC_ID,
45static void send_ins_z(
struct transport_tx *trans,
struct link_device *
dev)
47 pprz_msg_send_INS_Z(trans,
dev, AC_ID,
53 if (
ins_vn.ltp_initialized) {
54 pprz_msg_send_INS_REF(trans,
dev, AC_ID,
63 pprz_msg_send_VECTORNAV_INFO(trans,
dev, AC_ID,
65 &
ins_vn.vn_packet.chksm_error,
66 &
ins_vn.vn_packet.hdr_error,
71 &
ins_vn.vn_data.ypr_u.theta,
72 &
ins_vn.vn_data.ypr_u.psi);
75static void send_accel(
struct transport_tx *trans,
struct link_device *
dev)
78 pprz_msg_send_IMU_ACCEL(trans,
dev, AC_ID, &
id,
82static void send_gyro(
struct transport_tx *trans,
struct link_device *
dev)
85 pprz_msg_send_IMU_GYRO(trans,
dev, AC_ID, &
id,
92 pprz_msg_send_IMU_ACCEL_SCALED(trans,
dev, AC_ID, &
id,
99 pprz_msg_send_IMU_GYRO_SCALED(trans,
dev, AC_ID, &
id,
104#ifndef USE_INS_NAV_INIT
105#define USE_INS_NAV_INIT TRUE
121 sec_cnt =
ins_vn.vn_packet.counter - last_cnt;
125 if (
ins_vn.vn_rate < (PERIODIC_FREQUENCY*0.75)) {
132 if (last_pacc !=
gps.pacc) {
133 last_pacc =
gps.pacc;
136 gps.svinfos[0].cno = (
gps.svinfos[0].cno + 1) % 2;
140 last_cnt =
ins_vn.vn_packet.counter;
156 if (
ins_vn.vn_packet.msg_available) {
158 ins_vn.vn_packet.msg_available =
false;
175 ins_vn.vn_packet.msg_idx = 0;
176 ins_vn.vn_packet.msg_available =
false;
177 ins_vn.vn_packet.chksm_error = 0;
178 ins_vn.vn_packet.hdr_error = 0;
179 ins_vn.vn_packet.overrun_error = 0;
180 ins_vn.vn_packet.noise_error = 0;
181 ins_vn.vn_packet.framing_error = 0;
193 ins_vn.ltp_initialized =
true;
195 ins_vn.ltp_initialized =
false;
202#if PERIODIC_TELEMETRY
232 float pacc =
ins_vn.vn_data.pos_u[0];
233 if (
ins_vn.vn_data.pos_u[1] > pacc) {
234 pacc =
ins_vn.vn_data.pos_u[1];
236 if (
ins_vn.vn_data.pos_u[2] > pacc) {
237 pacc =
ins_vn.vn_data.pos_u[2];
266 static struct FloatRMat ltp_to_body_rmat;
286 VECT3_ASSIGN(ltp_accel, accel_meas_ltp.
x, accel_meas_ltp.
y, accel_meas_ltp.
z);
288 ins_vn.ltp_accel_f = ltp_accel;
292 ins_vn.lla_pos.lat = RadOfDeg((
float)
ins_vn.vn_data.pos_lla[0]);
293 ins_vn.lla_pos.lon = RadOfDeg((
float)
ins_vn.vn_data.pos_lla[1]);
294 ins_vn.lla_pos.alt = ((float)
ins_vn.vn_data.pos_lla[2]);
309 float speed = sqrt(
ins_vn.vn_data.vel_ned.x *
ins_vn.vn_data.vel_ned.x +
ins_vn.vn_data.vel_ned.y *
ins_vn.vn_data.vel_ned.y);
static void send_vn_info(struct transport_tx *trans, struct link_device *dev)
static struct uart_periph * dev
struct GpsState gps
global GPS state
#define GPS_VALID_POS_LLA_BIT
#define GPS_FIX_NONE
No GPS fix.
#define GPS_VALID_POS_ECEF_BIT
#define GPS_VALID_HMSL_BIT
#define GPS_FIX_3D
3D GPS fix
#define GPS_VALID_COURSE_BIT
void float_rmat_ratemult(struct FloatRates *rb, const struct FloatRMat *m_a2b, const struct FloatRates *ra)
rotate anglular rates by rotation matrix.
void float_quat_of_eulers(struct FloatQuat *q, const struct FloatEulers *e)
quat of euler roation 'ZYX'
void float_rmat_of_quat(struct FloatRMat *rm, const struct FloatQuat *q)
void float_rmat_transp_vmult(struct FloatVect3 *vb, const struct FloatRMat *m_b2a, const struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
void float_rmat_comp(struct FloatRMat *m_a2c, const struct FloatRMat *m_a2b, const struct FloatRMat *m_b2c)
Composition (multiplication) of two rotation matrices.
#define VECT3_ASSIGN(_a, _x, _y, _z)
#define RATES_BFP_OF_REAL(_ri, _rf)
#define ACCELS_BFP_OF_REAL(_ef, _ei)
#define INT32_VECT3_ZERO(_v)
#define LLA_BFP_OF_REAL(_o, _i)
static float wgs84_ellipsoid_to_geoid_f(float lat, float lon)
Get WGS84 ellipsoid/geoid separation.
static void orientationSetEulers_f(struct OrientationReps *orientation, struct FloatEulers *eulers)
Set vehicle body attitude from euler angles (float).
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
static void stateSetAccelNed_f(uint16_t id, struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
static struct NedCoor_i * stateGetAccelNed_i(void)
Get acceleration in NED coordinates (int).
static void stateSetNedToBodyRMat_f(uint16_t id, struct FloatRMat *ned_to_body_rmat)
Set vehicle body attitude from rotation matrix (float).
static struct EcefCoor_i * stateGetPositionEcef_i(void)
Get position in ECEF coordinates (int).
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
static void stateSetPositionLla_i(uint16_t id, struct LlaCoor_i *lla_pos)
Set position from LLA coordinates (int).
static void stateSetBodyRates_f(uint16_t id, struct FloatRates *body_rate)
Set vehicle body angular rate (float).
static void stateSetSpeedNed_f(uint16_t id, struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
static struct NedCoor_i * stateGetSpeedNed_i(void)
Get ground speed in local NED coordinates (int).
static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev)
static void send_gyro(struct transport_tx *trans, struct link_device *dev)
static void send_accel(struct transport_tx *trans, struct link_device *dev)
static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev)
void ins_init_origin_i_from_flightplan(uint16_t id, struct LtpDef_i *ltp_def)
initialize the local origin (ltp_def in fixed point) from flight plan position
static void send_ins(struct transport_tx *trans, struct link_device *dev)
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
void ins_vectornav_monitor(void)
Monitors vectornav data rate and changes GPS lock if the data rate is too low.
static void send_ins(struct transport_tx *trans, struct link_device *dev)
void ins_vectornav_set_pacc(void)
Find maximum uncertainty (NED) position accuracy in cm.
struct InsVectornav ins_vn
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
void ins_vectornav_set_sacc(void)
Set speed (velocity) uncertainty (NED) speed accuracy in cm/s.
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev)
static void send_gyro(struct transport_tx *trans, struct link_device *dev)
void ins_vectornav_init(void)
Initialize Vectornav struct.
void ins_vectornav_event(void)
Event handling for Vectornav.
static void send_vn_info(struct transport_tx *trans, struct link_device *dev)
static void send_accel(struct transport_tx *trans, struct link_device *dev)
static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev)
void ins_vectornav_propagate()
Propagate the received states into the vehicle state machine.
Vectornav VN-200 INS module.
#define INS_VN_BODY_TO_IMU_PHI
#define INS_VN_BODY_TO_IMU_PSI
#define INS_VN_BODY_TO_IMU_THETA
vector in North East Down coordinates Units: meters
volatile uint32_t nb_sec
full seconds since startup
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
void vn200_read_message(struct VNPacket *vn_packet, struct VNData *vn_data)
Read received message and populate data struct with new measurements.
void vn200_event(struct VNPacket *vnp)