HITL on a tiny !

conf/Makefile.arm7

@@ -120,8 +120,8 @@ ADEFS =  -D$(RUN_MODE)
 #CFLAGS =  -g0
 CFLAGS = $(CDEFS) $(CINCS)
 CFLAGS += -O$(OPT)
-CFLAGS += -Wall -Wcast-qual -Wimplicit 
-# -Wcast-align # Incompatible with GPS message parsing
+# CFLAGS += -malignment-traps
+CFLAGS += -Wall -Wcast-qual -Wimplicit -Wcast-align
 CFLAGS += -Wpointer-arith -Wswitch
 CFLAGS += -Wredundant-decls -Wreturn-type -Wshadow -Wunused
 CFLAGS += -Wa,-adhlns=$(OBJDIR)/$(notdir $(subst $(suffix $<),.lst,$<))

conf/airframes/microjet4.xml

@@ -33,25 +33,112 @@
     <set servo="AILEVON_RIGHT" value="$elevator - $aileron"/>
   </command_laws>
 
+  <section name="AUTO1" prefix="AUTO1_">
+    <define name="MAX_ROLL" value="0.6"/>
+    <define name="MAX_PITCH" value="0.6"/>
+  </section>
+
+  <section name="adc" prefix="ADC_CHANNEL_">
+    <define name="IR1" value="1"/>
+    <define name="IR2" value="2"/>
+    <define name="IR_TOP" value="3"/>
+    <define name="IR_NB_SAMPLES" value="16"/>
+    <define name="VSUPPLY" value="6"/>
+  </section>
+
+  <section name="INFRARED" prefix="IR_">
+    <define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+    <define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+    <define name="DEFAULT_CONTRAST" value="200"/>
+    <define name="RAD_OF_IR_CONTRAST" value="0.75"/>
+    <linear name="RollOfIrs" arity="2" coeff1="0" coeff2="-1"/>
+    <linear name="PitchOfIrs" arity="2" coeff1="-1" coeff2="0"/>
+    <define name="RAD_OF_IR_MAX_VALUE" value="0.0045"/>
+    <define name="RAD_OF_IR_MIN_VALUE" value="0.00075"/>
+    <define name="ADC_ROLL_NEUTRAL" value="-512"/>
+    <define name="ADC_PITCH_NEUTRAL" value="-512"/>
+  </section>
+ 
+  <section name="BAT">
+    <define name="MILLIAMP_PER_PERCENT" value="0.86"/>
+    <define name="VOLTAGE_ADC_A" value="0.0156"/>
+    <define name="VOLTAGE_ADC_B" value="-0.029"/>
+    <define name="VoltageOfAdc(adc)" value="(VOLTAGE_ADC_A * adc + VOLTAGE_ADC_B)"/>
+    <define name="LOW_BATTERY" value="9.3" unit="V"/>
+  </section>
+ 
+  <section name="MISC">
+    <define name="NOMINAL_AIRSPEED" value="10." unit="m/s"/>
+    <define name="CARROT" value="5." unit="s"/>
+  </section>
+ 
+  <section name="PID">
+    <define name="ROLL_PGAIN" value="5000."/>
+    <define name="PITCH_OF_ROLL" value="0.0"/>
+    <define name="PITCH_PGAIN" value="5000."/>
+    <define name="MAX_ROLL" value="0.35"/>
+    <define name="MAX_PITCH" value="0.35"/>
+    <define name="MIN_PITCH" value="-0.35"/>
+    <define name="AILERON_OF_GAZ" value="0.0"/>
+  </section>
+ 
+  <section name="ALT" prefix="CLIMB_">
+    <define name="PITCH_PGAIN" value="-0.1"/>
+    <define name="PITCH_IGAIN" value="0.025"/>
+    <define name="PGAIN" value="-0.03"/>
+    <define name="IGAIN" value="0.1"/>
+    <define name="MAX" value="1."/>
+    <define name="LEVEL_GAZ" value="0.50"/>
+    <define name="PITCH_OF_VZ_PGAIN" value="0.05"/>
+    <define name="GAZ_OF_CLIMB" value="0.2" unit="%/(m/s)"/>
+  </section>
+ 
+  <section name="NAV">
+    <define name="COURSE_PGAIN" value="-0.2"/>
+    <define name="ALTITUDE_PGAIN" value="-0.025"/>
+    <define name="NAV_PITCH" value="0."/>
+  </section>
+
+  <section name="SIMU">
+    <define name="ROLL_RESPONSE_FACTOR" value="2."/>
+    <define name="YAW_RESPONSE_FACTOR" value="1.35"/>
+    <define name="WEIGHT" value="1.3"/>
+  </section>
+ 
  <makefile>
 include $(PAPARAZZI_SRC)/conf/autopilot/tiny.makefile
 
