simulate rates p and q as well

sw/airborne/subsystems/ahrs/ahrs_sim.c

@@ -31,16 +31,21 @@
 extern float sim_phi;
 extern float sim_theta;
 extern float sim_psi;
+extern float sim_p;
+extern float sim_q;
 extern bool_t ahrs_sim_available;
 
 
 void compute_body_orientation_and_rates(void);
 
-void update_attitude_from_sim(void) {
+void update_ahrs_from_sim(void) {
   ahrs_float.ltp_to_imu_euler.phi = sim_phi;
   ahrs_float.ltp_to_imu_euler.theta = sim_theta;
   ahrs_float.ltp_to_imu_euler.psi = sim_psi;
 
+  ahrs_float.imu_rate.p = sim_p;
+  ahrs_float.imu_rate.q = sim_q;
+
   /* set quaternion and rotation matrix representations as well */
   FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler);
   FLOAT_RMAT_OF_EULERS(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_euler);
@@ -75,7 +80,7 @@ void ahrs_align(void)
    * body and imu have the same frame and always set to true value from sim
    */
 
-  update_attitude_from_sim();
+  update_ahrs_from_sim();
 
   /* Compute initial body orientation */
   compute_body_orientation_and_rates();

sw/airborne/subsystems/ahrs/ahrs_sim.h

@@ -35,11 +35,11 @@ extern float ins_roll_neutral;
 extern float ins_pitch_neutral;
 #endif
 
-extern void update_attitude_from_sim(void);
+extern void update_ahrs_from_sim(void);
 
 #define AhrsEvent(_available_callback) {        \
     if (ahrs_sim_available) {                   \
-      update_attitude_from_sim();               \
+      update_ahrs_from_sim();               \
       _available_callback();                    \
       ahrs_sim_available = FALSE;               \
     }                                           \

sw/simulator/flightModel.ml

@@ -69,6 +69,7 @@ module type SIG =
 let get_xyz state = (state.x, state.y, state.z)
 let get_time state = state.t
 let get_attitude state = (state.phi, state.theta, state.psi)
+let get_pq state = (state.phi_dot, state.theta_dot)
 
 let get_air_speed state = state.air_speed
 let set_air_speed state = fun s -> state.air_speed <- s

sw/simulator/flightModel.mli

@@ -33,6 +33,7 @@ type state
 val get_xyz : state -> meter * meter * meter
 val get_time : state -> float
 val get_attitude : state -> radian * radian * radian
+val get_pq : state -> radian_s * radian_s
 
 val set_air_speed : state -> meter_s -> unit
 val get_air_speed : state -> meter_s

sw/simulator/hitl.ml

@@ -97,7 +97,7 @@ module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct
     with
       exc -> prerr_endline (Printexc.to_string exc)
 
-  let attitude = fun phi theta psi ->
+  let attitude_and_rates = fun phi theta psi p q ->
     prerr_endline "HITL attitude sim not implemented..."
 
   let sep_reg = Str.regexp Pprz.separator

sw/simulator/sim.ml

@@ -65,8 +65,8 @@ module type AIRCRAFT =
     val infrared_and_airspeed : float -> float -> float -> float -> unit
 	(** [infrared ir_left ir_front ir_top air_speed] Called on timer *)
 
-    val attitude : float -> float -> float -> unit
-    (** [ahrs phi theta psi] Called on timer *)
+    val attitude_and_rates : float -> float -> float -> float -> float ->unit
+    (** [ahrs phi theta psi p q] Called on timer *)
 
     val gps : Gps.state -> unit
 	(** [gps state] Called on timer *)
@@ -224,8 +224,9 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
       Aircraft.gps s
 
     and ahrs_task = fun () ->
-      let (phi, theta, psi) = FlightModel.get_attitude !state in
-      Aircraft.attitude phi theta psi in
+      let (phi, theta, psi) = FlightModel.get_attitude !state
+      and p, q = FlightModel.get_pq !state in
+      Aircraft.attitude_and_rates phi theta psi p q in
 
     (** Sending to Flight Gear *)
     let fg_task = fun socket buffer () ->

sw/simulator/sim.mli

@@ -10,7 +10,7 @@ module type AIRCRAFT =
     val boot : Stdlib.value -> unit
     val commands : Stdlib.pprz_t array -> unit
     val infrared_and_airspeed : float -> float -> float -> float -> unit
-    val attitude: float -> float -> float -> unit
+    val attitude_and_rates : float -> float -> float -> float -> float -> unit
     val gps : Gps.state -> unit
   end
 

sw/simulator/sitl.ml

@@ -194,9 +194,9 @@ module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct
     (** ADC neutral is not taken into account in the soft sim (c.f. sim_ir.c)*)
     set_ir_and_airspeed (truncate ir_left) (truncate ir_front) (truncate ir_top) air_speed
 
-  external set_attitude : float -> float -> float -> unit = "set_attitude"
-  let attitude = fun phi theta psi ->
-    set_attitude phi theta psi
+  external provide_attitude_and_rates : float -> float -> float -> float -> float -> unit = "provide_attitude_and_rates"
+  let attitude_and_rates = fun phi theta psi p q ->
+    provide_attitude_and_rates phi theta psi p q
 
   external use_gps_pos: int -> int -> int -> float -> float -> float -> float -> float -> bool -> float -> float -> unit = "sim_use_gps_pos_bytecode" "sim_use_gps_pos"
   let gps = fun gps ->