lots of new bugs

conf/messages.xml

@@ -66,7 +66,8 @@
 
   <message name="PPRZ_MODE" ID="11" period="5" >
     <field name="ap_mode" type="uint8" values="MANUAL|AUTO1|AUTO2|HOME"/>
-    <field name="ap_altitude" type="uint8" values="MANUAL|AUTO_GAZ|AUTO_CLIMB|AUTO_ALT"/>
+    <field name="ap_gaz" type="uint8" values="MANUAL|AUTO_GAZ|AUTO_CLIMB|AUTO_ALT"/>
+    <field name="ap_lateral" type="uint8" values="MANUAL|ROLL_RATE|ROLL|COURSE"/>
     <field name="if_calib_mode" type="uint8" values="NONE|DOWN|UP"/>
     <field name="mcu1_status" type="uint8" values="LOST|OK|REALLY_LOST"/>	
     <field name="lls_calib" type="uint8" values="LLS_CALIB_MODE_OFF|LLS_CALIB_MODE_ON"/>
@@ -298,9 +299,9 @@
 
   <message name="AP_STATUS" ID="12">
     <field name="ac_id" type="string"/>
-    <field name="ap_mode" type="uint8" values="MANUAL|AUTO1|AUTO2|HOME"/>
+    <field name="ap_mode" type="string" values="MANUAL|AUTO1|AUTO2|HOME"/>
-    <field name="h_mode" type="uint8" values="NONE|ROLLDOT|ROLL|HEADING_DOT|HEADING|WP|ROUTE|CIRCLE"/>
+    <field name="lat_mode" type="string" values="MANUAL|ROLL_RATE|ROLL|COURSE"/>
-    <field name="v_mode" type="uint8" values="NONE|CLIMB|ALT|GLIDE|PARABOLIC"/>
+    <field name="gaz_mode" type="string" values="MANUAL|GAZ|CLIMB|ALT"/>
     <field name="gps_mode" type="uint8"/>
     <field name="flight_time" type="uint32"/>
   </message>
@@ -315,7 +316,7 @@
     <field name="target_north" type="float" unit="m"/>
     <field name="target_climb"   type="float" unit="m/s"/>
     <field name="target_alt"     type="float" unit="m"/>
-    <field name="target_heading" type="float" unit="m"/>
+    <field name="target_course" type="float" unit="deg"/>
   </message>
 
   <message name="CAM_STATUS" ID="14">

sw/airborne/autopilot/main.c

@@ -224,7 +224,7 @@ uint8_t ac_ident = AC_ID;
 /** \def EventPos(_cpt, _channel, _event)
  *  @@@@@ A FIXER @@@@@
  */
-#define PERIODIC_SEND_PPRZ_MODE() DOWNLINK_SEND_PPRZ_MODE(&pprz_mode, &vertical_mode, &inflight_calib_mode, &mcu1_status, &ir_estim_mode);
+#define PERIODIC_SEND_PPRZ_MODE() DOWNLINK_SEND_PPRZ_MODE(&pprz_mode, &vertical_mode, &lateral_mode, &inflight_calib_mode, &mcu1_status, &ir_estim_mode);
 #define PERIODIC_SEND_DESIRED() DOWNLINK_SEND_DESIRED(&desired_roll, &desired_pitch, &desired_x, &desired_y, &desired_altitude, &desired_climb);
 
 #define PERIODIC_SEND_NAVIGATION_REF()  DOWNLINK_SEND_NAVIGATION_REF(&nav_utm_east0, &nav_utm_north0, &nav_utm_zone0);
@@ -324,7 +324,7 @@ inline void radio_control_task( void ) {
     }
     mode_changed |= mcu1_status_update();
     if ( mode_changed )
