Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
ins_flow.c File Reference
#include "ins_flow.h"
#include "modules/core/abi.h"
#include "generated/airframe.h"
#include "mcu_periph/sys_time.h"
#include "autopilot.h"
#include "math/pprz_algebra_float.h"
#include "generated/flight_plan.h"
#include "modules/actuators/motor_mixing.h"
#include "stabilization.h"
#include <stdio.h>
#include "modules/datalink/telemetry.h"
#include "state.h"
Include dependency graph for ins_flow.c:

Go to the source code of this file.

Data Structures

struct  InsFlow

Macros

#define DEBUG_INS_FLOW   0
#define DEBUG_PRINT(...)
#define DEBUG_MAT_PRINT(...)
#define AHRS_ICQ_OUTPUT_ENABLED   TRUE
#define INS_FLOW_GYRO_ID   ABI_BROADCAST
#define INS_FLOW_ACCEL_ID   ABI_BROADCAST
#define INS_FLOW_IMU_ID   ABI_BROADCAST
#define INS_FLOW_GPS_ID   GPS_MULTI_ID
#define INS_OPTICAL_FLOW_ID   ABI_BROADCAST
#define INS_RPM_ID   ABI_BROADCAST
#define MOMENT_DELAY   20
#define OF_N_ROTORS   4
#define USE_STANDARD_PARAMS   0
#define PAR_IX   0
#define PAR_MASS   1
#define PAR_BASE   2
#define PAR_K0   3
#define PAR_K1   4
#define PAR_K2   5
#define PAR_K3   6
#define PAR_R0   7
#define PAR_R1   8
#define PAR_Q0   9
#define PAR_Q1   10
#define PAR_Q2   11
#define PAR_Q3   12
#define PAR_Q4   13
#define PAR_P0   14
#define PAR_P1   15
#define PAR_P2   16
#define PAR_P3   17
#define PAR_P4   18
#define PAR_KD   19
#define PAR_Q_TB   20
#define PAR_P_TB   21
#define PAR_PRED_ROLL_1   22
#define PAR_PRED_ROLL_2   23
#define PAR_PRED_ROLL_3   24

Functions

