diff --git a/conf/airframes/UCM/rover_steering.xml b/conf/airframes/UCM/rover_steering.xml
new file mode 100644
index 0000000000..324fb06e10
--- /dev/null
+++ b/conf/airframes/UCM/rover_steering.xml
@@ -0,0 +1,127 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/autopilot/rover_steering.xml b/conf/autopilot/rover_steering.xml
new file mode 100644
index 0000000000..8fb6410396
--- /dev/null
+++ b/conf/autopilot/rover_steering.xml
@@ -0,0 +1,122 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/flight_plans/UCM/steering_rover_gvfMission.xml b/conf/flight_plans/UCM/steering_rover_gvfMission.xml
new file mode 100644
index 0000000000..90583b3ad2
--- /dev/null
+++ b/conf/flight_plans/UCM/steering_rover_gvfMission.xml
@@ -0,0 +1,95 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/flight_plans/UCM/test_ucm_fisicas.xml b/conf/flight_plans/UCM/test_ucm_fisicas.xml
new file mode 100644
index 0000000000..c3a267b7c0
--- /dev/null
+++ b/conf/flight_plans/UCM/test_ucm_fisicas.xml
@@ -0,0 +1,85 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/board_matek_f765_car.xml b/conf/modules/board_matek_f765_car.xml
new file mode 100644
index 0000000000..b11fd511de
--- /dev/null
+++ b/conf/modules/board_matek_f765_car.xml
@@ -0,0 +1,41 @@
+
+
+
+
+
+ Autoload several onboard sensors and subsystems
+ for the Matek F765 Wing board with proper configuration fixed for rovers.
+ IMU ICM20602 (auto-detected from MPU6000 driver)
+ Baro (BMP280)
+ OSD (desactivated for rovers)
+ Normal back of the board is on the battery/ESC wires
+ Normal up of the board is on MCU side.
+
+
+
+
+ spi_master,baro_bmp280_i2c,current_sensor
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/fdm_rover.xml b/conf/modules/fdm_rover.xml
new file mode 100644
index 0000000000..86f092a1a3
--- /dev/null
+++ b/conf/modules/fdm_rover.xml
@@ -0,0 +1,18 @@
+
+
+
+
+
+ Rover FDM for NPS simulator
+
+ This module introduces the foundations to construct any rover physics for the NPS simulator.
+ If you have a non designed rover physiscs, in nps_fdm_rover.c you can find the instructions
+ to build it.
+
+
+
+
+
+
+
+
diff --git a/conf/modules/firmwares/rover.xml b/conf/modules/firmwares/rover.xml
index a7d83ee0fd..2a82d06fca 100644
--- a/conf/modules/firmwares/rover.xml
+++ b/conf/modules/firmwares/rover.xml
@@ -27,5 +27,9 @@ $(error "Rover firmware should use generated autopilot")
endif
+
+
+
+
diff --git a/conf/modules/guidance_rover_steering.xml b/conf/modules/guidance_rover_steering.xml
new file mode 100644
index 0000000000..3faaa80caf
--- /dev/null
+++ b/conf/modules/guidance_rover_steering.xml
@@ -0,0 +1,46 @@
+
+
+
+
+
+ Steering guidance for rover.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ @navigation
+ guidance,commands
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/gvf_module.xml b/conf/modules/gvf_module.xml
index f5b7ae21eb..7ab2456e0f 100644
--- a/conf/modules/gvf_module.xml
+++ b/conf/modules/gvf_module.xml
@@ -78,5 +78,13 @@ For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_
+
+
+
+
+
+
+
+
diff --git a/conf/radios/UCM/T16SZ_SBUS_rover.xml b/conf/radios/UCM/T16SZ_SBUS_rover.xml
new file mode 100644
index 0000000000..006a9bfab3
--- /dev/null
+++ b/conf/radios/UCM/T16SZ_SBUS_rover.xml
@@ -0,0 +1,63 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/firmwares/rover/guidance/rover_guidance_steering.c b/sw/airborne/firmwares/rover/guidance/rover_guidance_steering.c
new file mode 100644
index 0000000000..8ce33140d3
--- /dev/null
+++ b/sw/airborne/firmwares/rover/guidance/rover_guidance_steering.c
@@ -0,0 +1,119 @@
+/*
+ * Copyright (C) 2021 Jesús Bautista
+ * Hector García
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ */
+
+#define AUTOPILOT_CORE_GUIDANCE_C
+
+/** Mandatory dependencies header **/
+#include "firmwares/rover/guidance/rover_guidance_steering.h"
+
+#include "generated/airframe.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
+
+// 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;
+
+ last_speed_cmd = 0.0;
+ last_ap_mode = AP_MODE_KILL;
+
+ guidance_control.speed_error = 0.0;
+ guidance_control.kf = SR_MEASURED_KF;
+ guidance_control.kp = 10;
+ guidance_control.ki = 100;
+
+ init_pid_f(&rover_pid, guidance_control.kp, 0.f, guidance_control.ki, MAX_PPRZ*0.2);
+
+ // Based on autopilot state machine frequency
+ time_step = 1.f/PERIODIC_FREQUENCY;
+}
+
+/** CTRL functions **/
+// Steering control (GVF)
+void rover_guidance_steering_heading_ctrl(void)
+{
+ float delta = 0.0;
+ float omega = guidance_control.gvf_omega; //GVF give us this omega
+
+ // 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));
+ }
+
+ guidance_control.cmd.delta = BoundDelta(delta);
+}
+
+// Speed control (feed feed forward + propotional + integral controler) (PID)
+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
+ 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));
+}
+
+
+/** PID RESET function**/
+void rover_guidance_steering_pid_reset(void)
+{
+ // Reset speed PID
+ if (rover_pid.sum != 0) {
+ reset_pid_f(&rover_pid);
+ }
+}
+
+void rover_guidance_steering_kill(void)
+{
+ guidance_control.cmd.delta = 0.0;
+ guidance_control.cmd.speed = 0.0;
+}
diff --git a/sw/airborne/firmwares/rover/guidance/rover_guidance_steering.h b/sw/airborne/firmwares/rover/guidance/rover_guidance_steering.h
new file mode 100644
index 0000000000..4ddb9681ba
--- /dev/null
+++ b/sw/airborne/firmwares/rover/guidance/rover_guidance_steering.h
@@ -0,0 +1,153 @@
+/*
+ * Copyright (C) 2021 Jesús Bautista
+ * Hector García
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ */
+
+#ifndef ROVER_GUIDANCE_STEERING_H
+#define ROVER_GUIDANCE_STEERING_H
+
+/** Generated airframe.h from airframe.xml
+ * - fun: SetActuatorsFromCommands
+ * - var: commands
+ * - var: hardware and construction parameters
+ **/
+
+#include "std.h"
+#include
+
+#include "generated/airframe.h"
+
+// Check critical global definitiones
+#ifndef SERVO_MOTOR_THROTTLE
+#error "Steering rover firmware requires the servo MOTOR_THROTTLE"
+#endif
+
+#ifndef SERVO_MOTOR_STEERING
+#error "Steering rover firmware requires the servo MOTOR_STEERING"
+#endif
+
+#ifndef COMMAND_THROTTLE
+#error "Steering rover firmware requires the command COMMAND_THROTTLE"
+#endif
+
+#ifndef COMMAND_STEERING
+#error "Steering rover firmware requires the command COMMAND_STEERING"
+#endif
+
+
+/** Global variables definitions **/
+
+// 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
+#define MAX_DELTA 15.0
+#endif
+#ifndef MIN_DELTA
+#define MIN_DELTA MAX_DELTA
+#endif
+
+// This variables allow you to configurate de max and min
+// steering actuator signal. There is a mecanic limitation
+// for the actuator in the steering of our rover, so we have
+// to limit the actuator travel.
+#ifndef MAX_CMD_SHUT
+#define MAX_CMD_SHUT 0
+#endif
+#ifndef MIN_CMD_SHUT
+#define MIN_CMD_SHUT 0
+#endif
+
+// MIN_SPEED, MAX_SPEED: Min and max state speed (m/s)
+#ifndef MAX_SPEED
+#define MAX_SPEED 999.0 //We don't really use that variable
+#endif
+#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).
+
+// DRIVE_SHAFT_DISTANCE: Distance between front and rear wheels (m)
+#ifndef DRIVE_SHAFT_DISTANCE
+#define DRIVE_SHAFT_DISTANCE 0.25
+#warning "Construction variable DRIVE_SHAFT_DISTANCE for steering wheel rover not defined"
+#endif
+
+// SR_MEASURED_KF: Lineal feed forward control constant (have to be measured in new servos)
+#ifndef SR_MEASURED_KF
+#define SR_MEASURED_KF 10
+#warning "Construction constant SR_MEASURED_KF for steering wheel rover not defined"
+#endif
+
+
+/** Steering rover guidance STRUCTURES **/
+// High level commands
+typedef struct {
+ float speed;
+ float delta;
+} sr_cmd_t;
+
+// Main structure
+typedef struct {
+ sr_cmd_t cmd;
+ float gvf_omega;
+ float throttle;
+
+ float speed_error;
+ float kf;
+ float kp;
+ float ki;
+} rover_ctrl;
+
+extern rover_ctrl guidance_control;
+
+/** Steering rover guidance EXT FUNCTIONS **/
+extern void rover_guidance_steering_init(void);
+extern void rover_guidance_steering_heading_ctrl(void);
+extern void rover_guidance_steering_speed_ctrl(void);
+extern void rover_guidance_steering_pid_reset(void);
+extern void rover_guidance_steering_kill(void);
+
+
+/** MACROS **/
+// Bound delta
+#define BoundDelta(delta) (delta < -MIN_DELTA ? -MIN_DELTA : \
+ (delta > MAX_DELTA ? MAX_DELTA : \
+ delta));
+
+// Bound speed
+#define BoundSpeed(speed) (speed < MIN_SPEED ? MIN_SPEED : \
+ (speed > MAX_SPEED ? MAX_SPEED : \
+ speed));
+
+// Bound throttle
+#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));
+
+// This macro is for NAV state
+#define GetCmdFromThrottle(throttle) TRIM_PPRZ((int)throttle);
+
+// Set AP throttle value
+#define SetAPThrottleFromCommands(void) { \
+ autopilot.throttle = commands[COMMAND_THROTTLE]; \
+ }
+
+#endif // ROVER_GUIDANCE_STEERING_H
diff --git a/sw/airborne/firmwares/rover/navigation.h b/sw/airborne/firmwares/rover/navigation.h
index 538f9474b9..5d1e0f1840 100644
--- a/sw/airborne/firmwares/rover/navigation.h
+++ b/sw/airborne/firmwares/rover/navigation.h
@@ -174,6 +174,7 @@ extern void nav_home(void);
extern void nav_set_manual(float speed, float turn);
extern void nav_reset_reference(void) __attribute__((unused));
+extern void nav_reset_alt(void) __attribute__((unused));
extern void nav_periodic_task(void);
extern bool nav_is_in_flight(void);
diff --git a/sw/airborne/modules/guidance/gvf/gvf.c b/sw/airborne/modules/guidance/gvf/gvf.c
index 30568cb45c..f987d5b0f7 100644
--- a/sw/airborne/modules/guidance/gvf/gvf.c
+++ b/sw/airborne/modules/guidance/gvf/gvf.c
@@ -28,9 +28,19 @@
#include "modules/guidance/gvf/trajectories/gvf_line.h"
#include "modules/guidance/gvf/trajectories/gvf_sin.h"
+#ifdef FIXEDWING_FIRMWARE
#include "firmwares/fixedwing/nav.h"
#include "modules/nav/common_nav.h"
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
+#elif defined(ROVER_FIRMWARE)
+#include "state.h"
+#include "modules/nav/nav_rover_base.h"
+#include "firmwares/rover/navigation.h"
+struct EnuCoor_f gvf_draw_wp;
+#else
+#error "Firmware not supported by GVF!"
+#endif
+
#include "autopilot.h"
// Control
@@ -83,7 +93,7 @@ static void send_circle(struct transport_tx *trans, struct link_device *dev)
if (delta_T < 200)
if (gvf_trajectory.type == ELLIPSE &&
- (gvf_trajectory.p[2] == gvf_trajectory.p[3])) {
+ ((int)gvf_trajectory.p[2] == (int)gvf_trajectory.p[3])) {
pprz_msg_send_CIRCLE(trans, dev, AC_ID,
&gvf_trajectory.p[0], &gvf_trajectory.p[1],
&gvf_trajectory.p[2]);
@@ -156,11 +166,19 @@ void gvf_control_2D(float ke, float kn, float e,
{
gvf_t0 = get_sys_time_msec();
- struct FloatEulers *att = stateGetNedToBodyEulers_f();
- float ground_speed = stateGetHorizontalSpeedNorm_f();
- float course = stateGetHorizontalSpeedDir_f();
- float px_dot = ground_speed * sinf(course);
- float py_dot = ground_speed * cosf(course);
+ #if defined(FIXEDWING_FIRMWARE)
+ float ground_speed = stateGetHorizontalSpeedNorm_f();
+ float course = stateGetHorizontalSpeedDir_f();
+ float px_dot = ground_speed * sinf(course);
+ float py_dot = ground_speed * cosf(course);
+ #elif defined(ROVER_FIRMWARE)
+ // We assume that the course and psi
+ // of the rover (steering wheel) are the same
+ float course = stateGetNedToBodyEulers_f()->psi;
+ float px_dot = stateGetSpeedEnu_f()->x;
+ float py_dot = stateGetSpeedEnu_f()->y;
+ #endif
+
int s = gvf_control.s;
// gradient Phi
@@ -209,14 +227,28 @@ void gvf_control_2D(float ke, float kn, float e,
float omega = omega_d + kn * (mr_x * md_y - mr_y * md_x);
- // Coordinated turn
+ #if defined(FIXEDWING_FIRMWARE)
if (autopilot_get_mode() == AP_MODE_AUTO2) {
+
+ // Coordinated turn
+ struct FloatEulers *att = stateGetNedToBodyEulers_f();
+ float ground_speed = stateGetHorizontalSpeedNorm_f();
+
+ lateral_mode = LATERAL_MODE_ROLL;
+
h_ctl_roll_setpoint =
-atanf(omega * ground_speed / GVF_GRAVITY / cosf(att->theta));
BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
-
- lateral_mode = LATERAL_MODE_ROLL;
}
+
+ #elif defined(ROVER_FIRMWARE)
+ if (autopilot_get_mode() != AP_MODE_DIRECT) {
+ guidance_control.gvf_omega = omega; //TODO: set omega into external GVF variable
+ }
+
+ #else
+ #error GVF does not support your firmware yet
+ #endif
}
void gvf_set_direction(int8_t s)
@@ -224,6 +256,10 @@ void gvf_set_direction(int8_t s)
gvf_control.s = s;
}
+
+
+#ifdef FIXEDWING_FIRMWARE // FIXEDWING TRAJECTORIES
+
// STRAIGHT LINE
static void gvf_line(float a, float b, float heading)
@@ -242,8 +278,9 @@ static void gvf_line(float a, float b, float heading)
gvf_control_2D(1e-2 * gvf_line_par.ke, gvf_line_par.kn, e, &grad_line, &Hess_line);
gvf_control.error = e;
-
+
horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
+
gvf_segment.seg = 0;
}
@@ -260,7 +297,7 @@ bool gvf_line_XY1_XY2(float x1, float y1, float x2, float y2)
float zy = y2 - y1;
gvf_line_XY_heading(x1, y1, atan2f(zx, zy));
-
+
horizontal_mode = HORIZONTAL_MODE_ROUTE;
gvf_segment.seg = 1;
gvf_segment.x1 = x1;
@@ -295,6 +332,7 @@ bool gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1,
gvf_line(x1, y1, alpha);
horizontal_mode = HORIZONTAL_MODE_ROUTE;
+
gvf_segment.seg = 1;
gvf_segment.x1 = x1;
gvf_segment.y1 = y1;
@@ -348,7 +386,7 @@ bool gvf_segment_wp1_wp2(uint8_t wp1, uint8_t wp2)
bool gvf_line_wp_heading(uint8_t wp, float heading)
{
- heading = heading * M_PI / 180;
+ heading = RadOfDeg(heading);
float a = waypoints[wp].x;
float b = waypoints[wp].y;
@@ -370,15 +408,16 @@ bool gvf_ellipse_XY(float x, float y, float a, float b, float alpha)
gvf_trajectory.p[2] = a;
gvf_trajectory.p[3] = b;
gvf_trajectory.p[4] = alpha;
-
+
// SAFE MODE
if (a < 1 || b < 1) {
gvf_trajectory.p[2] = 60;
gvf_trajectory.p[3] = 60;
}
- if (gvf_trajectory.p[2] == gvf_trajectory.p[3]) {
+ if ((int)gvf_trajectory.p[2] == (int)gvf_trajectory.p[3]) {
horizontal_mode = HORIZONTAL_MODE_CIRCLE;
+
} else {
horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
}
@@ -447,7 +486,7 @@ bool gvf_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off, float A)
bool gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off, float A)
{
w = 2 * M_PI * w;
- alpha = alpha * M_PI / 180;
+ alpha = RadOfDeg(alpha);
float x = waypoints[wp].x;
float y = waypoints[wp].y;
@@ -457,3 +496,259 @@ bool gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off, float A)
return true;
}
+
+#elif defined(ROVER_FIRMWARE) // ROVER TRAJECTORIES
+
+// STRAIGHT LINE
+
+static void gvf_line(float a, float b, float heading)
+{
+ float e;
+ struct gvf_grad grad_line;
+ struct gvf_Hess Hess_line;
+
+ gvf_trajectory.type = 0;
+ gvf_trajectory.p[0] = a;
+ gvf_trajectory.p[1] = b;
+ gvf_trajectory.p[2] = heading;
+
+ gvf_line_info(&e, &grad_line, &Hess_line);
+ gvf_control.ke = gvf_line_par.ke;
+ gvf_control_2D(1e-2 * gvf_line_par.ke, gvf_line_par.kn, e, &grad_line, &Hess_line);
+
+ gvf_control.error = e;
+
+ nav.mode = NAV_MODE_WAYPOINT;
+
+ gvf_segment.seg = 0;
+}
+
+bool gvf_line_XY_heading(float a, float b, float heading)
+{
+ gvf_set_direction(1);
+ gvf_line(a, b, heading);
+ return true;
+}
+
+bool gvf_line_XY1_XY2(float x1, float y1, float x2, float y2)
+{
+ float zx = x2 - x1;
+ float zy = y2 - y1;
+
+ gvf_line_XY_heading(x1, y1, atan2f(zx, zy));
+
+ nav.mode = NAV_MODE_ROUTE;
+
+ gvf_segment.seg = 1;
+ gvf_segment.x1 = x1;
+ gvf_segment.y1 = y1;
+ gvf_segment.x2 = x2;
+ gvf_segment.y2 = y2;
+
+ // Send rover_base navigation data to draw segment (ROUTE)
+ nav_rover_base.goto_wp.from.x = x1;
+ nav_rover_base.goto_wp.from.y = y1;
+ nav_rover_base.goto_wp.to.x = x2;
+ nav_rover_base.goto_wp.to.y = y2;
+
+ return true;
+}
+
+bool gvf_line_wp1_wp2(uint8_t wp1, uint8_t wp2)
+{
+ float x1 = waypoint_get_x(wp1);
+ float y1 = waypoint_get_y(wp1);
+ float x2 = waypoint_get_x(wp2);
+ float y2 = waypoint_get_y(wp2);
+
+ return gvf_line_XY1_XY2(x1, y1, x2, y2);
+}
+
+bool gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1, float d2)
+{
+ int s = out_of_segment_area(x1, y1, x2, y2, d1, d2);
+ if (s != 0) {
+ gvf_control.s = s;
+ }
+
+ float zx = x2 - x1;
+ float zy = y2 - y1;
+ float alpha = atanf(zx / zy);
+
+ gvf_line(x1, y1, alpha);
+
+ nav.mode = NAV_MODE_ROUTE;
+
+ gvf_segment.seg = 1;
+ gvf_segment.x1 = x1;
+ gvf_segment.y1 = y1;
+ gvf_segment.x2 = x2;
+ gvf_segment.y2 = y2;
+
+ // Send rover_base navigation data to draw segment (ROUTE)
+ nav_rover_base.goto_wp.from.x = x1;
+ nav_rover_base.goto_wp.from.y = y1;
+ nav_rover_base.goto_wp.to.x = x2;
+ nav_rover_base.goto_wp.to.y = y2;
+
+ return true;
+}
+
+bool gvf_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float d2)
+{
+ float x1 = waypoint_get_x(wp1);
+ float y1 = waypoint_get_y(wp1);
+ float x2 = waypoint_get_x(wp2);
+ float y2 = waypoint_get_y(wp2);
+
+ return gvf_segment_loop_XY1_XY2(x1, y1, x2, y2, d1, d2);
+}
+
+bool gvf_segment_XY1_XY2(float x1, float y1, float x2, float y2)
+{
+ struct EnuCoor_f *p = stateGetPositionEnu_f();
+ float px = p->x - x1;
+ float py = p->y - y1;
+
+ float zx = x2 - x1;
+ float zy = y2 - y1;
+
+ float beta = atan2f(zy, zx);
+ float cosb = cosf(-beta);
+ float sinb = sinf(-beta);
+ float zxr = zx * cosb - zy * sinb;
+ float pxr = px * cosb - py * sinb;
+
+ if ((zxr > 0 && pxr > zxr) || (zxr < 0 && pxr < zxr)) {
+ return false;
+ }
+
+ return gvf_line_XY1_XY2(x1, y1, x2, y2);
+}
+
+bool gvf_segment_wp1_wp2(uint8_t wp1, uint8_t wp2)
+{
+ float x1 = waypoint_get_x(wp1);
+ float y1 = waypoint_get_y(wp1);
+ float x2 = waypoint_get_x(wp2);
+ float y2 = waypoint_get_y(wp2);
+
+ return gvf_segment_XY1_XY2(x1, y1, x2, y2);
+}
+
+bool gvf_line_wp_heading(uint8_t wp, float heading)
+{
+ heading = RadOfDeg(heading);
+
+ float a = waypoint_get_x(wp);
+ float b = waypoint_get_y(wp);
+
+ return gvf_line_XY_heading(a, b, heading);
+}
+
+// ELLIPSE
+
+bool gvf_ellipse_XY(float x, float y, float a, float b, float alpha)
+{
+ float e;
+ struct gvf_grad grad_ellipse;
+ struct gvf_Hess Hess_ellipse;
+
+ gvf_trajectory.type = 1;
+ gvf_trajectory.p[0] = x;
+ gvf_trajectory.p[1] = y;
+ gvf_trajectory.p[2] = a;
+ gvf_trajectory.p[3] = b;
+ gvf_trajectory.p[4] = alpha;
+
+ // SAFE MODE
+ if (a < 1 || b < 1) {
+ gvf_trajectory.p[2] = 60;
+ gvf_trajectory.p[3] = 60;
+ }
+
+ // Send rover_base navigation data to draw circle (CIRCLE)
+ if (gvf_trajectory.p[2] == gvf_trajectory.p[3]) {
+ nav.mode = NAV_MODE_CIRCLE;
+ nav_rover_base.circle.center.x = x;
+ nav_rover_base.circle.center.y = y;
+ nav_rover_base.circle.radius = a;
+ } else {
+ nav.mode = NAV_MODE_WAYPOINT;
+ }
+
+ gvf_ellipse_info(&e, &grad_ellipse, &Hess_ellipse);
+ gvf_control.ke = gvf_ellipse_par.ke;
+ gvf_control_2D(gvf_ellipse_par.ke, gvf_ellipse_par.kn,
+ e, &grad_ellipse, &Hess_ellipse);
+
+ gvf_control.error = e;
+
+ return true;
+}
+
+
+bool gvf_ellipse_wp(uint8_t wp, float a, float b, float alpha)
+{
+ gvf_ellipse_XY(waypoint_get_x(wp), waypoint_get_y(wp), a, b, alpha);
+ return true;
+}
+
+// SINUSOIDAL (if w = 0 and off = 0, then we just have the straight line case)
+
+bool gvf_sin_XY_alpha(float a, float b, float alpha, float w, float off, float A)
+{
+ float e;
+ struct gvf_grad grad_line;
+ struct gvf_Hess Hess_line;
+
+ gvf_trajectory.type = 2;
+ gvf_trajectory.p[0] = a;
+ gvf_trajectory.p[1] = b;
+ gvf_trajectory.p[2] = alpha;
+ gvf_trajectory.p[3] = w;
+ gvf_trajectory.p[4] = off;
+ gvf_trajectory.p[5] = A;
+
+ gvf_sin_info(&e, &grad_line, &Hess_line);
+ gvf_control.ke = gvf_sin_par.ke;
+ gvf_control_2D(1e-2 * gvf_sin_par.ke, gvf_sin_par.kn, e, &grad_line, &Hess_line);
+
+ gvf_control.error = e;
+
+ return true;
+}
+
+bool gvf_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off, float A)
+{
+ w = 2 * M_PI * w;
+
+ float x1 = waypoint_get_x(wp1);
+ float y1 = waypoint_get_y(wp1);
+ float x2 = waypoint_get_x(wp2);
+ float y2 = waypoint_get_y(wp2);
+
+ float zx = x1 - x2;
+ float zy = y1 - y2;
+
+ float alpha = atanf(zy / zx);
+
+ gvf_sin_XY_alpha(x1, y1, alpha, w, off, A);
+
+ return true;
+}
+
+bool gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off, float A)
+{
+ w = 2 * M_PI * w;
+ alpha = RadOfDeg(alpha);
+
+ float x = waypoint_get_x(wp);
+ float y = waypoint_get_y(wp);
+
+ gvf_sin_XY_alpha(x, y, alpha, w, off, A);
+
+ return true;
+}
+#endif
+
diff --git a/sw/simulator/nps/nps_autopilot_rover.c b/sw/simulator/nps/nps_autopilot_rover.c
new file mode 100644
index 0000000000..5e091dddb1
--- /dev/null
+++ b/sw/simulator/nps/nps_autopilot_rover.c
@@ -0,0 +1,211 @@
+/*
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+#include "nps_autopilot.h"
+
+#include "main_ap.h"
+#include "nps_sensors.h"
+#include "nps_radio_control.h"
+#include "nps_electrical.h"
+#include "nps_fdm.h"
+
+#include "modules/radio_control/radio_control.h"
+#include "modules/imu/imu.h"
+#include "mcu_periph/sys_time.h"
+#include "state.h"
+#include "modules/ahrs/ahrs.h"
+#include "modules/ins/ins.h"
+#include "math/pprz_algebra.h"
+
+/**
+#ifndef NPS_NO_MOTOR_MIXING
+#include "modules/actuators/motor_mixing.h"
+
+#if NPS_COMMANDS_NB != MOTOR_MIXING_NB_MOTOR
+#warning "NPS_COMMANDS_NB does not match MOTOR_MIXING_NB_MOTOR!"
+#endif
+#endif
+**/
+
+#include "modules/core/abi.h"
+
+#include "pprzlink/messages.h"
+#include "modules/datalink/downlink.h"
+
+// for datalink_time hack
+#include "modules/datalink/datalink.h"
+#include "modules/actuators/actuators.h"
+
+struct NpsAutopilot nps_autopilot;
+bool nps_bypass_ahrs;
+bool nps_bypass_ins;
+
+#ifndef NPS_BYPASS_AHRS
+#define NPS_BYPASS_AHRS FALSE
+#endif
+
+#ifndef NPS_BYPASS_INS
+#define NPS_BYPASS_INS FALSE
+#endif
+
+#if INDI_RPM_FEEDBACK
+#error "INDI_RPM_FEEDBACK can not be used in simulation!"
+#endif
+
+void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char *rc_dev)
+{
+ nps_autopilot.launch = TRUE;
+
+ nps_radio_control_init(type_rc, num_rc_script, rc_dev);
+ nps_electrical_init();
+
+ nps_bypass_ahrs = NPS_BYPASS_AHRS;
+ nps_bypass_ins = NPS_BYPASS_INS;
+
+ modules_mcu_init();
+ main_ap_init();
+
+}
+
+void nps_autopilot_run_systime_step(void)
+{
+ sys_tick_handler();
+}
+
+#include
+#include "modules/gps/gps.h"
+
+void nps_autopilot_run_step(double time)
+{
+
+ nps_electrical_run_step(time);
+
+#if RADIO_CONTROL && !RADIO_CONTROL_TYPE_DATALINK
+ if (nps_radio_control_available(time)) {
+ radio_control_feed();
+ main_ap_event();
+ }
+#endif
+
+ if (nps_sensors_gyro_available()) {
+ imu_feed_gyro_accel();
+ main_ap_event();
+ }
+
+ if (nps_sensors_mag_available()) {
+ imu_feed_mag();
+ main_ap_event();
+ }
+
+ if (nps_sensors_baro_available()) {
+ uint32_t now_ts = get_sys_time_usec();
+ float pressure = (float) sensors.baro.value;
+ AbiSendMsgBARO_ABS(BARO_SIM_SENDER_ID, now_ts, pressure);
+ main_ap_event();
+ }
+
+ if (nps_sensors_temperature_available()) {
+ AbiSendMsgTEMPERATURE(BARO_SIM_SENDER_ID, (float)sensors.temp.value);
+ main_ap_event();
+ }
+
+#if USE_AIRSPEED
+ if (nps_sensors_airspeed_available()) {
+ stateSetAirspeed_f((float)sensors.airspeed.value);
+ AbiSendMsgAIRSPEED(AIRSPEED_NPS_ID, (float)sensors.airspeed.value);
+ main_ap_event();
+ }
+#endif
+
+#if USE_SONAR
+ if (nps_sensors_sonar_available()) {
+ uint32_t now_ts = get_sys_time_usec();
+ float dist = (float) sensors.sonar.value;
+ if (dist >= 0.0) {
+ AbiSendMsgAGL(AGL_SONAR_NPS_ID, now_ts, dist);
+ }
+
+#ifdef SENSOR_SYNC_SEND_SONAR
+ uint16_t foo = 0;
+ DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &foo, &dist);
+#endif
+
+ main_ap_event();
+ }
+#endif
+
+#if USE_GPS
+ if (nps_sensors_gps_available()) {
+ gps_feed_value();
+ main_ap_event();
+ }
+#endif
+
+ if (nps_bypass_ahrs) {
+ sim_overwrite_ahrs();
+ }
+
+ if (nps_bypass_ins) {
+ sim_overwrite_ins();
+ }
+
+ main_ap_periodic();
+
+ /* feeding the fdm with raw low level commands */
+ for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) {
+ commands[i] = autopilot_get_motors_on() ? commands[i] : 0;
+ nps_autopilot.commands[i] = (double)commands[i] / MAX_PPRZ;
+ }
+
+ PRINT_CONFIG_MSG("Using throttle, steering commands because rover's fdm don't have explicit actuators.")
+}
+
+
+void sim_overwrite_ahrs(void)
+{
+
+ struct FloatQuat quat_f;
+ QUAT_COPY(quat_f, fdm.ltp_to_body_quat);
+ stateSetNedToBodyQuat_f(&quat_f);
+
+ struct FloatRates rates_f;
+ RATES_COPY(rates_f, fdm.body_ecef_rotvel);
+ stateSetBodyRates_f(&rates_f);
+
+}
+
+void sim_overwrite_ins(void)
+{
+
+ struct NedCoor_f ltp_pos;
+ VECT3_COPY(ltp_pos, fdm.ltpprz_pos);
+ stateSetPositionNed_f(<p_pos);
+
+ struct NedCoor_f ltp_speed;
+ VECT3_COPY(ltp_speed, fdm.ltpprz_ecef_vel);
+ stateSetSpeedNed_f(<p_speed);
+
+ struct NedCoor_f ltp_accel;
+ VECT3_COPY(ltp_accel, fdm.ltpprz_ecef_accel);
+ stateSetAccelNed_f(<p_accel);
+
+ // Here we make sure that ENU states are recalculated
+ stateCalcPositionEnu_i();
+}
diff --git a/sw/simulator/nps/nps_fdm_rover.c b/sw/simulator/nps/nps_fdm_rover.c
new file mode 100644
index 0000000000..e3625447d9
--- /dev/null
+++ b/sw/simulator/nps/nps_fdm_rover.c
@@ -0,0 +1,221 @@
+/*
+ * Copyright (C) 2021 Jesús Bautista
+ * Hector García
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ */
+
+#include "nps_fdm.h"
+
+#include
+#include
+
+#include "math/pprz_geodetic.h"
+#include "math/pprz_geodetic_double.h"
+#include "math/pprz_geodetic_float.h"
+#include "math/pprz_algebra.h"
+#include "math/pprz_algebra_float.h"
+#include "math/pprz_isa.h"
+
+#include "generated/airframe.h"
+#include "generated/flight_plan.h"
+
+#include "firmwares/rover/guidance/rover_guidance_steering.h"
+#include "state.h"
+
+// Check if rover firmware
+#ifndef ROVER_FIRMWARE
+#error "The module nps_fdm_rover is designed for rovers and doesn't support other firmwares!!"
+#endif
+
+// NpsFdm structure
+struct NpsFdm fdm;
+
+// Reference point
+static struct LtpDef_d ltpdef;
+
+// Static functions declaration
+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;
+
+
+/** 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.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();
+}
+
+void nps_fdm_run_step(bool launch __attribute__((unused)), double *commands, int commands_nb __attribute__((unused)))
+{
+
+ /****************************************************************************
+ PHYSICAL MODEL
+ -------------
+ The physical model of your rover goes here. This physics takes place in
+ the LTP plane (so we transfer every integration step to NED and ECEF at the end).
+ */
+
+ #ifdef COMMAND_STEERING // STEERING ROVER PHYSICS
+
+ // Steering rover cmds:
+ // COMMAND_STEERING -> delta parameter
+ // COMMAND_TRHOTTLE -> acceleration in heading direction
+
+ double delta = RadOfDeg(commands[COMMAND_STEERING] * MAX_DELTA);
+
+ /** 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 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];
+
+ // 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;
+
+ #else
+ #warning "The physics of this rover are not yet implemented in nps_fdm_rover!!"
+ #endif // STEERING ROVER PHYSICS
+
+
+ /****************************************************************************/
+ // 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);
+
+ /* 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);
+
+ /* 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;
+
+ // 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;
+
+}
+
+
+/**************************
+ ** Generating LTP plane **
+ **************************/
+
+static void init_ltp(void)
+{
+
+ struct LlaCoor_d llh_nav0; /* Height above the ellipsoid */
+ llh_nav0.lat = RadOfDeg((double)NAV_LAT0 / 1e7);
+ llh_nav0.lon = RadOfDeg((double)NAV_LON0 / 1e7);
+
+ 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;
+
+ fdm.ltp_g.x = 0.;
+ fdm.ltp_g.y = 0.;
+ fdm.ltp_g.z = 0.; // accel data are already with the correct format
+
+
+#ifdef AHRS_H_X
+ fdm.ltp_h.x = AHRS_H_X;
+ fdm.ltp_h.y = AHRS_H_Y;
+ 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;
+#endif
+
+}
+
+
+/*****************************************************/
+// Atmosphere function (we don't need that features) //
+void nps_fdm_set_wind(double speed __attribute__((unused)),
+ double dir __attribute__((unused)))
+{
+}
+
+void nps_fdm_set_wind_ned(double wind_north __attribute__((unused)),
+ double wind_east __attribute__((unused)),
+ double wind_down __attribute__((unused)))
+{
+}
+
+void nps_fdm_set_turbulence(double wind_speed __attribute__((unused)),
+ int turbulence_severity __attribute__((unused)))
+{
+}
+
+void nps_fdm_set_temperature(double temp __attribute__((unused)),
+ double h __attribute__((unused)))
+{
+}
+