Merge branch 'master' into nav

conf/Makefile.lpc21

@@ -32,7 +32,7 @@
 SRC_ARCH = arch/lpc21
 
 # Define programs and commands.
-HAVE_ARM_NONE_EABI_GCC := $(wildcard /usr/bin/arm-none-eabi-gcc)
+HAVE_ARM_NONE_EABI_GCC := $(shell which arm-none-eabi-gcc)
 ifeq ($(strip $(HAVE_ARM_NONE_EABI_GCC)),)
 CC		= arm-elf-gcc
 LD		= $(CC)

conf/Makefile.stm32

@@ -38,7 +38,7 @@ OPT   = s
 #OPT   = 0
 
 # Programs location
-TOOLCHAIN_DIR=/opt/paparazzi/stm32
+TOOLCHAIN_DIR=$(shell find -L /opt/paparazzi/stm32 ~/sat -maxdepth 1 -type d -name arm-none-eabi 2>/dev/null | head -n 1 | xargs dirname )
 GCC_BIN_DIR=$(TOOLCHAIN_DIR)/bin
 GCC_LIB_DIR=$(TOOLCHAIN_DIR)/arm-none-eabi/lib
 

conf/airframes/TUDelft/yapa_xsens.xml

@@ -0,0 +1,212 @@
+<!DOCTYPE airframe SYSTEM "../airframe.dtd">
+
+<!-- 
+	YAPA + XSens + XBee
+-->
+
+<airframe name="Yapa v1">
+
+  <modules>
+    <load name="TUDelft/blink_led_3.xml"/>
+  </modules>
+
+  <servos>
+    <servo name="MOTOR"         no="0" min="1000" neutral="1000" max="2000"/>
+    <servo name="AILEVON_LEFT"  no="2" min="1130" neutral="1575" max="1880"/>
+    <servo name="AILEVON_RIGHT" no="6" min="1980" neutral="1465" max="1170"/>
+  </servos>
+
+  <commands>
+    <axis name="THROTTLE" failsafe_value="0"/>
+    <axis name="ROLL"     failsafe_value="0"/>
+    <axis name="PITCH"    failsafe_value="0"/>
+    <axis name="HATCH"    failsafe_value="0"/>
+    <axis name="CAM_TILT" failsafe_value="0"/>
+    <axis name="CAM_PAN" failsafe_value="0"/>
+  </commands>
+
+  <rc_commands>
+    <set command="THROTTLE" value="@THROTTLE"/>
+    <set command="ROLL"     value="@ROLL"/>
+    <set command="PITCH"    value="@PITCH"/>
+  </rc_commands>
+
+  <section name="MIXER">
+    <define name="AILEVON_AILERON_RATE" value="0.75"/>
+    <define name="AILEVON_ELEVATOR_RATE" value="0.75"/>
+  </section>
+
+  <command_laws>
+    <let var="aileron"  value="@ROLL  * AILEVON_AILERON_RATE"/>
+    <let var="elevator" value="@PITCH * AILEVON_ELEVATOR_RATE"/>  
+    <set servo="MOTOR"           value="@THROTTLE"/>
+    <set servo="AILEVON_LEFT"  value="$elevator + $aileron"/>
+    <set servo="AILEVON_RIGHT" value="$elevator - $aileron"/>
+  </command_laws>
+
+  <section name="AUTO1" prefix="AUTO1_">
+    <define name="MAX_ROLL" value="0.85"/>
+    <define name="MAX_PITCH" value="0.6"/>
+  </section>
+
+  <section name="adc" prefix="ADC_CHANNEL_">
+    <define name="IR1" value="ADC_0"/>
+    <define name="IR2" value="ADC_1"/>
+    <define name="IR_TOP" value="ADC_2"/>
+    <define name="IR_NB_SAMPLES" value="16"/>
+
+    <define name="ONBOARDCAMA" value="ADC_4"/>
+    <define name="ONBOARDCAMA_NB_SAMPLES" value="16"/>
+    <define name="ONBOARDCAMB" value="ADC_5"/>
+    <define name="ONBOARDCAMB_NB_SAMPLES" value="16"/>
+
+
+    <define name="GYRO_ROLL" value="ADC_3"/>
+    <define name="GYRO_NB_SAMPLES" value="16"/> 
+
+  </section>
+
+  <section name="CAMERA" prefix="CAM_">
+    <define name="TILT0" value="0" unit="deg"/>
+    <define name="TILT_MIN" value="-20" unit="deg"/>
+    <define name="TILT_NEUTRAL" value="0" unit="deg"/>
+    <define name="TILT_MAX" value="20" unit="deg"/>
+    <define name="PAN0" value="0" unit="deg"/>
+    <define name="PAN_MIN" value="-20" unit="deg"/>
+    <define name="PAN_NEUTRAL" value="0" unit="deg"/>
+    <define name="PAN_MAX" value="20" unit="deg"/>
+  </section>
+
+  <section name="BAT">
+    <define name="MILLIAMP_AT_FULL_THROTTLE" value="2000"/>
+
+    <define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
+  </section>
+ 
+  <section name="MISC">
+    <define name="MINIMUM_AIRSPEED" value="12." unit="m/s"/>
+    <define name="NOMINAL_AIRSPEED" value="15." unit="m/s"/>
+    <define name="MAXIMUM_AIRSPEED" value="25." unit="m/s"/>
+    <define name="CARROT" value="5." unit="s"/>
+    <define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
+    <define name="CONTROL_RATE" value="60" unit="Hz"/>
+    <define name="ALT_KALMAN_ENABLED" value="TRUE"/>
+
+    <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
+    <define name="MIN_CIRCLE_RADIUS" value="50."/>
+    <!--uncommant API protocol in makefile uncommant after programed 
+    <define name="XBEE_INIT" value="&quot;ATCHC\rATID3332\rATPL4\rATRN1\rATTT80\rATBD3\rATWR\r&quot;"/>
+    <define name="NO_XBEE_API_INIT" value="TRUE"/>   -->
+  </section>
+ 
+  <section name="VERTICAL CONTROL" prefix="V_CTL_">
+
+    <!-- outer loop proportional gain -->
+    <define name="ALTITUDE_PGAIN" value="-0.04"/>
+    <!-- outer loop saturation -->
+    <define name="ALTITUDE_MAX_CLIMB" value="2."/>
+
+    <!-- auto throttle inner loop -->
+    <define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.35"/>
+    <define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.25"/>
+    <define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.80"/>
+    <define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
+    <define name="AUTO_THROTTLE_DASH_TRIM" value="-1000"/>
+    <define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.1" unit="%/(m/s)"/>
+    <define name="AUTO_THROTTLE_PGAIN" value="-0.02"/>
+    <define name="AUTO_THROTTLE_IGAIN" value="0.1"/>
+    <define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.1"/>
+
+    <!-- auto pitch inner loop -->
+    <define name="AUTO_PITCH_PGAIN" value="-0.05"/>
+    <define name="AUTO_PITCH_IGAIN" value="0.075"/>
+    <define name="AUTO_PITCH_MAX_PITCH" value="0.35"/>
+    <define name="AUTO_PITCH_MIN_PITCH" value="-0.35"/>
+
+   <define name="THROTTLE_SLEW" value="0.5"/>
+
+  </section>
+
+  <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
+    <define name="COURSE_PGAIN" value="-0.8"/>
+
+    <define name="ROLL_MAX_SETPOINT" value="0.7" unit="radians"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+
+    <define name="PITCH_PGAIN" value="-10000."/>
+    <define name="PITCH_DGAIN" value="1.5"/>
+
+    <define name="ELEVATOR_OF_ROLL" value="2500"/>
+
+    <define name="ROLL_ATTITUDE_GAIN" value="-7500"/>
+    <define name="ROLL_RATE_GAIN" value="-1500"/>
+  </section>
+
+  <section name="AGGRESSIVE" prefix="AGR_">
+    <define name="BLEND_START" value="20"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
+    <define name="BLEND_END" value="10"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes  CANNOT BE ZERO!!-->
+    <define name="CLIMB_THROTTLE" value="0.6"/><!-- Gaz for Aggressive Climb -->
+    <define name="CLIMB_PITCH" value="0.20"/><!-- Pitch for Aggressive Climb -->
+    <define name="DESCENT_THROTTLE" value="0.1"/><!-- Gaz for Aggressive Decent -->
+    <define name="DESCENT_PITCH" value="-0.15"/><!-- Pitch for Aggressive Decent -->
+    <define name="CLIMB_NAV_RATIO" value="0.8"/><!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
+    <define name="DESCENT_NAV_RATIO" value="1.0"/>
+    </section>
+
+  <section name="FAILSAFE" prefix="FAILSAFE_">
+	<define name="DELAY_WITHOUT_GPS" value="1" unit="s"/>
+    	<define name="DEFAULT_THROTTLE" value="0.3" unit="%"/>
+    	<define name="DEFAULT_ROLL" value="0.3" unit="rad"/>
+    	<define name="DEFAULT_PITCH" value="0.5" unit="rad"/>
+	<define name="HOME_RADIUS" value="100" unit="m"/>
+  </section>
+ <makefile>
+CONFIG = \"tiny_2_1_1.h\"
+
+include $(PAPARAZZI_SRC)/conf/autopilot/tiny.makefile
+
+FLASH_MODE=IAP
+
+XSENS_UART_NR = 0
+
+ap.CFLAGS +=  -DFBW -DAP -DBOARD_CONFIG=$(CONFIG) -DLED
+ap.srcs = sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c main_fbw.c main_ap.c main.c
+
+ap.srcs += commands.c
+
+ap.CFLAGS += -DACTUATORS=\"servos_4017_hw.h\" -DSERVOS_4017
+ap.srcs += $(SRC_ARCH)/servos_4017_hw.c actuators.c
+
+ap.CFLAGS += -DRADIO_CONTROL
+ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c  
+
+#ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B57600
+#ap.srcs += downlink.c $(SRC_ARCH)/uart_hw.c datalink.c pprz_transport.c
+
+# Maxstream API protocol
+ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=XBeeTransport -DXBEE_UART=Uart1 -DDATALINK=XBEE -DUART1_BAUD=B9600 -DTELEMETRY_MODE_FBW=1 
+ap.srcs += downlink.c $(SRC_ARCH)/uart_hw.c datalink.c xbee.c
+
+ap.CFLAGS += -DINTER_MCU -DUSE_MODULES
+ap.srcs += inter_mcu.c 
+
+ap.CFLAGS += -DADC -DUSE_ADC_0 -DUSE_ADC_1 -DUSE_ADC_2 -DUSE_ADC_4 -DUSE_ADC_5
+ap.srcs += $(SRC_ARCH)/adc_hw.c
+
+ap.srcs += gps_xsens.c gps.c latlong.c
+
+ap.CFLAGS += -DALT_KALMAN
+ap.srcs += estimator.c
+
+ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
+ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
+
+ap.srcs += nav_line.c nav_survey_rectangle.c
+
+# Config for SITL simulation
+include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
+sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN -DTRAFFIC_INFO
+sim.srcs += nav_survey_rectangle.c traffic_info.c nav_line.c
+  </makefile>
+</airframe>

