Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-23 21:36:28 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

Use altitude waypoints for EKF2

Browse Source
This commit is contained in:
Ewoud Smeur
2024-02-20 15:17:24 +01:00
committed by Freek van Tienen
parent 757c6372f0
commit 3bedf9576d
3 changed files with 19 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/ins_ekf2.xml
+3 -2
View File
@@ -25,7 +25,7 @@
<define name="INS_EKF2_FLOW_POS_X" value="0" description="Flow sensor X offset from CoG position [m]"/>
<define name="INS_EKF2_FLOW_POS_Y" value="0" description="Flow sensor Y offset from CoG position [m]"/>
<define name="INS_EKF2_FLOW_POS_Z" value="0" description="Flow sensor Z offset from CoG position [m]"/>
<define name="INS_EKF2_SONAR_MIN_RANGE" value="0.05" description="AGL sensor minimum range [m]"/>
<define name="INS_EKF2_SONAR_MAX_RANGE" value="3" description="AGL sensor maximum range [m]"/>
<define name="INS_EKF2_RANGE_MAIN_AGL" value="1" description="If enabled uses radar sensor as primary AGL source, if possible"/>
@@ -80,6 +80,7 @@
<define name="__PAPARAZZI" value="TRUE"/>
<define name="ECL_STANDALONE" value="TRUE"/>
<define name="IMU_INTEGRATION" value="TRUE"/> <!-- Needed to enable IMU integration -->
<define name="USE_ALT_LLA_WAYPOINTS" value="TRUE"/>
<!-- Compile needed ecl files -->
<file name="geo.cpp" dir="ecl/geo"/>
@@ -103,6 +104,6 @@
<file name="EKFGSF_yaw.cpp" dir="ecl/EKF"/>
<file name="sensor_range_finder.cpp" dir="ecl/EKF"/>
<file name="utils.cpp" dir="ecl/EKF"/>
</makefile>
</module>
sw/airborne/modules/nav/waypoints.c
+11 -3
View File
@@ -118,6 +118,14 @@ float waypoint_get_alt(uint8_t wp_id)
return 0.f;
}
float waypoint_get_lla_alt(uint8_t wp_id)
{
if (wp_id < nb_waypoint) {
return waypoints[wp_id].lla.alt/1000.f - state.ned_origin_i.lla.alt/1000.f;
}
return 0.f;
}
float waypoint_get_lat_deg(uint8_t wp_id)
{
if (wp_id < nb_waypoint) {
@@ -335,9 +343,9 @@ void waypoint_localize(uint8_t wp_id)
struct EnuCoor_i enu;
enu_of_lla_point_i(&enu, &state.ned_origin_i, &waypoints[wp_id].lla);
// convert ENU pos from cm to BFP with INT32_POS_FRAC
enu.x = POS_BFP_OF_REAL(enu.x) / 100;
enu.y = POS_BFP_OF_REAL(enu.y) / 100;
enu.z = POS_BFP_OF_REAL(enu.z) / 100;
enu.x = POS_BFP_OF_REAL((int64_t) enu.x) / 100;
enu.y = POS_BFP_OF_REAL((int64_t) enu.y) / 100;
enu.z = POS_BFP_OF_REAL((int64_t) enu.z) / 100;
waypoints[wp_id].enu_i = enu;
SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_I);
ENU_FLOAT_OF_BFP(waypoints[wp_id].enu_f, waypoints[wp_id].enu_i);
sw/airborne/modules/nav/waypoints.h
+5
View File
@@ -53,7 +53,11 @@ extern struct Waypoint waypoints[];
/* aliases for backwards compatibilty */
#define WaypointX(_wp) waypoint_get_x(_wp)
#define WaypointY(_wp) waypoint_get_y(_wp)
#if USE_ALT_LLA_WAYPOINTS
#define WaypointAlt(_wp) waypoint_get_lla_alt(_wp)
#else
#define WaypointAlt(_wp) waypoint_get_alt(_wp)
#endif
#define Height(_h) (_h)
extern void waypoints_init(void);
@@ -72,6 +76,7 @@ extern float waypoint_get_x(uint8_t wp_id);
extern float waypoint_get_y(uint8_t wp_id);
/** Get altitude of waypoint in meters (above reference) */
extern float waypoint_get_alt(uint8_t wp_id);
extern float waypoint_get_lla_alt(uint8_t wp_id);
/** Get latitude of waypoint in deg */
extern float waypoint_get_lat_deg(uint8_t wp_id);
/** Get latitude of waypoint in rad */