diff --git a/conf/airframes/ENAC/rover.xml b/conf/airframes/ENAC/rover.xml
new file mode 100644
index 0000000000..1f706e57f7
--- /dev/null
+++ b/conf/airframes/ENAC/rover.xml
@@ -0,0 +1,118 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/autopilot/rover.xml b/conf/autopilot/rover.xml
new file mode 100644
index 0000000000..c251992c35
--- /dev/null
+++ b/conf/autopilot/rover.xml
@@ -0,0 +1,105 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/firmwares/rover.makefile b/conf/firmwares/rover.makefile
new file mode 100644
index 0000000000..514c91528a
--- /dev/null
+++ b/conf/firmwares/rover.makefile
@@ -0,0 +1,167 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# Copyright (C) 2018 Gautier Hattenberger
+#
+# 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.
+#
+#
+
+CFG_SHARED=$(PAPARAZZI_SRC)/conf/firmwares/subsystems/shared
+
+SRC_BOARD=boards/$(BOARD)
+SRC_FIRMWARE=firmwares/rover
+SRC_SUBSYSTEMS=subsystems
+SRC_MODULES=modules
+
+SRC_ARCH=arch/$(ARCH)
+
+ROTORCRAFT_INC = -I$(SRC_FIRMWARE) -I$(SRC_BOARD)
+
+ap.ARCHDIR = $(ARCH)
+
+
+VPATH += $(PAPARAZZI_HOME)/var/share
+
+######################################################################
+##
+## COMMON ROVER ALL TARGETS (AP)
+##
+
+$(TARGET).CFLAGS += $(ROTORCRAFT_INC)
+$(TARGET).CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+$(TARGET).CFLAGS += -DPERIPHERALS_AUTO_INIT
+$(TARGET).srcs += mcu.c
+$(TARGET).srcs += $(SRC_ARCH)/mcu_arch.c
+
+# frequency of main periodic
+PERIODIC_FREQUENCY ?= 100
+$(TARGET).CFLAGS += -DPERIODIC_FREQUENCY=$(PERIODIC_FREQUENCY)
+
+ifdef AHRS_PROPAGATE_FREQUENCY
+$(TARGET).CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(AHRS_PROPAGATE_FREQUENCY)
+endif
+
+ifdef AHRS_CORRECT_FREQUENCY
+$(TARGET).CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY)
+endif
+
+ifdef AHRS_MAG_CORRECT_FREQUENCY
+$(TARGET).CFLAGS += -DAHRS_MAG_CORRECT_FREQUENCY=$(AHRS_MAG_CORRECT_FREQUENCY)
+endif
+
+
+#
+# Systime
+#
+$(TARGET).srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c
+ifeq ($(ARCH), linux)
+# seems that we need to link against librt for glibc < 2.17
+$(TARGET).LDFLAGS += -lrt
+endif
+
+
+#
+# Math functions
+#
+$(TARGET).srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c math/pprz_trig_int.c math/pprz_orientation_conversion.c math/pprz_algebra_int.c math/pprz_algebra_float.c math/pprz_algebra_double.c math/pprz_stat.c
+
+$(TARGET).srcs += subsystems/settings.c
+$(TARGET).srcs += $(SRC_ARCH)/subsystems/settings_arch.c
+
+$(TARGET).srcs += subsystems/actuators.c
+$(TARGET).srcs += subsystems/commands.c
+
+$(TARGET).srcs += state.c
+
+#
+# BARO_BOARD (if existing/configured)
+#
+include $(CFG_SHARED)/baro_board.makefile
+
+#
+# Main
+#
+# based on ChibiOS
+#
+ifeq ($(RTOS), chibios)
+$(TARGET).srcs += $(SRC_FIRMWARE)/main_chibios.c
+endif # RTOS
+$(TARGET).srcs += $(SRC_FIRMWARE)/main_ap.c
+$(TARGET).srcs += autopilot.c
+$(TARGET).srcs += $(SRC_FIRMWARE)/autopilot_firmware.c
+$(TARGET).srcs += $(SRC_FIRMWARE)/autopilot_utils.c
+ifeq ($(USE_GENERATED_AUTOPILOT), TRUE)
+$(TARGET).srcs += $(SRC_FIRMWARE)/autopilot_generated.c
+$(TARGET).CFLAGS += -DUSE_GENERATED_AUTOPILOT=1
+else
+$(error "Rover firmware should use generated autopilot")
+endif
+
+
+
+######################################################################
+##
+## COMMON HARDWARE SUPPORT FOR ALL TARGETS
+##
+
+$(TARGET).srcs += mcu_periph/i2c.c
+$(TARGET).srcs += $(SRC_ARCH)/mcu_periph/i2c_arch.c
+
+include $(CFG_SHARED)/uart.makefile
+
+
+#
+# Electrical subsystem / Analog Backend
+#
+$(TARGET).CFLAGS += -DUSE_ADC
+$(TARGET).srcs += $(SRC_ARCH)/mcu_periph/adc_arch.c
+$(TARGET).srcs += subsystems/electrical.c
+
+
+######################################################################
+##
+## HARDWARE SUPPORT FOR ALL NON-SIMULATION TARGETS (ap)
+##
+ifeq ($(ARCH), stm32)
+ns_srcs += $(SRC_ARCH)/mcu_periph/gpio_arch.c
+endif
+
+ifeq ($(ARCH), chibios)
+ns_srcs += $(SRC_ARCH)/mcu_periph/gpio_arch.c
+endif
+
+#
+# LEDs
+#
+ns_CFLAGS += -DUSE_LED
+ifneq ($(SYS_TIME_LED),none)
+ns_CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED)
+endif
+
+ifeq ($(ARCH), stm32)
+ns_srcs += $(SRC_ARCH)/led_hw.c
+endif
+
+######################################################################
+##
+## Final Target Allocations
+##
+
+ap.CFLAGS += $(ns_CFLAGS)
+ap.srcs += $(ns_srcs)
+
diff --git a/conf/modules/guidance_rover.xml b/conf/modules/guidance_rover.xml
new file mode 100644
index 0000000000..66ebc8cd16
--- /dev/null
+++ b/conf/modules/guidance_rover.xml
@@ -0,0 +1,28 @@
+
+
+
+
+
+ Base guidance code for rover
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/ins_gps_passthrough.xml b/conf/modules/ins_gps_passthrough.xml
index 120c62bcfc..4b26605e65 100644
--- a/conf/modules/ins_gps_passthrough.xml
+++ b/conf/modules/ins_gps_passthrough.xml
@@ -21,4 +21,9 @@
+
+
+
+
+
diff --git a/conf/modules/nav_rover_base.xml b/conf/modules/nav_rover_base.xml
new file mode 100644
index 0000000000..c736a7ff18
--- /dev/null
+++ b/conf/modules/nav_rover_base.xml
@@ -0,0 +1,31 @@
+
+
+
+
+
+ Basic navigation functions for Rovers
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/telemetry_xbee_api.xml b/conf/modules/telemetry_xbee_api.xml
index fc550cfe32..e62ec86fc3 100644
--- a/conf/modules/telemetry_xbee_api.xml
+++ b/conf/modules/telemetry_xbee_api.xml
@@ -43,5 +43,9 @@
+
+
+
+
diff --git a/conf/settings/rover_basic.xml b/conf/settings/rover_basic.xml
new file mode 100644
index 0000000000..b1756c8351
--- /dev/null
+++ b/conf/settings/rover_basic.xml
@@ -0,0 +1,16 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/telemetry/default_rover.xml b/conf/telemetry/default_rover.xml
new file mode 100644
index 0000000000..dd94af5c00
--- /dev/null
+++ b/conf/telemetry/default_rover.xml
@@ -0,0 +1,56 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/boards/chimera/chibios/v1.0/chimera.h b/sw/airborne/boards/chimera/chibios/v1.0/chimera.h
index d9126fec7c..0a435683d9 100644
--- a/sw/airborne/boards/chimera/chibios/v1.0/chimera.h
+++ b/sw/airborne/boards/chimera/chibios/v1.0/chimera.h
@@ -477,20 +477,51 @@
#define I2C_FAST_400KHZ_DNF0_100NS_PCLK54MHZ_TIMINGR (STM32_TIMINGR_PRESC(0U) | \
STM32_TIMINGR_SCLDEL(10U) | STM32_TIMINGR_SDADEL(0U) | \
STM32_TIMINGR_SCLH(34U) | STM32_TIMINGR_SCLL(86U))
+#define I2C_STD_100KHZ_DNF0_100NS_PCLK54MHZ_TIMINGR (STM32_TIMINGR_PRESC(1U) | \
+ STM32_TIMINGR_SCLDEL(9U) | STM32_TIMINGR_SDADEL(0U) | \
+ STM32_TIMINGR_SCLH(105U) | STM32_TIMINGR_SCLL(153U))
+
+#ifndef I2C1_CLOCK_SPEED
#define I2C1_CLOCK_SPEED 400000
+#endif
+
+#if I2C1_CLOCK_SPEED == 400000
#define I2C1_CFG_DEF { \
.timingr = I2C_FAST_400KHZ_DNF0_100NS_PCLK54MHZ_TIMINGR, \
.cr1 = STM32_CR1_DNF(0), \
.cr2 = 0 \
}
+#elif I2C1_CLOCK_SPEED == 100000
+#define I2C1_CFG_DEF { \
+ .timingr = I2C_STD_100KHZ_DNF0_100NS_PCLK54MHZ_TIMINGR, \
+ .cr1 = STM32_CR1_DNF(0), \
+ .cr2 = 0 \
+}
+#else
+#error "Unknown I2C1 clock speed"
+#endif
+
+#ifndef I2C2_CLOCK_SPEED
#define I2C2_CLOCK_SPEED 400000
+#endif
+
+#if I2C2_CLOCK_SPEED == 400000
#define I2C2_CFG_DEF { \
.timingr = I2C_FAST_400KHZ_DNF0_100NS_PCLK54MHZ_TIMINGR, \
.cr1 = STM32_CR1_DNF(0), \
.cr2 = 0 \
}
+#elif I2C2_CLOCK_SPEED == 100000
+#define I2C2_CFG_DEF { \
+ .timingr = I2C_STD_100KHZ_DNF0_100NS_PCLK54MHZ_TIMINGR, \
+ .cr1 = STM32_CR1_DNF(0), \
+ .cr2 = 0 \
+}
+#else
+#error "Unknown I2C2 clock speed"
+#endif
/**
* SPI Config
diff --git a/sw/airborne/firmwares/rover/autopilot_firmware.c b/sw/airborne/firmwares/rover/autopilot_firmware.c
new file mode 100644
index 0000000000..a48d2800d0
--- /dev/null
+++ b/sw/airborne/firmwares/rover/autopilot_firmware.c
@@ -0,0 +1,144 @@
+/*
+ * Copyright (C) 2018 Gautier Hattenberger
+ *
+ * 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
+ * .
+ */
+
+/**
+ * @file firmwares/rover/autopilot.c
+ *
+ * Rover specific autopilot interface
+ * and initialization
+ */
+
+#include "firmwares/rover/autopilot_firmware.h"
+
+#include "generated/modules.h"
+
+#include
+#include "subsystems/electrical.h"
+#include "subsystems/datalink/telemetry.h"
+#include "subsystems/radio_control.h"
+
+#if USE_GPS
+#include "subsystems/gps.h"
+#else
+#if NO_GPS_NEEDED_FOR_NAV
+#define GpsIsLost() FALSE
+#else
+#define GpsIsLost() TRUE
+#endif
+#endif
+
+
+/* Geofence exceptions */
+#include "modules/nav/nav_geofence.h"
+
+#if USE_MOTOR_MIXING
+#include "subsystems/actuators/motor_mixing.h"
+#endif
+
+static void send_status(struct transport_tx *trans, struct link_device *dev)
+{
+#if USE_GPS
+ uint8_t fix = gps.fix;
+#else
+ uint8_t fix = 0;
+#endif
+ uint8_t motors_on = autopilot.motors_on;
+ uint16_t time_sec = sys_time.nb_sec;
+ pprz_msg_send_ROVER_STATUS(trans, dev, AC_ID,
+ &radio_control.status, &radio_control.frame_rate,
+ &fix, &autopilot.mode, &motors_on,
+ &electrical.vsupply, &time_sec);
+}
+
+static void send_energy(struct transport_tx *trans, struct link_device *dev)
+{
+ uint16_t e = electrical.energy;
+ if (fabs(electrical.energy) >= INT16_MAX) {
+ e = INT16_MAX;
+ }
+ float vsup = ((float)electrical.vsupply) / 10.0f;
+ float curs = ((float)electrical.current) / 1000.0f;
+ float power = vsup * curs;
+ pprz_msg_send_ENERGY(trans, dev, AC_ID, &vsup, &curs, &e, &power);
+}
+
+static void send_fp(struct transport_tx *trans, struct link_device *dev)
+{
+ int32_t carrot_east = POS_BFP_OF_REAL(nav.carrot.x);
+ int32_t carrot_north = POS_BFP_OF_REAL(nav.carrot.y);
+ int32_t carrot_up = 0;
+ int32_t carrot_heading = ANGLE_BFP_OF_REAL(nav.heading);
+ int32_t cmd = (int32_t)(ABS(commands[COMMAND_SPEED]));
+ pprz_msg_send_ROTORCRAFT_FP(trans, dev, AC_ID,
+ &(stateGetPositionEnu_i()->x),
+ &(stateGetPositionEnu_i()->y),
+ &(stateGetPositionEnu_i()->z),
+ &(stateGetSpeedEnu_i()->x),
+ &(stateGetSpeedEnu_i()->y),
+ &(stateGetSpeedEnu_i()->z),
+ &(stateGetNedToBodyEulers_i()->phi),
+ &(stateGetNedToBodyEulers_i()->theta),
+ &(stateGetNedToBodyEulers_i()->psi),
+ &carrot_east,
+ &carrot_north,
+ &carrot_up,
+ &carrot_heading,
+ &cmd,
+ &autopilot.flight_time);
+}
+
+
+#ifdef RADIO_CONTROL
+static void send_rotorcraft_rc(struct transport_tx *trans, struct link_device *dev)
+{
+#ifdef RADIO_KILL_SWITCH
+ int16_t _kill_switch = radio_control.values[RADIO_KILL_SWITCH];
+#else
+ int16_t _kill_switch = 42;
+#endif
+ pprz_msg_send_ROTORCRAFT_RADIO_CONTROL(trans, dev, AC_ID,
+ &radio_control.values[RADIO_ROLL],
+ &radio_control.values[RADIO_PITCH],
+ &radio_control.values[RADIO_YAW],
+ &radio_control.values[RADIO_THROTTLE],
+ &radio_control.values[RADIO_MODE],
+ &_kill_switch,
+ &radio_control.status);
+}
+#endif
+
+void autopilot_firmware_init(void)
+{
+ // register messages
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROVER_STATUS, send_status);
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ENERGY, send_energy);
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_FP, send_fp);
+#ifdef RADIO_CONTROL
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_RADIO_CONTROL, send_rotorcraft_rc);
+#endif
+}
+
+/** autopilot event function
+ *
+ */
+void autopilot_event(void)
+{
+}
+
diff --git a/sw/airborne/firmwares/rover/autopilot_firmware.h b/sw/airborne/firmwares/rover/autopilot_firmware.h
new file mode 100644
index 0000000000..9673301b8a
--- /dev/null
+++ b/sw/airborne/firmwares/rover/autopilot_firmware.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) 2018 Gautier Hattenberger
+ *
+ * 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
+ * .
+ */
+
+/**
+ * @file firmwares/rover/autopilot_firmware.h
+ *
+ * Rover specific autopilot interface
+ * and initialization
+ */
+
+#ifndef AUTOPILOT_FIRMWARE_H
+#define AUTOPILOT_FIRMWARE_H
+
+#include "std.h"
+#include "autopilot.h"
+
+extern void autopilot_firmware_init(void);
+
+#endif /* AUTOPILOT_FIRMWARE_H */
diff --git a/sw/airborne/firmwares/rover/autopilot_generated.c b/sw/airborne/firmwares/rover/autopilot_generated.c
new file mode 100644
index 0000000000..e2cb7dba98
--- /dev/null
+++ b/sw/airborne/firmwares/rover/autopilot_generated.c
@@ -0,0 +1,101 @@
+/*
+ * Copyright (C) 2018 Gautier Hattenberger
+ *
+ * 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
+ * .
+ */
+
+/**
+ * @file firmwares/rover/autopilot_generated.c
+ *
+ * Generated autopilot implementation.
+ *
+ */
+#define AUTOPILOT_CORE_AP_C
+
+#include "firmwares/rotorcraft/autopilot_generated.h"
+#include "autopilot.h"
+
+#include "subsystems/radio_control.h"
+#include "subsystems/commands.h"
+#include "subsystems/actuators.h"
+#include "subsystems/settings.h"
+#include "subsystems/datalink/telemetry.h"
+
+#include "generated/settings.h"
+
+
+void autopilot_generated_init(void)
+{
+ // call generated init
+ autopilot_core_ap_init();
+ // copy generated mode to public mode (set at startup)
+ autopilot.mode = autopilot_mode_ap;
+}
+
+
+void autopilot_generated_periodic(void)
+{
+
+ autopilot_core_ap_periodic_task();
+
+ // copy generated mode to public mode (may be changed on internal exceptions)
+ autopilot.mode = autopilot_mode_ap;
+
+ /* Reset ground detection _after_ running flight plan
+ */
+ if (!autopilot_in_flight()) {
+ autopilot.ground_detected = false;
+ autopilot.detect_ground_once = false;
+ }
+
+}
+
+/** AP mode setting handler
+ *
+ * FIXME generated something else for this?
+ */
+void autopilot_generated_SetModeHandler(float mode)
+{
+ autopilot_generated_set_mode(mode);
+}
+
+
+void autopilot_generated_set_mode(uint8_t new_autopilot_mode)
+{
+ autopilot_core_ap_set_mode(new_autopilot_mode);
+ // copy generated mode to public mode
+ autopilot.mode = autopilot_mode_ap;
+}
+
+
+void autopilot_generated_set_motors_on(bool motors_on)
+{
+ if (ap_ahrs_is_aligned() && motors_on
+#ifdef AP_MODE_KILL
+ && autopilot.mode != AP_MODE_KILL
+#endif
+ ) {
+ autopilot.motors_on = true;
+ } else {
+ autopilot.motors_on = false;
+ }
+}
+
+
+void autopilot_generated_on_rc_frame(void)
+{
+}
diff --git a/sw/airborne/firmwares/rover/autopilot_generated.h b/sw/airborne/firmwares/rover/autopilot_generated.h
new file mode 100644
index 0000000000..1de31f4f9e
--- /dev/null
+++ b/sw/airborne/firmwares/rover/autopilot_generated.h
@@ -0,0 +1,45 @@
+/*
+ * Copyright (C) 2018 Gautier Hattenberger
+ *
+ * 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
+ * .
+ */
+
+/**
+ * @file firmwares/rover/autopilot_generated.h
+ *
+ * Autopilot generated implementation
+ * Calls the code generated from autopilot XML file
+ *
+ */
+
+#ifndef AUTOPILOT_GENERATED_H
+#define AUTOPILOT_GENERATED_H
+
+#include "std.h"
+#include "generated/autopilot_core_ap.h"
+#include "generated/airframe.h"
+#include "state.h"
+
+extern void autopilot_generated_init(void);
+extern void autopilot_generated_periodic(void);
+extern void autopilot_generated_on_rc_frame(void);
+extern void autopilot_generated_set_mode(uint8_t new_autopilot_mode);
+extern void autopilot_generated_SetModeHandler(float new_autopilot_mode); // handler for dl_setting
+extern void autopilot_generated_set_motors_on(bool motors_on);
+
+#endif /* AUTOPILOT_GENERATED_H */
+
diff --git a/sw/airborne/firmwares/rover/autopilot_rc_helpers.h b/sw/airborne/firmwares/rover/autopilot_rc_helpers.h
new file mode 100644
index 0000000000..3f7416026d
--- /dev/null
+++ b/sw/airborne/firmwares/rover/autopilot_rc_helpers.h
@@ -0,0 +1,115 @@
+/*
+ * Copyright (C) 2012 The Paparazzi Team
+ *
+ * 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.
+ */
+
+/**
+ * @file firmwares/rotorcraft/autopilot_rc_helpers.h
+ *
+ * Some helper functions to check RC sticks.
+ */
+
+#ifndef AUTOPILOT_RC_HELPERS_H
+#define AUTOPILOT_RC_HELPERS_H
+
+#include "generated/airframe.h"
+#include "subsystems/radio_control.h"
+
+#define AUTOPILOT_THROTTLE_THRESHOLD (MAX_PPRZ / 20)
+#define AUTOPILOT_YAW_THRESHOLD (MAX_PPRZ * 19 / 20)
+#ifndef AUTOPILOT_STICK_CENTER_THRESHOLD
+#define AUTOPILOT_STICK_CENTER_THRESHOLD (MAX_PPRZ * 1 / 20)
+#endif
+
+#define THROTTLE_STICK_DOWN() \
+ (radio_control.values[RADIO_THROTTLE] < AUTOPILOT_THROTTLE_THRESHOLD)
+#define YAW_STICK_PUSHED() \
+ (radio_control.values[RADIO_YAW] > AUTOPILOT_YAW_THRESHOLD || \
+ radio_control.values[RADIO_YAW] < -AUTOPILOT_YAW_THRESHOLD)
+#define YAW_STICK_CENTERED() \
+ (radio_control.values[RADIO_YAW] < AUTOPILOT_STICK_CENTER_THRESHOLD && \
+ radio_control.values[RADIO_YAW] > -AUTOPILOT_STICK_CENTER_THRESHOLD)
+#define PITCH_STICK_CENTERED() \
+ (radio_control.values[RADIO_PITCH] < AUTOPILOT_STICK_CENTER_THRESHOLD && \
+ radio_control.values[RADIO_PITCH] > -AUTOPILOT_STICK_CENTER_THRESHOLD)
+#define ROLL_STICK_CENTERED() \
+ (radio_control.values[RADIO_ROLL] < AUTOPILOT_STICK_CENTER_THRESHOLD && \
+ radio_control.values[RADIO_ROLL] > -AUTOPILOT_STICK_CENTER_THRESHOLD)
+
+/** RC mode switch position helper
+ * switch positions threshold are evenly spaced
+ *
+ * @param[in] chan RC mode channel number
+ * @param[in] pos switch position to be tested
+ * @param[in] max maximum number of position of the switch
+ * @return true if current position is the same as requested (and RC status is OK)
+ */
+static inline bool rc_mode_switch(uint8_t chan, uint8_t pos, uint8_t max)
+{
+ if (radio_control.status != RC_OK) return false;
+ if (pos >= max) return false;
+ int32_t v = (int32_t)radio_control.values[chan] - MIN_PPRZ;
+ // round final value
+ int32_t p = (((((int32_t)max - 1) * 10 * v) / (MAX_PPRZ - MIN_PPRZ)) + 5) / 10;
+ Bound(p, 0, max - 1); // just in case
+ return pos == (uint8_t)p;
+}
+
+/** Convenience macro for 3way switch
+ */
+#ifdef RADIO_MODE
+#define RCMode0() rc_mode_switch(RADIO_MODE, 0, 3)
+#define RCMode1() rc_mode_switch(RADIO_MODE, 1, 3)
+#define RCMode2() rc_mode_switch(RADIO_MODE, 2, 3)
+#endif
+
+static inline bool rc_attitude_sticks_centered(void)
+{
+ return ROLL_STICK_CENTERED() && PITCH_STICK_CENTERED() && YAW_STICK_CENTERED();
+}
+
+#ifdef RADIO_KILL_SWITCH
+static inline bool kill_switch_is_on(void)
+{
+ if (radio_control.values[RADIO_KILL_SWITCH] < 0) {
+ return true;
+ } else {
+ return false;
+ }
+}
+#else
+static inline bool kill_switch_is_on(void)
+{
+ return false;
+}
+#endif
+
+static inline uint8_t percent_from_rc(int channel)
+{
+ int per = (MAX_PPRZ + (int32_t)radio_control.values[channel]) * 50 / MAX_PPRZ;
+ if (per < 0) {
+ per = 0;
+ } else if (per > 100) {
+ per = 100;
+ }
+ return per;
+}
+
+
+#endif /* AUTOPILOT_RC_HELPERS_H */
diff --git a/sw/airborne/firmwares/rover/autopilot_utils.c b/sw/airborne/firmwares/rover/autopilot_utils.c
new file mode 100644
index 0000000000..677e0f56f4
--- /dev/null
+++ b/sw/airborne/firmwares/rover/autopilot_utils.c
@@ -0,0 +1,66 @@
+/*
+ * Copyright (C) 2018 Gautier Hattenberger
+ *
+ * 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
+ * .
+ */
+
+/**
+ * @file firmwares/rover/autopilot_utils.c
+ *
+ * Utility functions and includes for autopilots
+ *
+ */
+
+#include "firmwares/rotorcraft/autopilot_utils.h"
+#include "autopilot.h"
+#include "firmwares/rotorcraft/autopilot_rc_helpers.h"
+#include "state.h"
+#include "subsystems/radio_control.h"
+
+/** Display descent speed in failsafe mode if needed */
+PRINT_CONFIG_VAR(FAILSAFE_DESCENT_SPEED)
+
+
+// Utility functions
+#ifndef AUTOPILOT_DISABLE_AHRS_KILL
+bool ap_ahrs_is_aligned(void)
+{
+ return stateIsAttitudeValid();
+}
+#else
+PRINT_CONFIG_MSG("Using AUTOPILOT_DISABLE_AHRS_KILL")
+bool ap_ahrs_is_aligned(void)
+{
+ return true;
+}
+#endif
+
+#define THRESHOLD_1_PPRZ (MIN_PPRZ / 2)
+#define THRESHOLD_2_PPRZ (MAX_PPRZ / 2)
+
+/** get autopilot mode as set by RADIO_MODE 3-way switch */
+uint8_t ap_mode_of_3way_switch(void)
+{
+ if (radio_control.values[RADIO_MODE] > THRESHOLD_2_PPRZ) {
+ return MODE_AUTO2;
+ } else if (radio_control.values[RADIO_MODE] > THRESHOLD_1_PPRZ) {
+ return MODE_AUTO1;
+ } else {
+ return MODE_MANUAL;
+ }
+}
+
diff --git a/sw/airborne/firmwares/rover/autopilot_utils.h b/sw/airborne/firmwares/rover/autopilot_utils.h
new file mode 100644
index 0000000000..afe788c0d0
--- /dev/null
+++ b/sw/airborne/firmwares/rover/autopilot_utils.h
@@ -0,0 +1,43 @@
+/*
+ * Copyright (C) 2018 Gautier Hattenberger
+ *
+ * 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
+ * .
+ */
+
+/**
+ * @file firmwares/rover/autopilot_utils.h
+ *
+ * Utility functions and includes for autopilots
+ *
+ */
+
+#ifndef AUTOPILOT_UTILS_H
+#define AUTOPILOT_UTILS_H
+
+#include "std.h"
+#include "subsystems/commands.h"
+
+extern bool ap_ahrs_is_aligned(void);
+extern uint8_t ap_mode_of_3way_switch(void);
+
+extern void set_rotorcraft_commands(pprz_t *cmd_out, int32_t *cmd_in, bool in_flight, bool motors_on);
+
+// in case, backward compatibility macro
+#define SetRotorcraftCommands(_cmd, _in_flight, _motors_on) set_rotorcraft_commands(commands, _cmd, _in_flight, _motors_on)
+
+#endif // AUTOPILOT_UTILS_H
+
diff --git a/sw/airborne/firmwares/rover/guidance/rover_guidance.c b/sw/airborne/firmwares/rover/guidance/rover_guidance.c
new file mode 100644
index 0000000000..bdd4879ec3
--- /dev/null
+++ b/sw/airborne/firmwares/rover/guidance/rover_guidance.c
@@ -0,0 +1,250 @@
+/*
+ * Copyright (C) 2018 Gautier Hattenberger
+ *
+ * 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
+ * .
+ */
+
+/** @file firmwares/rover/guidance/rover_guidance.c
+ * Basic guidance for rover.
+ * Implement standard PID control loop to track a navigation target.
+ * Guidance "modes" are using the autopilot generation with the "guidance"
+ * state machine.
+ */
+
+#define AUTOPILOT_CORE_GUIDANCE_C
+
+#include "firmwares/rover/guidance/rover_guidance.h"
+#include "generated/airframe.h"
+#include "generated/autopilot_core_guidance.h"
+#include "state.h"
+
+struct RoverGuidance rover_guidance;
+
+#if PERIODIC_TELEMETRY
+#include "subsystems/datalink/telemetry.h"
+// TODO rover guidance messages
+#endif
+
+void rover_guidance_init(void)
+{
+ FLOAT_VECT2_ZERO(rover_guidance.sp.pos);
+ rover_guidance.sp.heading = 0.0f;
+ rover_guidance.speed_pid.p = ROVER_GUIDANCE_SPEED_PGAIN;
+ rover_guidance.speed_pid.i = ROVER_GUIDANCE_SPEED_IGAIN;
+ rover_guidance.speed_pid.d = ROVER_GUIDANCE_SPEED_DGAIN;
+ rover_guidance.speed_pid.err = 0.f;
+ rover_guidance.speed_pid.d_err = 0.f;
+ rover_guidance.speed_pid.sum_err = 0.f;
+ rover_guidance.turn_pid.p = ROVER_GUIDANCE_TURN_PGAIN;
+ rover_guidance.turn_pid.i = ROVER_GUIDANCE_TURN_IGAIN;
+ rover_guidance.turn_pid.d = ROVER_GUIDANCE_TURN_DGAIN;
+ rover_guidance.turn_pid.err = 0.f;
+ rover_guidance.turn_pid.d_err = 0.f;
+ rover_guidance.turn_pid.sum_err = 0.f;
+
+#if PERIODIC_TELEMETRY
+ // TODO register messages
+#endif
+
+ // from code generation
+ autopilot_core_guidance_init();
+}
+
+void rover_guidance_periodic(void)
+{
+ // from code generation
+ autopilot_core_guidance_periodic_task();
+}
+
+static float compute_pid(struct RoverGuidancePID *pid)
+{
+ return pid->p * pid->err + pid->d * pid->d_err + pid->i * pid->sum_err;
+}
+
+#define MAX_POS_ERR 10.f // max position error
+#define MAX_SPEED_ERR 10.f // max speed error
+#define MAX_INTEGRAL_CMD (MAX_PPRZ / 10.f) // 10% of max command
+#define PROXIMITY_DIST 0.5f // proximity distance TODO improve
+
+void rover_guidance_run(float *heading_sp)
+{
+ // compute position error
+ struct FloatVect2 pos_err;
+ VECT2_DIFF(pos_err, rover_guidance.sp.pos, *stateGetPositionEnu_f());
+ rover_guidance.speed_pid.err = float_vect2_norm(&pos_err);
+ BoundAbs(rover_guidance.speed_pid.err, MAX_POS_ERR);
+
+ // speed and heading update when far enough
+ if (rover_guidance.speed_pid.err > PROXIMITY_DIST) {
+ // compute speed error
+ rover_guidance.speed_pid.d_err = - stateGetHorizontalSpeedNorm_f();
+ BoundAbs(rover_guidance.speed_pid.d_err, MAX_SPEED_ERR);
+ // integral
+ rover_guidance.speed_pid.sum_err = 0.f; // nothing for now
+ // run PID
+ rover_guidance.cmd.motor_speed = MAX_PPRZ * compute_pid(&rover_guidance.speed_pid);
+ rover_guidance.cmd.motor_speed = TRIM_PPRZ(rover_guidance.cmd.motor_speed);
+
+ // if not close to WP, compute desired heading
+ rover_guidance.sp.heading = atan2f(pos_err.x, pos_err.y);
+ *heading_sp = rover_guidance.sp.heading; // update nav sp
+ }
+ else {
+ rover_guidance.cmd.motor_speed = 0.f;
+ // use nav heading ref and run angular control
+ rover_guidance.sp.heading = *heading_sp;
+ }
+
+ // angular error
+ rover_guidance.turn_pid.err = rover_guidance.sp.heading - stateGetNedToBodyEulers_f()->psi;
+ NormRadAngle(rover_guidance.turn_pid.err);
+ // turn rate error
+ rover_guidance.turn_pid.d_err = - stateGetBodyRates_f()->r;
+ // integral
+ rover_guidance.turn_pid.sum_err = 0.f; // nothing for now
+ // run PID
+ rover_guidance.cmd.motor_turn = MAX_PPRZ * compute_pid(&rover_guidance.turn_pid);
+ rover_guidance.cmd.motor_turn = TRIM_PPRZ(rover_guidance.cmd.motor_turn);
+}
+
+void rover_guidance_enter(void)
+{
+ ClearBit(rover_guidance.sp.mask, 5);
+ ClearBit(rover_guidance.sp.mask, 7);
+
+ /* horizontal position setpoint from navigation/flightplan */
+ //INT32_VECT2_NED_OF_ENU(rover_guidance.sp.pos, navigation_carrot); TODO move to AP xml
+
+ //nav_heading = stateGetNedToBodyEulers_i()->psi; TODO move to AP xml
+ rover_guidance.sp.heading = stateGetNedToBodyEulers_f()->psi;
+
+ // TODO reset integral part ?
+}
+
+// TODO from AP xml
+//void rover_guidance_from_nav(bool in_flight)
+//{
+// if (!in_flight) {
+// rover_guidance_nav_enter();
+// }
+//
+// if (horizontal_mode == HORIZONTAL_MODE_MANUAL) {
+// stabilization_cmd[COMMAND_ROLL] = nav_cmd_roll;
+// stabilization_cmd[COMMAND_PITCH] = nav_cmd_pitch;
+// stabilization_cmd[COMMAND_YAW] = nav_cmd_yaw;
+// } else if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) {
+// struct Int32Eulers sp_cmd_i;
+// sp_cmd_i.phi = nav_roll;
+// sp_cmd_i.theta = nav_pitch;
+// sp_cmd_i.psi = nav_heading;
+// stabilization_attitude_set_rpy_setpoint_i(&sp_cmd_i);
+// stabilization_attitude_run(in_flight);
+// } else {
+// INT32_VECT2_NED_OF_ENU(rover_guidance.sp.pos, navigation_carrot);
+//
+// /* set psi command */
+// rover_guidance.sp.heading = ANGLE_FLOAT_OF_BFP(nav_heading);
+// FLOAT_ANGLE_NORMALIZE(rover_guidance.sp.heading);
+//
+// /* compute x,y earth commands */
+// rover_guidance_traj_run(in_flight);
+// /* set final attitude setpoint */
+// int32_t heading_sp_i = ANGLE_BFP_OF_REAL(rover_guidance.sp.heading);
+// stabilization_attitude_set_earth_cmd_i(&rover_guidance_cmd_earth,
+// heading_sp_i);
+// stabilization_attitude_run(in_flight);
+// }
+//}
+
+void rover_guidance_set_speed_igain(uint32_t igain)
+{
+ rover_guidance.speed_pid.i = igain;
+ rover_guidance.speed_pid.sum_err = 0.f;
+}
+
+void rover_guidance_set_turn_igain(uint32_t igain)
+{
+ rover_guidance.turn_pid.i = igain;
+ rover_guidance.turn_pid.sum_err = 0.f;
+}
+
+
+#ifdef ROVER_GUIDANCE_MODE_GUIDED
+void rover_guidance_guided_run(bool in_flight)
+{
+ /* rover_guidance.sp.pos and rover_guidance.sp.heading need to be set from external source */
+ if (!in_flight) {
+ rover_guidance_hover_enter();
+ }
+
+ /* compute x,y earth commands */
+ rover_guidance_traj_run(in_flight);
+}
+
+bool rover_guidance_set_guided_pos(float x, float y)
+{
+ if (rover_guidance.mode == ROVER_GUIDANCE_MODE_GUIDED) {
+ ClearBit(rover_guidance.sp.mask, 5);
+ rover_guidance.sp.pos.x = x;
+ rover_guidance.sp.pos.y = y;
+ return true;
+ }
+ return false;
+}
+
+bool rover_guidance_set_guided_heading(float heading)
+{
+ if (rover_guidance.mode == ROVER_GUIDANCE_MODE_GUIDED) {
+ ClearBit(rover_guidance.sp.mask, 7);
+ rover_guidance.sp.heading = heading;
+ FLOAT_ANGLE_NORMALIZE(rover_guidance.sp.heading);
+ return true;
+ }
+ return false;
+}
+
+bool rover_guidance_set_guided_body_vel(float vx, float vy)
+{
+ float psi = stateGetNedToBodyEulers_f()->psi;
+ float newvx = cosf(-psi) * vx + sinf(-psi) * vy;
+ float newvy = -sinf(-psi) * vx + cosf(-psi) * vy;
+ return rover_guidance_set_guided_vel(newvx, newvy);
+}
+
+bool rover_guidance_set_guided_vel(float vx, float vy)
+{
+ if (rover_guidance.mode == ROVER_GUIDANCE_MODE_GUIDED) {
+ SetBit(rover_guidance.sp.mask, 5);
+ rover_guidance.sp.speed.x = vx;
+ rover_guidance.sp.speed.y = vy;
+ return true;
+ }
+ return false;
+}
+
+bool rover_guidance_set_guided_heading_rate(float rate)
+{
+ if (rover_guidance.mode == ROVER_GUIDANCE_MODE_GUIDED) {
+ SetBit(rover_guidance.sp.mask, 7);
+ rover_guidance.sp.heading_rate = rate;
+ return true;
+ }
+ return false;
+}
+
+#endif
+
diff --git a/sw/airborne/firmwares/rover/guidance/rover_guidance.h b/sw/airborne/firmwares/rover/guidance/rover_guidance.h
new file mode 100644
index 0000000000..52913955aa
--- /dev/null
+++ b/sw/airborne/firmwares/rover/guidance/rover_guidance.h
@@ -0,0 +1,136 @@
+/*
+ * Copyright (C) 2018 Gautier Hattenberger
+ *
+ * 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
+ * .
+ */
+
+/** @file firmwares/rover/guidance/rover_guidance.h
+ * Basic guidance for rover.
+ * Implement standard PID control loop to track a navigation target.
+ * Guidance "modes" are using the autopilot generation with the "guidance"
+ * state machine.
+ */
+
+#ifndef ROVER_GUIDANCE_H
+#define ROVER_GUIDANCE_H
+
+
+#include "math/pprz_algebra_float.h"
+#include "generated/airframe.h"
+#include "std.h"
+
+struct RoverGuidanceSetpoint {
+ /** position setpoint in NED.
+ */
+ struct FloatVect2 pos; ///< position setpoint
+ struct FloatVect2 speed; ///< speed setpoint
+ float heading; ///< heading setpoint
+ uint8_t mask; ///< bit 5: vx & vy, bit 6: vz, bit 7: vyaw
+};
+
+// TODO write a generic PID somewhere else ?
+struct RoverGuidancePID {
+ float p;
+ float i;
+ float d;
+ float err;
+ float d_err;
+ float sum_err;
+};
+
+struct RoverGuidanceControl {
+ float motor_speed;
+ float motor_turn;
+};
+
+struct RoverGuidance {
+ struct RoverGuidanceSetpoint sp; ///< setpoints
+ struct RoverGuidanceControl cmd; ///< commands
+ struct RoverGuidancePID speed_pid; ///< motor speed controller
+ struct RoverGuidancePID turn_pid; ///< turn rate controller
+};
+
+extern struct RoverGuidance rover_guidance;
+
+extern void rover_guidance_init(void);
+extern void rover_guidance_periodic(void); // call state machine
+extern void rover_guidance_run(float *heading_sp); // run control loop
+extern void rover_guidance_enter(void);
+
+extern void rover_guidance_set_speed_igain(uint32_t igain);
+extern void rover_guidance_set_turn_igain(uint32_t igain);
+
+#ifdef GUIDANCE_MODE_GUIDED
+/** Run GUIDED mode control
+ */
+extern void rover_guidance_guided_run(bool in_flight);
+
+/** Set horizontal position setpoint in GUIDED mode.
+ * @param x North position (local NED frame) in meters.
+ * @param y East position (local NED frame) in meters.
+ * @return TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED)
+ */
+extern bool rover_guidance_set_guided_pos(float x, float y);
+
+/** Set heading setpoint in GUIDED mode.
+ * @param heading Setpoint in radians.
+ * @return TRUE if setpoint was set (currently in GUIDANCE_H_MODE_GUIDED)
+ */
+extern bool rover_guidance_set_guided_heading(float heading);
+
+/** Set body relative horizontal velocity setpoint in GUIDED mode.
+ * @param vx forward velocity (body frame) in meters/sec.
+ * @param vy right velocity (body frame) in meters/sec.
+ * @return TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED)
+ */
+extern bool rover_guidance_set_guided_body_vel(float vx, float vy);
+
+/** Set horizontal velocity setpoint in GUIDED mode.
+ * @param vx North velocity (local NED frame) in meters/sec.
+ * @param vy East velocity (local NED frame) in meters/sec.
+ * @return TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED)
+ */
+extern bool rover_guidance_set_guided_vel(float vx, float vy);
+
+/** Set heading rate setpoint in GUIDED mode.
+ * @param rate Heading rate in radians.
+ * @return TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED)
+ */
+extern bool rover_guidance_set_guided_heading_rate(float rate);
+#endif
+
+//static inline void rover_guidance_SetMaxSpeed(float speed)
+//{
+// gh_set_max_speed(speed);
+//}
+//
+//static inline void rover_guidance_SetOmega(float omega)
+//{
+// gh_set_omega(omega);
+//}
+//
+//static inline void rover_guidance_SetZeta(float zeta)
+//{
+// gh_set_zeta(zeta);
+//}
+//
+//static inline void rover_guidance_SetTau(float tau)
+//{
+// gh_set_tau(tau);
+//}
+
+#endif /* ROVER_GUIDANCE_H */
diff --git a/sw/airborne/firmwares/rover/main_ap.c b/sw/airborne/firmwares/rover/main_ap.c
new file mode 100644
index 0000000000..47138891ce
--- /dev/null
+++ b/sw/airborne/firmwares/rover/main_ap.c
@@ -0,0 +1,289 @@
+/*
+ * Copyright (C) 2018 Gautier Hattenberger
+ *
+ * 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
+ * .
+ *
+ */
+
+/**
+ * @file firmwares/rover/main_ap.c
+ *
+ * Rover main loop.
+ */
+
+#define MODULES_C
+
+#define ABI_C
+
+#include
+#include "mcu.h"
+#include "mcu_periph/sys_time.h"
+#include "led.h"
+
+#include "subsystems/datalink/telemetry.h"
+#include "subsystems/datalink/datalink.h"
+#include "subsystems/datalink/downlink.h"
+#include "subsystems/settings.h"
+
+#include "subsystems/commands.h"
+#include "subsystems/actuators.h"
+
+#if USE_IMU
+#include "subsystems/imu.h"
+#endif
+#if USE_GPS
+#include "subsystems/gps.h"
+#endif
+
+#if USE_BARO_BOARD
+#include "subsystems/sensors/baro.h"
+PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BOARD)
+#endif
+
+#include "subsystems/electrical.h"
+
+#include "autopilot.h"
+
+#include "subsystems/radio_control.h"
+
+#include "subsystems/ahrs.h"
+
+#include "state.h"
+
+#include "firmwares/rover/main_ap.h"
+
+#include "generated/modules.h"
+#include "subsystems/abi.h"
+
+// needed for stop-gap measure waypoints_localize_all()
+#include "subsystems/navigation/waypoints.h"
+
+
+/* if PRINT_CONFIG is defined, print some config options */
+PRINT_CONFIG_VAR(PERIODIC_FREQUENCY)
+/* SYS_TIME_FREQUENCY/PERIODIC_FREQUENCY should be an integer, otherwise the timer will not be correct */
+#if !(SYS_TIME_FREQUENCY/PERIODIC_FREQUENCY*PERIODIC_FREQUENCY == SYS_TIME_FREQUENCY)
+#warning "The SYS_TIME_FREQUENCY can not be divided by PERIODIC_FREQUENCY. Make sure this is the case for correct timing."
+#endif
+
+/* TELEMETRY_FREQUENCY is defined in generated/periodic_telemetry.h
+ * defaults to 60Hz or set by TELEMETRY_FREQUENCY configure option in airframe file
+ */
+PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY)
+
+/* MODULES_FREQUENCY is defined in generated/modules.h
+ * according to main_freq parameter set for modules in airframe file
+ */
+PRINT_CONFIG_VAR(MODULES_FREQUENCY)
+
+#ifndef BARO_PERIODIC_FREQUENCY
+#define BARO_PERIODIC_FREQUENCY 50
+#endif
+PRINT_CONFIG_VAR(BARO_PERIODIC_FREQUENCY)
+
+#if USE_AHRS && USE_IMU && (defined AHRS_PROPAGATE_FREQUENCY)
+#if (AHRS_PROPAGATE_FREQUENCY > PERIODIC_FREQUENCY)
+#warning "PERIODIC_FREQUENCY should be least equal or greater than AHRS_PROPAGATE_FREQUENCY"
+INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY to at least ", AHRS_PROPAGATE_FREQUENCY)
+#endif
+#endif
+
+tid_t main_periodic_tid; ///< id for main_periodic() timer
+tid_t modules_tid; ///< id for modules_periodic_task() timer
+tid_t failsafe_tid; ///< id for failsafe_check() timer
+tid_t radio_control_tid; ///< id for radio_control_periodic_task() timer
+tid_t electrical_tid; ///< id for electrical_periodic() timer
+tid_t telemetry_tid; ///< id for telemetry_periodic() timer
+#if USE_BARO_BOARD
+tid_t baro_tid; ///< id for baro_periodic() timer
+#endif
+
+void main_init(void)
+{
+ mcu_init();
+
+#if defined(PPRZ_TRIG_INT_COMPR_FLASH)
+ pprz_trig_int_init();
+#endif
+
+ electrical_init();
+
+ stateInit();
+
+ actuators_init();
+
+#if USE_MOTOR_MIXING
+ motor_mixing_init();
+#endif
+
+ radio_control_init();
+
+#if USE_BARO_BOARD
+ baro_init();
+#endif
+
+#if USE_AHRS
+ ahrs_init();
+#endif
+
+ autopilot_init();
+
+ modules_init();
+
+ // call autopilot implementation init after guidance modules init
+ // it will set startup mode
+ autopilot_generated_init();
+
+ /* temporary hack:
+ * Since INS is now a module, LTP_DEF is not yet initialized when autopilot_init is called
+ * This led to the problem that global waypoints were not "localized",
+ * so as a stop-gap measure we localize them all (again) here..
+ */
+ //waypoints_localize_all();
+
+ settings_init();
+
+ mcu_int_enable();
+
+#if DOWNLINK
+ downlink_init();
+#endif
+
+ // register the timers for the periodic functions
+ main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
+#if PERIODIC_FREQUENCY != MODULES_FREQUENCY
+ modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
+#endif
+ radio_control_tid = sys_time_register_timer((1. / 60.), NULL);
+ failsafe_tid = sys_time_register_timer(0.05, NULL);
+ electrical_tid = sys_time_register_timer(0.1, NULL);
+ telemetry_tid = sys_time_register_timer((1. / TELEMETRY_FREQUENCY), NULL);
+#if USE_BARO_BOARD
+ baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL);
+#endif
+
+#if USE_IMU
+ // send body_to_imu from here for now
+ AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
+#endif
+
+ // Do a failsafe check first
+ failsafe_check();
+
+}
+
+void handle_periodic_tasks(void)
+{
+ if (sys_time_check_and_ack_timer(main_periodic_tid)) {
+ main_periodic();
+#if PERIODIC_FREQUENCY == MODULES_FREQUENCY
+ /* Use the main periodc freq timer for modules if the freqs are the same
+ * This is mainly useful for logging each step.
+ */
+ modules_periodic_task();
+#else
+ }
+ /* separate timer for modules, since it has a different freq than main */
+ if (sys_time_check_and_ack_timer(modules_tid)) {
+ modules_periodic_task();
+#endif
+ }
+ if (sys_time_check_and_ack_timer(radio_control_tid)) {
+ radio_control_periodic_task();
+ }
+ if (sys_time_check_and_ack_timer(failsafe_tid)) {
+ failsafe_check();
+ }
+ if (sys_time_check_and_ack_timer(electrical_tid)) {
+ electrical_periodic();
+ }
+ if (sys_time_check_and_ack_timer(telemetry_tid)) {
+ telemetry_periodic();
+ }
+#if USE_BARO_BOARD
+ if (sys_time_check_and_ack_timer(baro_tid)) {
+ baro_periodic();
+ }
+#endif
+}
+
+void main_periodic(void)
+{
+ /* run control loops */
+ autopilot_periodic();
+ /* set actuators */
+ //actuators_set(autopilot_get_motors_on());
+ SetActuatorsFromCommands(commands, autopilot_get_mode());
+
+ if (autopilot_in_flight()) {
+ RunOnceEvery(PERIODIC_FREQUENCY, autopilot.flight_time++);
+ }
+
+#if defined DATALINK || defined SITL
+ RunOnceEvery(PERIODIC_FREQUENCY, datalink_time++);
+#endif
+
+ RunOnceEvery(10, LED_PERIODIC());
+}
+
+void telemetry_periodic(void)
+{
+ static uint8_t boot = true;
+
+ /* initialisation phase during boot */
+ if (boot) {
+#if DOWNLINK
+ autopilot_send_version();
+#endif
+ boot = false;
+ }
+ /* then report periodicly */
+ else {
+#if PERIODIC_TELEMETRY
+ periodic_telemetry_send_Main(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device);
+#endif
+ }
+}
+
+/** mode to enter when RC is lost while using a mode with RC input (not AP_MODE_NAV) */
+#ifndef RC_LOST_MODE
+#define RC_LOST_MODE AP_MODE_FAILSAFE
+#endif
+
+void failsafe_check(void)
+{
+ autopilot_check_in_flight(autopilot_get_motors_on());
+}
+
+void main_event(void)
+{
+ /* event functions for mcu peripherals: i2c, usb_serial.. */
+ mcu_event();
+
+ if (autopilot.use_rc) {
+ RadioControlEvent(autopilot_on_rc_frame);
+ }
+
+#if USE_BARO_BOARD
+ BaroEvent();
+#endif
+
+ autopilot_event();
+
+ modules_event_task();
+}
+
diff --git a/sw/airborne/firmwares/rover/main_ap.h b/sw/airborne/firmwares/rover/main_ap.h
new file mode 100644
index 0000000000..c8c58c8522
--- /dev/null
+++ b/sw/airborne/firmwares/rover/main_ap.h
@@ -0,0 +1,40 @@
+/*
+ * Copyright (C) 2018 Gautier Hattenberger
+ *
+ * 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
+ * .
+ */
+
+/**
+ * @file firmwares/rover/main_ap.h
+ *
+ * Rover main loop.
+ */
+
+#ifndef MAIN_H
+#define MAIN_H
+
+extern void main_init(void);
+extern void main_event(void);
+extern void handle_periodic_tasks(void);
+
+extern void main_periodic(void);
+extern void telemetry_periodic(void);
+extern void failsafe_check(void);
+
+
+#endif /* MAIN_H */
+
diff --git a/sw/airborne/firmwares/rover/main_chibios.c b/sw/airborne/firmwares/rover/main_chibios.c
new file mode 100644
index 0000000000..ad7fd2a294
--- /dev/null
+++ b/sw/airborne/firmwares/rover/main_chibios.c
@@ -0,0 +1,102 @@
+/*
+ * Copyright (C) 2018 Gautier Hattenberger, Alexandre Bustico
+ *
+ * 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
+ * .
+ */
+
+/**
+ * @file firmwares/rover/main_chibios.c
+ */
+
+#include "mcu_periph/sys_time.h"
+#include "mcu.h"
+#include
+
+#ifndef SYS_TIME_FREQUENCY
+#error SYS_TIME_FREQUENCY should be defined in Makefile.chibios or airframe.xml and be equal to CH_CFG_ST_FREQUENCY
+#elif SYS_TIME_FREQUENCY != CH_CFG_ST_FREQUENCY
+#error SYS_TIME_FREQUENCY should be equal to CH_CFG_ST_FREQUENCY
+#elif CH_CFG_ST_FREQUENCY < (2 * PERIODIC_FREQUENCY)
+#error CH_CFG_ST_FREQUENCY and SYS_TIME_FREQUENCY should be >= 2 x PERIODIC_FREQUENCY
+#endif
+
+#include "firmwares/rotorcraft/main_ap.h"
+
+#ifndef THD_WORKING_AREA_MAIN
+#define THD_WORKING_AREA_MAIN 8192
+#endif
+
+/*
+ * PPRZ/AP thread
+ * Also include FBW on single MCU
+ */
+static void thd_ap(void *arg);
+THD_WORKING_AREA(wa_thd_ap, THD_WORKING_AREA_MAIN);
+static thread_t *apThdPtr = NULL;
+
+/**
+ * Main function
+ */
+int main(void)
+{
+ // Init
+ main_init();
+
+ chThdSleepMilliseconds(100);
+
+ // Create threads
+ apThdPtr = chThdCreateStatic(wa_thd_ap, sizeof(wa_thd_ap), NORMALPRIO, thd_ap, NULL);
+
+ // Main loop, do nothing
+ while (TRUE) {
+ chThdSleepMilliseconds(1000);
+ }
+ return 0;
+}
+
+/*
+ * PPRZ/AP thread
+ *
+ * Call PPRZ AP periodic and event functions
+ */
+static void thd_ap(void *arg)
+{
+ (void) arg;
+ chRegSetThreadName("AP");
+
+ while (!chThdShouldTerminateX()) {
+ handle_periodic_tasks();
+ main_event();
+ chThdSleepMicroseconds(500);
+ }
+
+ chThdExit(0);
+}
+
+/*
+ * Terminate autopilot threads
+ * Wait until proper stop
+ */
+void pprz_terminate_autopilot_threads(void)
+{
+ if (apThdPtr != NULL) {
+ chThdTerminate(apThdPtr);
+ chThdWait(apThdPtr);
+ apThdPtr = NULL;
+ }
+}
+
diff --git a/sw/airborne/firmwares/rover/main_chibios.h b/sw/airborne/firmwares/rover/main_chibios.h
new file mode 100644
index 0000000000..0744c70e6a
--- /dev/null
+++ b/sw/airborne/firmwares/rover/main_chibios.h
@@ -0,0 +1,35 @@
+/*
+ * Copyright (C) 2018 Gautier Hattenberger, Alexandre Bustico
+ *
+ * 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
+ * .
+ */
+
+/**
+ * @file firmwares/rover/main_chibios.h
+ */
+
+#ifndef MAIN_CHIBIOS_H
+#define MAIN_CHIBIOS_H
+
+#include
+
+/** Terminate all autopilot threads
+ * Wait until proper stop
+ */
+extern void pprz_terminate_autopilot_threads(void);
+
+#endif /* MAIN_CHIBIOS_H */
diff --git a/sw/airborne/firmwares/rover/navigation.c b/sw/airborne/firmwares/rover/navigation.c
new file mode 100644
index 0000000000..bf7e598a62
--- /dev/null
+++ b/sw/airborne/firmwares/rover/navigation.c
@@ -0,0 +1,308 @@
+/*
+ * Copyright (C) 2018 Gautier Hattenberger
+ *
+ * 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
+ * .
+ */
+
+/**
+ * @file firmwares/rover/navigation.c
+ *
+ * Rover navigation functions.
+ */
+
+
+#define NAV_C
+
+#include "firmwares/rover/navigation.h"
+
+#include "pprz_debug.h"
+#include "subsystems/gps.h" // needed by auto_nav from the flight plan
+#include "subsystems/ins.h"
+#include "state.h"
+
+#include "autopilot.h"
+#include "generated/modules.h"
+#include "generated/flight_plan.h"
+
+#include "math/pprz_algebra_int.h"
+
+#include "subsystems/datalink/downlink.h"
+#include "pprzlink/messages.h"
+#include "mcu_periph/uart.h"
+
+
+
+struct RoverNavigation nav;
+
+uint8_t last_wp __attribute__((unused));
+
+const float max_dist_from_home = MAX_DIST_FROM_HOME;
+const float max_dist2_from_home = MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME;
+float failsafe_mode_dist2 = FAILSAFE_MODE_DISTANCE * FAILSAFE_MODE_DISTANCE;
+
+
+void set_exception_flag(uint8_t flag_num)
+{
+ nav.exception_flag[flag_num] = true;
+}
+
+
+#if PERIODIC_TELEMETRY
+#include "subsystems/datalink/telemetry.h"
+
+static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
+{
+ static uint8_t i;
+ i++;
+ if (i >= nb_waypoint) { i = 0; }
+ pprz_msg_send_WP_MOVED_ENU(trans, dev, AC_ID,
+ &i,
+ &(waypoints[i].enu_i.x),
+ &(waypoints[i].enu_i.y),
+ &(waypoints[i].enu_i.z));
+}
+#endif
+
+void nav_init(void)
+{
+ waypoints_init();
+
+ nav_block = 0;
+ nav_stage = 0;
+
+ nav.mode = NAV_MODE_WAYPOINT;
+
+ VECT3_COPY(nav.target, waypoints[WP_HOME].enu_f);
+ VECT3_COPY(nav.carrot, waypoints[WP_HOME].enu_f);
+ nav.heading = 0.f;
+ nav.radius = DEFAULT_CIRCLE_RADIUS;
+ nav.speed = 0.f;
+ nav.turn = 0.f;
+ nav.shift = 0.f;
+
+ nav.too_far_from_home = false;
+ nav.dist2_to_home = 0.f;
+
+#if PERIODIC_TELEMETRY
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WP_MOVED, send_wp_moved);
+#endif
+
+ // generated init function
+ auto_nav_init();
+}
+
+
+void nav_run(void)
+{
+ VECT2_COPY(nav.carrot, nav.target);
+}
+
+
+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;
+}
+
+
+/** Reset the geographic reference to the current GPS fix */
+void nav_reset_reference(void)
+{
+ ins_reset_local_origin();
+ /* update local ENU coordinates of global waypoints */
+ waypoints_localize_all();
+}
+
+void nav_reset_alt(void)
+{
+ ins_reset_altitude_ref();
+ waypoints_localize_all();
+}
+
+void nav_init_stage(void)
+{
+ VECT3_COPY(nav.last_pos, *stateGetPositionEnu_f());
+ stage_time = 0;
+ //nav_circle_radians = 0; FIXME
+}
+
+void nav_periodic_task(void)
+{
+ RunOnceEvery(NAV_FREQ, { stage_time++; block_time++; });
+
+ //nav.dist2_to_wp = 0; FIXME
+
+ /* from flight_plan.h */
+ auto_nav();
+
+ /* run carrot loop */
+ nav_run();
+}
+
+bool nav_is_in_flight(void)
+{
+ return autopilot_in_flight();
+}
+
+/** Home mode navigation */
+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();
+}
+
+/** Returns squared horizontal distance to given point */
+float get_dist2_to_point(struct EnuCoor_f *p)
+{
+ struct EnuCoor_f *pos = stateGetPositionEnu_f();
+ struct FloatVect2 pos_diff = {
+ .x = p->x - pos->x,
+ .y = p->y - pos->y
+ };
+ return pos_diff.x * pos_diff.x + pos_diff.y * pos_diff.y;
+}
+
+/** Returns squared horizontal distance to given waypoint */
+float get_dist2_to_waypoint(uint8_t wp_id)
+{
+ return get_dist2_to_point(&waypoints[wp_id].enu_f);
+}
+
+/** Computes squared distance to the HOME waypoint potentially sets
+ * #too_far_from_home
+ */
+void compute_dist2_to_home(void)
+{
+ nav.dist2_to_home = get_dist2_to_waypoint(WP_HOME);
+ nav.too_far_from_home = nav.dist2_to_home > max_dist2_from_home;
+#ifdef InGeofenceSector
+ struct EnuCoor_f *pos = stateGetPositionEnu_f();
+ nav.too_far_from_home = nav.too_far_from_home || !(InGeofenceSector(pos->x, pos->y));
+#endif
+}
+
+/** Set nav_heading in radians. */
+void nav_set_heading_rad(float rad)
+{
+ nav.heading = rad;
+ NormCourseRad(nav.heading);
+}
+
+/** Set nav_heading in degrees. */
+void nav_set_heading_deg(float deg)
+{
+ nav_set_heading_rad(RadOfDeg(deg));
+}
+
+/** Set heading to point towards x,y position in local coordinates */
+void nav_set_heading_towards(float x, float y)
+{
+ struct FloatVect2 target = {x, y};
+ struct FloatVect2 pos_diff;
+ VECT2_DIFF(pos_diff, target, *stateGetPositionEnu_f());
+ // don't change heading if closer than 0.5m to target
+ if (VECT2_NORM2(pos_diff) > 0.25f) {
+ nav.heading = atan2f(pos_diff.x, pos_diff.y);
+ }
+}
+
+/** Set heading in the direction of a waypoint */
+void nav_set_heading_towards_waypoint(uint8_t wp)
+{
+ nav_set_heading_towards(WaypointX(wp), WaypointY(wp));
+}
+
+/** Set heading in the direction of the target*/
+void nav_set_heading_towards_target(void)
+{
+ nav_set_heading_towards(nav.target.x, nav.target.y);
+}
+
+/** Set heading to the current yaw angle */
+void nav_set_heading_current(void)
+{
+ nav.heading = stateGetNedToBodyEulers_f()->psi;
+}
+
+void nav_set_failsafe(void)
+{
+#ifdef AP_MODE_FAILSAFE
+ autopilot_set_mode(AP_MODE_FAILSAFE);
+#endif
+}
+
+
+/** Register functions
+ */
+
+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;
+ nav.nav_route = nav_route;
+ nav.nav_approaching = nav_approaching;
+}
+
+void nav_register_circle(nav_rover_circle nav_circle)
+{
+ nav.nav_circle = nav_circle;
+}
+
+void nav_register_oval(nav_rover_oval_init nav_oval_init, nav_rover_oval nav_oval)
+{
+ nav.nav_oval_init = nav_oval_init;
+ nav.nav_oval = nav_oval;
+}
+
+
diff --git a/sw/airborne/firmwares/rover/navigation.h b/sw/airborne/firmwares/rover/navigation.h
new file mode 100644
index 0000000000..bf4f58894b
--- /dev/null
+++ b/sw/airborne/firmwares/rover/navigation.h
@@ -0,0 +1,318 @@
+/*
+ * Copyright (C) 2018 Gautier Hattenberger
+ *
+ * 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
+ * .
+ */
+
+/**
+ * @file firmwares/rover/navigation.h
+ *
+ * Rover navigation functions.
+ */
+
+#ifndef NAVIGATION_H
+#define NAVIGATION_H
+
+#include "std.h"
+#include "math/pprz_geodetic_float.h"
+#include "state.h"
+#include "subsystems/navigation/waypoints.h"
+#include "subsystems/navigation/common_flight_plan.h"
+#include "autopilot.h"
+
+/** default approaching_time for a wp */ // FIXME
+#ifndef CARROT
+#define CARROT 0.f
+#endif
+
+#ifndef CARROT_DIST
+#define CARROT_DIST 2.f
+#endif
+
+#ifndef NAV_FREQ
+#define NAV_FREQ 16
+#endif
+
+
+/** default nav_circle_radius in meters */
+#ifndef DEFAULT_CIRCLE_RADIUS
+#define DEFAULT_CIRCLE_RADIUS 6.0f
+#endif
+
+/** minimum horizontal distance to waypoint to mark as arrived */
+#ifndef ARRIVED_AT_WAYPOINT
+#define ARRIVED_AT_WAYPOINT 3.0f
+#endif
+
+/** Maximum distance from HOME waypoint before going into failsafe mode */
+#ifndef FAILSAFE_MODE_DISTANCE
+#define FAILSAFE_MODE_DISTANCE (1.2*MAX_DIST_FROM_HOME)
+#endif
+
+/** Nav modes */
+#define NAV_MODE_WAYPOINT 0
+#define NAV_MODE_ROUTE 1
+#define NAV_MODE_CIRCLE 2
+#define NAV_MODE_HEADING 3
+#define NAV_MODE_MANUAL 4
+
+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);
+typedef void (*nav_rover_circle)(struct EnuCoor_f *wp_center, float radius);
+typedef void (*nav_rover_oval_init)(void);
+typedef void (*nav_rover_oval)(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius);
+
+
+/** General Navigation structure
+ */
+struct RoverNavigation {
+ // mode
+ uint8_t mode; // nav mode
+
+ // commands
+ struct EnuCoor_f target; ///< final target
+ struct EnuCoor_f carrot; ///< carrot position
+ float heading; ///< heading setpoint (in radians)
+ float radius; ///< radius setpoint
+ float speed; ///< speed setpoint
+ float turn; ///< turn rate setpoint
+ float shift; ///< lateral shift (in meters)
+
+ // misc
+ float dist2_to_home; ///< squared distance to home waypoint
+ bool too_far_from_home; ///< too_far flag
+ float failsafe_mode_dist2; ///< maximum squared distance to home wp before going to failsafe mode
+ bool exception_flag[10]; ///< array of flags that might be used in flight plans
+ struct EnuCoor_f last_pos; ///< last stage position
+
+ // pointers to basic nav functions
+ nav_rover_goto nav_goto;
+ nav_rover_route nav_route;
+ nav_rover_approaching nav_approaching;
+ nav_rover_circle nav_circle;
+ nav_rover_oval_init nav_oval_init;
+ nav_rover_oval nav_oval;
+};
+
+extern struct RoverNavigation nav;
+
+/** Registering functions
+ */
+extern void nav_register_goto_wp(nav_rover_goto nav_goto,
+ nav_rover_route nav_route,
+ nav_rover_approaching nav_approaching);
+extern void nav_register_circle(nav_rover_circle nav_circle);
+extern void nav_register_oval(nav_rover_oval_init nav_oval_init, nav_rover_oval nav_oval);
+// TODO: eight, survey
+
+
+/*****************************************************************
+ * macros to ensure compatibility between fixedwing and rotorcraft
+ *****************************************************************/
+
+/// Get current x (east) position in local coordinates
+#define GetPosX() (stateGetPositionEnu_f()->x)
+/// 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)
+/**
+ * Get current altitude reference for local coordinates.
+ * This is the ground_alt from the flight plan at first,
+ * 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)
+
+
+/** Normalize a degree angle between 0 and 359 */
+#define NormCourse(x) { \
+ while (x < 0) x += 360; \
+ while (x >= 360) x -= 360; \
+ }
+/** Normalize a rad angle between 0 and 2*PI */
+#define NormCourseRad(x) { \
+ while (x < 0) x += 2*M_PI; \
+ while (x >= 2*M_PI) x -= 2*M_PI; \
+ }
+
+extern uint8_t last_wp __attribute__((unused));
+
+extern void nav_init(void);
+extern void nav_run(void);
+
+extern void set_exception_flag(uint8_t flag_num);
+
+extern float get_dist2_to_waypoint(uint8_t wp_id);
+extern float get_dist2_to_point(struct EnuCoor_f *p);
+extern void compute_dist2_to_home(void);
+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_periodic_task(void);
+
+extern bool nav_is_in_flight(void);
+
+/** heading utility functions */
+extern void nav_set_heading_rad(float rad);
+extern void nav_set_heading_deg(float deg);
+extern void nav_set_heading_towards(float x, float y);
+extern void nav_set_heading_towards_waypoint(uint8_t wp);
+extern void nav_set_heading_towards_target(void);
+extern void nav_set_heading_current(void);
+
+extern void nav_set_failsafe(void);
+
+/* switching motors on/off */
+static inline void NavKillThrottle(void)
+{
+ if (autopilot_get_mode() == AP_MODE_NAV) { autopilot_set_motors_on(FALSE); }
+}
+static inline void NavResurrect(void)
+{
+ if (autopilot_get_mode() == AP_MODE_NAV) { autopilot_set_motors_on(TRUE); }
+}
+
+
+#define NavSetManual(_roll, _pitch, _yaw) _Pragma("GCC error \"Manual mode in flight plan for fixedwing is not available\"")
+#define NavSetFailsafe nav_set_failsafe
+
+#define NavSetGroundReferenceHere nav_reset_reference
+#define NavSetAltitudeReferenceHere nav_reset_alt
+
+#define NavSetWaypointHere waypoint_set_here_2d
+#define NavCopyWaypoint waypoint_copy
+#define NavCopyWaypointPositionOnly waypoint_position_copy
+
+
+/** Check the time spent in a radius of 'ARRIVED_AT_WAYPOINT' around a wp */
+bool nav_check_wp_time(struct EnuCoor_f *wp, float stay_time);
+#define NavCheckWaypointTime(wp, time) nav_check_wp_time(&waypoints[wp].enu_f, time)
+
+
+/***********************************************************
+ * macros used by flight plan to set different modes
+ **********************************************************/
+
+// command direct turn rate from FP roll [-1.; 1.]
+#define NavAttitude(_roll) { \
+ nav.mode = NAV_MODE_MANUAL; \
+ nav.turn = _roll; \
+ BoundAbs(nav.turn, 1.f); \
+}
+
+// command direct motor speed from FP throttle [-1.; 1.]
+#define NavVerticalThrottleMode(_speed) { \
+ nav.speed = _speed; \
+ BoundAbs(nav.speed, 1.f); \
+}
+
+/** Set the heading of the rover, nothing else */
+#define NavHeading nav_set_heading_rad
+
+
+
+/***********************************************************
+ * built in navigation routines
+ **********************************************************/
+
+/*********** Navigation to waypoint *************************************/
+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
+}
+
+/*********** Navigation along a line *************************************/
+static inline void NavSegment(uint8_t wp_start, uint8_t wp_end)
+{
+ nav.mode = NAV_MODE_ROUTE;
+ if (nav.nav_route) {
+ nav.nav_route(&waypoints[wp_start].enu_f, &waypoints[wp_end].enu_f);
+ }
+}
+
+static inline bool NavApproaching(uint8_t wp, float approaching_time)
+{
+ if (nav.nav_approaching) {
+ return nav.nav_approaching(&waypoints[wp].enu_f, NULL, approaching_time);
+ }
+ else {
+ return true;
+ }
+}
+
+static inline bool NavApproachingFrom(uint8_t to, uint8_t from, float approaching_time)
+{
+ if (nav.nav_approaching) {
+ return nav.nav_approaching(&waypoints[to].enu_f, &waypoints[from].enu_f, approaching_time);
+ }
+ else {
+ return true;
+ }
+}
+
+/*********** Navigation on a circle **************************************/
+static inline void NavCircleWaypoint(uint8_t wp_center, float radius)
+{
+ nav.mode = NAV_MODE_CIRCLE;
+ if (nav.nav_circle) {
+ nav.nav_circle(&waypoints[wp_center].enu_f, radius);
+ }
+}
+
+/*********** Navigation along an oval *************************************/
+static inline void nav_oval_init(void)
+{
+ if (nav.nav_oval_init) {
+ nav.nav_oval_init();
+ }
+}
+
+static inline void Oval(uint8_t wp1, uint8_t wp2, float radius)
+{
+ if (nav.nav_oval) {
+ nav.nav_oval(&waypoints[wp1].enu_f, &waypoints[wp2].enu_f, radius);
+ }
+}
+
+
+/** Settings handlers
+ */
+#define navigation_IncreaseShift(x) { if (x==0) nav.shift = 0; else nav.shift += x; }
+
+#define navigation_SetNavRadius(x) { if (x==1) nav.radius = DEFAULT_CIRCLE_RADIUS; else if (x==-1) nav.radius = -DEFAULT_CIRCLE_RADIUS; else nav.radius = x; }
+
+
+/* follow another aircraft TODO */
+#define NavFollow nav_follow(_id, _dist, _height) {}
+
+
+/** Unused compat macros
+ */
+
+#define NavGlide(_start_wp, _wp) {}
+#define NavVerticalAutoThrottleMode(_pitch) {}
+#define NavVerticalAutoPitchMode(_throttle) {}
+#define NavVerticalAltitudeMode(_alt, _pre_climb) {}
+#define NavVerticalClimbMode(_climb) {}
+
+
+#endif /* NAVIGATION_H */
diff --git a/sw/airborne/firmwares/rover/rover_datalink.c b/sw/airborne/firmwares/rover/rover_datalink.c
new file mode 100644
index 0000000000..5cbd5b1a47
--- /dev/null
+++ b/sw/airborne/firmwares/rover/rover_datalink.c
@@ -0,0 +1,74 @@
+/*
+ * Copyright (C) 2018 Gautier Hattenberger
+ *
+ * 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
+ * .
+ */
+
+/**
+ * @file firmwares/rover/rover_datalink.c
+ * Handling of messages coming from ground and other A/Cs.
+ *
+ */
+
+#include "subsystems/datalink/datalink.h"
+
+#include "pprzlink/dl_protocol.h"
+
+#ifdef USE_NAVIGATION
+#include "firmwares/rover/navigation.h"
+#include "math/pprz_geodetic_int.h"
+#endif
+
+#include "autopilot.h"
+
+void firmware_parse_msg(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf)
+{
+ uint8_t msg_id = IdOfPprzMsg(buf);
+
+ /* parse telemetry messages coming from ground station */
+ switch (msg_id) {
+
+#ifdef USE_NAVIGATION
+ case DL_BLOCK : {
+ if (DL_BLOCK_ac_id(buf) != AC_ID) { break; }
+ nav_goto_block(DL_BLOCK_block_id(buf));
+ }
+ break;
+
+ case DL_MOVE_WP : {
+ uint8_t ac_id = DL_MOVE_WP_ac_id(buf);
+ if (ac_id != AC_ID) { break; }
+ if (stateIsLocalCoordinateValid()) {
+ uint8_t wp_id = DL_MOVE_WP_wp_id(buf);
+ struct LlaCoor_i lla;
+ lla.lat = DL_MOVE_WP_lat(buf);
+ lla.lon = DL_MOVE_WP_lon(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;
+ waypoint_move_lla(wp_id, &lla);
+ }
+ }
+ break;
+#endif /* USE_NAVIGATION */
+
+ default:
+ break;
+ }
+}
diff --git a/sw/airborne/firmwares/rover/rover_telemetry.c b/sw/airborne/firmwares/rover/rover_telemetry.c
new file mode 100644
index 0000000000..7f04eecfd0
--- /dev/null
+++ b/sw/airborne/firmwares/rover/rover_telemetry.c
@@ -0,0 +1,27 @@
+/*
+ * Copyright (C) 2018 Gautier Hattenberger
+ *
+ * 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
+ * .
+ */
+
+/* PERIODIC_C_MAIN is defined before generated/periodic_telemetry.h
+ * in order to implement telemetry_mode_Main_*
+ */
+#define PERIODIC_C_MAIN
+
+#include "generated/periodic_telemetry.h"
+
diff --git a/sw/airborne/modules/nav/nav_rover_base.c b/sw/airborne/modules/nav/nav_rover_base.c
new file mode 100644
index 0000000000..8ffeb4bd22
--- /dev/null
+++ b/sw/airborne/modules/nav/nav_rover_base.c
@@ -0,0 +1,304 @@
+/*
+ * Copyright (C) 2018 Gautier Hattenberger
+ *
+ * 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
+ * .
+ */
+
+/**
+ * @file "modules/nav/nav_rover_base.c"
+ * @author 2018 Gautier Hattenberger
+ * Basic navigation functions for Rovers
+ */
+
+#include "modules/nav/nav_rover_base.h"
+
+struct RoverNavBase nav_rover_base;
+
+/** Implement basic nav function
+ */
+
+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);
+ nav.mode = NAV_MODE_WAYPOINT;
+}
+
+static void nav_route(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
+{
+ struct FloatVect2 wp_diff, pos_diff;
+ VECT2_DIFF(wp_diff, *wp_end, *wp_start);
+ VECT2_DIFF(pos_diff, *stateGetPositionEnu_f(), *wp_start);
+ // leg length
+ float leg_length2 = Max((wp_diff.x * wp_diff.x + wp_diff.y * wp_diff.y), 0.1f);
+ nav_rover_base.goto_wp.leg_length = sqrtf(leg_length2);
+ // leg progress
+ nav_rover_base.goto_wp.leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / nav_rover_base.goto_wp.leg_length;
+ nav_rover_base.goto_wp.leg_progress += Max(CARROT_DIST, 0.f);
+ // next pos on leg
+ struct FloatVect2 progress_pos;
+ VECT2_SMUL(progress_pos, wp_diff, nav_rover_base.goto_wp.leg_progress / nav_rover_base.goto_wp.leg_length);
+ VECT2_SUM(nav.target, *wp_start, progress_pos);
+
+ nav_rover_base.goto_wp.from = *wp_start;
+ nav_rover_base.goto_wp.to = *wp_end;
+ nav_rover_base.goto_wp.dist2_to_wp = get_dist2_to_point(wp_end);
+ nav.mode = NAV_MODE_ROUTE;
+}
+
+static bool nav_approaching(struct EnuCoor_f *wp, struct EnuCoor_f *from, float approaching_time)
+{
+ float dist_to_point;
+ struct FloatVect2 diff;
+ struct EnuCoor_f *pos = stateGetPositionEnu_f();
+
+ /* if an approaching_time is given, estimate diff after approching_time secs */
+ if (approaching_time > 0.f) {
+ struct FloatVect2 estimated_pos;
+ struct FloatVect2 estimated_progress;
+ struct EnuCoor_f *speed = stateGetSpeedEnu_f();
+ VECT2_SMUL(estimated_progress, *speed, approaching_time);
+ VECT2_SUM(estimated_pos, *pos, estimated_progress);
+ VECT2_DIFF(diff, *wp, estimated_pos);
+ }
+ /* else use current position */
+ else {
+ VECT2_DIFF(diff, *wp, *pos);
+ }
+ /* compute distance of estimated/current pos to target wp
+ */
+ //nav.dist_to_point = float_vect2_norm(&diff); FIXME
+ dist_to_point = float_vect2_norm(&diff);
+
+ /* return TRUE if we have arrived */
+ if (dist_to_point < ARRIVED_AT_WAYPOINT) {
+ return true;
+ }
+
+ /* if coming from a valid waypoint */
+ if (from != NULL) {
+ /* return TRUE if normal line at the end of the segment is crossed */
+ struct FloatVect2 from_diff;
+ VECT2_DIFF(from_diff, *wp, *from);
+ return (diff.x * from_diff.x + diff.y * from_diff.y < 0.f);
+ }
+
+ return false;
+}
+
+static void nav_circle(struct EnuCoor_f *wp_center, float radius)
+{
+ struct FloatVect2 pos_diff;
+ VECT2_DIFF(pos_diff, *stateGetPositionEnu_f(), *wp_center);
+ // store last qdr
+ float last_qdr = nav_rover_base.circle.qdr;
+ // compute qdr
+ nav_rover_base.circle.qdr = atan2f(pos_diff.y, pos_diff.x);
+ // increment circle radians
+ float trigo_diff = nav_rover_base.circle.qdr - last_qdr;
+ NormRadAngle(trigo_diff);
+ nav_rover_base.circle.radians += trigo_diff;
+
+ // direction of rotation
+ float sign_radius = radius > 0.f ? 1.f : -1.f;
+ // absolute radius
+ float abs_radius = fabsf(radius);
+ if (abs_radius > 0.1f) {
+ // carrot_angle
+ float carrot_angle = CARROT_DIST / abs_radius;
+ carrot_angle = Min(carrot_angle, M_PI / 4);
+ carrot_angle = Max(carrot_angle, M_PI / 16);
+ carrot_angle = nav_rover_base.circle.qdr - sign_radius * carrot_angle;
+ // compute setpoint
+ VECT2_ASSIGN(pos_diff, abs_radius * cosf(carrot_angle), abs_radius * sinf(carrot_angle));
+ VECT2_SUM(nav.target, *wp_center, pos_diff);
+ }
+ else {
+ // radius is too small, direct to center
+ VECT2_COPY(nav.target, *wp_center);
+ }
+
+ nav_rover_base.circle.center = *wp_center;
+ nav_rover_base.circle.radius = radius;
+ nav.mode = NAV_MODE_CIRCLE;
+}
+
+/**
+ * Navigation along a figure O. One side leg is defined by waypoints [p1] and [p2].
+ * The navigation goes through 4 states: OC1 (half circle next to [p1]),
+ * OR21 (route [p2] to [p1], OC2 (half circle next to [p2]) and OR12 (opposite leg).
+ * Initial state is the route along the desired segment (OC2).
+ */
+#ifndef LINE_START_FUNCTION
+#define LINE_START_FUNCTION {}
+#endif
+#ifndef LINE_STOP_FUNCTION
+#define LINE_STOP_FUNCTION {}
+#endif
+
+static void _nav_oval_init(void)
+{
+ nav_rover_base.oval.status = OC2;
+ nav_rover_base.oval.count = 0;
+}
+
+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 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);
+
+ /* 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_i p1_out = { waypoints[p1].enu_i.x + 2 * radius * -u_y,
+ waypoints[p1].enu_i.y + 2 * radius * u_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;
+ }
+ int32_t qdr_anticipation = ANGLE_BFP_OF_REAL(radius > 0 ? -15 : 15);
+
+ switch (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;
+ 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++;
+ 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;
+ 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;
+ InitStage();
+ LINE_STOP_FUNCTION;
+ }
+ return;
+
+ default: /* Should not occur !!! Doing nothing */
+ return;
+ }
+#endif
+}
+
+#if PERIODIC_TELEMETRY
+#include "subsystems/datalink/telemetry.h"
+
+static void send_segment(struct transport_tx *trans, struct link_device *dev)
+{
+ pprz_msg_send_SEGMENT(trans, dev, AC_ID,
+ &nav_rover_base.goto_wp.from.x,
+ &nav_rover_base.goto_wp.from.y,
+ &nav_rover_base.goto_wp.to.x,
+ &nav_rover_base.goto_wp.to.y);
+}
+
+static void send_circle(struct transport_tx *trans, struct link_device *dev)
+{
+ pprz_msg_send_CIRCLE(trans, dev, AC_ID,
+ &nav_rover_base.circle.center.x,
+ &nav_rover_base.circle.center.y,
+ &nav_rover_base.circle.radius);
+}
+
+static void send_nav_status(struct transport_tx *trans, struct link_device *dev)
+{
+ float dist_home = sqrtf(nav.dist2_to_home);
+ float dist_wp = sqrtf(nav_rover_base.goto_wp.dist2_to_wp);
+ pprz_msg_send_ROTORCRAFT_NAV_STATUS(trans, dev, AC_ID,
+ &block_time, &stage_time,
+ &dist_home, &dist_wp,
+ &nav_block, &nav_stage,
+ &nav.mode);
+ if (nav.mode == NAV_MODE_ROUTE) {
+ send_segment(trans, dev);
+ } else if (nav.mode == NAV_MODE_CIRCLE) {
+ send_circle(trans, dev);
+ }
+}
+#endif
+
+/** Init and register nav functions
+ */
+void nav_rover_init(void)
+{
+ nav_rover_base.circle.radius = DEFAULT_CIRCLE_RADIUS;
+ nav_rover_base.goto_wp.leg_progress = 0.f;
+ nav_rover_base.goto_wp.leg_length = 1.f;
+
+ // register nav functions
+ nav_register_goto_wp(nav_goto, nav_route, nav_approaching);
+ nav_register_circle(nav_circle);
+ nav_register_oval(_nav_oval_init, nav_oval);
+
+#if PERIODIC_TELEMETRY
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_NAV_STATUS, send_nav_status);
+#endif
+
+}
+
+
diff --git a/sw/airborne/modules/nav/nav_rover_base.h b/sw/airborne/modules/nav/nav_rover_base.h
new file mode 100644
index 0000000000..013533b7cc
--- /dev/null
+++ b/sw/airborne/modules/nav/nav_rover_base.h
@@ -0,0 +1,87 @@
+/*
+ * Copyright (C) 2018 Gautier Hattenberger
+ *
+ * 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
+ * .
+ */
+
+/**
+ * @file "modules/nav/nav_rover_base.h"
+ * @author 2018 Gautier Hattenberger
+ * Basic navigation functions for Rovers
+ */
+
+#ifndef NAV_ROVER_BASE_H
+#define NAV_ROVER_BASE_H
+
+#include "firmwares/rover/navigation.h"
+
+/** Waypoint and route pattern
+ */
+struct RoverNavGoto {
+ struct EnuCoor_f from; ///< start WP position
+ struct EnuCoor_f to; ///< end WP position
+ float dist2_to_wp; ///< squared distance to next waypoint
+ float leg_progress; ///< progress over leg
+ float leg_length; ///< leg length
+};
+
+/** Circle pattern
+ */
+struct RoverNavCircle {
+ struct EnuCoor_f center; ///< center WP position
+ float radius; ///< radius in meters
+ float qdr; ///< qdr in radians
+ float radians; ///< incremental angular distance
+};
+
+/** Oval pattern
+ */
+enum oval_status { OR12, OC2, OR21, OC1 };
+struct RoverNavOval {
+ enum oval_status status; ///< oval status
+ uint8_t count; ///< number of laps
+};
+
+/** Basic Nav struct
+ */
+struct RoverNavBase {
+ struct RoverNavGoto goto_wp;
+ struct RoverNavCircle circle;
+ struct RoverNavOval oval;
+};
+
+extern struct RoverNavBase nav_rover_base;
+
+extern void nav_rover_init(void);
+
+
+/** Macros for circle nav
+ */
+#define NavCircleCount() (fabsf(nav_rover_base.circle.radians) / (2*M_PI))
+#define NavCircleQdr() ({ float qdr = DegOfRad(M_PI_2 - nav_rover_base.circle.qdr); NormCourse(qdr); qdr; })
+
+#define CloseDegAngles(_c1, _c2) ({ float _diff = _c1 - _c2; NormCourse(_diff); 350 < _diff || _diff < 10; })
+#define CloseRadAngles(_c1, _c2) ({ float _diff = _c1 - _c2; NormRadAngle(_diff); fabsf(_diff) < 0.0177; })
+
+/** True if x (in degrees) is close to the current QDR (less than 10 degrees)
+ */
+#define NavQdrCloseTo(x) CloseDegAngles(x, NavCircleQdr())
+#define NavCourseCloseTo(x) CloseDegAngles(x, DegOfRad(stateGetHorizontalSpeedDir_f()))
+
+
+#endif
+
diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink
index af581eec19..07bc6cfc72 160000
--- a/sw/ext/pprzlink
+++ b/sw/ext/pprzlink
@@ -1 +1 @@
-Subproject commit af581eec19e85d173ec51489db634c09c08dfbc6
+Subproject commit 07bc6cfc72c8ac1b9654a11ade6ecdfdf4f62508