conf/messages.xml

@@ -1615,6 +1615,7 @@
     <field name="link_imu_nb_err" type="uint32"/>
     <field name="blmc_nb_err"  type="uint8"/>
     <field name="rc_status"    type="uint8" values="OK|LOST|REALLY_LOST"/>
+    <field name="rc_rate"      type="uint8" unit="Hz"/>
     <field name="gps_status"   type="uint8" values="NO_FIX|NA|NA|3Dfix"/>
     <field name="ap_mode"      type="uint8" values="FAILSAFE|KILL|RATE_DIRECT|ATTITUDE_DIRECT|RATE_RC_CLIMB|ATTITUDE_RC_CLIMB|ATTITUDE_CLIMB|RATE_Z_HOLD|ATTITUDE_Z_HOLD|HOVER_DIRECT|HOVER_CLIMB|HOVER_Z_HOLD|NAV"/>
     <field name="ap_in_flight" type="uint8" values="ON_GROUND|IN_FLIGHT"/>

paparazzi.osx

@@ -1,4 +1,4 @@
-#!/usr/local/bin/ocamlrun /usr/local/bin/ocaml
+#!/opt/local/bin/ocamlrun /opt/local/bin/ocaml
 #load "unix.cma";;
 let (//) = Filename.concat
 let dirname = Filename.dirname Sys.argv.(0)

sw/airborne/arch/lpc21/i2c_hw.c

@@ -33,16 +33,16 @@
 // I2C Automaton //
 ///////////////////
 
-static inline void I2cSendStart(struct i2c_periph* p) {
+__attribute__ ((always_inline)) static inline void I2cSendStart(struct i2c_periph* p) {
   p->status = I2CStartRequested;
   ((i2cRegs_t *)(p->reg_addr))->conset = _BV(STA);
 }
 
-static inline void I2cSendAck(void* reg) {
+__attribute__ ((always_inline)) static inline void I2cSendAck(void* reg) {
   ((i2cRegs_t *)reg)->conset = _BV(AA);
 }
 
-static inline void I2cEndOfTransaction(struct i2c_periph* p) {
+__attribute__ ((always_inline)) static inline void I2cEndOfTransaction(struct i2c_periph* p) {
   // handle fifo here
   p->trans_extract_idx++;
   if (p->trans_extract_idx >= I2C_TRANSACTION_QUEUE_LEN)
@@ -56,18 +56,18 @@ static inline void I2cEndOfTransaction(struct i2c_periph* p) {
   }
 }
 
-static inline void I2cFinished(struct i2c_periph* p, struct i2c_transaction* t) {
+__attribute__ ((always_inline)) static inline void I2cFinished(struct i2c_periph* p, struct i2c_transaction* t) {
   // transaction finished with success
   t->status = I2CTransSuccess;
   I2cEndOfTransaction(p);
 }
 
-static inline void I2cSendStop(struct i2c_periph* p, struct i2c_transaction* t) {
+__attribute__ ((always_inline)) static inline void I2cSendStop(struct i2c_periph* p, struct i2c_transaction* t) {
   ((i2cRegs_t *)(p->reg_addr))->conset = _BV(STO);
   I2cFinished(p,t);
 }
 
-static inline void I2cFail(struct i2c_periph* p, struct i2c_transaction* t) {
+__attribute__ ((always_inline)) static inline void I2cFail(struct i2c_periph* p, struct i2c_transaction* t) {
   ((i2cRegs_t *)(p->reg_addr))->conset = _BV(STO);
   t->status = I2CTransFailed;
   p->status = I2CFailed;
@@ -75,24 +75,24 @@ static inline void I2cFail(struct i2c_periph* p, struct i2c_transaction* t) {
   I2cEndOfTransaction(p);
 }
 
-static inline void I2cSendByte(void* reg, uint8_t b) {
+__attribute__ ((always_inline)) static inline void I2cSendByte(void* reg, uint8_t b) {
   ((i2cRegs_t *)reg)->dat = b;
 }
 
-static inline void I2cReceive(void* reg, bool_t ack) {
+__attribute__ ((always_inline)) static inline void I2cReceive(void* reg, bool_t ack) {
   if (ack) ((i2cRegs_t *)reg)->conset = _BV(AA);
   else ((i2cRegs_t *)reg)->conclr = _BV(AAC);
 }
 
-static inline void I2cClearStart(void* reg) {
+__attribute__ ((always_inline)) static inline void I2cClearStart(void* reg) {
   ((i2cRegs_t *)reg)->conclr = _BV(STAC);
 }
 
-static inline void I2cClearIT(void* reg) {
+__attribute__ ((always_inline)) static inline void I2cClearIT(void* reg) {
   ((i2cRegs_t *)reg)->conclr = _BV(SIC);
 }
 
-static inline void I2cAutomaton(int32_t state, struct i2c_periph* p) {
+__attribute__ ((always_inline)) static inline void I2cAutomaton(int32_t state, struct i2c_periph* p) {
   struct i2c_transaction* trans = p->trans[p->trans_extract_idx];
   switch (state) {
     case I2C_START:

sw/airborne/firmwares/rotorcraft/actuators/supervision.c

@@ -59,13 +59,13 @@ void supervision_init(void) {
   supervision.nb_failure = 0;
 }
 
-static inline void offset_commands(int32_t offset) {
+__attribute__ ((always_inline)) static inline void offset_commands(int32_t offset) {
   uint8_t j;
   for (j=0; j<SUPERVISION_NB_MOTOR; j++)
     supervision.commands[j] += (offset);
 }
 
-static inline void bound_commands(void) {
+__attribute__ ((always_inline)) static inline void bound_commands(void) {
   uint8_t j;
   for (j=0; j<SUPERVISION_NB_MOTOR; j++)
     Bound(supervision.commands[j],

sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c

@@ -221,7 +221,7 @@ void guidance_h_run(bool_t  in_flight) {
 //#define MAX_BANK (65536)
 #define MAX_BANK (98000)
 
-static inline void  guidance_h_hover_run(void) {
+__attribute__ ((always_inline)) static inline void  guidance_h_hover_run(void) {
 
   /* compute position error    */
   VECT2_DIFF(guidance_h_pos_err, ins_ltp_pos, guidance_h_pos_sp);
@@ -279,7 +279,7 @@ static inline void  guidance_h_hover_run(void) {
 #define NAV_MAX_BANK BFP_OF_REAL(0.35,REF_ANGLE_FRAC)
 #define HOLD_DISTANCE POS_BFP_OF_REAL(10.)
 
-static inline void  guidance_h_nav_run(bool_t in_flight) {
+__attribute__ ((always_inline)) static inline void  guidance_h_nav_run(bool_t in_flight) {
 
   /* convert our reference to generic representation */
 #ifdef GUIDANCE_H_USE_REF
@@ -368,7 +368,7 @@ static inline void  guidance_h_nav_run(bool_t in_flight) {
 
 }
 
-static inline void guidance_h_hover_enter(void) {
+__attribute__ ((always_inline)) static inline void guidance_h_hover_enter(void) {
 
   VECT2_COPY(guidance_h_pos_sp, ins_ltp_pos);
 
@@ -378,7 +378,7 @@ static inline void guidance_h_hover_enter(void) {
 
 }
 
-static inline void guidance_h_nav_enter(void) {
+__attribute__ ((always_inline)) static inline void guidance_h_nav_enter(void) {
 
   INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, navigation_carrot);
   struct Int32Vect2 pos,speed,zero;

sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c

@@ -82,7 +82,7 @@ int32_t guidance_v_z_sum_err;
   }
 
 
-static inline void run_hover_loop(bool_t in_flight);
+__attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flight);
 
 
 void guidance_v_init(void) {
@@ -232,7 +232,7 @@ void guidance_v_run(bool_t in_flight) {
 
 #define MAX_BANK_COEF (BFP_OF_REAL(RadOfDeg(30.),INT32_TRIG_FRAC))
 
-static inline void run_hover_loop(bool_t in_flight) {
+__attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flight) {
 
   /* convert our reference to generic representation */
   int64_t tmp  = gv_z_ref>>(GV_Z_REF_FRAC - INT32_POS_FRAC);

sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h

@@ -96,14 +96,14 @@ int64_t gv_z_ref;
 int32_t gv_zd_ref;
 int32_t gv_zdd_ref;
 
-static inline void gv_set_ref(int32_t alt, int32_t speed, int32_t accel) {
+__attribute__ ((always_inline)) static inline void gv_set_ref(int32_t alt, int32_t speed, int32_t accel) {
   int64_t new_z = ((int64_t)alt)<<(GV_Z_REF_FRAC - INT32_POS_FRAC);
   gv_z_ref   = new_z;
   gv_zd_ref  = speed>>(INT32_SPEED_FRAC - GV_ZD_REF_FRAC);
   gv_zdd_ref = accel>>(INT32_ACCEL_FRAC - GV_ZDD_REF_FRAC);
 }
 
-static inline void gv_update_ref_from_z_sp(int32_t z_sp) {
+__attribute__ ((always_inline)) static inline void gv_update_ref_from_z_sp(int32_t z_sp) {
 
   gv_z_ref  += gv_zd_ref;
   gv_zd_ref += gv_zdd_ref;
@@ -135,7 +135,7 @@ static inline void gv_update_ref_from_z_sp(int32_t z_sp) {
 }
 
 
-static inline void gv_update_ref_from_zd_sp(int32_t zd_sp) {
+__attribute__ ((always_inline)) static inline void gv_update_ref_from_zd_sp(int32_t zd_sp) {
 
   gv_z_ref  += gv_zd_ref;
   gv_zd_ref += gv_zdd_ref;

sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c

@@ -154,7 +154,7 @@ void ahrs_update_mag(void) {
 }
 
 /* measures phi and theta assuming no dynamic acceleration ?!! */
-static inline void get_phi_theta_measurement_fom_accel(int32_t* phi_meas, int32_t* theta_meas, struct Int32Vect3 accel) {
+__attribute__ ((always_inline)) static inline void get_phi_theta_measurement_fom_accel(int32_t* phi_meas, int32_t* theta_meas, struct Int32Vect3 accel) {
 
   INT32_ATAN2(*phi_meas, -accel.y, -accel.z);
   int32_t cphi;
@@ -167,7 +167,7 @@ static inline void get_phi_theta_measurement_fom_accel(int32_t* phi_meas, int32_
 }
 
 /* measure psi by projecting magnetic vector in local tangeant plan */
-static inline void get_psi_measurement_from_mag(int32_t* psi_meas, int32_t phi_est, int32_t theta_est, struct Int32Vect3 mag) {
+__attribute__ ((always_inline)) static inline void get_psi_measurement_from_mag(int32_t* psi_meas, int32_t phi_est, int32_t theta_est, struct Int32Vect3 mag) {
 
   int32_t sphi;
   PPRZ_ITRIG_SIN(sphi, phi_est);
@@ -196,7 +196,7 @@ static inline void get_psi_measurement_from_mag(int32_t* psi_meas, int32_t phi_e
 
 /* Compute ltp to imu rotation in quaternion and rotation matrice representation
    from the euler angle representation */
-static inline void compute_imu_quat_and_rmat_from_euler(void) {
+__attribute__ ((always_inline)) static inline void compute_imu_quat_and_rmat_from_euler(void) {
 
   /* Compute LTP to IMU quaternion */
   INT32_QUAT_OF_EULERS(ahrs.ltp_to_imu_quat, ahrs.ltp_to_imu_euler);
@@ -205,7 +205,7 @@ static inline void compute_imu_quat_and_rmat_from_euler(void) {
 
 }
 
-static inline void compute_body_orientation(void) {
+__attribute__ ((always_inline)) static inline void compute_body_orientation(void) {
 
   /* Compute LTP to BODY quaternion */
   INT32_QUAT_COMP_INV(ahrs.ltp_to_body_quat, ahrs.ltp_to_imu_quat, imu.body_to_imu_quat);

sw/airborne/subsystems/ins/vf_float.c

@@ -122,7 +122,7 @@ void vff_propagate(float accel) {
   // update covariance
   Pp = Pm - K*H*Pm;
 */
-static inline void update_z_conf(float z_meas, float conf) {
+__attribute__ ((always_inline)) static inline void update_z_conf(float z_meas, float conf) {
   vff_z_meas = z_meas;
 
   const float y = z_meas - vff_z;
@@ -179,7 +179,7 @@ void vff_update_z_conf(float z_meas, float conf) {
   // update covariance
   Pp = Pm - K*H*Pm;
 */
-static inline void update_vz_conf(float vz, float conf) {
+__attribute__ ((always_inline)) static inline void update_vz_conf(float vz, float conf) {
   const float yd = vz - vff_zdot;
   const float S = vff_P[1][1] + conf;
   const float K1 = vff_P[0][1] * 1/S;

sw/ground_segment/tmtc/booz_server.ml

@@ -182,6 +182,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
           Wind.update ac_name a.gspeed a.course
   | "ROTORCRAFT_STATUS" ->
       a.fbw.rc_status <- get_rc_status (ivalue "rc_status");
+      a.fbw.rc_rate   <- ivalue "frame_rate";
       a.gps_mode      <- check_index (ivalue "gps_status") gps_modes "GPS_MODE";
       a.ap_mode       <- check_index (get_pprz_mode (ivalue "ap_mode")) ap_modes "BOOZ_AP_MODE";
       a.kill_mode     <- ivalue "ap_motors_on" == 0;

sw/ground_segment/tmtc/fw_server.ml

@@ -103,6 +103,8 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
     a.last_msg_date <- U.gettimeofday ();
   match msg.Pprz.name with
     "GPS" ->
+      a.gps_mode <- check_index (ivalue "mode") gps_modes "GPS_MODE";
+      if a.gps_mode = _3D then begin
       let p = { LL.utm_x = fvalue "utm_east" /. 100.;
         utm_y = fvalue "utm_north" /. 100.;
         utm_zone = ivalue "utm_zone" } in
@@ -114,9 +116,9 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
       if !heading_from_course then
         a.heading <- a.course;
       a.agl     <- a.alt -. float (try Srtm.of_wgs84 a.pos with _ -> 0);
-      a.gps_mode <- check_index (ivalue "mode") gps_modes "GPS_MODE";
       if a.gspeed > 3. && a.ap_mode = _AUTO2 then
 	      Wind.update ac_name a.gspeed a.course
+      end
   | "GPS_LLA" ->
       let lat = ivalue "lat"
       and lon = ivalue "lon" in
@@ -143,9 +145,6 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
         Some nav_ref ->
           let x = (try fvalue "x" with _ -> fvalue "desired_x")
           and y = (try fvalue "y" with _ -> fvalue "desired_y") in
-	        (*let target_utm = LL.utm_add (LL.utm_of nav_ref) (x, y) in
-	        let target_geo = LL.of_utm WGS84 target_utm in 
-          a.desired_pos <- target_geo;*)
           a.desired_pos <- Aircraft.add_pos_to_nav_ref nav_ref (x, y);
       | None -> ()
       end;

sw/ground_segment/tmtc/server.ml

@@ -151,8 +151,7 @@ let send_cam_status = fun a ->
 	  let alpha = -. a.course in
 	  let east = dx *. cos alpha -. dy *. sin alpha
 	  and north = dx *. sin alpha +. dy *. cos alpha in
-	  let utm = LL.utm_add (LL.utm_of WGS84 a.pos) (east, north) in
-	  let wgs84 = LL.of_utm WGS84 utm in
+    let wgs84 = Aircraft.add_pos_to_nav_ref (Geo a.pos) (east, north) in
 	  let twgs84 = Aircraft.add_pos_to_nav_ref nav_ref a.cam.target in
 	  let values = ["ac_id", Pprz.String a.id; 
 			"cam_lat", Pprz.Float ((Rad>>Deg)wgs84.posn_lat);

sw/ground_segment/tmtc/server_globals.ml

@@ -8,6 +8,7 @@ let _AUTO2 = 2
 let gaz_modes = [|"MANUAL";"GAZ";"CLIMB";"ALT"|]
 let lat_modes = [|"MANUAL";"ROLL_RATE";"ROLL";"COURSE"|]
 let gps_modes = [|"NOFIX";"DRO";"2D";"3D";"GPSDRO"|]
+let _3D = 3
 let gps_hybrid_modes = [|"OFF";"ON"|]
 let horiz_modes = [|"WAYPOINT";"ROUTE";"CIRCLE";"ATTITUDE"|]