-      DOWNLINK_SEND_PPRZ_MODE(&pprz_mode, &vertical_mode, &inflight_calib_mode, &mcu1_status, &ir_estim_mode);
+      PERIODIC_SEND_PPRZ_MODE();
     
     /** If Auto1 mode, compute \a desired_roll and \a desired_pitch from 
       * \a RADIO_ROLL and \a RADIO_PITCH \n

sw/ground_segment/cockpit/Paparazzi/PFD.pm

@@ -33,7 +33,7 @@ sub populate {
      -alt            => [S_NOINIT,   S_PASSIVE, S_RDWR,   S_OVRWRT, S_NOPRPG, 0.],
      -target_alt     => [S_NOINIT,   S_PASSIVE, S_RDWR,   S_OVRWRT, S_NOPRPG, 0.],
      -heading        => [S_NOINIT,   S_PASSIVE, S_RDWR,   S_OVRWRT, S_NOPRPG, 0.],
-     -target_heading => [S_NOINIT,   S_PASSIVE, S_RDWR,   S_OVRWRT, S_NOPRPG, 0.],
+     -target_course => [S_NOINIT,   S_PASSIVE, S_RDWR,   S_OVRWRT, S_NOPRPG, 0.],
      -vz             => [S_NOINIT,   S_PASSIVE, S_RDWR,   S_OVRWRT, S_NOPRPG, 0.],
      
      -ap_mode     => [S_NOINIT, S_PRPGONLY, S_RDWR,   S_OVRWRT, S_CHILDREN,    0],
@@ -63,7 +63,7 @@ sub selected_ac {
 #  print "###########\n";
 #  print "in PFD selected_ac $previous_ac $new_ac\n";
 
-  my @fields = ('roll', 'pitch', 'speed', 'alt', 'target_alt', 'target_heading', 'gps_mode', 'ap_mode');
+  my @fields = ('roll', 'pitch', 'speed', 'alt', 'target_alt', 'target_course', 'gps_mode', 'ap_mode');
 
   foreach my $field ( @fields ) {
     $previous_ac->detach($self, $field, [\&foo_cbk, $field]) if ($previous_ac);
@@ -177,7 +177,7 @@ sub build_gui() {
 						  -repeat_legend => 3,
 						);
   $self->connectoptions(-heading, S_TO, [$self->{heading_scale}, -value]);
-  $self->connectoptions(-target_heading, S_TO, [$self->{heading_scale}, -target_value]);
+  $self->connectoptions(-target_course, S_TO, [$self->{heading_scale}, -target_value]);
   $self->{alt_scale} = Paparazzi::LensScale->new( -zinc => $zinc, 
 						  -parent_grp => $self->{main_group},
 						  -origin => [0.8*$width, 0.25*$height],

sw/ground_segment/tmtc/receive.ml

@@ -41,6 +41,21 @@ let logs_path = Env.paparazzi_home // "var" // "logs"
 let conf_xml = Xml.parse_file (Env.paparazzi_home // "conf" // "conf.xml")
 let srtm_path = Env.paparazzi_home // "data" // "srtm"
 
+(** Should be read from messages.xml *)
+let ap_modes = [|"MANUAL";"AUTO1";"AUTO2";"HOME"|]
+let gaz_modes = [|"MANUAL";"GAZ";"CLIMB";"ALT"|]
+let lat_modes = [|"MANUAL";"ROLL_RATE";"ROLL";"COURSE"|]
+
+let check_index = fun i t where ->
+  if i < 0 || i >= Array.length t then begin
+    Debug.call 'E' (fun f -> fprintf f "Wrong index in %s: %d" where i);
+    -1
+  end else
+    i
+
+let get_indexed_value = fun t i ->
+  if i >= 0 then t.(i) else "UNK"
+  
 type ac_cam = {
     mutable phi : float; (* Rad, right = >0 *)
     mutable theta : float; (* Rad, front = >0 *)
@@ -97,6 +112,8 @@ type aircraft = {
     mutable desired_east    : float;
     mutable desired_north    : float;
     mutable desired_altitude    : float;
+    mutable desired_course : float;
+    mutable desired_climb : float;
     mutable gspeed  : float;
     mutable course  : float;
     mutable alt     : float;
@@ -110,14 +127,17 @@ type aircraft = {
     mutable amp : float;
     mutable energy  : float;
     mutable ap_mode : int;
-    mutable ap_altitude : int;
+    mutable gaz_mode : int;
+    mutable lateral_mode : int;
     cam : ac_cam;
     mutable gps_mode : int;
     inflight_calib : inflight_calib;
     infrared : infrared;
     fbw : fbw;
     svinfo : svinfo array;
-    mutable flight_time : int
+    mutable flight_time : int;
+    mutable stage_time : int;
+    mutable block_time : int
   }
 
 (** The aircrafts store *)
@@ -176,7 +196,8 @@ let log_and_parse = fun log ac_name a msg values ->
     | "DESIRED" ->
 	a.desired_east <- fvalue "desired_x";
 	a.desired_north <- fvalue "desired_y";
-	a.desired_altitude <- fvalue "desired_altitude"
+	a.desired_altitude <- fvalue "desired_altitude";
+	a.desired_climb <- fvalue "desired_climb"
     | "NAVIGATION_REF" ->
 	a.nav_ref_east <- fvalue "utm_east";
 	a.nav_ref_north <- fvalue "utm_north"
@@ -185,15 +206,19 @@ let log_and_parse = fun log ac_name a msg values ->
 	a.pitch <- (Deg>>Rad) (fvalue "theta")
     | "NAVIGATION" -> 
 	a.cur_block <- ivalue "cur_block";
-	a.cur_stage <- ivalue "cur_stage"
+	a.cur_stage <- ivalue "cur_stage";
+	a.desired_course <- fvalue "desired_course" /. 10.
     | "BAT" ->
 	a.throttle <- fvalue "desired_gaz" /. 9600. *. 100.;
 	a.flight_time <- ivalue "flight_time";
 	a.rpm <- a.throttle *. 100.;
-	a.bat <- fvalue "voltage" /. 10.
+	a.bat <- fvalue "voltage" /. 10.;
+	a.stage_time <- ivalue "stage_time";
+	a.block_time <- ivalue "block_time"
     | "PPRZ_MODE" ->
-	a.ap_mode <- ivalue "ap_mode";
+	a.ap_mode <- check_index (ivalue "ap_mode") ap_modes "AP_MODE";
-	a.ap_altitude <- ivalue "ap_altitude";
+	a.gaz_mode <- check_index (ivalue "ap_gaz") gaz_modes "AP_GAZ";
+	a.lateral_mode <- check_index (ivalue "ap_lateral") lat_modes "AP_LAT";
 	a.inflight_calib.if_mode <- ivalue "if_calib_mode";
 	let mcu1_status = ivalue "mcu1_status" in
 	(** c.f. link_autopilot.h *)
@@ -322,9 +347,14 @@ let send_aircraft_msg = fun ac ->
     let values = ["ac_id", Pprz.String ac; 
 		  "cur_block", Pprz.Int a.cur_block;
 		  "cur_stage", Pprz.Int a.cur_stage;
+		  "stage_time", Pprz.Int a.stage_time;
+		  "block_time", Pprz.Int a.block_time;
 		  "target_east", f (a.nav_ref_east+.a.desired_east);
 		  "target_north", f (a.nav_ref_north+.a.desired_north);
-		  "target_alt", Pprz.Float a.desired_altitude] in
+		  "target_alt", Pprz.Float a.desired_altitude;
+		  "target_climb", Pprz.Float a.desired_climb;
+		  "target_course", Pprz.Float a.desired_course
+		] in
     Ground_Pprz.message_send my_id "NAV_STATUS" values;
 
     let values = ["ac_id", Pprz.String ac; 
@@ -335,11 +365,15 @@ let send_aircraft_msg = fun ac ->
 		  "amp", f a.amp;
 		  "energy", f a.energy] in
     Ground_Pprz.message_send my_id "ENGINE_STATUS" values;
-
+    
+    let ap_mode = get_indexed_value ap_modes a.ap_mode in
+    let gaz_mode = get_indexed_value gaz_modes a.gaz_mode in
+    let lat_mode = get_indexed_value lat_modes a.lateral_mode in
     let values = ["ac_id", Pprz.String ac; 
 		  "flight_time", Pprz.Int a.flight_time;
-		  "ap_mode", Pprz.Int a.ap_mode; 
+		  "ap_mode", Pprz.String ap_mode; 
-		  "v_mode", Pprz.Int a.ap_altitude;
+		  "gaz_mode", Pprz.String gaz_mode;
+		  "lat_mode", Pprz.String lat_mode;
 		  "gps_mode", Pprz.Int a.gps_mode] in
     Ground_Pprz.message_send my_id "AP_STATUS" values;
 
@@ -353,15 +387,20 @@ let send_aircraft_msg = fun ac ->
   | x -> prerr_endline (Printexc.to_string x)
       
 let new_aircraft = fun id ->
-    { id = id ; roll = 0.; pitch = 0.; nav_ref_east = 0.; nav_ref_north = 0.; desired_east = 0.; desired_north = 0.; gspeed=0.; course = 0.; alt=0.; climb=0.; cur_block=0; cur_stage=0; throttle = 0.; rpm = 0.; temp = 0.; bat = 0.; amp = 0.; energy = 0.; ap_mode=0; ap_altitude=0; gps_mode =0;
+    { id = id ; roll = 0.; pitch = 0.; nav_ref_east = 0.; nav_ref_north = 0.; desired_east = 0.; desired_north = 0.; 
+      desired_course = 0.;
+	gspeed=0.; course = 0.; alt=0.; climb=0.; cur_block=0; cur_stage=0; throttle = 0.; rpm = 0.; temp = 0.; bat = 0.; amp = 0.; energy = 0.; ap_mode= -1;
+      gaz_mode= -1; lateral_mode= -1;
+      gps_mode =0;
       desired_altitude = 0.;
+      desired_climb = 0.;
       pos = { utm_x = 0.; utm_y = 0.; utm_zone = 0 };
       cam = { phi = 0.; theta = 0. };
       inflight_calib = { if_mode = 1 ; if_val1 = 0.; if_val2 = 0.};
       infrared = infrared_init;
       fbw = { rc_status = "???"; rc_mode = "???" };
       svinfo = Array.create gps_nb_channels svinfo_init;
-      flight_time = 0;
+      flight_time = 0; stage_time = 0; block_time = 0
     }
     
 let register_aircraft = fun name a ->

sw/lib/perl/Paparazzi/Aircraft.pm

@@ -49,11 +49,11 @@ sub populate {
 		    climb         => [S_NOINIT,   S_PASSIVE,  S_RDWR,   S_OVRWRT, S_NOPRPG, 0.],
 
 		    ap_mode       => [S_NOINIT,   S_PASSIVE,  S_RDWR,   S_OVRWRT, S_NOPRPG, 0.],
-		    h_mode        => [S_NOINIT,   S_PASSIVE,  S_RDWR,   S_OVRWRT, S_NOPRPG, 0.],
+		    lat_mode        => [S_NOINIT,   S_PASSIVE,  S_RDWR,   S_OVRWRT, S_NOPRPG, 0.],
-		    v_mode        => [S_NOINIT,   S_PASSIVE,  S_RDWR,   S_OVRWRT, S_NOPRPG, 0.],
+		    gaz_mode        => [S_NOINIT,   S_PASSIVE,  S_RDWR,   S_OVRWRT, S_NOPRPG, 0.],
 		    target_climb  => [S_NOINIT,   S_PASSIVE,  S_RDWR,   S_OVRWRT, S_NOPRPG, 0.],
 		    target_alt    => [S_NOINIT,   S_PASSIVE,  S_RDWR,   S_OVRWRT, S_NOPRPG, 0.],
-		    target_heading=> [S_NOINIT,   S_PASSIVE,  S_RDWR,   S_OVRWRT, S_NOPRPG, 0.],
+		    target_course=> [S_NOINIT,   S_PASSIVE,  S_RDWR,   S_OVRWRT, S_NOPRPG, 0.],
 		    gps_mode      => [S_NOINIT,   S_PASSIVE,  S_RDWR,   S_OVRWRT, S_NOPRPG, 0.],
 
 		    cur_block     => [S_NOINIT,   S_PASSIVE,  S_RDWR,   S_OVRWRT, S_NOPRPG, 0.],