diff --git a/conf/Makefile.nps b/conf/Makefile.nps
index 146590f3c3..3fc929dcca 100644
--- a/conf/Makefile.nps
+++ b/conf/Makefile.nps
@@ -26,7 +26,6 @@
 
 SRC_ARCH = arch/sim
 
-CC = g++
 OPT ?= 2
 SIMDIR = $(PAPARAZZI_SRC)/sw/simulator
 
@@ -39,6 +38,14 @@ CFLAGS += $($(TARGET).CFLAGS)
 CFLAGS += $(LOCAL_CFLAGS)
 CFLAGS += -O$(OPT)
 CFLAGS += -g
+CFLAGS += -std=gnu99
+
+CXXFLAGS  = -W -Wall
+CXXFLAGS += $(INCLUDES)
+CXXFLAGS += $($(TARGET).CFLAGS)
+CXXFLAGS += $(LOCAL_CFLAGS)
+CXXFLAGS += -O$(OPT)
+CXXFLAGS += -g
 
 LDFLAGS	+= $($(TARGET).LDFLAGS)
 
@@ -48,7 +55,8 @@ LDFLAGS	+= $($(TARGET).LDFLAGS)
 
 $(TARGET).srcsnd = $(notdir $($(TARGET).srcs))
 $(TARGET).objso	= $($(TARGET).srcs:%.c=$(OBJDIR)/%.o)
-$(TARGET).objs	= $($(TARGET).objso:%.S=$(OBJDIR)/%.o)
+$(TARGET).objsoxx = $($(TARGET).objso:%.cpp=$(OBJDIR)/%.o)
+$(TARGET).objs	= $($(TARGET).objsoxx:%.S=$(OBJDIR)/%.o)
 
 all compile: check_jsbsim $(OBJDIR)/simsitl
 
@@ -59,7 +67,7 @@ check_jsbsim:
 
 $(OBJDIR)/simsitl : $($(TARGET).objs)
 	@echo LD $@
-	$(Q)$(CC) $(CFLAGS) -o $@ $($(TARGET).objs) $(LDFLAGS)
+	$(Q)$(CXX) $(CXXFLAGS) -o $@ $($(TARGET).objs) $(LDFLAGS)
 
 
 %.s: %.c
@@ -74,9 +82,9 @@ $(OBJDIR)/%.s: %.c
 	$(CC) $(CFLAGS) -S -o $@ $<
 
 $(OBJDIR)/%.s: %.cpp
-	@echo CC $@
+	@echo CXX $@
 	$(Q)test -d $(dir $@) || mkdir -p $(dir $@)
-	$(CC) $(CFLAGS) -S -o $@ $<
+	$(CXX) $(CXXFLAGS) -S -o $@ $<
 
 $(OBJDIR)/%.o: %.c $(OBJDIR)/../Makefile.ac
 	@echo CC $@
@@ -84,20 +92,26 @@ $(OBJDIR)/%.o: %.c $(OBJDIR)/../Makefile.ac
 	$(Q)$(CC) $(CFLAGS) -c -o $@ $<
 
 $(OBJDIR)/%.o: %.cpp $(OBJDIR)/../Makefile.ac
-	@echo CC $@
+	@echo CXX $@
 	$(Q)test -d $(dir $@) || mkdir -p $(dir $@)
-	$(Q)$(CC) $(CFLAGS) -c -o $@ $<
+	$(Q)$(CXX) $(CXXFLAGS) -c -o $@ $<
 
 .PHONY: all compile check_jsbsim
 
 #
 # Dependencies
 #
-$(OBJDIR)/.depend:
-	@test -d $(OBJDIR) || mkdir -p $(OBJDIR)
-	@echo DEPEND $@
-	$(Q)$(CC) -MM -MG $(CFLAGS) $($(TARGET).srcs) | sed 's|\([^\.]*\.o\)|$(OBJDIR)/\1|' > $@
 
