Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-02 05:17:03 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

State interface input filter (#3409)

Browse Source 
* [generator] generate unique ID and names table for modules
* [state] add accessors for the state origin
* [state] don't access directly to state origin, use getters
* [state] filter inputs with module ID
* [ahrs] convert AHRS modules to state input filter
- selection of ahrs with settings from each module
- init functions for each ahrs modules
- remove old chimu
* update unit test
* [ins] decoupled INS implementation
To allow multiple INS at the same time:
- remove weak functions from ins.c
- remove the INS_TYPE_H define
- use ABI message to trigger INS reset
* [state] protect for c++
This commit is contained in:
Gautier Hattenberger
2024-12-03 22:34:27 +01:00
committed by GitHub
parent 32b0dc6944
commit dfd8e93927
109 changed files with 1168 additions and 2239 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/navigation.c
+5 -5
View File
@@ -130,8 +130,8 @@ void nav_parse_MOVE_WP(uint8_t *buf)
/* WP_alt from message is alt above MSL in mm
* lla.alt is above ellipsoid in mm
*/
lla.alt = DL_MOVE_WP_alt(buf) - state.ned_origin_i.hmsl +
state.ned_origin_i.lla.alt;
lla.alt = DL_MOVE_WP_alt(buf) - stateGetHmslOrigin_i() +
stateGetLlaOrigin_i().alt;
waypoint_move_lla(wp_id, &lla);
}
}
@@ -224,7 +224,7 @@ static inline void nav_set_altitude(void)
// altitude mode
if (fabsf(nav.fp_altitude - last_alt) > 0.2f) {
nav.nav_altitude = nav.fp_altitude;
flight_altitude = nav.nav_altitude + state.ned_origin_f.hmsl;
flight_altitude = nav.nav_altitude + stateGetHmslOrigin_f();
last_alt = nav.fp_altitude;
}
}
@@ -233,14 +233,14 @@ static inline void nav_set_altitude(void)
/** Reset the geographic reference to the current GPS fix */
void nav_reset_reference(void)
{
ins_reset_local_origin();
AbiSendMsgINS_RESET(0, INS_RESET_REF);
/* update local ENU coordinates of global waypoints */
waypoints_localize_all();
}
void nav_reset_alt(void)
{
ins_reset_altitude_ref();
AbiSendMsgINS_RESET(0, INS_RESET_VERTICAL_REF);
waypoints_localize_all();
}
sw/airborne/firmwares/rotorcraft/navigation.h
+3 -3
View File
@@ -180,7 +180,7 @@ extern float flight_altitude; // hmsl flight altitude in meters
/// Get current y (north) position in local coordinates
#define GetPosY() (stateGetPositionEnu_f()->y)
/// Get current altitude above MSL
#define GetPosAlt() (stateGetPositionEnu_f()->z+state.ned_origin_f.hmsl)
#define GetPosAlt() (stateGetPositionEnu_f()->z+stateGetHmslOrigin_f())
/// Get current height above reference
#define GetPosHeight() (stateGetPositionEnu_f()->z)
/**
@@ -189,7 +189,7 @@ extern float flight_altitude; // hmsl flight altitude in meters
* but might be updated later through a call to NavSetGroundReferenceHere() or
* NavSetAltitudeReferenceHere(), e.g. in the GeoInit flight plan block.
*/
#define GetAltRef() (state.ned_origin_f.hmsl)
#define GetAltRef() (stateGetHmslOrigin_f())
extern void nav_init(void);
@@ -381,7 +381,7 @@ static inline void NavGlide(uint8_t wp_start, uint8_t wp_end)
#define nav_SetNavRadius(x) {}
#define navigation_SetFlightAltitude(x) { \
flight_altitude = x; \
nav.nav_altitude = flight_altitude - state.ned_origin_f.hmsl; \
nav.nav_altitude = flight_altitude - stateGetHmslOrigin_f(); \
}
/** Unused compat macros
sw/airborne/firmwares/rover/navigation.c
+4 -4
View File
@@ -126,8 +126,8 @@ void nav_parse_MOVE_WP(uint8_t *buf)
/* WP_alt from message is alt above MSL in mm
* lla.alt is above ellipsoid in mm
*/
lla.alt = DL_MOVE_WP_alt(buf) - state.ned_origin_i.hmsl +
state.ned_origin_i.lla.alt;
lla.alt = DL_MOVE_WP_alt(buf) - stateGetHmslOrigin_i() +
stateGetLlaOrigin_i().alt;
waypoint_move_lla(wp_id, &lla);
}
}
@@ -175,14 +175,14 @@ bool nav_check_wp_time(struct EnuCoor_f *wp, float stay_time)
/** Reset the geographic reference to the current GPS fix */
void nav_reset_reference(void)
{
ins_reset_local_origin();
AbiSendMsgINS_RESET(0, INS_RESET_REF);
/* update local ENU coordinates of global waypoints */
waypoints_localize_all();
}
void nav_reset_alt(void)
{
ins_reset_altitude_ref();
AbiSendMsgINS_RESET(0, INS_RESET_VERTICAL_REF);
waypoints_localize_all();
}
sw/airborne/firmwares/rover/navigation.h
+2 -2
View File
@@ -135,7 +135,7 @@ extern void nav_register_oval(nav_rover_oval_init nav_oval_init, nav_rover_oval
/// Get current y (north) position in local coordinates
#define GetPosY() (stateGetPositionEnu_f()->y)
/// Get current altitude above MSL
#define GetPosAlt() (stateGetPositionEnu_f()->z+state.ned_origin_f.hmsl)
#define GetPosAlt() (stateGetPositionEnu_f()->z+stateGetHmslOrigin_f())
/// Get current height above reference
#define GetPosHeight() (stateGetPositionEnu_f()->z)
/**
@@ -144,7 +144,7 @@ extern void nav_register_oval(nav_rover_oval_init nav_oval_init, nav_rover_oval
* but might be updated later through a call to NavSetGroundReferenceHere() or
* NavSetAltitudeReferenceHere(), e.g. in the GeoInit flight plan block.
*/
#define GetAltRef() (state.ned_origin_f.hmsl)
#define GetAltRef() (stateGetHmslOrigin_f())
/** Normalize a degree angle between 0 and 359 */