svinfo simul

conf/messages.xml

@@ -4,7 +4,7 @@
  <!-- messages from modem or sim to server -->
  <class name="telemetry_ap" ID="0x40">
  
-   <message name="BOOT" ID="1">
+   <message name="BOOT" id="2">
      <field name="version" type="uint16"></field>
    </message>
 

sw/airborne/autopilot/gps.h

@@ -61,8 +61,8 @@ struct svinfo {
   uint8_t flags;
   uint8_t qi;
   uint8_t cno;
-  int8_t elev;
-  int16_t azim;
+  int8_t elev; /** deg */
+  int16_t azim; /** deg */
 };
 
 extern struct svinfo gps_svinfos[NB_CHANNELS];

sw/airborne/autopilot/main.c

@@ -529,6 +529,7 @@ void use_gps_pos( void ) {
   if (i == gps_nb_channels) i = 0;
   if (i < gps_nb_channels)
     DOWNLINK_SEND_SVINFO(&i, &gps_svinfos[i].svid, &gps_svinfos[i].flags, &gps_svinfos[i].qi, &gps_svinfos[i].cno, &gps_svinfos[i].elev, &gps_svinfos[i].azim);
+  i++;
 
   if (!estimator_flight_time && (estimator_hspeed_mod > MIN_SPEED_FOR_TAKEOFF)) {
     estimator_flight_time = 1;

sw/ground_segment/tmtc/receive.ml

@@ -232,7 +232,7 @@ let log_and_parse = fun log ac_name a msg values ->
 	  azim = ivalue "Azim";
 	};
 	if i = 0 then
-	  a.svinfo_nb_channels <- a.svinfo_last_channel;
+	  a.svinfo_nb_channels <- a.svinfo_last_channel + 1;
 	a.svinfo_last_channel <- i
     | _ -> ()
 
@@ -254,7 +254,7 @@ let send_cam_status = fun a ->
     let h = a.alt -. float (Srtm.of_utm a.pos) in
     let east = a.pos.utm_x +. h *. tan (a.cam.phi -. a.roll)
     and north = a.pos.utm_y +. h *. tan (a.cam.theta +. a.pitch) in
-    let values = ["cam_east", Pprz.Float east; "cam_north", Pprz.Float north] in
+    let values = ["ac_id", Pprz.String a.id; "cam_east", Pprz.Float east; "cam_north", Pprz.Float north] in
     Ground_Pprz.message_send my_id "CAM_STATUS" values
 
 let send_if_calib = fun a ->
@@ -298,8 +298,9 @@ let send_svsinfo = fun a ->
       concat azim a.svinfo.(i).azim
     done;
     let f = fun s r -> (s, Pprz.String !r) in
-    let vs = [f "SVID" svid; f "Flags" flags; f "QI" qi; 
-	      f "CNO" cno; f "Elev" elev; f "Azim" azim] in
+    let vs = ["ac_id", Pprz.String a.id; 
+	      f "svid" svid; f "flags" flags; f "qi" qi; 
+	      f "cno" cno; f "elev" elev; f "azim" azim] in
     Ground_Pprz.message_send my_id "SVSINFO" vs
   end
 
@@ -345,7 +346,8 @@ let send_aircraft_msg = fun ac ->
     send_cam_status a;
     send_if_calib a;
     send_fbw a;
-    send_infrared a
+    send_infrared a;
+    send_svsinfo a
   with
     Not_found -> prerr_endline ac
   | x -> prerr_endline (Printexc.to_string x)

sw/simulator/sim_ap.c

@@ -21,9 +21,6 @@ bool_t launch;
 bool_t link_fbw_receive_valid;
 uint8_t gps_nb_ovrn, modem_nb_ovrn, link_fbw_fbw_nb_err, link_fbw_nb_err;
 
-struct svinfo gps_svinfos[NB_CHANNELS];
-uint8_t gps_nb_channels;
-
 
 uint8_t ac_id;
 

sw/simulator/sim_gps.c

@@ -5,6 +5,7 @@
 #include "airframe.h"
 #include "flight_plan.h"
 #include "autopilot.h"
+#include "gps.h"
 
 #include <caml/mlvalues.h>
 
@@ -15,8 +16,10 @@ float   gps_fspeed;  /* m/s     */
 float   gps_fclimb;  /* m/s     */
 float   gps_fcourse; /* rad     */
 int32_t gps_utm_east, gps_utm_north;
-int8_t gps_utm_zone;
+uint8_t gps_utm_zone;
 float gps_east, gps_north; /* m */
+struct svinfo gps_svinfos[NB_CHANNELS];
+uint8_t gps_nb_channels = 0;
 
 value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, value cl, value t) {
   gps_mode = 3;
@@ -31,7 +34,21 @@ value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, valu
 
   gps_east = gps_utm_east / 100 - NAV_UTM_EAST0;
   gps_north = gps_utm_north / 100 - NAV_UTM_NORTH0;
-    
+
+  /** Space vehicle info simulation */
+  gps_nb_channels=7;
+  int i;
+  static int time;
+  time++;
+  for(i = 0; i < gps_nb_channels; i++) {
+    gps_svinfos[i].svid = 7 + i;
+    gps_svinfos[i].elev = (cos(((100*i)+time)/100.) + 1) * 45;
+    gps_svinfos[i].azim = (time/gps_nb_channels + 20 * i) % 360;
+    gps_svinfos[i].cno = 40 + sin(time/100.) * 10.;
+    gps_svinfos[i].flags = 0x01;
+    gps_svinfos[i].qi = (int)((time / 1000.) + i) % 8;
+  }
+      
   use_gps_pos(); /* From main.c */
   return Val_unit;
 }