untabify

2026-05-26 08:22:43 +08:00 · 2011-12-03 17:30:14 +01:00
parent 0e3bc1cdfa
commit c0ef220c32
3 changed files with 100 additions and 100 deletions
@@ -45,28 +45,28 @@ static inline void ahrs_update_mag_2d(void);

 /* in place quaternion first order integration with constante rotational velocity */
 /*  */
-#define INT32_QUAT_INTEGRATE_FI(_q, _hr, _omega, _f) {			\
-    _hr.qi += -_omega.p*_q.qx - _omega.q*_q.qy - _omega.r*_q.qz;	\
-    _hr.qx +=  _omega.p*_q.qi + _omega.r*_q.qy - _omega.q*_q.qz;	\
-    _hr.qy +=  _omega.q*_q.qi - _omega.r*_q.qx + _omega.p*_q.qz;	\
-    _hr.qz +=  _omega.r*_q.qi + _omega.q*_q.qx - _omega.p*_q.qy;	\
-									\
-    ldiv_t _div = ldiv(_hr.qi, ((1<<INT32_RATE_FRAC)*_f*2));		\
-    _q.qi+= _div.quot;							\
-    _hr.qi = _div.rem;							\
-									\
-    _div = ldiv(_hr.qx, ((1<<INT32_RATE_FRAC)*_f*2));			\
-    _q.qx+= _div.quot;							\
-    _hr.qx = _div.rem;							\
-									\
-    _div = ldiv(_hr.qy, ((1<<INT32_RATE_FRAC)*_f*2));			\
-    _q.qy+= _div.quot;							\
-    _hr.qy = _div.rem;							\
-									\
-    _div = ldiv(_hr.qz, ((1<<INT32_RATE_FRAC)*_f*2));			\
-    _q.qz+= _div.quot;							\
-    _hr.qz = _div.rem;							\
-									\
+#define INT32_QUAT_INTEGRATE_FI(_q, _hr, _omega, _f) {              \
+    _hr.qi += -_omega.p*_q.qx - _omega.q*_q.qy - _omega.r*_q.qz;    \
+    _hr.qx +=  _omega.p*_q.qi + _omega.r*_q.qy - _omega.q*_q.qz;    \
+    _hr.qy +=  _omega.q*_q.qi - _omega.r*_q.qx + _omega.p*_q.qz;    \
+    _hr.qz +=  _omega.r*_q.qi + _omega.q*_q.qx - _omega.p*_q.qy;    \
+                                                                    \
+    ldiv_t _div = ldiv(_hr.qi, ((1<<INT32_RATE_FRAC)*_f*2));        \
+    _q.qi+= _div.quot;                                              \
+    _hr.qi = _div.rem;                                              \
+                                                                    \
+    _div = ldiv(_hr.qx, ((1<<INT32_RATE_FRAC)*_f*2));               \
+    _q.qx+= _div.quot;                                              \
+    _hr.qx = _div.rem;                                              \
+                                                                    \
+    _div = ldiv(_hr.qy, ((1<<INT32_RATE_FRAC)*_f*2));               \
+    _q.qy+= _div.quot;                                              \
+    _hr.qy = _div.rem;                                              \
+                                                                    \
+    _div = ldiv(_hr.qz, ((1<<INT32_RATE_FRAC)*_f*2));               \
+    _q.qz+= _div.quot;                                              \
+    _hr.qz = _div.rem;                                              \
+                                                                    \
  }


@@ -160,8 +160,8 @@ void ahrs_propagate(void) {
 void ahrs_update_accel(void) {

  struct Int32Vect3 c2 = { RMAT_ELMT(ahrs.ltp_to_imu_rmat, 0,2),
-			   RMAT_ELMT(ahrs.ltp_to_imu_rmat, 1,2),
-			   RMAT_ELMT(ahrs.ltp_to_imu_rmat, 2,2)};
+                           RMAT_ELMT(ahrs.ltp_to_imu_rmat, 1,2),
+                           RMAT_ELMT(ahrs.ltp_to_imu_rmat, 2,2)};
  struct Int32Vect3 residual;
 #ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN
  // FIXME: check overflow ?
@@ -216,8 +216,8 @@ void ahrs_update_mag(void) {

 static inline void ahrs_update_mag_full(void) {
  const struct Int32Vect3 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X),
-					  MAG_BFP_OF_REAL(AHRS_H_Y),
-					  MAG_BFP_OF_REAL(AHRS_H_Z)};
+                                          MAG_BFP_OF_REAL(AHRS_H_Y),
+                                          MAG_BFP_OF_REAL(AHRS_H_Z)};
  struct Int32Vect3 expected_imu;
  INT32_RMAT_VMULT(expected_imu, ahrs.ltp_to_imu_rmat, expected_ltp);