+$(OBJDIR)/%.d: %.c
+	@echo DEP $@
+	$(Q)test -d $(dir $@) || mkdir -p $(dir $@)
+	$(Q)$(CC) -MM -MG $(CFLAGS) $< | sed 's|\([^\.]*\.o\)|$(OBJDIR)/\1|' > $(OBJDIR)/$*.d
+
+$(OBJDIR)/%.d: %.cpp $(OBJDIR)/../Makefile.ac
+	@echo DEP $@
+	$(Q)test -d $(dir $@) || mkdir -p $(dir $@)
+	$(Q)$(CXX) -MM -MG $(CXXFLAGS) $< | sed 's|\([^\.]*\.o\)|$(OBJDIR)/\1|' > $(OBJDIR)/$*.d
+ 
 ifneq ($(MAKECMDGOALS),clean)
--include $(OBJDIR)/.depend
+-include $($(TARGET).objs:.o=.d)
 endif
diff --git a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile
index 96a7d39e8c..41b359beb5 100644
--- a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile
+++ b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile
@@ -39,7 +39,7 @@ endif
 
 
 nps.srcs += $(NPSDIR)/nps_main.c                 \
-       $(NPSDIR)/nps_fdm_jsbsim.c                \
+       $(NPSDIR)/nps_fdm_jsbsim.cpp              \
        $(NPSDIR)/nps_random.c                    \
        $(NPSDIR)/nps_sensors.c                   \
        $(NPSDIR)/nps_sensors_utils.c             \
diff --git a/sw/airborne/arch/sim/subsystems/settings_arch.c b/sw/airborne/arch/sim/subsystems/settings_arch.c
index 94bfbb9e62..ea69ba9f12 100644
--- a/sw/airborne/arch/sim/subsystems/settings_arch.c
+++ b/sw/airborne/arch/sim/subsystems/settings_arch.c
@@ -28,14 +28,10 @@
 
 #include "subsystems/settings.h"
 
