Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 03:57:45 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

untabify

Browse Source
This commit is contained in:
Felix Ruess
2011-12-03 17:30:14 +01:00
parent 0e3bc1cdfa
commit c0ef220c32
3 changed files with 100 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c
+27 -27
View File
@@ -45,28 +45,28 @@ static inline void ahrs_update_mag_2d(void);
/* in place quaternion first order integration with constante rotational velocity */ /* in place quaternion first order integration with constante rotational velocity */
/* */ /* */
#define INT32_QUAT_INTEGRATE_FI(_q, _hr, _omega, _f) { \ #define INT32_QUAT_INTEGRATE_FI(_q, _hr, _omega, _f) { \
_hr.qi += -_omega.p*_q.qx - _omega.q*_q.qy - _omega.r*_q.qz; \ _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.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.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; \ _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)); \ ldiv_t _div = ldiv(_hr.qi, ((1<<INT32_RATE_FRAC)*_f*2)); \
_q.qi+= _div.quot; \ _q.qi+= _div.quot; \
_hr.qi = _div.rem; \ _hr.qi = _div.rem; \
\ \
_div = ldiv(_hr.qx, ((1<<INT32_RATE_FRAC)*_f*2)); \ _div = ldiv(_hr.qx, ((1<<INT32_RATE_FRAC)*_f*2)); \
_q.qx+= _div.quot; \ _q.qx+= _div.quot; \
_hr.qx = _div.rem; \ _hr.qx = _div.rem; \
\ \
_div = ldiv(_hr.qy, ((1<<INT32_RATE_FRAC)*_f*2)); \ _div = ldiv(_hr.qy, ((1<<INT32_RATE_FRAC)*_f*2)); \
_q.qy+= _div.quot; \ _q.qy+= _div.quot; \
_hr.qy = _div.rem; \ _hr.qy = _div.rem; \
\ \
_div = ldiv(_hr.qz, ((1<<INT32_RATE_FRAC)*_f*2)); \ _div = ldiv(_hr.qz, ((1<<INT32_RATE_FRAC)*_f*2)); \
_q.qz+= _div.quot; \ _q.qz+= _div.quot; \
_hr.qz = _div.rem; \ _hr.qz = _div.rem; \
\ \
} }
@@ -160,8 +160,8 @@ void ahrs_propagate(void) {
void ahrs_update_accel(void) { void ahrs_update_accel(void) {
struct Int32Vect3 c2 = { RMAT_ELMT(ahrs.ltp_to_imu_rmat, 0,2), 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, 1,2),
RMAT_ELMT(ahrs.ltp_to_imu_rmat, 2,2)}; RMAT_ELMT(ahrs.ltp_to_imu_rmat, 2,2)};
struct Int32Vect3 residual; struct Int32Vect3 residual;
#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN #ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN
// FIXME: check overflow ? // FIXME: check overflow ?
@@ -216,8 +216,8 @@ void ahrs_update_mag(void) {
static inline void ahrs_update_mag_full(void) { static inline void ahrs_update_mag_full(void) {
const struct Int32Vect3 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X), 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_Y),
MAG_BFP_OF_REAL(AHRS_H_Z)}; MAG_BFP_OF_REAL(AHRS_H_Z)};
struct Int32Vect3 expected_imu; struct Int32Vect3 expected_imu;
INT32_RMAT_VMULT(expected_imu, ahrs.ltp_to_imu_rmat, expected_ltp); 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) { static inline void ahrs_update_mag_2d(void) {
const struct Int32Vect2 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X), 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; struct Int32Vect3 measured_ltp;
INT32_RMAT_TRANSP_VMULT(measured_ltp, ahrs.ltp_to_imu_rmat, imu.mag); INT32_RMAT_TRANSP_VMULT(measured_ltp, ahrs.ltp_to_imu_rmat, imu.mag);
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
+4 -4
View File
@@ -44,9 +44,9 @@ static inline void compute_body_orientation(void);
#define PI_INTEG_EULER (INT32_ANGLE_PI * F_UPDATE) #define PI_INTEG_EULER (INT32_ANGLE_PI * F_UPDATE)
#define TWO_PI_INTEG_EULER (INT32_ANGLE_2_PI * F_UPDATE) #define TWO_PI_INTEG_EULER (INT32_ANGLE_2_PI * F_UPDATE)
#define INTEG_EULER_NORMALIZE(_a) { \ #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; \
while (_a < -PI_INTEG_EULER) _a += TWO_PI_INTEG_EULER; \ while (_a < -PI_INTEG_EULER) _a += TWO_PI_INTEG_EULER; \
} }
void ahrs_init(void) { 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_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, 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.measure, ahrs_impl.hi_res_euler);
EULERS_COPY(ahrs_impl.measurement, ahrs_impl.hi_res_euler); EULERS_COPY(ahrs_impl.measurement, ahrs_impl.hi_res_euler);
sw/simulator/sim.ml
+69 -69
View File
@@ -53,28 +53,28 @@ let cb_register = fun closure ->
module type AIRCRAFT = module type AIRCRAFT =
sig sig
val init : int -> GPack.box -> unit val init : int -> GPack.box -> unit
(** [init ac_id box] *) (** [init ac_id box] *)
val boot : Stdlib.value -> unit val boot : Stdlib.value -> unit
(** [boot time_acceleration] *) (** [boot time_acceleration] *)
val commands : pprz_t array -> unit val commands : pprz_t array -> unit
(** Called once at init *) (** Called once at init *)
val infrared_and_airspeed : float -> float -> float -> float -> unit val infrared_and_airspeed : float -> float -> float -> float -> unit
(** [infrared ir_left ir_front ir_top air_speed] Called on timer *) (** [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 *) (** [ahrs phi theta psi p q r] Called on timer *)
val gps : Gps.state -> unit val gps : Gps.state -> unit
(** [gps state] Called on timer *) (** [gps state] Called on timer *)
end end
module type AIRCRAFT_ITL = 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_sizeof : unit -> int = "fg_sizeof"
external fg_msg : string -> float -> float -> float -> float -> float -> float -> unit = "fg_msg_bytecode" "fg_msg_native" 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 = let ground_alt =
Srtm.add_path (Env.paparazzi_home ^ "/data/srtm"); Srtm.add_path (Env.paparazzi_home ^ "/data/srtm");
try try
float (Srtm.of_wgs84 !pos0) float (Srtm.of_wgs84 !pos0)
with Srtm.Tile_not_found x -> with Srtm.Tile_not_found x ->
float_attrib flight_plan "ground_alt" in float_attrib flight_plan "ground_alt" in
ref ground_alt ref ground_alt
let main () = let main () =
@@ -171,18 +171,18 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
let ask_for_world_env = fun () -> let ask_for_world_env = fun () ->
try 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 float = fun f -> Pprz.Float f in
let values = ["east", float x; "north", float y; "up", float z; let values = ["east", float x; "north", float y; "up", float z;
"lat", float ((Rad>>Deg)gps_sol.Gps.wgs84.posn_lat); "lat", float ((Rad>>Deg)gps_sol.Gps.wgs84.posn_lat);
"long", float ((Rad>>Deg)gps_sol.Gps.wgs84.posn_long); "long", float ((Rad>>Deg)gps_sol.Gps.wgs84.posn_long);
"alt", float gps_sol.Gps.alt ] in "alt", float gps_sol.Gps.alt ] in
Ground_Pprz.message_req "sim" "WORLD_ENV" values world_update Ground_Pprz.message_req "sim" "WORLD_ENV" values world_update
with 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 in
ignore (GMain.Timeout.add 1000 (fun () -> ask_for_world_env (); true)); 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 () -> let fm_task = fun () ->
FM.do_commands !state commands; FM.do_commands !state commands;
let agl = let agl =
if !noground then max_float if !noground then max_float
else else
match !last_gps_state with match !last_gps_state with
Some s -> Some s ->
begin begin
try s.Gps.alt -. float (Srtm.of_wgs84 s.Gps.wgs84) with try s.Gps.alt -. float (Srtm.of_wgs84 s.Gps.wgs84) with
_ -> s.Gps.alt _ -> s.Gps.alt
end end
| None -> 0. in | None -> 0. in
FM.state_update !state FM.nominal_airspeed (!wind_x, !wind_y, !wind_z) agl fm_period FM.state_update !state FM.nominal_airspeed (!wind_x, !wind_y, !wind_z) agl fm_period
and ir_task = fun () -> and ir_task = fun () ->
@@ -231,33 +231,33 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
(** Sending to Flight Gear *) (** Sending to Flight Gear *)
let fg_task = fun socket buffer () -> let fg_task = fun socket buffer () ->
match !last_gps_state with match !last_gps_state with
None -> () None -> ()
| Some s -> | Some s ->
let lat = s.Gps.wgs84.Latlong.posn_lat let lat = s.Gps.wgs84.Latlong.posn_lat
and lon = s.Gps.wgs84.Latlong.posn_long and lon = s.Gps.wgs84.Latlong.posn_long
and alt = s.Gps.alt and alt = s.Gps.alt
(* and theta_ = s.Gps.course *) (* and theta_ = s.Gps.course *)
and (phi, theta, psi) = FlightModel.get_attitude !state in and (phi, theta, psi) = FlightModel.get_attitude !state in
fg_msg buffer lat lon alt phi theta psi; 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"; **) (** for i = 0 to String.length buffer - 1 do fprintf stderr "%x " (Char.code buffer.[i]) done; fprintf stderr "\n"; **)
try try
ignore (Unix.send socket buffer 0 (String.length buffer) []) ignore (Unix.send socket buffer 0 (String.length buffer) [])
with with
Unix.Unix_error (e,f,a) -> Printf.fprintf stderr "Error fg: %s (%s(%s))\n" (Unix.error_message e) f a Unix.Unix_error (e,f,a) -> Printf.fprintf stderr "Error fg: %s (%s(%s))\n" (Unix.error_message e) f a
in in
let set_pos = fun _ -> let set_pos = fun _ ->
let current_pos = Latlong.string_of !pos0 in let current_pos = Latlong.string_of !pos0 in
begin begin
match GToolbox.input_string ~title:"Setting geographic position" ~text:current_pos "Geographic position" with match GToolbox.input_string ~title:"Setting geographic position" ~text:current_pos "Geographic position" with
Some s -> pos0 := Latlong.of_string s Some s -> pos0 := Latlong.of_string s
| _ -> () | _ -> ()
end; end;
begin begin
let text = string_of_float !alt0 in let text = string_of_float !alt0 in
match GToolbox.input_string ~title:"Setting initial altitude" ~text "Geographic altitude" with match GToolbox.input_string ~title:"Setting initial altitude" ~text "Geographic altitude" with
Some s -> alt0 := float_of_string s Some s -> alt0 := float_of_string s
| _ -> () | _ -> ()
end end
in in
@@ -270,14 +270,14 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
(** Connection to Flight Gear client *) (** Connection to Flight Gear client *)
if !fg_client <> "" then if !fg_client <> "" then
try try
let inet_addr = Unix.inet_addr_of_string !fg_client in let inet_addr = Unix.inet_addr_of_string !fg_client in
let socket = Unix.socket Unix.PF_INET Unix.SOCK_DGRAM 0 in let socket = Unix.socket Unix.PF_INET Unix.SOCK_DGRAM 0 in
Unix.connect socket (Unix.ADDR_INET (inet_addr, 5501)); Unix.connect socket (Unix.ADDR_INET (inet_addr, 5501));
let buffer = String.create (fg_sizeof ()) in let buffer = String.create (fg_sizeof ()) in
Stdlib.timer ~scale:time_scale fg_period (fg_task socket buffer) Stdlib.timer ~scale:time_scale fg_period (fg_task socket buffer)
with with
e -> fprintf stderr "Error while connecting to fg: %s" (Printexc.to_string e) e -> fprintf stderr "Error while connecting to fg: %s" (Printexc.to_string e)
in in
let take_off = fun () -> FlightModel.set_air_speed !state FM.nominal_airspeed 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 if not !autoboot then begin
let s = GButton.button ~label:"Boot" ~packing:(hbox#pack) () in let s = GButton.button ~label:"Boot" ~packing:(hbox#pack) () in
let callback = fun () -> let callback = fun () ->
set_pos_and_boot (); set_pos_and_boot ();
s#misc#set_sensitive false in s#misc#set_sensitive false in
ignore (s#connect#clicked ~callback) ignore (s#connect#clicked ~callback)
end else end else
set_pos_and_boot (); set_pos_and_boot ();
@@ -304,17 +304,17 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
if not !autolaunch then begin if not !autolaunch then begin
let t = GButton.button ~label:"Launch" ~packing:hbox#pack () in let t = GButton.button ~label:"Launch" ~packing:hbox#pack () in
let callback = fun () -> let callback = fun () ->
take_off (); take_off ();
t#misc#set_sensitive false in t#misc#set_sensitive false in
ignore (t#connect#clicked ~callback); ignore (t#connect#clicked ~callback);
(* Monitor an AUTO2 launch to disable the button *) (* Monitor an AUTO2 launch to disable the button *)
let monitor = fun () -> let monitor = fun () ->
if FlightModel.get_air_speed !state > 0. then begin if FlightModel.get_air_speed !state > 0. then begin
t#misc#set_sensitive false; t#misc#set_sensitive false;
false false
end else end else
true in true in
ignore (GMain.Timeout.add 1000 monitor) ignore (GMain.Timeout.add 1000 monitor)
end else end else
take_off (); take_off ();