+ap.CFLAGS +=  -DFBW -DAP -DCONFIG=\"tiny.h\" -DLED -DTIME_LED=1
+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_4015_hw.h\" -DSERVOS_4015
 ap.srcs += $(SRC_ARCH)/servos_4015_hw.c
 
-ap.CFLAGS += -DRADIO_CONTROL
+ap.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE=RC_FUTABA
 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
-ap.srcs += downlink.c pprz_transport.c $(SRC_ARCH)/uart_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=B9600
+ap.srcs += downlink.c pprz_transport.c $(SRC_ARCH)/uart_hw.c datalink.c
 
 ap.CFLAGS += -DINTER_MCU
 ap.srcs += inter_mcu.c 
 
-ap.CFLAGS += -DGPS -DUBX -DUSE_UART1 -DGPS_LINK=Uart1
+ap.CFLAGS += -DADC -DUSE_AD0_1 -DUSE_AD0_2 -DUSE_AD0_3
+ap.srcs += $(SRC_ARCH)/adc_hw.c
+
+ap.CFLAGS += -DGPS -DUBX -DUSE_UART1 -DGPS_LINK=Uart1 -DUART1_BAUD=B19200 -DGPS_CONFIGURE
 ap.srcs += gps_ubx.c gps.c
 
+ap.CFLAGS += -DINFRARED 
+ap.srcs += infrared.c estimator.c
+
+ap.srcs += nav.c pid.c
+
+# Harware In The Loop
+ap.CFLAGS += -DHITL
+
+
   </makefile>
 </airframe>

conf/airframes/twinjet1.xml

@@ -138,13 +138,13 @@ fbw.srcs += $(SRC_ARCH)/adc_hw.c
 ap.CFLAGS += -DAP -DCONFIG=\"classix.h\" -DLED -DTIME_LED=2
 ap.srcs = sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c main_ap.c main.c
 
-ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_INPUT -DPPRZ_UART=Uart0 -DDATALINK
+ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
 ap.srcs += downlink.c pprz_transport.c $(SRC_ARCH)/uart_hw.c datalink.c traffic_info.c
 
 ap.CFLAGS += -DINTER_MCU -DMCU_SPI_LINK
 ap.srcs += inter_mcu.c link_mcu.c spi.c $(SRC_ARCH)/spi_hw.c
 
-ap.CFLAGS += -DGPS -DUBX -DUSE_UART1 -DGPS_LINK=Uart1
+ap.CFLAGS += -DGPS -DUBX -DUSE_UART1 -DGPS_LINK=Uart1 -DUART1_BAUD=B38400
 ap.srcs += gps_ubx.c gps.c
 
 ap.CFLAGS += -DADC -DUSE_AD0_1 -DUSE_AD0_2 -DUSE_AD0_3
@@ -155,8 +155,6 @@ ap.srcs += infrared.c estimator.c
 
 ap.srcs += nav.c pid.c
 
-
-
 test.CFLAGS += -DFBW -DCONFIG=\"classix.h\" -DLED -DTIME_LED=1 -DACTUATORS=\"servos_4017_hw.h\" -DSERVOS_4017 -DSERVOS_4017_CLOCK_FALLING  -DUSE_UART0 -DDATALINK -DPPRZ_INPUT -DPPRZ_UART=Uart0
 test.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c pprz_transport.c setup_actuators.c $(SRC_ARCH)/uart_hw.c $(SRC_ARCH)/servos_4017_hw.c main.c
 

conf/airframes/twinstar4.xml

@@ -131,7 +131,7 @@ ap.srcs += $(SRC_ARCH)/modem_hw.c
 ap.CFLAGS += -DRADIO_CONTROL_CALIB
 ap.srcs += if_calib.c
 
