diff --git a/conf/airframes/ENAC/rover/rover_2_wheels.xml b/conf/airframes/ENAC/rover/rover_2_wheels.xml
index b9a2b5f03e..4094642824 100644
--- a/conf/airframes/ENAC/rover/rover_2_wheels.xml
+++ b/conf/airframes/ENAC/rover/rover_2_wheels.xml
@@ -1,4 +1,4 @@
-
+
@@ -16,6 +16,11 @@
+
+
+
+
+
@@ -98,6 +103,7 @@
+
diff --git a/conf/airframes/ENAC/rover/rover_e_maxx.xml b/conf/airframes/ENAC/rover/rover_e_maxx.xml
new file mode 100644
index 0000000000..a0af15e638
--- /dev/null
+++ b/conf/airframes/ENAC/rover/rover_e_maxx.xml
@@ -0,0 +1,147 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/autopilot/rover.xml b/conf/autopilot/rover.xml
index 6f47a139a0..c6aa92b7cb 100644
--- a/conf/autopilot/rover.xml
+++ b/conf/autopilot/rover.xml
@@ -4,16 +4,18 @@
+
+
+
+
+
-
-
-
@@ -33,7 +35,7 @@
-
+
@@ -83,8 +85,8 @@
-
-
+
+
@@ -94,8 +96,8 @@
-
-
+
+
diff --git a/conf/autopilot/rover_steering.xml b/conf/autopilot/rover_steering.xml
index 62f48ba1f0..e90895552c 100644
--- a/conf/autopilot/rover_steering.xml
+++ b/conf/autopilot/rover_steering.xml
@@ -2,7 +2,13 @@
-
+
+
+
+
+
+
+
@@ -11,8 +17,6 @@
-
-
diff --git a/conf/autopilot/rover_steering_cruise.xml b/conf/autopilot/rover_steering_cruise.xml
index c246d422d6..402521fda8 100644
--- a/conf/autopilot/rover_steering_cruise.xml
+++ b/conf/autopilot/rover_steering_cruise.xml
@@ -2,7 +2,13 @@
-
+
+
+
+
+
+
+
@@ -11,8 +17,6 @@
-
-
diff --git a/conf/autopilot/rover_steering_pid.xml b/conf/autopilot/rover_steering_pid.xml
new file mode 100644
index 0000000000..b6064f5544
--- /dev/null
+++ b/conf/autopilot/rover_steering_pid.xml
@@ -0,0 +1,129 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/flight_plans/ENAC/rover_enac.xml b/conf/flight_plans/ENAC/rover_enac.xml
new file mode 100644
index 0000000000..a7259a8b63
--- /dev/null
+++ b/conf/flight_plans/ENAC/rover_enac.xml
@@ -0,0 +1,78 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/guidance_rover.xml b/conf/modules/guidance_rover.xml
index 88455e42b1..f0d6724335 100644
--- a/conf/modules/guidance_rover.xml
+++ b/conf/modules/guidance_rover.xml
@@ -26,7 +26,7 @@
-
+
diff --git a/conf/modules/guidance_rover_steering.xml b/conf/modules/guidance_rover_steering.xml
index 720a17b015..db967b2901 100644
--- a/conf/modules/guidance_rover_steering.xml
+++ b/conf/modules/guidance_rover_steering.xml
@@ -15,6 +15,10 @@
+
+
+
+
@@ -22,15 +26,15 @@
-
-
+
+
- @navigation,gvf_common
+ @navigation
guidance,commands
diff --git a/conf/modules/mission_rover.xml b/conf/modules/mission_rover.xml
new file mode 100644
index 0000000000..3ed4eabf34
--- /dev/null
+++ b/conf/modules/mission_rover.xml
@@ -0,0 +1,22 @@
+
+
+
+
+
+ Specific interface for mission control of rover.
+
+
+
+ mission_common
+ mission
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/radios/FrSky_X-Lite_car.xml b/conf/radios/FrSky_X-Lite_car.xml
new file mode 100644
index 0000000000..e44d8dc7d8
--- /dev/null
+++ b/conf/radios/FrSky_X-Lite_car.xml
@@ -0,0 +1,13 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/firmwares/rover/guidance/rover_guidance_steering.c b/sw/airborne/firmwares/rover/guidance/rover_guidance_steering.c
index 51bff3d301..75e50b9005 100644
--- a/sw/airborne/firmwares/rover/guidance/rover_guidance_steering.c
+++ b/sw/airborne/firmwares/rover/guidance/rover_guidance_steering.c
@@ -1,6 +1,7 @@
/*
- * Copyright (C) 2021 Jesús Bautista
+ * Copyright (C) 2021 Jesús Bautista
* Hector García
+ * 2025 Gautier Hattenberger
*
* This file is part of paparazzi.
*
@@ -25,90 +26,131 @@
#include "firmwares/rover/guidance/rover_guidance_steering.h"
#include "generated/airframe.h"
+#include "generated/autopilot_core_guidance.h"
-#include "modules/actuators/actuators_default.h"
-#include "modules/radio_control/radio_control.h"
#include "autopilot.h"
-#include "navigation.h"
#include "state.h"
#include "filters/pid.h" // Used for p+i speed controller
-#include
-#include
+#ifndef ROVER_GUIDANCE_POS_KP
+#define ROVER_GUIDANCE_POS_KP 1.f
+#endif
+
+#ifndef ROVER_GUIDANCE_HEADING_KP
+#define ROVER_GUIDANCE_HEADING_KP 1.f
+#endif
+
+#ifndef ROVER_GUIDANCE_SPEED_KP
+#define ROVER_GUIDANCE_SPEED_KP 10.f
+#endif
+
+#ifndef ROVER_GUIDANCE_SPEED_KI
+#define ROVER_GUIDANCE_SPEED_KI 100.f
+#endif
+
+#ifndef ROVER_GUIDANCE_MAX_POS_ERR
+#define ROVER_GUIDANCE_MAX_POS_ERR 10.f // max position error
+#endif
+
+#ifndef ROVER_GUIDANCE_MAX_SPEED
+#define ROVER_GUIDANCE_MAX_SPEED 10.f
+#endif
+
+#ifndef ROVER_GUIDANCE_PROXIMITY_DISTANCE
+#define ROVER_GUIDANCE_PROXIMITY_DISTANCE 2.f // proximity distance TODO improve with hysteresis ?
+#endif
// Guidance control main variables
rover_ctrl guidance_control;
static struct PID_f rover_pid;
static float time_step;
-static float last_speed_cmd;
-static uint8_t last_ap_mode;
/** INIT function **/
void rover_guidance_steering_init(void)
{
- guidance_control.cmd.delta = 0.0;
- guidance_control.cmd.speed = 0.0;
- guidance_control.throttle = 0.0;
+ guidance_control.cmd.delta = 0.f;
+ guidance_control.cmd.speed = 0.f;
+ guidance_control.throttle = 0.f;
- last_speed_cmd = 0.0;
- last_ap_mode = AP_MODE_KILL;
-
- guidance_control.speed_error = 0.0;
+ guidance_control.pos_kp = ROVER_GUIDANCE_POS_KP;
+ guidance_control.heading_kp = ROVER_GUIDANCE_HEADING_KP;
+ guidance_control.speed_error = 0.f;
guidance_control.kf = SR_MEASURED_KF;
- guidance_control.kp = 10;
- guidance_control.ki = 100;
+ guidance_control.kp = ROVER_GUIDANCE_SPEED_KP;
+ guidance_control.ki = ROVER_GUIDANCE_SPEED_KI;
- init_pid_f(&rover_pid, guidance_control.kp, 0.f, guidance_control.ki, MAX_PPRZ*0.2);
+ init_pid_f(&rover_pid, guidance_control.kp, 0.f, guidance_control.ki, MAX_PPRZ*0.2f);
// Based on autopilot state machine frequency
time_step = 1.f/PERIODIC_FREQUENCY;
}
+void rover_guidance_steering_periodic(void)
+{
+ // from code generation
+ autopilot_core_guidance_periodic_task();
+}
+
/** CTRL functions **/
// Steering control (GVF)
void rover_guidance_steering_heading_ctrl(float omega) //GVF give us this omega
{
- float delta = 0.0;
-
// Speed is bounded to avoid GPS noise while driving at small velocity
- float speed = BoundSpeed(stateGetHorizontalSpeedNorm_f());
-
- if (fabs(omega)>0.0) {
- delta = DegOfRad(-atanf(omega*DRIVE_SHAFT_DISTANCE/speed));
- }
+ float speed = BoundSpeed(stateGetHorizontalSpeedNorm_f());
+ // Compute steering angle
+ float delta = DegOfRad(atanf(omega*DRIVE_SHAFT_DISTANCE/speed));
guidance_control.cmd.delta = BoundDelta(delta);
}
// Speed control (feed feed forward + propotional + integral controler) (PID)
-void rover_guidance_steering_speed_ctrl(void)
+void rover_guidance_steering_speed_ctrl(void)
{
- // - Looking for setting update
- if (guidance_control.kp != rover_pid.g[0] || guidance_control.ki != rover_pid.g[2]) {
- set_gains_pid_f(&rover_pid, guidance_control.kp, 0.f, guidance_control.ki);
- }
- if (guidance_control.cmd.speed != last_speed_cmd) {
- last_speed_cmd = guidance_control.cmd.speed;
- //reset_pid_f(&rover_pid);
- }
-
- // - Updating PID
+ // Updating PID
guidance_control.speed_error = (guidance_control.cmd.speed - stateGetHorizontalSpeedNorm_f());
update_pid_f(&rover_pid, guidance_control.speed_error, time_step);
guidance_control.throttle = BoundThrottle(guidance_control.kf*guidance_control.cmd.speed + get_pid_f(&rover_pid));
}
+void rover_guidance_steering_setpoints(struct EnuCoor_f pos_sp, float *heading_sp)
+{
+ // compute position error
+ struct FloatVect2 pos_2d;
+ VECT2_COPY(pos_2d, pos_sp);
+ struct FloatVect2 pos_err;
+ VECT2_DIFF(pos_err, pos_2d, *stateGetPositionEnu_f());
+ float pos_err_norm = float_vect2_norm(&pos_err);
+ BoundAbs(pos_err_norm, ROVER_GUIDANCE_MAX_POS_ERR);
+
+ // speed and heading update when far enough
+ if (pos_err_norm > ROVER_GUIDANCE_PROXIMITY_DISTANCE) {
+ // compute speed error
+ guidance_control.cmd.speed = guidance_control.pos_kp * pos_err_norm;
+ Bound(guidance_control.cmd.speed, 0.f, ROVER_GUIDANCE_MAX_SPEED);
+ // if not close to WP, compute desired heading
+ guidance_control.heading_sp = atan2f(pos_err.x, pos_err.y);
+ // update nav sp
+ *heading_sp = guidance_control.heading_sp;
+ // angular error
+ float heading_err = guidance_control.heading_sp - stateGetNedToBodyEulers_f()->psi;
+ NormRadAngle(heading_err);
+ // compute omega setpoint
+ guidance_control.omega_sp = guidance_control.heading_kp * heading_err;
+ }
+ else {
+ guidance_control.cmd.speed = 0.f;
+ guidance_control.heading_sp = *heading_sp;
+ guidance_control.omega_sp = 0.f;
+ }
+}
/** PID RESET function**/
void rover_guidance_steering_pid_reset(void)
{
- // Reset speed PID
- if (rover_pid.sum != 0) {
- reset_pid_f(&rover_pid);
- }
+ reset_pid_f(&rover_pid);
}
void rover_guidance_steering_kill(void)
@@ -116,3 +158,17 @@ void rover_guidance_steering_kill(void)
guidance_control.cmd.delta = 0.0;
guidance_control.cmd.speed = 0.0;
}
+
+void rover_guidance_steering_set_speed_pgain(float pgain)
+{
+ guidance_control.kp = pgain;
+ set_gains_pid_f(&rover_pid, guidance_control.kp, 0.f, guidance_control.ki);
+}
+
+void rover_guidance_steering_set_speed_igain(float igain)
+{
+ guidance_control.ki = igain;
+ set_gains_pid_f(&rover_pid, guidance_control.kp, 0.f, guidance_control.ki);
+ rover_pid.sum = 0.f;
+}
+
diff --git a/sw/airborne/firmwares/rover/guidance/rover_guidance_steering.h b/sw/airborne/firmwares/rover/guidance/rover_guidance_steering.h
index 406abd0cc4..d6840a4627 100644
--- a/sw/airborne/firmwares/rover/guidance/rover_guidance_steering.h
+++ b/sw/airborne/firmwares/rover/guidance/rover_guidance_steering.h
@@ -1,6 +1,7 @@
/*
- * Copyright (C) 2021 Jesús Bautista
+ * Copyright (C) 2021 Jesús Bautista
* Hector García
+ * 2025 Gautier Hattenberger
*
* This file is part of paparazzi.
*
@@ -30,6 +31,7 @@
#include "std.h"
#include
+#include "math/pprz_geodetic_float.h"
#include "generated/airframe.h"
@@ -56,10 +58,10 @@
// MIN_DELTA, MAX_DELTA: Min and max wheels turning angle (deg)
// You should measure this angle if you want to have an
// efficient control in your steering
-#ifndef MAX_DELTA
+#ifndef MAX_DELTA
#define MAX_DELTA 15.0
#endif
-#ifndef MIN_DELTA
+#ifndef MIN_DELTA
#define MIN_DELTA MAX_DELTA
#endif
@@ -70,15 +72,15 @@
#ifndef MAX_CMD_SHUT
#define MAX_CMD_SHUT 0
#endif
-#ifndef MIN_CMD_SHUT
+#ifndef MIN_CMD_SHUT
#define MIN_CMD_SHUT 0
#endif
// MIN_SPEED, MAX_SPEED: Min and max state speed (m/s)
-#ifndef MAX_SPEED
+#ifndef MAX_SPEED
#define MAX_SPEED 999.0 //We don't really use that variable
#endif
-#ifndef MIN_SPEED
+#ifndef MIN_SPEED
#define MIN_SPEED 0.2 //But this one is mandatory because we have
#endif //to deal with GPS noise (and 1/v in guidance control).
@@ -106,7 +108,11 @@ typedef struct {
typedef struct {
sr_cmd_t cmd;
float throttle;
+ float heading_sp; ///< heading setpoint
+ float omega_sp; ///< omega setpoint
+ float pos_kp;
+ float heading_kp;
float speed_error;
float kf;
float kp;
@@ -117,11 +123,15 @@ extern rover_ctrl guidance_control;
/** Steering rover guidance EXT FUNCTIONS **/
extern void rover_guidance_steering_init(void);
+extern void rover_guidance_steering_periodic(void); // call state machine
extern void rover_guidance_steering_heading_ctrl(float omega);
extern void rover_guidance_steering_speed_ctrl(void);
+extern void rover_guidance_steering_setpoints(struct EnuCoor_f pos_sp, float *heading_sp);
extern void rover_guidance_steering_pid_reset(void);
extern void rover_guidance_steering_kill(void);
+extern void rover_guidance_steering_set_speed_pgain(float pgain);
+extern void rover_guidance_steering_set_speed_igain(float igain);
/** MACROS **/
// Bound delta
@@ -138,8 +148,8 @@ extern void rover_guidance_steering_kill(void);
#define BoundThrottle(throttle) TRIM_PPRZ((int)throttle)
// Set low level commands from high level commands
-#define GetCmdFromDelta(delta) (delta >= 0 ? -delta/MAX_DELTA * (MAX_PPRZ - (int)MAX_CMD_SHUT) : \
- -delta/MIN_DELTA * (MAX_PPRZ - (int)MIN_CMD_SHUT))
+#define GetCmdFromDelta(delta) (delta >= 0 ? delta/MAX_DELTA * (MAX_PPRZ - (int)MAX_CMD_SHUT) : \
+ delta/MIN_DELTA * (MAX_PPRZ - (int)MIN_CMD_SHUT))
// This macro is for NAV state
#define GetCmdFromThrottle(throttle) (autopilot_throttle_killed() ? 0 : TRIM_PPRZ((int)throttle))
diff --git a/sw/airborne/firmwares/rover/navigation.c b/sw/airborne/firmwares/rover/navigation.c
index b7776e2215..699d9b6d2c 100644
--- a/sw/airborne/firmwares/rover/navigation.c
+++ b/sw/airborne/firmwares/rover/navigation.c
@@ -134,41 +134,39 @@ void nav_parse_MOVE_WP(uint8_t *buf)
bool nav_check_wp_time(struct EnuCoor_f *wp, float stay_time)
{
- (void) wp;
- (void) stay_time;
- return true;
-// uint16_t time_at_wp;
-// float dist_to_point;
-// static uint16_t wp_entry_time = 0;
-// static bool wp_reached = false;
-// static struct EnuCoor_i wp_last = { 0, 0, 0 };
-// struct Int32Vect2 diff;
-//
-// if ((wp_last.x != wp->x) || (wp_last.y != wp->y)) {
-// wp_reached = false;
-// wp_last = *wp;
-// }
-//
-// VECT2_DIFF(diff, *wp, *stateGetPositionEnu_i());
-// struct FloatVect2 diff_f = {POS_FLOAT_OF_BFP(diff.x), POS_FLOAT_OF_BFP(diff.y)};
-// dist_to_point = float_vect2_norm(&diff_f);
-// if (dist_to_point < ARRIVED_AT_WAYPOINT) {
-// if (!wp_reached) {
-// wp_reached = true;
-// wp_entry_time = autopilot.flight_time;
-// time_at_wp = 0;
-// } else {
-// time_at_wp = autopilot.flight_time - wp_entry_time;
-// }
-// } else {
-// time_at_wp = 0;
-// wp_reached = false;
-// }
-// if (time_at_wp > stay_time) {
-// INT_VECT3_ZERO(wp_last);
-// return true;
-// }
-// return false;
+ uint16_t time_at_wp;
+ float dist_to_point;
+ static uint16_t wp_entry_time = 0;
+ static bool wp_reached = false;
+ static struct EnuCoor_i wp_last = { 0, 0, 0 };
+ struct EnuCoor_i wp_i;
+ struct FloatVect2 diff;
+
+ ENU_BFP_OF_REAL(wp_i, *wp);
+ if ((wp_last.x != wp_i.x) || (wp_last.y != wp_i.y)) {
+ wp_reached = false;
+ wp_last = wp_i;
+ }
+
+ VECT2_DIFF(diff, *wp, *stateGetPositionEnu_f());
+ dist_to_point = float_vect2_norm(&diff);
+ if (dist_to_point < ARRIVED_AT_WAYPOINT) {
+ if (!wp_reached) {
+ wp_reached = true;
+ wp_entry_time = autopilot.flight_time;
+ time_at_wp = 0;
+ } else {
+ time_at_wp = autopilot.flight_time - wp_entry_time;
+ }
+ } else {
+ time_at_wp = 0;
+ wp_reached = false;
+ }
+ if (time_at_wp > stay_time) {
+ INT_VECT3_ZERO(wp_last);
+ return true;
+ }
+ return false;
}
@@ -190,15 +188,15 @@ void nav_init_stage(void)
{
VECT3_COPY(nav.last_pos, *stateGetPositionEnu_f());
stage_time = 0;
- //nav_circle_radians = 0; FIXME
+ if (nav.nav_stage_init) {
+ nav.nav_stage_init();
+ }
}
void nav_periodic_task(void)
{
RunOnceEvery(NAVIGATION_FREQUENCY, { stage_time++; block_time++; });
- //nav.dist2_to_wp = 0; FIXME
-
/* from flight_plan.h */
auto_nav();
@@ -217,8 +215,6 @@ void nav_home(void)
nav.mode = NAV_MODE_WAYPOINT;
VECT3_COPY(nav.target, waypoints[WP_HOME].enu_f);
- //nav.dist2_to_wp = nav.dist2_to_home; FIXME
-
/* run carrot loop */
nav_run();
}
@@ -307,6 +303,11 @@ void nav_set_failsafe(void)
/** Register functions
*/
+void nav_register_stage_init(nav_rover_stage_init nav_stage_init)
+{
+ nav.nav_stage_init = nav_stage_init;
+}
+
void nav_register_goto_wp(nav_rover_goto nav_goto, nav_rover_route nav_route, nav_rover_approaching nav_approaching)
{
nav.nav_goto = nav_goto;
diff --git a/sw/airborne/firmwares/rover/navigation.h b/sw/airborne/firmwares/rover/navigation.h
index 4544ae2e4b..5a9385d815 100644
--- a/sw/airborne/firmwares/rover/navigation.h
+++ b/sw/airborne/firmwares/rover/navigation.h
@@ -40,7 +40,7 @@
#endif
#ifndef CARROT_DIST
-#define CARROT_DIST 2.f
+#define CARROT_DIST 5.f
#endif
/** default navigation frequency */
@@ -75,6 +75,7 @@
#define NAV_MODE_HEADING 3
#define NAV_MODE_MANUAL 4
+typedef void (*nav_rover_stage_init)(void);
typedef void (*nav_rover_goto)(struct EnuCoor_f *wp);
typedef void (*nav_rover_route)(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end);
typedef bool (*nav_rover_approaching)(struct EnuCoor_f *wp_to, struct EnuCoor_f *wp_from, float approaching_time);
@@ -106,6 +107,7 @@ struct RoverNavigation {
struct EnuCoor_f last_pos; ///< last stage position
// pointers to basic nav functions
+ nav_rover_stage_init nav_stage_init;
nav_rover_goto nav_goto;
nav_rover_route nav_route;
nav_rover_approaching nav_approaching;
@@ -118,6 +120,7 @@ extern struct RoverNavigation nav;
/** Registering functions
*/
+extern void nav_register_stage_init(nav_rover_stage_init nav_stage_init);
extern void nav_register_goto_wp(nav_rover_goto nav_goto,
nav_rover_route nav_route,
nav_rover_approaching nav_approaching);
@@ -243,8 +246,9 @@ bool nav_check_wp_time(struct EnuCoor_f *wp, float stay_time);
static inline void NavGotoWaypoint(uint8_t wp)
{
nav.mode = NAV_MODE_WAYPOINT;
- VECT3_COPY(nav.target, waypoints[wp].enu_f);
- //nav.dist2_to_wp = get_dist2_to_waypoint(wp); FIXME
+ if (nav.nav_goto) {
+ nav.nav_goto(&waypoints[wp].enu_f);
+ }
}
/*********** Navigation along a line *************************************/
diff --git a/sw/airborne/modules/mission/mission_rover_nav.c b/sw/airborne/modules/mission/mission_rover_nav.c
new file mode 100644
index 0000000000..350c84bea6
--- /dev/null
+++ b/sw/airborne/modules/mission/mission_rover_nav.c
@@ -0,0 +1,255 @@
+/** @file modules/mission/mission_rover_nav.c
+ * @brief mission navigation for rovers
+ *
+ * Implement specific navigation routines for the mission control
+ * of a rover.
+ */
+
+#include
+#include "modules/mission/mission_common.h"
+#include "autopilot.h"
+#include "firmwares/rover/navigation.h"
+#include "generated/flight_plan.h"
+
+// Buffer zone in [m] before MAX_DIST_FROM_HOME
+#define BUFFER_ZONE_DIST 10
+
+/// Utility function: converts lla (int) to local point (float)
+bool mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla)
+{
+ // return FALSE if there is no valid local coordinate system
+ if (!state.ned_initialized_i) {
+ return false;
+ }
+
+ // For a rover, GPS z altitude is ignored. The altitude is forced to 0.
+ lla->alt = 0;
+
+ // Compute ENU components from LLA with respect to ltp origin
+ struct EnuCoor_i tmp_enu_point_i;
+ enu_of_lla_point_i(&tmp_enu_point_i, stateGetNedOrigin_i(), lla);
+ struct EnuCoor_f tmp_enu_point_f;
+ // result of enu_of_lla_point_i is in cm, convert to float in m
+ VECT3_SMUL(tmp_enu_point_f, tmp_enu_point_i, 0.01f);
+
+ // Bound the new waypoint with max distance from home
+ struct FloatVect2 home;
+ home.x = waypoint_get_x(WP_HOME);
+ home.y = waypoint_get_y(WP_HOME);
+ struct FloatVect2 vect_from_home;
+ VECT2_DIFF(vect_from_home, tmp_enu_point_f, home);
+
+ // Saturate the mission wp not to overflow max_dist_from_home
+ // including a buffer zone before limits
+ float dist_to_home = float_vect2_norm(&vect_from_home);
+ dist_to_home += BUFFER_ZONE_DIST;
+ if (dist_to_home > MAX_DIST_FROM_HOME) {
+ VECT2_SMUL(vect_from_home, vect_from_home, (MAX_DIST_FROM_HOME / dist_to_home));
+ }
+ // set new point (2D)
+ point->x = home.x + vect_from_home.x;
+ point->y = home.y + vect_from_home.y;
+ point->z = 0.0f; // to be sure the altitude is at 0
+
+ return true;
+}
+
+// navigation time step
+static const float dt_navigation = 1.0f / ((float)NAVIGATION_FREQUENCY);
+
+// last_mission_wp, last target wp from mission elements, not used actively and kept for future implementations
+struct EnuCoor_f last_mission_wp = { 0.f, 0.f, 0.f };
+
+/** Navigation function to a single waypoint (2D)
+*/
+static inline bool mission_nav_wp(struct _mission_element *el)
+{
+ struct EnuCoor_f *target_wp = &(el->element.mission_wp.wp);
+
+ //Check proximity and wait for 'duration' seconds in proximity circle if desired
+ if (nav.nav_approaching(target_wp, NULL, CARROT)) {
+ last_mission_wp = *target_wp;
+
+ if (el->duration > 0.f) {
+ if (nav_check_wp_time(target_wp, el->duration)) { return false; }
+ } else { return false; }
+
+ }
+ //Go to Mission Waypoint
+ nav.nav_goto(target_wp);
+
+ return true;
+}
+
+/** Navigation function on a circle
+*/
+
+static inline bool mission_nav_circle(struct _mission_element *el)
+{
+ struct EnuCoor_f *center_wp = &(el->element.mission_circle.center);
+ float radius = el->element.mission_circle.radius;
+
+ //Draw the desired circle for a 'duration' time
+ nav.nav_circle(center_wp, radius);
+
+ if (el->duration > 0.f && mission.element_time >= el->duration) {
+ return false;
+ }
+
+ if (el-> duration <= 0.f){
+ return false;
+ }
+
+ return true;
+}
+
+
+/** Navigation function along a segment (2D)
+*/
+static inline bool mission_nav_segment(struct _mission_element *el)
+{
+ struct EnuCoor_f *from_wp = &(el->element.mission_segment.from);
+ struct EnuCoor_f *to_wp = &(el->element.mission_segment.to);
+
+ //Check proximity and wait for 'duration' seconds in proximity circle if desired
+ if (nav.nav_approaching(to_wp, from_wp, CARROT)) {
+ last_mission_wp = *to_wp;
+
+ if (el->duration > 0.f) {
+ if (nav_check_wp_time(to_wp, el->duration)) { return false; }
+ } else { return false; }
+ }
+
+ //Route Between from-to
+ nav.nav_route(from_wp, to_wp);
+
+ return true;
+}
+
+
+#ifndef MISSION_PATH_SKIP_GOTO
+#define MISSION_PATH_SKIP_GOTO FALSE
+#endif
+
+/** Navigation function along a path (2D)
+*/
+static inline bool mission_nav_path(struct _mission_element *el)
+{
+ if (el->element.mission_path.nb == 0) {
+ return false; // nothing to do
+ }
+
+ if (el->element.mission_path.path_idx == 0) { //first wp of path
+ el->element.mission_wp.wp = el->element.mission_path.path[0];
+ if (MISSION_PATH_SKIP_GOTO || !mission_nav_wp(el)) { el->element.mission_path.path_idx++; }
+ }
+
+ else if (el->element.mission_path.path_idx < el->element.mission_path.nb) { //standart wp of path
+
+ struct EnuCoor_f *from_wp = &(el->element.mission_path.path[(el->element.mission_path.path_idx) - 1]);
+ struct EnuCoor_f *to_wp = &(el->element.mission_path.path[el->element.mission_path.path_idx]);
+
+ //Check proximity and wait for t seconds in proximity circle if desired
+ if (nav.nav_approaching(to_wp, from_wp, CARROT)) {
+ last_mission_wp = *to_wp;
+
+ if (el->duration > 0.f) {
+ if (nav_check_wp_time(to_wp, el->duration)) {
+ el->element.mission_path.path_idx++;
+ }
+ } else { el->element.mission_path.path_idx++; }
+ }
+ //Route Between from-to
+ nav.nav_route(from_wp, to_wp);
+ } else { return false; } //end of path
+
+ return true;
+}
+
+/** Call custom navigation function
+ */
+static inline bool mission_nav_custom(struct _mission_custom *custom, bool init)
+{
+ if (init) {
+ return custom->reg->cb(custom->nb, custom->params, MissionInit);
+ } else {
+ return custom->reg->cb(custom->nb, custom->params, MissionRun);
+ }
+}
+
+/** Implement waiting pattern
+ * Only called when MISSION_WAIT_TIMEOUT is not 0
+ */
+#ifndef MISSION_WAIT_TIMEOUT
+#define MISSION_WAIT_TIMEOUT 30 // wait 30 seconds before ending mission
+#endif
+
+static bool mission_wait_started = false;
+#if MISSION_WAIT_TIMEOUT
+static float mission_wait_time = 0.f;
+static struct _mission_element mission_wait_wp;
+static bool mission_wait_pattern(void) {
+ if (!mission_wait_started) {
+ mission_wait_wp.element.mission_wp.wp = *stateGetPositionEnu_f();
+ mission_wait_time = 0.f;
+ mission_wait_started = true;
+ }
+ mission_nav_wp(&mission_wait_wp);
+ mission_wait_time += dt_navigation;
+
+ return (mission_wait_time < (float)MISSION_WAIT_TIMEOUT); // keep flying until TIMEOUT
+}
+#else
+static bool mission_wait_pattern(void) {
+ return false; // no TIMEOUT, end mission now
+}
+#endif
+
+int mission_run()
+{
+ // current element
+ struct _mission_element *el = NULL;
+ if ((el = mission_get()) == NULL) {
+ return mission_wait_pattern();
+ }
+ mission_wait_started = false;
+ bool el_running = false;
+
+ switch (el->type) {
+ case MissionWP:
+ el_running = mission_nav_wp(el);
+ break;
+
+ case MissionCircle:
+ el_running = mission_nav_circle(el);
+ break;
+
+ case MissionSegment:
+ el_running = mission_nav_segment(el);
+ break;
+
+ case MissionPath:
+ el_running = mission_nav_path(el);
+ break;
+
+ case MissionCustom:
+ el_running = mission_nav_custom(&(el->element.mission_custom), mission.element_time < dt_navigation);
+ break;
+
+ default:
+ // invalid type or pattern not yet handled
+ break;
+ }
+
+ // increment element_time
+ mission.element_time += dt_navigation;
+
+ if (!el_running) {
+ // reset timer
+ mission.element_time = 0.;
+ // go to next element
+ mission.current_idx = (mission.current_idx + 1) % MISSION_ELEMENT_NB;
+ }
+ return true;
+}
+
diff --git a/sw/airborne/modules/nav/nav_rover_base.c b/sw/airborne/modules/nav/nav_rover_base.c
index 9f6cda64da..6e3c0c665f 100644
--- a/sw/airborne/modules/nav/nav_rover_base.c
+++ b/sw/airborne/modules/nav/nav_rover_base.c
@@ -31,10 +31,17 @@ struct RoverNavBase nav_rover_base;
/** Implement basic nav function
*/
+static void nav_stage_init(void)
+{
+ nav_rover_base.circle.radians = 0.f;
+ nav_rover_base.goto_wp.leg_progress = 0.f;
+}
+
static void nav_goto(struct EnuCoor_f *wp)
{
nav_rover_base.goto_wp.to = *wp;
nav_rover_base.goto_wp.dist2_to_wp = get_dist2_to_point(wp);
+ VECT3_COPY(nav.target, *wp);
nav.mode = NAV_MODE_WAYPOINT;
}
@@ -158,82 +165,76 @@ static void _nav_oval_init(void)
static void nav_oval(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius)
{
- (void) wp1;
- (void) wp2;
- (void) radius;
-#if 0
- radius = - radius; /* Historical error ? */
- int32_t alt = waypoints[p1].enu_i.z;
- waypoints[p2].enu_i.z = alt;
+ float alt = wp1->z;
+ wp2->z = alt;
- float p2_p1_x = waypoints[p1].enu_f.x - waypoints[p2].enu_f.x;
- float p2_p1_y = waypoints[p1].enu_f.y - waypoints[p2].enu_f.y;
- float d = sqrtf(p2_p1_x * p2_p1_x + p2_p1_y * p2_p1_y);
-
- /* Unit vector from p1 to p2 */
- int32_t u_x = POS_BFP_OF_REAL(p2_p1_x / d);
- int32_t u_y = POS_BFP_OF_REAL(p2_p1_y / d);
+ struct FloatVect2 dir;
+ VECT2_DIFF(dir, *wp1, *wp2);
+ float_vect2_normalize(&dir);
/* The half circle centers and the other leg */
- struct EnuCoor_i p1_center = { waypoints[p1].enu_i.x + radius * -u_y,
- waypoints[p1].enu_i.y + radius * u_x,
- alt
+ struct EnuCoor_f p1_center = {
+ wp1->x + radius * dir.y,
+ wp1->y - radius * dir.x,
+ alt
};
- struct EnuCoor_i p1_out = { waypoints[p1].enu_i.x + 2 * radius * -u_y,
- waypoints[p1].enu_i.y + 2 * radius * u_x,
- alt
+ struct EnuCoor_f p1_out = {
+ wp1->x + 2.f * radius * dir.y,
+ wp1->y - 2.f * radius * dir.x,
+ alt
+ };
+ struct EnuCoor_f p2_in = {
+ wp2->x + 2.f * radius * dir.y,
+ wp2->y - 2.f * radius * dir.x,
+ alt
+ };
+ struct EnuCoor_f p2_center = {
+ wp2->x + radius * dir.y,
+ wp2->y - radius * dir.x,
+ alt
};
- struct EnuCoor_i p2_in = { waypoints[p2].enu_i.x + 2 * radius * -u_y,
- waypoints[p2].enu_i.y + 2 * radius * u_x,
- alt
- };
- struct EnuCoor_i p2_center = { waypoints[p2].enu_i.x + radius * -u_y,
- waypoints[p2].enu_i.y + radius * u_x,
- alt
- };
-
- int32_t qdr_out_2 = INT32_ANGLE_PI - int32_atan2_2(u_y, u_x);
- int32_t qdr_out_1 = qdr_out_2 + INT32_ANGLE_PI;
- if (radius < 0) {
- qdr_out_2 += INT32_ANGLE_PI;
- qdr_out_1 += INT32_ANGLE_PI;
+ float qdr_out_2 = M_PI - atan2f(dir.y, dir.x);
+ float qdr_out_1 = qdr_out_2 + M_PI;
+ if (radius > 0.f) {
+ qdr_out_2 += M_PI;
+ qdr_out_1 += M_PI;
}
- int32_t qdr_anticipation = ANGLE_BFP_OF_REAL(radius > 0 ? -15 : 15);
+ float qdr_anticipation = (radius > 0.f ? 15.f : -15.f);
- switch (oval_status) {
+ switch (nav_rover_base.oval.status) {
case OC1 :
- nav_circle(&p1_center, POS_BFP_OF_REAL(-radius));
- if (NavQdrCloseTo(INT32_DEG_OF_RAD(qdr_out_1) - qdr_anticipation)) {
- oval_status = OR12;
+ nav.nav_circle(&p1_center, radius);
+ if (NavQdrCloseTo(DegOfRad(qdr_out_1) - qdr_anticipation)) {
+ nav_rover_base.oval.status = OR12;
InitStage();
LINE_START_FUNCTION;
}
return;
case OR12:
- nav_route(&p1_out, &p2_in);
- if (nav_approaching_from(&p2_in, &p1_out, CARROT)) {
- oval_status = OC2;
- nav_oval_count++;
+ nav.nav_route(&p1_out, &p2_in);
+ if (nav.nav_approaching(&p2_in, &p1_out, CARROT)) {
+ nav_rover_base.oval.status = OC2;
+ nav_rover_base.oval.count++;
InitStage();
LINE_STOP_FUNCTION;
}
return;
case OC2 :
- nav_circle(&p2_center, POS_BFP_OF_REAL(-radius));
- if (NavQdrCloseTo(INT32_DEG_OF_RAD(qdr_out_2) - qdr_anticipation)) {
- oval_status = OR21;
+ nav.nav_circle(&p2_center, radius);
+ if (NavQdrCloseTo(DegOfRad(qdr_out_2) - qdr_anticipation)) {
+ nav_rover_base.oval.status = OR21;
InitStage();
LINE_START_FUNCTION;
}
return;
case OR21:
- nav_route(&waypoints[p2].enu_i, &waypoints[p1].enu_i);
- if (nav_approaching_from(&waypoints[p1].enu_i, &waypoints[p2].enu_i, CARROT)) {
- oval_status = OC1;
+ nav.nav_route(wp2, wp1);
+ if (nav.nav_approaching(wp1, wp2, CARROT)) {
+ nav_rover_base.oval.status = OC1;
InitStage();
LINE_STOP_FUNCTION;
}
@@ -242,7 +243,6 @@ static void nav_oval(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius)
default: /* Should not occur !!! Doing nothing */
return;
}
-#endif
}
#if PERIODIC_TELEMETRY
@@ -295,6 +295,7 @@ void nav_rover_init(void)
nav_rover_base.goto_wp.leg_length = 1.f;
// register nav functions
+ nav_register_stage_init(nav_stage_init);
nav_register_goto_wp(nav_goto, nav_route, nav_approaching);
nav_register_circle(nav_circle);
nav_register_oval(_nav_oval_init, nav_oval);
diff --git a/sw/simulator/nps/nps_fdm_rover.c b/sw/simulator/nps/nps_fdm_rover.c
index e3625447d9..e0eb2305f4 100644
--- a/sw/simulator/nps/nps_fdm_rover.c
+++ b/sw/simulator/nps/nps_fdm_rover.c
@@ -1,5 +1,5 @@
/*
- * Copyright (C) 2021 Jesús Bautista
+ * Copyright (C) 2021 Jesús Bautista
* Hector García
*
* This file is part of paparazzi.
@@ -24,6 +24,7 @@
#include
#include
+#include "std.h"
#include "math/pprz_geodetic.h"
#include "math/pprz_geodetic_double.h"
#include "math/pprz_geodetic_float.h"
@@ -33,15 +34,63 @@
#include "generated/airframe.h"
#include "generated/flight_plan.h"
-
-#include "firmwares/rover/guidance/rover_guidance_steering.h"
+#include "generated/modules.h"
#include "state.h"
-// Check if rover firmware
+// Check if rover firmware
#ifndef ROVER_FIRMWARE
#error "The module nps_fdm_rover is designed for rovers and doesn't support other firmwares!!"
#endif
+#if (defined COMMAND_STEERING) && (defined COMMAND_THROTTLE) // STEERING ROVER PHYSICS
+
+// Friction coef, proportional to speed
+#ifndef NPS_ROVER_FRICTION
+#define NPS_ROVER_FRICTION 0.01f
+#endif
+
+//// Used for car-like rover (steering wheels)
+
+// Maximum acceleration (m/s²)
+#ifndef NPS_ROVER_ACCELERATION
+#define NPS_ROVER_ACCELERATION 1.f
+#endif
+
+/** Physical model parameters **/
+static float mu = NPS_ROVER_FRICTION;
+
+#endif
+
+//// Used for 2-wheels rover
+
+#if (defined COMMAND_TURN) && (defined COMMAND_SPEED) // 2 wheels rover dynamic
+
+// Maximum speed (m/s)
+#ifndef NPS_ROVER_MAX_SPEED
+#define NPS_ROVER_MAX_SPEED 2.f
+#endif
+
+// Maximum turn rate (rad/s)
+#ifndef NPS_ROVER_TURN_RATE
+#define NPS_ROVER_TURN_RATE 3.f
+#endif
+
+// Accel 1st order time constant
+#ifndef NPS_ROVER_ACCEL_TAU
+#define NPS_ROVER_ACCEL_TAU 0.2f
+#endif
+
+// Turn rate 1st order time constant
+#ifndef NPS_ROVER_TURN_TAU
+#define NPS_ROVER_TURN_TAU 0.1f
+#endif
+
+#include "filters/low_pass_filter.h"
+static struct FirstOrderLowPass speed_filter;
+static struct FirstOrderLowPass turn_filter;
+
+#endif // 2 wheels
+
// NpsFdm structure
struct NpsFdm fdm;
@@ -52,36 +101,37 @@ static struct LtpDef_d ltpdef;
static void init_ltp(void);
/** Physical model structures **/
-static struct EnuCoor_d rover_pos;
-static struct EnuCoor_d rover_vel;
-static struct EnuCoor_d rover_acc;
-
-/** Physical model parameters **/
-static float mu = 0.01;
+static struct NedCoor_d rover_pos;
+static struct NedCoor_d rover_vel;
+static struct NedCoor_d rover_acc;
/** NPS FDM rover init ***************************/
void nps_fdm_init(double dt)
{
fdm.init_dt = dt; // (1 / simulation freq)
- fdm.curr_dt = 0.001; // ¿Configurable from GCS?
- fdm.time = dt;
+ fdm.curr_dt = dt;
+ fdm.time = 0;
fdm.on_ground = TRUE;
-
fdm.nan_count = 0;
fdm.pressure = -1;
fdm.pressure_sl = PPRZ_ISA_SEA_LEVEL_PRESSURE;
fdm.total_pressure = -1;
fdm.dynamic_pressure = -1;
fdm.temperature = -1;
-
fdm.ltpprz_to_body_eulers.psi = 0.0;
init_ltp();
+
+#if (defined COMMAND_TURN) && (defined COMMAND_SPEED) // 2 wheels rover dynamic
+ init_first_order_low_pass(&speed_filter, NPS_ROVER_ACCEL_TAU, fdm.curr_dt, 0.f);
+ init_first_order_low_pass(&turn_filter, NPS_ROVER_TURN_TAU, fdm.curr_dt, 0.f);
+#endif
}
void nps_fdm_run_step(bool launch __attribute__((unused)), double *commands, int commands_nb __attribute__((unused)))
-{
+{
+ fdm.time += fdm.curr_dt;
/****************************************************************************
PHYSICAL MODEL
@@ -90,71 +140,80 @@ void nps_fdm_run_step(bool launch __attribute__((unused)), double *commands, int
the LTP plane (so we transfer every integration step to NED and ECEF at the end).
*/
- #ifdef COMMAND_STEERING // STEERING ROVER PHYSICS
+ // From previous step...
+ ned_of_ecef_point_d(&rover_pos, <pdef, &fdm.ecef_pos);
+ ned_of_ecef_vect_d(&rover_vel, <pdef, &fdm.ecef_ecef_vel);
+ double phi = fdm.ltpprz_to_body_eulers.psi;
- // Steering rover cmds:
+ #if (defined COMMAND_STEERING) && (defined COMMAND_THROTTLE) // STEERING ROVER PHYSICS
+
+ // Steering rover cmds:
// COMMAND_STEERING -> delta parameter
- // COMMAND_TRHOTTLE -> acceleration in heading direction
-
- double delta = RadOfDeg(commands[COMMAND_STEERING] * MAX_DELTA);
+ // COMMAND_THROTTLE -> acceleration in heading direction
/** Physical model for car-like robots .................. **/
- // From previous step...
- enu_of_ecef_point_d(&rover_pos, <pdef, &fdm.ecef_pos);
- enu_of_ecef_vect_d(&rover_vel, <pdef, &fdm.ecef_ecef_vel);
+ double delta = RadOfDeg(commands[COMMAND_STEERING] * MAX_DELTA);
double speed = FLOAT_VECT2_NORM(rover_vel);
- double phi = fdm.ltpprz_to_body_eulers.psi;
- double phi_d = tan(delta) / DRIVE_SHAFT_DISTANCE * speed;
-
- // Setting accelerations
- rover_acc.x = commands[COMMAND_THROTTLE] * cos(phi) - speed * (sin(phi) * phi_d + cos(phi) * mu);
- rover_acc.y = commands[COMMAND_THROTTLE] * sin(phi) + speed * (cos(phi) * phi_d - sin(phi) * mu);
- double phi_dd = tan(delta) / DRIVE_SHAFT_DISTANCE * commands[COMMAND_THROTTLE];
+ double phi_d = tan(delta) / DRIVE_SHAFT_DISTANCE * speed;
+ double accel = NPS_ROVER_ACCELERATION * commands[COMMAND_THROTTLE];
+ // NED accel = R(psi) [accel; |V|*phi_d] - mu*V
+ rover_acc.x = accel * cos(phi) - speed * phi_d * sin(phi) - mu * rover_vel.x;
+ rover_acc.y = accel * sin(phi) + speed * phi_d * cos(phi) - mu * rover_vel.y;
// Velocities (EULER INTEGRATION)
rover_vel.x += rover_acc.x * fdm.curr_dt;
rover_vel.y += rover_acc.y * fdm.curr_dt;
- phi_d = tan(delta) / DRIVE_SHAFT_DISTANCE * speed;
- // Positions
- rover_pos.x += rover_vel.x * fdm.curr_dt;
- rover_pos.y += rover_vel.y * fdm.curr_dt;
- phi += phi_d * fdm.curr_dt;
-
- // phi have to be contained in [-180º,180º). So:
- phi = (phi > M_PI)? - 2*M_PI + phi : (phi < -M_PI)? 2*M_PI + phi : phi;
+ #elif (defined COMMAND_TURN) && (defined COMMAND_SPEED) // 2 wheels rover dynamic
+
+ double phi_sp = NPS_ROVER_TURN_RATE * commands[COMMAND_TURN];
+ double phi_d = update_first_order_low_pass(&turn_filter, phi_sp);
+ double speed_sp = NPS_ROVER_MAX_SPEED * commands[COMMAND_SPEED];
+ double speed = update_first_order_low_pass(&speed_filter, speed_sp);
+ double vx = speed * cos(phi);
+ double vy = speed * sin(phi);
+ rover_acc.x = (vx - rover_vel.x) / fdm.curr_dt;
+ rover_acc.y = (vy - rover_vel.y) / fdm.curr_dt;
+ rover_vel.x = vx;
+ rover_vel.y = vy;
#else
#warning "The physics of this rover are not yet implemented in nps_fdm_rover!!"
#endif // STEERING ROVER PHYSICS
-
+ // Positions
+ rover_pos.x += rover_vel.x * fdm.curr_dt;
+ rover_pos.y += rover_vel.y * fdm.curr_dt;
+ // phi have to be contained in [-180º,180º). So:
+ phi += phi_d * fdm.curr_dt;
+ NormRadAngle(phi);
/****************************************************************************/
// Coordenates transformations |
// ----------------------------|
/* in ECEF */
- ecef_of_enu_point_d(&fdm.ecef_pos, <pdef, &rover_pos);
- ecef_of_enu_vect_d(&fdm.ecef_ecef_vel, <pdef, &rover_vel);
- ecef_of_enu_vect_d(&fdm.ecef_ecef_accel, <pdef, &rover_acc);
+ ecef_of_ned_point_d(&fdm.ecef_pos, <pdef, &rover_pos);
+ ecef_of_ned_vect_d(&fdm.ecef_ecef_vel, <pdef, &rover_vel);
+ ecef_of_ned_vect_d(&fdm.ecef_ecef_accel, <pdef, &rover_acc);
/* in NED */
ned_of_ecef_point_d(&fdm.ltpprz_pos, <pdef, &fdm.ecef_pos);
ned_of_ecef_vect_d(&fdm.ltpprz_ecef_vel, <pdef, &fdm.ecef_ecef_vel);
ned_of_ecef_vect_d(&fdm.ltpprz_ecef_accel, <pdef, &fdm.ecef_ecef_accel);
+ /* Height above sea level */
+ fdm.hmsl = ltpdef.hmsl - rover_pos.z;
+
/* Eulers */
fdm.ltpprz_to_body_eulers.psi = phi;
-
- // ENU to NED and exporting heading (psi euler angle)
- fdm.ltp_to_body_eulers.psi = - phi + M_PI / 2;
+ fdm.ltp_to_body_eulers.psi = phi;
// Exporting Eulers to AHRS (in quaternions)
double_quat_of_eulers(&fdm.ltp_to_body_quat, &fdm.ltp_to_body_eulers);
// Angular vel & acc
- fdm.body_ecef_rotvel.r = phi_d;
- fdm.body_ecef_rotaccel.r = phi_dd;
+ fdm.body_ecef_rotaccel.r = (phi_d - fdm.body_ecef_rotvel.r) / fdm.curr_dt;
+ fdm.body_ecef_rotvel.r = phi_d;
}
@@ -165,18 +224,19 @@ void nps_fdm_run_step(bool launch __attribute__((unused)), double *commands, int
static void init_ltp(void)
{
-
- struct LlaCoor_d llh_nav0; /* Height above the ellipsoid */
+ struct LlaCoor_d llh_nav0;
llh_nav0.lat = RadOfDeg((double)NAV_LAT0 / 1e7);
llh_nav0.lon = RadOfDeg((double)NAV_LON0 / 1e7);
+ llh_nav0.alt = (double)(NAV_ALT0 + NAV_MSL0) / 1000.; /* Height above the ellipsoid */
struct EcefCoor_d ecef_nav0;
-
ecef_of_lla_d(&ecef_nav0, &llh_nav0);
ltp_def_from_ecef_d(<pdef, &ecef_nav0);
- fdm.ecef_pos = ecef_nav0;
+ ltpdef.hmsl = (double)NAV_ALT0 / 1000.; /* Height above the geoid */
+ fdm.ecef_pos = ecef_nav0;
+ fdm.hmsl = ltpdef.hmsl;
fdm.ltp_g.x = 0.;
fdm.ltp_g.y = 0.;
fdm.ltp_g.z = 0.; // accel data are already with the correct format
@@ -188,9 +248,9 @@ static void init_ltp(void)
fdm.ltp_h.z = AHRS_H_Z;
PRINT_CONFIG_MSG("Using magnetic field as defined in airframe file.")
#else
- fdm.ltp_h.x = 0.4912;
- fdm.ltp_h.y = 0.1225;
- fdm.ltp_h.z = 0.8624;
+ fdm.ltp_h.x = 1.;
+ fdm.ltp_h.y = 0.;
+ fdm.ltp_h.z = 0.;
#endif
}