38#if PREFLIGHT_CHECKS && !defined(AUTOPILOT_DISABLE_AHRS_KILL)
60 state.speed_status = 0;
61 state.accel_status = 0;
62 state.ned_to_body_orientation.status = 0;
63 state.rate_status = 0;
64 state.wind_air_status = 0;
65 state.ned_initialized_i =
false;
66 state.ned_initialized_f =
false;
67 state.utm_initialized_f =
false;
76#ifdef FIXEDWING_FIRMWARE
77 state.fixed_wing_flight =
true;
79 state.fixed_wing_flight =
false;
83 state.utm_origin_f.zone = 0;
86#if PREFLIGHT_CHECKS && !defined(AUTOPILOT_DISABLE_AHRS_KILL)
95 state.origin_input_filter = flag;
98 state.pos_input_filter = flag;
101 state.speed_input_filter = flag;
104 state.accel_input_filter = flag;
107 state.attitude_input_filter = flag;
110 state.rates_input_filter = flag;
113 state.wind_air_input_filter = flag;
133 if (
state.ned_initialized_i) {
134 return state.ned_origin_i.lla;
135 }
else if (
state.ned_initialized_f) {
136 LLA_BFP_OF_REAL(lla_i, state.ned_origin_f.lla);
138 }
else if (
state.utm_initialized_f) {
139 struct LlaCoor_f lla_f;
140 lla_of_utm_f(&lla_f, &state.utm_origin_f);
141 LLA_BFP_OF_REAL(lla_i, lla_f);
152 if (
state.ned_initialized_f) {
153 return state.ned_origin_f.lla;
154 }
else if (
state.ned_initialized_i) {
155 LLA_FLOAT_OF_BFP(lla_f, state.ned_origin_i.lla);
157 }
else if (
state.utm_initialized_f) {
158 lla_of_utm_f(&lla_f, &state.utm_origin_f);
169 if (
state.ned_initialized_i) {
170 return state.ned_origin_i.ecef;
171 }
else if (
state.ned_initialized_f) {
172 ECEF_BFP_OF_REAL(ecef_i, state.ned_origin_f.ecef);
184 if (
state.ned_initialized_f) {
185 return state.ned_origin_f.ecef;
186 }
else if (
state.ned_initialized_i) {
187 ECEF_FLOAT_OF_BFP(ecef_f, state.ned_origin_i.ecef);
198 if (
state.ned_initialized_i) {
199 return state.ned_origin_i.hmsl;
200 }
else if (
state.ned_initialized_f) {
202 }
else if (
state.utm_initialized_f) {
212 if (
state.ned_initialized_f) {
213 return state.ned_origin_f.hmsl;
214 }
else if (
state.ned_initialized_i) {
216 }
else if (
state.utm_initialized_f) {
217 return state.utm_origin_f.alt;
262 if (
state.ned_initialized_i) {
290 }
else if (
state.utm_initialized_f) {
342 if (
state.ned_initialized_i) {
370 }
else if (
state.utm_initialized_f) {
441 }
else if (
state.ned_initialized_i) {
469 }
else if (
state.utm_initialized_f) {
533 }
else if (
state.utm_initialized_f) {
595 if (
state.ned_initialized_f) {
617 }
else if (
state.utm_initialized_f) {
662 if (
state.ned_initialized_f) {
690 }
else if (
state.utm_initialized_f) {
746 }
else if (
state.ned_initialized_f) {
774 }
else if (
state.utm_initialized_f) {
836 if (
state.ned_initialized_i) {
855 }
else if (
state.utm_initialized_f) {
886 if (
state.ned_initialized_i) {
906 }
else if (
state.utm_initialized_f) {
1040 if (
state.ned_initialized_f) {
1059 }
else if (
state.utm_initialized_f) {
1090 if (
state.ned_initialized_f) {
1109 }
else if (
state.utm_initialized_f) {
1194 state.h_speed_dir_f = atan2f(
state.ned_speed_f.y,
state.ned_speed_f.x);
1196 state.h_speed_dir_f = atan2f(
state.enu_speed_f.x,
state.enu_speed_f.y);
1200 state.h_speed_dir_f = atan2f(
state.ned_speed_f.y,
state.ned_speed_f.x);
1204 state.h_speed_dir_f = atan2f(
state.enu_speed_f.x,
state.enu_speed_f.y);
1233 else if (
state.ned_initialized_i) {
1266 else if (
state.ned_initialized_i) {
1298 else if (
state.ned_initialized_f) {
1331 else if (
state.ned_initialized_f) {
if(GpsFixValid() &&e_identification_started)
#define FLOAT_VECT2_NORM(_v)
#define RATES_BFP_OF_REAL(_ri, _rf)
#define SPEEDS_BFP_OF_REAL(_ef, _ei)
#define ACCELS_BFP_OF_REAL(_ef, _ei)
#define RATES_FLOAT_OF_BFP(_rf, _ri)
#define SPEEDS_FLOAT_OF_BFP(_ef, _ei)
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
uint32_t int32_sqrt(uint32_t in)
#define INT32_COURSE_NORMALIZE(_a)
#define SPEED_FLOAT_OF_BFP(_ai)
#define SPEED_BFP_OF_REAL(_af)
#define UTM_OF_NED_ADD(_utm, _pos, _utm0)
#define ENU_OF_UTM_DIFF(_pos, _utm1, _utm2)
#define NED_OF_UTM_DIFF(_pos, _utm1, _utm2)
#define UTM_OF_ENU_ADD(_utm, _pos, _utm0)
void ned_of_lla_pos_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCoor_i *lla)
Convert a point from LLA to local NED.
#define ECEF_FLOAT_OF_BFP(_o, _i)
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
Convert a LLA to ECEF.
#define ENU_FLOAT_OF_BFP(_o, _i)
#define ECEF_BFP_OF_REAL(_o, _i)
#define ENU_BFP_OF_REAL(_o, _i)
#define LLA_BFP_OF_REAL(_o, _i)
#define NED_BFP_OF_REAL(_o, _i)
void ned_of_ecef_pos_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Convert a ECEF position to local NED.
#define NED_FLOAT_OF_BFP(_o, _i)
void enu_of_ecef_pos_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Convert a ECEF position to local ENU.
#define INT32_VECT3_ENU_OF_NED(_o, _i)
#define VECT3_ENU_OF_NED(_o, _i)
void ecef_of_enu_pos_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu)
Convert a local ENU position to ECEF.
#define VECT3_NED_OF_ENU(_o, _i)
void ecef_of_ned_vect_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct NedCoor_i *ned)
Rotate a vector from NED to ECEF.
void ned_of_ecef_vect_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Rotate a vector from ECEF to NED.
#define LLA_FLOAT_OF_BFP(_o, _i)
#define INT32_VECT3_NED_OF_ENU(_o, _i)
void enu_of_lla_pos_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla)
Convert a point from LLA to local ENU.
void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in)
Convert a ECEF to LLA.
void enu_of_ecef_vect_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Rotate a vector from ECEF to ENU.
void ecef_of_ned_pos_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct NedCoor_i *ned)
Convert a local NED position to ECEF.
vector in EarthCenteredEarthFixed coordinates
vector in Latitude, Longitude and Altitude
void stateCalcAccelNed_i(void)
void stateCalcAccelEcef_i(void)
void stateCalcAccelEcef_f(void)
void stateCalcAccelNed_f(void)
static bool stateIsAttitudeValid(void)
Test if attitudes are valid.
void stateSetInputFilter(uint8_t type, uint16_t flag)
set the input filter for a specified type of data.
Structure holding vehicle state data.
void stateCalcPositionNed_f(void)
struct EcefCoor_f stateGetEcefOrigin_f(void)
Get the ECEF position of the frame origin (float).
void stateCalcPositionUtm_f(void)
void stateCalcPositionEnu_f(void)
void stateCalcPositionEcef_f(void)
void stateCalcPositionLla_f(void)
int32_t stateGetHmslOrigin_i(void)
Get the HMSL of the frame origin (int).
void stateCalcPositionEcef_i(void)
float stateGetHmslOrigin_f(void)
Get the HMSL of the frame origin (float).
struct LlaCoor_i stateGetLlaOrigin_i(void)
Get the LLA position of the frame origin (int).
void stateCalcPositionNed_i(void)
void stateCalcPositionLla_i(void)
Calculate LLA (int) from any other available representation.
struct LlaCoor_f stateGetLlaOrigin_f(void)
Get the LLA position of the frame origin (float).
struct EcefCoor_i stateGetEcefOrigin_i(void)
Get the ECEF position of the frame origin (int).
void stateCalcPositionEnu_i(void)
void stateCalcBodyRates_f(void)
void stateCalcBodyRates_i(void)
void stateCalcSpeedEnu_f(void)
void stateCalcSpeedNed_f(void)
void stateCalcHorizontalSpeedDir_i(void)
void stateCalcHorizontalSpeedNorm_f(void)
void stateCalcHorizontalSpeedNorm_i(void)
void stateCalcSpeedEcef_f(void)
void stateCalcSpeedEnu_i(void)
void stateCalcHorizontalSpeedDir_f(void)
void stateCalcSpeedNed_i(void)
void stateCalcSpeedEcef_i(void)
void stateCalcAirspeed_i(void)
void stateCalcVerticalWindspeed_i(void)
void stateCalcAirspeed_f(void)
void stateCalcHorizontalWindspeed_f(void)
void stateCalcHorizontalWindspeed_i(void)
void stateCalcVerticalWindspeed_f(void)
void ecef_of_enu_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct EnuCoor_f *enu)
void enu_of_ecef_point_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct EcefCoor_f *ecef)
void ned_of_ecef_point_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef)
void lla_of_utm_f(struct LlaCoor_f *lla, struct UtmCoor_f *utm)
void ecef_of_ned_vect_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct NedCoor_f *ned)
void ecef_of_lla_f(struct EcefCoor_f *out, struct LlaCoor_f *in)
void ecef_of_ned_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct NedCoor_f *ned)
void enu_of_lla_point_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct LlaCoor_f *lla)
void ned_of_lla_point_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct LlaCoor_f *lla)
void ned_of_ecef_vect_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef)
void enu_of_ecef_vect_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct EcefCoor_f *ecef)
void lla_of_ecef_f(struct LlaCoor_f *out, struct EcefCoor_f *in)
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
vector in EarthCenteredEarthFixed coordinates
vector in Latitude, Longitude and Altitude
int32_t int32_atan2(int32_t y, int32_t x)
void preflight_error(struct preflight_result_t *result, const char *fmt,...)
Register a preflight error used inside the preflight checking functions.
void preflight_success(struct preflight_result_t *result, const char *fmt,...)
Register a preflight success used inside the preflight checking functions.
void preflight_check_register(struct preflight_check_t *check, preflight_check_f func)
Register a preflight check and add it to the linked list.
API to get/set the generic vehicle states.
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.