-int32_t persistent_write(uint32_t ptr, uint32_t size) {
-  ptr=ptr;
-  size=size;
+int32_t persistent_write(uint32_t ptr UNUSED, uint32_t size UNUSED) {
   return -1;
 }
 
-int32_t persistent_read(uint32_t ptr, uint32_t size) {
-  ptr=ptr;
-  size=size;
+int32_t persistent_read(uint32_t ptr UNUSED, uint32_t size UNUSED) {
   return -1;
 }
diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c
index d9754431a8..118987b9e5 100644
--- a/sw/airborne/firmwares/rotorcraft/navigation.c
+++ b/sw/airborne/firmwares/rotorcraft/navigation.c
@@ -56,7 +56,7 @@ struct EnuCoor_i navigation_carrot;
 
 struct EnuCoor_i nav_last_point;
 
-uint8_t last_wp __attribute__ ((unused));
+uint8_t last_wp UNUSED;
 
 /** Maximum distance from HOME waypoint before going into failsafe mode */
 #ifndef FAILSAFE_MODE_DISTANCE
@@ -180,7 +180,7 @@ void nav_init(void) {
 #endif
 }
 
-static inline void nav_advance_carrot(void) {
+static inline void UNUSED nav_advance_carrot(void) {
   struct EnuCoor_i *pos = stateGetPositionEnu_i();
   /* compute a vector to the waypoint */
   struct Int32Vect2 path_to_waypoint;
diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h
index 44e5523259..c1773e91d9 100644
--- a/sw/airborne/firmwares/rotorcraft/navigation.h
+++ b/sw/airborne/firmwares/rotorcraft/navigation.h
@@ -136,7 +136,7 @@ extern void nav_circle(struct EnuCoor_i * wp_center, int32_t radius);
 }
 
 #define NavCircleCount() (abs(nav_circle_radians) / INT32_ANGLE_2_PI)
-#define NavCircleQdr() ({ int32_t qdr = INT32_DEG_OF_RAD(INT32_ANGLE_2_PI_2 - nav_circle_qdr) >> INT32_ANGLE_FRAC; NormCourse(qdr); qdr; })
+#define NavCircleQdr() ({ int32_t qdr = INT32_DEG_OF_RAD(INT32_ANGLE_2_PI_2 - nav_circle_qdr) >> INT32_ANGLE_FRAC; NormCourse(qdr); qdr; })
 
 /** True if x (in degrees) is close to the current QDR (less than 10 degrees)*/
 #define NavQdrCloseTo(x) {}
diff --git a/sw/airborne/math/pprz_geodetic_double.c b/sw/airborne/math/pprz_geodetic_double.c
index 5a9d9bcec6..6811704513 100644
--- a/sw/airborne/math/pprz_geodetic_double.c
+++ b/sw/airborne/math/pprz_geodetic_double.c
@@ -140,7 +140,7 @@ double gc_of_gd_lat_d(double gd_lat, double hmsl) {
 
 #include "math/pprz_geodetic_utm.h"
 
-static inline double isometric_latitude_d(double phi, double e) {
+static inline double UNUSED isometric_latitude_d(double phi, double e) {
   return log (tan (M_PI_4 + phi / 2.0)) - e / 2.0 * log((1.0 + e * sin(phi)) / (1.0 - e * sin(phi)));
 }
 
diff --git a/sw/airborne/math/pprz_geodetic_double.h b/sw/airborne/math/pprz_geodetic_double.h
index 5903506386..5eab7c5e02 100644
--- a/sw/airborne/math/pprz_geodetic_double.h
+++ b/sw/airborne/math/pprz_geodetic_double.h
@@ -30,6 +30,10 @@
 #ifndef PPRZ_GEODETIC_DOUBLE_H
 #define PPRZ_GEODETIC_DOUBLE_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #include "pprz_geodetic.h"
 #include "pprz_algebra_double.h"
 #include "std.h"
@@ -114,5 +118,8 @@ extern void ecef_of_ned_vect_d(struct EcefCoor_d* ecef, struct LtpDef_d* def, st
 
 extern double gc_of_gd_lat_d(double gd_lat, double hmsl);
 
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
 
 #endif /* PPRZ_GEODETIC_DOUBLE_H */
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
index 06c4a7ea3c..e322002d31 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
@@ -142,7 +142,7 @@ float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
 struct AhrsIntCmplQuat ahrs_impl;
 
 static inline void set_body_state_from_quat(void);
-static inline void ahrs_update_mag_full(void);
+static inline void UNUSED ahrs_update_mag_full(void);
 static inline void ahrs_update_mag_2d(void);
 
 #if PERIODIC_TELEMETRY
diff --git a/sw/include/std.h b/sw/include/std.h
index 2af2b0ef9d..bfd2d66c67 100644
--- a/sw/include/std.h
+++ b/sw/include/std.h
@@ -226,4 +226,10 @@ static inline bool_t str_equal(const char * a, const char * b) {
   return TRUE;
 }
 
+#ifdef __GNUC__
+#  define UNUSED __attribute__((__unused__))
+#else
+#  define UNUSED
+#endif
+
 #endif /* STD_H */
diff --git a/sw/simulator/nps/nps_fdm.h b/sw/simulator/nps/nps_fdm.h
index 77e6098b0d..96c21889c1 100644
--- a/sw/simulator/nps/nps_fdm.h
+++ b/sw/simulator/nps/nps_fdm.h
@@ -22,6 +22,9 @@
 #ifndef NPS_FDM
 #define NPS_FDM
 
+#ifdef __cplusplus
+extern "C" {
+#endif
 
 #include "std.h"
 #include "math/pprz_geodetic_double.h"
@@ -96,4 +99,8 @@ extern void nps_fdm_init(double dt);
 extern void nps_fdm_run_step(bool_t launch, double* commands, int commands_nb);
 extern void nps_fdm_set_wind(double speed, double dir, int turbulence_severity);
 
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
 #endif /* NPS_FDM */
diff --git a/sw/simulator/nps/nps_fdm_jsbsim.c b/sw/simulator/nps/nps_fdm_jsbsim.cpp
similarity index 98%
rename from sw/simulator/nps/nps_fdm_jsbsim.c
rename to sw/simulator/nps/nps_fdm_jsbsim.cpp
index 7f80b04a0e..abec4d1905 100644
--- a/sw/simulator/nps/nps_fdm_jsbsim.c
+++ b/sw/simulator/nps/nps_fdm_jsbsim.cpp
@@ -31,6 +31,7 @@
 #include <stdio.h>
 
 // ignore stupid warnings in JSBSim
+#pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wunused-parameter"
 #include <FGFDMExec.h>
 #pragma GCC diagnostic pop
@@ -475,17 +476,17 @@ static void init_ltp(void) {
 
 
 #if !NPS_CALC_GEO_MAG && defined(AHRS_H_X)
-#pragma message "Using magnetic field as defined in airframe file (AHRS section)."
+PRINT_CONFIG_MSG("Using magnetic field as defined in airframe file (AHRS section).")
   fdm.ltp_h.x = AHRS_H_X;
   fdm.ltp_h.y = AHRS_H_Y;
   fdm.ltp_h.z = AHRS_H_Z;
 #elif !NPS_CALC_GEO_MAG && defined(INS_H_X)
-#pragma message "Using magnetic field as defined in airframe file (INS section)."
+PRINT_CONFIG_MSG("Using magnetic field as defined in airframe file (INS section).")
   fdm.ltp_h.x = INS_H_X;
   fdm.ltp_h.y = INS_H_Y;
   fdm.ltp_h.z = INS_H_Z;
 #else
-#pragma message "Using WMM2010 model to calculate magnetic field at simulated location."
+PRINT_CONFIG_MSG("Using WMM2010 model to calculate magnetic field at simulated location.")
   /* calculation of magnetic field according to WMM2010 model */
   double gha[MAXCOEFF];
 
diff --git a/sw/simulator/nps/nps_radio_control_joystick.c b/sw/simulator/nps/nps_radio_control_joystick.c
index 4b164777b2..88b3d1dab9 100644
--- a/sw/simulator/nps/nps_radio_control_joystick.c
+++ b/sw/simulator/nps/nps_radio_control_joystick.c
@@ -114,7 +114,7 @@
 #define JS_NB_BUTTONS 0
 #endif
 
-NpsJoystick nps_joystick;
+struct NpsJoystick nps_joystick;
 SDL_Joystick *sdl_joystick;
 SDL_Event sdl_event;
 
diff --git a/sw/simulator/nps/nps_random.c b/sw/simulator/nps/nps_random.c
index 0619af89ed..0edc28f514 100644
--- a/sw/simulator/nps/nps_random.c
+++ b/sw/simulator/nps/nps_random.c
@@ -31,8 +31,6 @@ static unsigned long int randlcg(void);
 #endif
 
 
-
-
 void double_vect3_add_gaussian_noise(struct DoubleVect3* vect, struct DoubleVect3* std_dev) {
   vect->x += get_gaussian_noise() * std_dev->x;
   vect->y += get_gaussian_noise() * std_dev->y;
diff --git a/sw/simulator/nps/nps_sensor_gps.c b/sw/simulator/nps/nps_sensor_gps.c
index 1ef6abe838..9a63423a32 100644
--- a/sw/simulator/nps/nps_sensor_gps.c
+++ b/sw/simulator/nps/nps_sensor_gps.c
@@ -79,7 +79,7 @@ void nps_sensor_gps_run_step(struct NpsSensorGps* gps, double time) {
    */
   /* convert current ecef reading to lla */
   struct LlaCoor_d cur_lla_reading;
-  lla_of_ecef_d(&cur_lla_reading, (EcefCoor_d*) &cur_pos_reading);
+  lla_of_ecef_d(&cur_lla_reading, (struct EcefCoor_d*) &cur_pos_reading);
 
   /* store that for later and retrieve a previously stored data */
   UpdateSensorLatency(time, &cur_lla_reading, &gps->lla_history, gps->pos_latency, &gps->lla_pos);