-ap.CFLAGS += -DDATALINK
+ap.CFLAGS += -DDATALINK=WAVECARD
 ap.srcs += traffic_info.c datalink.c
 ap.CFLAGS += -DWAVECARD -DWAVECARD_UART=Uart0 -DUSE_UART0
 ap.srcs += wavecard.c

conf/autopilot/tiny.h

@@ -6,14 +6,19 @@
 
 /* Master oscillator freq.       */
 #define FOSC (14745600) 
+
 /* PLL multiplier                */
 #define PLL_MUL (4)         
+
 /* CPU clock freq.               */
 #define CCLK (FOSC * PLL_MUL) 
-/* Peripheral bus speed divider */
-#define PBSD 2    
+
+/* Peripheral bus speed mask 0x00->4, 0x01-> 1, 0x02 -> 2   */
+#define PBSD_BITS 0x00    
+#define PBSD_VAL 4
+
 /* Peripheral bus clock freq. */
-#define PCLK (CCLK / PBSD) 
+#define PCLK (CCLK / PBSD_VAL) 
 
 #define LED_1_BANK 1
 #define LED_1_PIN 28

conf/autopilot/tiny.makefile

@@ -6,8 +6,6 @@ ap.ARCHDIR = $(ARCHI)
 ap.ARCH = arm7tdmi
 ap.TARGET = autopilot
 ap.TARGETDIR = autopilot
-ap.CFLAGS +=  -DFBW -DAP -DCONFIG=\"tiny.h\"
-ap.srcs = sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c main_fbw.c main_ap_2.c main.c
 
 LPC21ISP_BAUD = 115200
-LPC21ISP_XTAL = 14746
+LPC21ISP_XTAL = 14746

conf/flight_plans/muret4.xml

@@ -15,6 +15,17 @@
       <setting VAR="ir_roll_neutral" RANGE="-60." RC="gain_2_up" TYPE="int16"/>
     </mode>
   </rc_control>
+
+  <dl_settings>
+      <dl_setting var="ir_roll_neutral" min="-0.3" max="0.3" step="0.01"/>
+      <dl_setting var="ir_pitch_neutral" min="-0.3" max="0.3" step="0.01"/>
+      <dl_setting var="roll_pgain" min="1000" max="15000" step="250"/>
+      <dl_setting var="pitch_pgain" min="1000" max="15000" step="250"/>
+      <dl_setting var="climb_level_gaz" min="0.3" max="0.7" step="0.05"/>
+      <dl_setting var="climb_pgain" min="-0.5" max="-0.01" step="0.01"/>
+      <dl_setting var="course_pgain" min="-1" max="-0.1" step="0.05"/>
+  </dl_settings>
+
   <waypoints utm_x0="360284.8" utm_y0="4813595.5">
     <waypoint name="AF" x="267.5" y="35.0" lat="43.4625948985" lon="1.27618631405" alt="215."/>
     <waypoint name="3" x="25.0" y="285.0" lat="43.464799741" lon="1.27312587027" alt="250."/>

conf/messages.xml

@@ -262,18 +262,19 @@
 
 <class name="datalink">
  <message name="ACINFO" ID="1">
-  <field name="ac_id" type="uint8"/>
+  <field name="course" type="int16" unit="decideg"></field>
   <field name="utm_east"  type="int32" unit="cm"></field>
   <field name="utm_north"  type="int32" unit="cm"></field>
-  <field name="course" type="int16" unit="decideg"></field>
   <field name="alt"    type="int32" unit="cm"></field>
+  <field name="itow"  type="uint32" unit="ms"></field>
   <field name="speed"  type="uint16" unit="cm/s"></field>
   <field name="climb"  type="int16" unit="cm/s"></field>
-  <field name="itow"  type="uint32" unit="ms"></field>
+  <field name="ac_id" type="uint8"/>
  </message>
 
  <message name="MOVE_WP" ID="2">
   <field name="wp_id" type="uint8"/>
+  <field name="pad0" type="uint8"/>
   <field name="utm_east"  type="int32" unit="cm"></field>
   <field name="utm_north"  type="int32" unit="cm"></field>
   <field name="alt"    type="int32" unit="cm"></field>