@@ -242,7 +242,7 @@ static inline void ahrs_update_mag_full(void) {
 static inline void ahrs_update_mag_2d(void) {

  const struct Int32Vect2 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X),
-					  MAG_BFP_OF_REAL(AHRS_H_Y)};
+                                          MAG_BFP_OF_REAL(AHRS_H_Y)};

  struct Int32Vect3 measured_ltp;
  INT32_RMAT_TRANSP_VMULT(measured_ltp, ahrs.ltp_to_imu_rmat, imu.mag);
@@ -44,9 +44,9 @@ static inline void compute_body_orientation(void);

 #define PI_INTEG_EULER     (INT32_ANGLE_PI * F_UPDATE)
 #define TWO_PI_INTEG_EULER (INT32_ANGLE_2_PI * F_UPDATE)
-#define INTEG_EULER_NORMALIZE(_a) {				\
-    while (_a >  PI_INTEG_EULER)  _a -= TWO_PI_INTEG_EULER;	\
-    while (_a < -PI_INTEG_EULER)  _a += TWO_PI_INTEG_EULER;	\
+#define INTEG_EULER_NORMALIZE(_a) {                         \
+    while (_a >  PI_INTEG_EULER)  _a -= TWO_PI_INTEG_EULER; \
+    while (_a < -PI_INTEG_EULER)  _a += TWO_PI_INTEG_EULER; \
  }

 void ahrs_init(void) {
@@ -78,7 +78,7 @@ void ahrs_align(void) {

  get_phi_theta_measurement_fom_accel(&ahrs_impl.hi_res_euler.phi, &ahrs_impl.hi_res_euler.theta, ahrs_aligner.lp_accel);
  get_psi_measurement_from_mag(&ahrs_impl.hi_res_euler.psi,
-			       ahrs_impl.hi_res_euler.phi/F_UPDATE, ahrs_impl.hi_res_euler.theta/F_UPDATE, ahrs_aligner.lp_mag);
+                               ahrs_impl.hi_res_euler.phi/F_UPDATE, ahrs_impl.hi_res_euler.theta/F_UPDATE, ahrs_aligner.lp_mag);

  EULERS_COPY(ahrs_impl.measure, ahrs_impl.hi_res_euler);
  EULERS_COPY(ahrs_impl.measurement, ahrs_impl.hi_res_euler);
@@ -53,28 +53,28 @@ let cb_register = fun closure ->


 module type AIRCRAFT =
-  sig
-    val init : int -> GPack.box -> unit
+sig
+  val init : int -> GPack.box -> unit
    (** [init ac_id box] *)
-    val boot : Stdlib.value -> unit
+  val boot : Stdlib.value -> unit
    (** [boot time_acceleration] *)

-    val commands : pprz_t array -> unit
-	(** Called once at init *)
+  val commands : pprz_t array -> unit
+    (** Called once at init *)

-    val infrared_and_airspeed : float -> float -> float -> float -> unit
-	(** [infrared ir_left ir_front ir_top air_speed] Called on timer *)
+  val infrared_and_airspeed : float -> float -> float -> float -> unit
+    (** [infrared ir_left ir_front ir_top air_speed] Called on timer *)

-    val attitude_and_rates : float -> float -> float -> float -> float -> float ->unit
+  val attitude_and_rates : float -> float -> float -> float -> float -> float ->unit
    (** [ahrs phi theta psi p q r] Called on timer *)

-    val gps : Gps.state -> unit
-	(** [gps state] Called on timer *)
-  end
+  val gps : Gps.state -> unit
+  (** [gps state] Called on timer *)
+end


 module type AIRCRAFT_ITL =
-    functor (A : Data.MISSION) -> functor (FM: FlightModel.SIG) -> AIRCRAFT
+  functor (A : Data.MISSION) -> functor (FM: FlightModel.SIG) -> AIRCRAFT

 external fg_sizeof : unit -> int = "fg_sizeof"
 external fg_msg : string -> float -> float -> float -> float -> float -> float -> unit = "fg_msg_bytecode" "fg_msg_native"
@@ -118,9 +118,9 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
    let ground_alt =
      Srtm.add_path (Env.paparazzi_home ^ "/data/srtm");
      try
-	float (Srtm.of_wgs84 !pos0)
+        float (Srtm.of_wgs84 !pos0)
      with Srtm.Tile_not_found x ->
-	float_attrib flight_plan "ground_alt" in
+        float_attrib flight_plan "ground_alt" in
    ref ground_alt

  let main () =
@@ -171,18 +171,18 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct

    let ask_for_world_env = fun () ->
      try
-	let (x, y, z) = FlightModel.get_xyz !state in
+        let (x, y, z) = FlightModel.get_xyz !state in

-	let gps_sol = compute_gps_state (x,y,z) (FlightModel.get_time !state) in
+        let gps_sol = compute_gps_state (x,y,z) (FlightModel.get_time !state) in

-	let float = fun f -> Pprz.Float f in
-	let values = ["east", float x; "north", float y; "up", float z;
-		      "lat", float ((Rad>>Deg)gps_sol.Gps.wgs84.posn_lat);
-		      "long", float ((Rad>>Deg)gps_sol.Gps.wgs84.posn_long);
-		      "alt", float gps_sol.Gps.alt ] in
-	Ground_Pprz.message_req "sim" "WORLD_ENV" values world_update
+        let float = fun f -> Pprz.Float f in
+        let values = ["east", float x; "north", float y; "up", float z;
+                      "lat", float ((Rad>>Deg)gps_sol.Gps.wgs84.posn_lat);
+                      "long", float ((Rad>>Deg)gps_sol.Gps.wgs84.posn_long);
+                      "alt", float gps_sol.Gps.alt ] in
+        Ground_Pprz.message_req "sim" "WORLD_ENV" values world_update
      with
-	exc -> fprintf stderr "Error in sim: %s\n%!" (Printexc.to_string exc)
+          exc -> fprintf stderr "Error in sim: %s\n%!" (Printexc.to_string exc)
    in

    ignore (GMain.Timeout.add 1000 (fun () -> ask_for_world_env (); true));
@@ -191,15 +191,15 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
    let fm_task = fun () ->
      FM.do_commands !state commands;
      let agl =
-	if !noground then max_float
-	else
-	  match !last_gps_state with
-	    Some s ->
-	      begin
-		try s.Gps.alt -. float (Srtm.of_wgs84 s.Gps.wgs84) with
-		  _ -> s.Gps.alt
-	      end
-	  | None -> 0. in
+        if !noground then max_float
+        else
+          match !last_gps_state with
+              Some s ->
+                begin
+                  try s.Gps.alt -. float (Srtm.of_wgs84 s.Gps.wgs84) with
+                      _ -> s.Gps.alt
+                end
+            | None -> 0. in
      FM.state_update !state FM.nominal_airspeed (!wind_x, !wind_y, !wind_z) agl fm_period

    and ir_task = fun () ->
@@ -231,33 +231,33 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
    (** Sending to Flight Gear *)
    let fg_task = fun socket buffer () ->
      match !last_gps_state with
-	None -> ()
-      | Some s ->
-	  let lat = s.Gps.wgs84.Latlong.posn_lat
-	  and lon = s.Gps.wgs84.Latlong.posn_long
-	  and alt = s.Gps.alt
-(*	  and theta_ = s.Gps.course *)
-	  and (phi, theta, psi) = FlightModel.get_attitude !state in
-	  fg_msg buffer lat lon alt phi theta psi;
-(**       for i = 0 to String.length buffer - 1 do fprintf stderr "%x " (Char.code buffer.[i]) done; fprintf stderr "\n"; **)
-      try
-	ignore (Unix.send socket buffer 0 (String.length buffer) [])
-      with
-	Unix.Unix_error (e,f,a) -> Printf.fprintf stderr "Error fg: %s (%s(%s))\n" (Unix.error_message e) f a
+          None -> ()
+        | Some s ->
+          let lat = s.Gps.wgs84.Latlong.posn_lat
+          and lon = s.Gps.wgs84.Latlong.posn_long
+          and alt = s.Gps.alt
+      (*    and theta_ = s.Gps.course *)
+          and (phi, theta, psi) = FlightModel.get_attitude !state in
+          fg_msg buffer lat lon alt phi theta psi;
+      (**       for i = 0 to String.length buffer - 1 do fprintf stderr "%x " (Char.code buffer.[i]) done; fprintf stderr "\n"; **)
+          try
+            ignore (Unix.send socket buffer 0 (String.length buffer) [])
+          with
+              Unix.Unix_error (e,f,a) -> Printf.fprintf stderr "Error fg: %s (%s(%s))\n" (Unix.error_message e) f a
    in

    let set_pos = fun _ ->
      let current_pos = Latlong.string_of !pos0 in
      begin
-	match GToolbox.input_string ~title:"Setting geographic position" ~text:current_pos "Geographic position"  with
-	  Some s -> pos0 := Latlong.of_string s
-	| _ -> ()
+        match GToolbox.input_string ~title:"Setting geographic position" ~text:current_pos "Geographic position"  with
+            Some s -> pos0 := Latlong.of_string s
+          | _ -> ()
      end;
      begin
-	let text = string_of_float !alt0 in
-	match GToolbox.input_string ~title:"Setting initial altitude" ~text "Geographic altitude"  with
-	  Some s -> alt0 := float_of_string s
-	| _ -> ()
+        let text = string_of_float !alt0 in
+        match GToolbox.input_string ~title:"Setting initial altitude" ~text "Geographic altitude"  with
+            Some s -> alt0 := float_of_string s
+          | _ -> ()
      end
    in

@@ -270,14 +270,14 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct

      (** Connection to Flight Gear client *)
      if !fg_client <> "" then
-	try
-	  let inet_addr = Unix.inet_addr_of_string !fg_client in
-	  let socket = Unix.socket Unix.PF_INET Unix.SOCK_DGRAM 0 in
-	  Unix.connect socket (Unix.ADDR_INET (inet_addr, 5501));
-	  let buffer = String.create (fg_sizeof ()) in
-	  Stdlib.timer ~scale:time_scale fg_period (fg_task socket buffer)
-	with
-	  e -> fprintf stderr "Error while connecting to fg: %s" (Printexc.to_string e)
+        try
+          let inet_addr = Unix.inet_addr_of_string !fg_client in
+          let socket = Unix.socket Unix.PF_INET Unix.SOCK_DGRAM 0 in
+          Unix.connect socket (Unix.ADDR_INET (inet_addr, 5501));
+          let buffer = String.create (fg_sizeof ()) in
+          Stdlib.timer ~scale:time_scale fg_period (fg_task socket buffer)
+        with
+            e -> fprintf stderr "Error while connecting to fg: %s" (Printexc.to_string e)
    in

    let take_off = fun () -> FlightModel.set_air_speed !state FM.nominal_airspeed in
@@ -295,8 +295,8 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
    if not !autoboot then begin
      let s = GButton.button ~label:"Boot" ~packing:(hbox#pack) () in
      let callback = fun () ->
-	set_pos_and_boot ();
-	s#misc#set_sensitive false in
+        set_pos_and_boot ();
+        s#misc#set_sensitive false in
      ignore (s#connect#clicked ~callback)
    end else
      set_pos_and_boot ();
@@ -304,17 +304,17 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
    if not !autolaunch then begin
      let t = GButton.button ~label:"Launch" ~packing:hbox#pack () in
      let callback = fun () ->
-	take_off ();
-	t#misc#set_sensitive false in
+        take_off ();
+        t#misc#set_sensitive false in
      ignore (t#connect#clicked ~callback);

      (* Monitor an AUTO2 launch to disable the button *)
      let monitor = fun () ->
-	if FlightModel.get_air_speed !state > 0. then begin
-	  t#misc#set_sensitive false;
-	  false
-	end else
-	  true in
+        if FlightModel.get_air_speed !state > 0. then begin
+          t#misc#set_sensitive false;
+          false
+        end else
+          true in
      ignore (GMain.Timeout.add 1000 monitor)
    end else
      take_off ();