static void gyro_cb (uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
static void accel_cb (uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
static void gps_cb (uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
void ins_optical_flow_cb (uint8_t sender_id, uint32_t stamp, int32_t flow_x, int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence)
static void ins_act_feedback_cb (uint8_t sender_id, struct act_feedback_t *feedback, uint8_t num_act)
static void aligner_cb (uint8_t sender_id, uint32_t stamp, struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
static void reset_cb (uint8_t sender_id UNUSED, uint8_t flag)
static void print_ins_flow_state (void)
static void print_true_state (void)
static void set_body_state_from_quat (void)
 Rotate angles and rates from imu to body frame and set state.
static void ins_reset_filter (void)
static void send_quat (struct transport_tx *trans, struct link_device *dev)
static void send_euler (struct transport_tx *trans, struct link_device *dev)
static void send_bias (struct transport_tx *trans, struct link_device *dev)
static void send_geo_mag (struct transport_tx *trans, struct link_device *dev)
static void send_filter_status (struct transport_tx *trans, struct link_device *dev)
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_flow (struct transport_tx *trans, struct link_device *dev)
void ins_flow_init (void)
void ins_flow_update (void)

Variables

static abi_event gyro_ev
static abi_event accel_ev
static abi_event gps_ev
static abi_event ins_optical_flow_ev
static abi_event ins_act_feedback_ev
static abi_event aligner_ev
static abi_event reset_ev
static uint32_t ahrs_icq_last_stamp
static uint8_t ahrs_flow_id = AHRS_COMP_ID_FLOW
 Component ID for FLOW.
struct InsFlow ins_flow
float moments [MOMENT_DELAY] = {0.}
int moment_ind
float OF_X [N_STATES_OF_KF] = {0.}
float OF_Q [N_STATES_OF_KF][N_STATES_OF_KF] = {{0.}}
float OF_P [N_STATES_OF_KF][N_STATES_OF_KF] = {{0.}}
float OF_R [N_MEAS_OF_KF][N_MEAS_OF_KF] = {{0.}}
float RPM_FACTORS [OF_N_ROTORS]
float of_time
float of_prev_time
float lp_factor
float lp_factor_strong
bool reset_filter
int use_filter
bool run_filter
uint32_t counter
float thrust_factor
float GT_phi
float GT_theta
float parameters [22] = {0.219033, 0.376572, 0.184002, 0.096388, 0.240843, 0.172390, 0.133111, 0.495885, 0.357086, 0.233624, 0.125611, 1.661682, 0.136735, 2.812652, 0.715887, 0.166932, 0.371409, 1.043920, 0.840683, 0.567703, 0.192238, 0.301966}

Data Structure Documentation

◆ InsFlow

struct InsFlow

Definition at line 133 of file ins_flow.c.

Collaboration diagram for InsFlow:
Data Fields
float divergence
float lp_gyro_bias_pitch
float lp_gyro_bias_roll
float lp_gyro_pitch
float lp_gyro_roll
float lp_roll_command
float lp_thrust
struct NedCoor_i ltp_accel
struct LtpDef_i ltp_def
bool ltp_initialized
struct NedCoor_i ltp_pos
struct NedCoor_i ltp_speed
bool new_flow_measurement
float optical_flow_x
float optical_flow_y
uint16_t RPM[8]
uint8_t RPM_num_act
float thrust_factor
float vision_time

Macro Definition Documentation

◆ AHRS_ICQ_OUTPUT_ENABLED

#define AHRS_ICQ_OUTPUT_ENABLED   TRUE

Definition at line 62 of file ins_flow.c.

◆ DEBUG_INS_FLOW

#define DEBUG_INS_FLOW   0

Definition at line 49 of file ins_flow.c.

Referenced by ins_flow_update().

◆ DEBUG_MAT_PRINT

#define DEBUG_MAT_PRINT ( ...)

Definition at line 58 of file ins_flow.c.

Referenced by ins_flow_update().

◆ DEBUG_PRINT

#define DEBUG_PRINT ( ...)

Definition at line 57 of file ins_flow.c.

Referenced by ins_flow_update().

◆ INS_FLOW_ACCEL_ID

#define INS_FLOW_ACCEL_ID   ABI_BROADCAST

Definition at line 74 of file ins_flow.c.

Referenced by ins_flow_init().

◆ INS_FLOW_GPS_ID

#define INS_FLOW_GPS_ID   GPS_MULTI_ID

Definition at line 87 of file ins_flow.c.

Referenced by ins_flow_init().

◆ INS_FLOW_GYRO_ID

#define INS_FLOW_GYRO_ID   ABI_BROADCAST

Definition at line 68 of file ins_flow.c.

Referenced by ins_flow_init().

◆ INS_FLOW_IMU_ID

#define INS_FLOW_IMU_ID   ABI_BROADCAST

Definition at line 80 of file ins_flow.c.

Referenced by ins_flow_init().

◆ INS_OPTICAL_FLOW_ID

#define INS_OPTICAL_FLOW_ID   ABI_BROADCAST

Definition at line 93 of file ins_flow.c.

Referenced by ins_flow_init().

◆ INS_RPM_ID

#define INS_RPM_ID   ABI_BROADCAST

Definition at line 99 of file ins_flow.c.

Referenced by ins_flow_init().

◆ MOMENT_DELAY

#define MOMENT_DELAY   20

Definition at line 168 of file ins_flow.c.

◆ OF_N_ROTORS

#define OF_N_ROTORS   4

Definition at line 177 of file ins_flow.c.

Referenced by ins_flow_update(), and send_ins_flow().

◆ PAR_BASE

#define PAR_BASE   2

Definition at line 257 of file ins_flow.c.

◆ PAR_IX

#define PAR_IX   0

Definition at line 255 of file ins_flow.c.

Referenced by ins_flow_update().

◆ PAR_K0

#define PAR_K0   3

Definition at line 258 of file ins_flow.c.

Referenced by ins_flow_init().

◆ PAR_K1

#define PAR_K1   4

Definition at line 259 of file ins_flow.c.

Referenced by ins_flow_init().

◆ PAR_K2

#define PAR_K2   5

Definition at line 260 of file ins_flow.c.

Referenced by ins_flow_init().

◆ PAR_K3

#define PAR_K3   6

Definition at line 261 of file ins_flow.c.

Referenced by ins_flow_init().

◆ PAR_KD

#define PAR_KD   19

Definition at line 274 of file ins_flow.c.

Referenced by ins_flow_update().

◆ PAR_MASS

#define PAR_MASS   1

Definition at line 256 of file ins_flow.c.

Referenced by ins_flow_update(), and send_ins_flow().

◆ PAR_P0

#define PAR_P0   14

Definition at line 269 of file ins_flow.c.

Referenced by ins_reset_filter().

◆ PAR_P1

#define PAR_P1   15

Definition at line 270 of file ins_flow.c.

Referenced by ins_reset_filter().

◆ PAR_P2

#define PAR_P2   16

Definition at line 271 of file ins_flow.c.

Referenced by ins_reset_filter().

◆ PAR_P3

#define PAR_P3   17

Definition at line 272 of file ins_flow.c.

Referenced by ins_reset_filter().

◆ PAR_P4

#define PAR_P4   18

Definition at line 273 of file ins_flow.c.

Referenced by ins_reset_filter().

◆ PAR_P_TB

#define PAR_P_TB   21

Definition at line 276 of file ins_flow.c.

Referenced by ins_reset_filter().

◆ PAR_PRED_ROLL_1

#define PAR_PRED_ROLL_1   22

Definition at line 277 of file ins_flow.c.

◆ PAR_PRED_ROLL_2

#define PAR_PRED_ROLL_2   23

Definition at line 278 of file ins_flow.c.

◆ PAR_PRED_ROLL_3

#define PAR_PRED_ROLL_3   24

Definition at line 279 of file ins_flow.c.

Referenced by ins_flow_update().

◆ PAR_Q0

#define PAR_Q0   9

Definition at line 264 of file ins_flow.c.

Referenced by ins_flow_init().

◆ PAR_Q1

#define PAR_Q1   10

Definition at line 265 of file ins_flow.c.

Referenced by ins_flow_init().

◆ PAR_Q2

#define PAR_Q2   11

Definition at line 266 of file ins_flow.c.

Referenced by ins_flow_init().

◆ PAR_Q3

#define PAR_Q3   12

Definition at line 267 of file ins_flow.c.

Referenced by ins_flow_init().

◆ PAR_Q4

#define PAR_Q4   13

Definition at line 268 of file ins_flow.c.

Referenced by ins_flow_init().

◆ PAR_Q_TB

#define PAR_Q_TB   20

Definition at line 275 of file ins_flow.c.

Referenced by ins_flow_init().

◆ PAR_R0

#define PAR_R0   7

Definition at line 262 of file ins_flow.c.

Referenced by ins_flow_init().

◆ PAR_R1

#define PAR_R1   8

Definition at line 263 of file ins_flow.c.

Referenced by ins_flow_init().

◆ USE_STANDARD_PARAMS

#define USE_STANDARD_PARAMS   0

Definition at line 194 of file ins_flow.c.

Function Documentation

◆ accel_cb()

void accel_cb ( uint8_t sender_id,
uint32_t stamp,
struct Int32Vect3 * accel )
static

Definition at line 1373 of file ins_flow.c.

References ahrs_icq, ahrs_icq_update_accel(), PRINT_CONFIG_MSG(), PRINT_CONFIG_VAR(), and set_body_state_from_quat().

Here is the call graph for this function:

◆ aligner_cb()

void aligner_cb ( uint8_t sender_id,
uint32_t stamp,
struct Int32Rates * lp_gyro,
struct Int32Vect3 * lp_accel,
struct Int32Vect3 * lp_mag )
static

Definition at line 1530 of file ins_flow.c.

References ahrs_icq, ahrs_icq_align(), and set_body_state_from_quat().

Here is the call graph for this function:

◆ gps_cb()

◆ gyro_cb()

void gyro_cb ( uint8_t sender_id,
uint32_t stamp,
struct Int32Rates * gyro )
static

◆ ins_act_feedback_cb()

void ins_act_feedback_cb ( uint8_t sender_id UNUSED,
struct act_feedback_t * feedback,
uint8_t num_act )
static

Definition at line 1444 of file ins_flow.c.

References ins_flow, act_feedback_t::rpm, and UNUSED.

Referenced by ins_flow_init().

Here is the caller graph for this function:

◆ ins_flow_init()

◆ ins_flow_update()

◆ ins_optical_flow_cb()

void ins_optical_flow_cb ( uint8_t sender_id UNUSED,
uint32_t stamp,
int32_t flow_x UNUSED,
int32_t flow_y UNUSED,
int32_t flow_der_x UNUSED,
int32_t flow_der_y UNUSED,
float quality UNUSED,
float size_divergence )

Definition at line 669 of file ins_flow.c.

References ins_flow, and UNUSED.

Referenced by ins_flow_init().

Here is the caller graph for this function:

◆ ins_reset_filter()

void ins_reset_filter ( void )
static

◆ print_ins_flow_state()

void print_ins_flow_state ( void )
static

Definition at line 688 of file ins_flow.c.

References CONSTANT_ALT_FILTER, OF_ANGLE_DOT_IND, OF_ANGLE_IND, OF_THETA_IND, OF_THRUST_BIAS, OF_THRUST_BIAS_IND, OF_TWO_DIM, OF_V_IND, OF_VX_IND, OF_X, OF_Z_DOT_IND, and OF_Z_IND.

Referenced by ins_flow_update().

Here is the caller graph for this function:

◆ print_true_state()

void print_true_state ( void )
static

Definition at line 713 of file ins_flow.c.

References FloatRates::p, FloatEulers::phi, stateGetBodyRates_f(), stateGetNedToBodyEulers_f(), stateGetPositionNed_f(), stateGetSpeedNed_f(), NedCoor_f::y, and NedCoor_f::z.

Referenced by ins_flow_update().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ reset_cb()

void reset_cb ( uint8_t sender_id UNUSED,
uint8_t flag )
static

Definition at line 657 of file ins_flow.c.

References ecef_int_from_gps(), gps, ins_flow, INS_RESET_REF, ltp_def_from_ecef_i(), stateSetLocalOrigin_i(), and UNUSED.

Referenced by gps_cb().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ send_bias()

void send_bias ( struct transport_tx * trans,
struct link_device * dev )
static

Definition at line 331 of file ins_flow.c.

References ahrs_flow_id, ahrs_icq, and dev.

◆ send_euler()

void send_euler ( struct transport_tx * trans,
struct link_device * dev )
static

Definition at line 315 of file ins_flow.c.

References ahrs_flow_id, ahrs_icq, dev, int32_eulers_of_quat(), Int32Eulers::phi, Int32Eulers::psi, stateGetNedToBodyEulers_i(), and Int32Eulers::theta.

Here is the call graph for this function:

◆ send_filter_status()

void send_filter_status ( struct transport_tx * trans,
struct link_device * dev )
static

Definition at line 348 of file ins_flow.c.

References ahrs_flow_id, ahrs_icq, ahrs_icq_last_stamp, dev, get_sys_time_usec(), and val.

Here is the call graph for this function:

◆ send_geo_mag()

void send_geo_mag ( struct transport_tx * trans,
struct link_device * dev )
static

◆ send_ins()

void send_ins ( struct transport_tx * trans,
struct link_device * dev )
static

Definition at line 360 of file ins_flow.c.

References dev, and ins_flow.

◆ send_ins_flow()

◆ send_ins_ref()

void send_ins_ref ( struct transport_tx * trans,
struct link_device * dev )
static

Definition at line 376 of file ins_flow.c.

References dev, and ins_flow.

◆ send_quat()

void send_quat ( struct transport_tx * trans,
struct link_device * dev )
static

Definition at line 299 of file ins_flow.c.

References ahrs_flow_id, ahrs_icq, dev, Int32Quat::qi, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, and stateGetNedToBodyQuat_i().

Here is the call graph for this function:

◆ set_body_state_from_quat()

void set_body_state_from_quat ( void )
static

Rotate angles and rates from imu to body frame and set state.

Definition at line 1399 of file ins_flow.c.

References ahrs_icq, OrientationReps::eulers_f, GT_phi, GT_theta, OF_ANGLE_IND, OF_THETA_IND, OF_TWO_DIM, OF_X, orientationGetEulers_f(), orientationGetQuat_i(), ORREP_EULER_F, ORREP_QUAT_I, FloatEulers::phi, OrientationReps::quat_i, stateSetBodyRates_i(), stateSetNedToBodyQuat_i(), OrientationReps::status, FloatEulers::theta, USE_ANGLE, and use_filter.

Referenced by accel_cb(), aligner_cb(), gyro_cb(), and ins_flow_init().

Here is the call graph for this function:
Here is the caller graph for this function:

Variable Documentation

◆ accel_ev

abi_event accel_ev
static

Definition at line 105 of file ins_flow.c.

◆ ahrs_flow_id

uint8_t ahrs_flow_id = AHRS_COMP_ID_FLOW
static

Component ID for FLOW.

Definition at line 129 of file ins_flow.c.

Referenced by send_bias(), send_euler(), send_filter_status(), send_geo_mag(), and send_quat().

◆ ahrs_icq_last_stamp

uint32_t ahrs_icq_last_stamp
static

Definition at line 128 of file ins_flow.c.

◆ aligner_ev

abi_event aligner_ev
static

Definition at line 109 of file ins_flow.c.

◆ counter

◆ gps_ev

abi_event gps_ev
static

Definition at line 106 of file ins_flow.c.

◆ GT_phi

float GT_phi

Definition at line 190 of file ins_flow.c.

Referenced by ins_flow_init(), send_ins_flow(), and set_body_state_from_quat().

◆ GT_theta

float GT_theta

Definition at line 191 of file ins_flow.c.

Referenced by ins_flow_init(), send_ins_flow(), and set_body_state_from_quat().

◆ gyro_ev

abi_event gyro_ev
static

Definition at line 104 of file ins_flow.c.

◆ ins_act_feedback_ev

abi_event ins_act_feedback_ev
static

Definition at line 108 of file ins_flow.c.

Referenced by ins_flow_init().

◆ ins_flow

◆ ins_optical_flow_ev

abi_event ins_optical_flow_ev
static

Definition at line 107 of file ins_flow.c.

Referenced by ins_flow_init().

◆ lp_factor

◆ lp_factor_strong

float lp_factor_strong

Definition at line 183 of file ins_flow.c.

Referenced by ins_flow_init(), and ins_flow_update().

◆ moment_ind

int moment_ind

Definition at line 170 of file ins_flow.c.

Referenced by ins_flow_init().

◆ moments

float moments[MOMENT_DELAY] = {0.}

Definition at line 169 of file ins_flow.c.

Referenced by evaluate_obm_moments(), and find_contour().

◆ OF_P

float OF_P[N_STATES_OF_KF][N_STATES_OF_KF] = {{0.}}

Definition at line 174 of file ins_flow.c.

Referenced by ins_flow_update(), and ins_reset_filter().

◆ of_prev_time

float of_prev_time

Definition at line 181 of file ins_flow.c.

Referenced by ins_flow_init(), and ins_flow_update().

◆ OF_Q

float OF_Q[N_STATES_OF_KF][N_STATES_OF_KF] = {{0.}}

Definition at line 173 of file ins_flow.c.

Referenced by ins_flow_init(), and ins_flow_update().

◆ OF_R

float OF_R[N_MEAS_OF_KF][N_MEAS_OF_KF] = {{0.}}

Definition at line 175 of file ins_flow.c.

Referenced by ins_flow_init(), and ins_flow_update().

◆ of_time

float of_time

Definition at line 180 of file ins_flow.c.

Referenced by ins_flow_init(), and ins_flow_update().

◆ OF_X

◆ parameters

float parameters[22] = {0.219033, 0.376572, 0.184002, 0.096388, 0.240843, 0.172390, 0.133111, 0.495885, 0.357086, 0.233624, 0.125611, 1.661682, 0.136735, 2.812652, 0.715887, 0.166932, 0.371409, 1.043920, 0.840683, 0.567703, 0.192238, 0.301966}

◆ reset_ev

abi_event reset_ev
static

Definition at line 110 of file ins_flow.c.

◆ reset_filter

bool reset_filter

Definition at line 184 of file ins_flow.c.

Referenced by ins_flow_init(), and ins_flow_update().

◆ RPM_FACTORS

float RPM_FACTORS[OF_N_ROTORS]

Definition at line 178 of file ins_flow.c.

Referenced by ins_flow_init(), ins_flow_update(), and send_ins_flow().

◆ run_filter

bool run_filter

Definition at line 186 of file ins_flow.c.

Referenced by ins_flow_init(), and ins_flow_update().

◆ thrust_factor

float thrust_factor

Definition at line 188 of file ins_flow.c.

Referenced by ins_flow_update(), and send_ins_flow().

◆ use_filter

int use_filter

Definition at line 185 of file ins_flow.c.

Referenced by gps_cb(), ins_flow_init(), send_ins_flow(), and set_body_state_from_quat().