@@ -285,6 +286,7 @@
 
  <message name="SETTING" ID="4">
   <field name="index" type="uint8"/>
+  <field name="pad0" type="uint8"/>
   <field name="value" type="float"/>
  </message>
 
@@ -304,8 +306,8 @@
  </message>
 
  <message name="SET_ACTUATOR" id="100">
-   <field name="no" type="uint8"/>
    <field name="value" type="uint16"/>
+   <field name="no" type="uint8"/>
  </message>
 </class>
 

sw/airborne/arm7/uart_hw.c

@@ -29,7 +29,7 @@ static void uart0_init_param( uint16_t baud, uint8_t mode, uint8_t fmode);
 
 void uart0_init_tx( void ) {
   /**  uart0_init_param(UART_BAUD(38400), UART_8N1, UART_FIFO_8); **/
-  uart0_init_param(UART_BAUD(9600), UART_8N1, UART_FIFO_8);
+  uart0_init_param(UART0_BAUD, UART_8N1, UART_FIFO_8);
 }
 
 void uart0_init_rx( void ) {}
@@ -224,7 +224,7 @@ uint8_t  uart1_tx_running;
 static void uart1_init_param( uint16_t baud, uint8_t mode, uint8_t fmode);
 
 void uart1_init_tx( void ) {
-  uart1_init_param(UART_BAUD(38400), UART_8N1, UART_FIFO_8);
+  uart1_init_param(UART1_BAUD, UART_8N1, UART_FIFO_8);
 }
 
 void uart1_init_rx( void ) {}

sw/airborne/datalink.c

@@ -28,6 +28,7 @@
 #define DATALINK_C
 
 #include <inttypes.h>
+#include <string.h>
 #include "traffic_info.h"
 #include "nav.h"
 #include "datalink.h"
@@ -78,6 +79,7 @@ void dl_parse_msg(void) {
   /** Infrared and GPS sensors are replaced by messages on the datalink */
   else if (msg_id == DL_HITL_INFRARED) {
     /** This code simulates infrared.c:ir_update() */
+    DOWNLINK_SEND_DEBUG1(10, dl_buffer);
     ir_roll = DL_HITL_INFRARED_roll(dl_buffer);
     ir_pitch = DL_HITL_INFRARED_pitch(dl_buffer);
   } else if (msg_id == DL_HITL_UBX) {
@@ -99,7 +101,10 @@ void dl_parse_msg(void) {
 #endif
 #ifdef DlSetting
   else if (msg_id == DL_SETTING) {
-    DlSetting(DL_SETTING_index(dl_buffer), DL_SETTING_value(dl_buffer));
+    uint8_t i = DL_SETTING_index(dl_buffer);
+    float var = DL_SETTING_value(dl_buffer);
+    DlSetting(i, var);
+    DOWNLINK_SEND_DL_VALUE(&i, &var);
   }
 #endif /** Else there is no dl_settings section in the flight plan */
 }

sw/airborne/datalink.h

@@ -41,7 +41,7 @@ EXTERN bool_t dl_msg_available;
 /** Flag provided to control calls to ::dl_parse_msg. NOT used in this module*/
 
 #define MSG_SIZE 128
-EXTERN char dl_buffer[MSG_SIZE];
+EXTERN uint8_t dl_buffer[MSG_SIZE]  __attribute__ ((aligned));
 
 void dl_parse_msg(void);
 /** Should be called when chars are available in dl_buffer */

sw/airborne/gps_ubx.c

@@ -49,7 +49,8 @@ bool_t gps_pos_available;
 uint8_t ubx_id, ubx_class;
 
 #define UBX_MAX_PAYLOAD 255
-uint8_t ubx_msg_buf[UBX_MAX_PAYLOAD];
+
+uint8_t ubx_msg_buf[UBX_MAX_PAYLOAD] __attribute__ ((aligned));
 
 #define RadianOfDeg(d) ((d)/180.*3.1415927)
 

sw/airborne/main_ap.c

@@ -508,6 +508,9 @@ void init_ap( void ) {
   /** Reset the wavecard during the init pause */
   wc_reset();
 #endif
+#if defined GPS && defined GPS_CONFIGURE
+  gps_configure();
+#endif
 
   /************ Internal status ***************/
   estimator_init();
@@ -539,9 +542,11 @@ void init_ap( void ) {
 /*********** EVENT ***********************************************************/
 void event_task_ap( void ) {
 #ifdef GPS
+#ifndef HITL /** else comes through the datalink */
   if (GpsBuffer()) {
     ReadGpsBuffer();
   }
+#endif
   if (gps_msg_received) {
     /* parse and use GPS messages */
     parse_gps_msg();
@@ -553,7 +558,18 @@ void event_task_ap( void ) {
   }
 #endif /** GPS */
 
-#ifdef WAVECARD
+#if defined DATALINK 
+
+
+#if DATALINK == PPRZ
+  if (PprzBuffer()) {
+    ReadPprzBuffer();
+    if (pprz_msg_received) {
+      pprz_parse_payload();
+      pprz_msg_received = FALSE;
+    }
+  }
+#elif DATALINK == WAVECARD
   if (WavecardBuffer()) {
     ReadWavecardBuffer();
     if (wc_msg_received) {
@@ -562,24 +578,13 @@ void event_task_ap( void ) {
       wc_msg_received = FALSE;
     }
   }
-#endif /** WAVECARD */
+#endif
 
-#ifdef PPRZ_INPUT
-  if (PprzBuffer()) {
-    ReadPprzBuffer();
-    if (pprz_msg_received) {
-      pprz_msg_received = FALSE;
-      pprz_parse_payload();
-    }
-  }
-#endif /** PPRZ_INPUT */
-
-#ifdef DATALINK
   if (dl_msg_available) {
     dl_parse_msg();
     dl_msg_available = FALSE;
   }
-#endif
+#endif /** DATALINK */
 
 #ifdef TELEMETER
   /** Handling of data sent by the device (initiated by srf08_receive() */

sw/ground_segment/tmtc/link.ml

@@ -87,7 +87,7 @@ exception NotSendingToThis
 let airborne_device = fun ac_id airframes device ->
   let ac_device = try Some (List.assoc ac_id airframes) with Not_found -> None in
   match ac_device, device with
-    _, Pprz -> Uart
+    None, Pprz -> Uart
   | (Some (WavecardDevice _ as ac_device), Wavecard) |
     (Some (MaxstreamDevice _ as ac_device), Maxstream) ->
       ac_device

sw/lib/ocaml/convert.c

@@ -46,6 +46,13 @@ c_sprint_float(value s, value index, value f) {
   return Val_unit;
 }
 
+value
+c_sprint_int16(value s, value index, value f) {
+  int16_t *p = (int16_t*) (String_val(s) + Int_val(index));
+  *p = (int16_t)Int_val(f);
+  return Val_unit;
+}
+
 value
 c_sprint_int32(value s, value index, value x) {
   char *p = String_val(s) + Int_val(index);

sw/lib/ocaml/pprz.ml

@@ -74,6 +74,7 @@ external int8_of_bytes : string -> int -> int = "c_int8_of_indexed_bytes"
 external int16_of_bytes : string -> int -> int = "c_int16_of_indexed_bytes"
 external sprint_float : string -> int -> float -> unit = "c_sprint_float"
 external sprint_int32 : string -> int -> int32 -> unit = "c_sprint_int32"
+external sprint_int16 : string -> int -> int -> unit = "c_sprint_int16"
 
 let types = [
   ("uint8",  { format = "%u"; glib_type = "guint8"; inttype = "uint8_t";  size = 1; value=Int 42 });
@@ -237,6 +238,7 @@ let rec sprint_value = fun buf i _type v ->
     Scalar ("int8"|"uint8"), Int x -> buf.[i] <- Char.chr x; sizeof _type
   | Scalar "float", Float f -> sprint_float buf i f; sizeof _type
   | Scalar "int32", Int32 x -> sprint_int32 buf i x; sizeof _type
+  | Scalar "int16", Int x -> sprint_int16 buf i x; sizeof _type
   | Scalar ("int32" | "uint32"), Int value ->
       assert (_type <> Scalar "uint32" || value >= 0);
       buf.[i+3] <- byte (value asr 24);