add estimator_r (rate on z axis) for fixedwing (for future use)

sw/airborne/arch/sim/jsbsim_ahrs.c

@@ -13,15 +13,17 @@ float sim_theta;  ///< in radians
 float sim_psi;    ///< in radians
 float sim_p;      ///< in radians/s
 float sim_q;      ///< in radians/s
+float sim_r;      ///< in radians/s
 bool_t ahrs_sim_available;
 
 // Updates from jsbsim
-void provide_attitude_and_rates(float phi, float theta, float psi, float p, float q) {
+void provide_attitude_and_rates(float phi, float theta, float psi, float p, float q, float r) {
   sim_phi = phi;
   sim_theta = theta;
   sim_psi = psi;
   sim_p = p;
   sim_q = q;
+  sim_r = r;
 
   ahrs_sim_available = TRUE;
 }

sw/airborne/arch/sim/jsbsim_hw.h

@@ -52,7 +52,7 @@ void sim_use_gps_pos(double lat, double lon, double alt, double course, double g
 void sim_update_sv(void);
 
 void set_ir(double roll, double pitch);
-void provide_attitude_and_rates(float phi, float theta, float psi, float p, float q);
+void provide_attitude_and_rates(float phi, float theta, float psi, float p, float q, float r);
 
 void update_bat(double bat);
 

sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c

@@ -22,6 +22,7 @@ extern float sim_phi;
 extern float sim_theta;
 extern float sim_p;
 extern float sim_q;
+extern float sim_r;
 
 void ArduIMU_init( void ) {}
 void ArduIMU_periodic( void ) {
@@ -30,6 +31,7 @@ void ArduIMU_periodic( void ) {
   estimator_theta = sim_theta - ins_pitch_neutral;
   estimator_p = sim_p;
   estimator_q = sim_q;
+  estimator_r = sim_r;
 }
 void ArduIMU_periodicGPS( void ) {}
 void ArduIMU_event( void ) {}

sw/airborne/arch/sim/sim_ahrs.c

@@ -14,15 +14,25 @@ float sim_theta;  ///< in radians
 float sim_psi;    ///< in radians
 float sim_p;      ///< in radians/s
 float sim_q;      ///< in radians/s
+float sim_r;      ///< in radians/s
 bool_t ahrs_sim_available;
 
 // Updates from Ocaml sim
-value provide_attitude_and_rates(value phi, value theta, value psi, value p, value q) {
+
+value provide_attitude(value phi, value theta, value psi) {
   sim_phi = Double_val(phi);
   sim_theta = Double_val(theta);
   sim_psi = - Double_val(psi) + M_PI/2.;
-  sim_p = Double_val(p);
-  sim_q = Double_val(q);
+
+  ahrs_sim_available = TRUE;
+
+  return Val_unit;
+}
+
+value provide_rates(value p, value q, value r) {
+  sim_p = Double_val(p);
+  sim_q = Double_val(q);
+  sim_r = Double_val(r);
 
   ahrs_sim_available = TRUE;
 

sw/airborne/estimator.c

@@ -53,6 +53,7 @@ float estimator_theta;
 /* rates in radians per second */
 float estimator_p;
 float estimator_q;
+float estimator_r;
 
 /* flight time in seconds */
 uint16_t estimator_flight_time;
@@ -96,7 +97,7 @@ void estimator_init( void ) {
 
   EstimatorSetSpeedPol ( 0., 0., 0.);
 
-  EstimatorSetRate(0., 0.);
+  EstimatorSetRate(0., 0., 0.);
 
 #ifdef USE_AIRSPEED
   EstimatorSetAirspeed( 0. );

sw/airborne/estimator.h

@@ -56,6 +56,7 @@ extern float estimator_z_dot;
 /* rates in radians per second */
 extern float estimator_p;
 extern float estimator_q;
+extern float estimator_r;
 
 /* flight time in seconds */
 extern uint16_t estimator_flight_time;
@@ -128,7 +129,7 @@ extern void alt_kalman( float );
 #define EstimatorSetAtt(phi, psi, theta) { estimator_phi = phi; estimator_psi = psi; estimator_theta = theta; }
 #define EstimatorSetPhiPsi(phi, psi) { estimator_phi = phi; estimator_psi = psi; }
 
-#define EstimatorSetRate(p, q) { estimator_p = p; estimator_q = q; }
+#define EstimatorSetRate(p, q, r) { estimator_p = p; estimator_q = q; estimator_r = r; }
 
 
 #endif /* ESTIMATOR_H */

sw/airborne/modules/ins/ins_arduimu_basic.c

@@ -189,6 +189,7 @@ void ArduIMU_event( void ) {
     estimator_theta = arduimu_eulers.theta - ins_pitch_neutral;
     estimator_p = arduimu_rates.p;
     estimator_q = arduimu_rates.q;
+    estimator_r = arduimu_rates.r;
     ardu_ins_trans.status = I2CTransDone;
 
 #ifdef ARDUIMU_SYNC_SEND

sw/airborne/modules/ins/ins_chimu_spi.c

@@ -87,7 +87,7 @@ void parse_ins_msg( void )
     }
 
     EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta);
-    EstimatorSetRate(CHIMU_DATA.m_sensor.rate[0],CHIMU_DATA.m_attrates.euler.theta);
+    EstimatorSetRate(CHIMU_DATA.m_sensor.rate[0],CHIMU_DATA.m_attrates.euler.theta,0.); // FIXME rate r
       }
       else if(CHIMU_DATA.m_MsgID==0x02)
       {

sw/airborne/modules/ins/ins_chimu_uart.c

@@ -88,7 +88,7 @@ void parse_ins_msg( void )
 	}
 
 	EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta);
-	//EstimatorSetRate(ins_p,ins_q);
+	//EstimatorSetRate(ins_p,ins_q,ins_r);
 
 	DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &CHIMU_DATA.m_attitude.euler.phi, &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi);
 

sw/airborne/modules/ins/ins_xsens.c

@@ -303,10 +303,10 @@ void handle_ins_msg( void) {
   // Send to Estimator (Control)
 #ifdef XSENS_BACKWARDS
   EstimatorSetAtt((-ins_phi+ins_roll_neutral), (ins_psi + RadOfDeg(180)), (-ins_theta+ins_pitch_neutral));
-  EstimatorSetRate(-ins_p,-ins_q);
+  EstimatorSetRate(-ins_p,-ins_q, ins_r);
 #else
   EstimatorSetAtt(ins_phi+ins_roll_neutral, ins_psi, ins_theta+ins_pitch_neutral);
-  EstimatorSetRate(ins_p,ins_q);
+  EstimatorSetRate(ins_p, ins_q, ins_r);
 #endif
 
   // Position

sw/airborne/subsystems/ahrs/ahrs_float_dcm.c

@@ -142,6 +142,7 @@ void ahrs_update_fw_estimator( void )
 
   estimator_p = ahrs_float.body_rate.p;
   estimator_q = ahrs_float.body_rate.q;
+  estimator_r = ahrs_float.body_rate.r;
 /*
   RunOnceEvery(6,DOWNLINK_SEND_RMAT_DEBUG(DefaultChannel,
     &(DCM_Matrix[0][0]),

sw/airborne/subsystems/ahrs/ahrs_infrared.c

@@ -59,6 +59,9 @@ void ahrs_propagate(void) {
 #ifdef ADC_CHANNEL_GYRO_Q
   ahrs_float.body_rate.q = RATE_FLOAT_OF_BFP(imu.gyro.q);
 #endif
+#ifdef ADC_CHANNEL_GYRO_R
+  ahrs_float.body_rate.r = RATE_FLOAT_OF_BFP(imu.gyro.r);
+#endif
 }
 
 void ahrs_update_accel(void) {
@@ -118,5 +121,6 @@ void ahrs_update_fw_estimator(void)
 
   estimator_p = ahrs_float.body_rate.p;
   estimator_q = ahrs_float.body_rate.q;
+  estimator_r = ahrs_float.body_rate.r;
 
 }

sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c

@@ -348,6 +348,7 @@ void ahrs_update_fw_estimator(void)
   RATES_FLOAT_OF_BFP(rates, ahrs.body_rate);
   estimator_p = rates.p;
   estimator_q = rates.q;
+  estimator_r = rates.r;
 
 }
 #endif //AHRS_UPDATE_FW_ESTIMATOR

sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c

@@ -316,6 +316,7 @@ void ahrs_update_fw_estimator(void)
   RATES_FLOAT_OF_BFP(rates, ahrs.body_rate);
   estimator_p = rates.p;
   estimator_q = rates.q;
+  estimator_r = rates.r;
 
 }
 #endif //AHRS_UPDATE_FW_ESTIMATOR

sw/airborne/subsystems/ahrs/ahrs_sim.c

@@ -33,6 +33,7 @@ extern float sim_theta;
 extern float sim_psi;
 extern float sim_p;
 extern float sim_q;
+extern float sim_r;
 extern bool_t ahrs_sim_available;
 
 
@@ -45,6 +46,7 @@ void update_ahrs_from_sim(void) {
 
   ahrs_float.imu_rate.p = sim_p;
   ahrs_float.imu_rate.q = sim_q;
+  ahrs_float.imu_rate.r = sim_r;
 
   /* set quaternion and rotation matrix representations as well */
   FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler);
@@ -134,5 +136,6 @@ void ahrs_update_fw_estimator(void)
 
   estimator_p = ahrs_float.body_rate.p;
   estimator_q = ahrs_float.body_rate.q;
+  estimator_r = ahrs_float.body_rate.r;
 }
 #endif //AHRS_UPDATE_FW_ESTIMATOR

sw/simulator/flightModel.ml

@@ -46,6 +46,7 @@ type state = {
     mutable theta : radian;
     mutable theta_dot : radian_s;
     mutable phi_dot : radian_s;
+    mutable psi_dot : radian_s;
     mutable delta_a : float;
     mutable delta_b : float;
     mutable thrust : float;
@@ -69,7 +70,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_pqr state = (state.phi_dot, state.theta_dot, state.psi_dot)
 
 let get_air_speed state = state.air_speed
 let set_air_speed state = fun s -> state.air_speed <- s
@@ -170,7 +171,7 @@ module Make(A:Data.MISSION) = struct
 
   let init route = {
     start = Unix.gettimeofday (); t = 0.; x = 0.; y = 0. ; z = 0.;
-    psi = route; phi = 0.; phi_dot = 0.; theta_dot = 0.;
+    psi = route; phi = 0.; phi_dot = 0.; theta_dot = 0.; psi_dot = 0.;
     delta_a = 0.; delta_b = 0.; thrust = 0.; air_speed = 0.;
     theta = 0.; z_dot = 0.
   }
@@ -195,8 +196,8 @@ module Make(A:Data.MISSION) = struct
       state.phi <- norm_angle (state.phi +. state.phi_dot *. dt);
       state.phi <- bound state.phi (-.max_phi) max_phi;
 
-      let psi_dot = -. g /. state.air_speed *. tan (yaw_response_factor *. state.phi) in
-      state.psi <- norm_angle (state.psi +. psi_dot *. dt);
+      state.psi_dot <- -. g /. state.air_speed *. tan (yaw_response_factor *. state.phi);
+      state.psi <- norm_angle (state.psi +. state.psi_dot *. dt);
 
       (* Aerodynamic pitching moment coeff, proportional to elevator;
         No Thrust moment, so null (0) for steady flight *)

sw/simulator/flightModel.mli

@@ -33,7 +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 get_pqr : state -> radian_s * 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_and_rates = fun phi theta psi p q ->
+  let attitude_and_rates = fun phi theta psi p q r ->
     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_and_rates : float -> float -> float -> float -> float ->unit
-    (** [ahrs phi theta psi p q] Called on timer *)
+    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 *)
@@ -225,8 +225,8 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
 
     and ahrs_task = fun () ->
       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
+      and p, q, r = FlightModel.get_pqr !state in
+      Aircraft.attitude_and_rates phi theta psi p q r 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_and_rates : float -> float -> float -> float -> float -> unit
+    val attitude_and_rates : float -> float -> float -> float -> float -> float -> unit
     val gps : Gps.state -> unit
   end
 

sw/simulator/sim_ac_fw.c

@@ -192,9 +192,10 @@ void copy_outputs_from_jsbsim(FGFDMExec* FDMExec) {
   double yaw    = get_value(FDMExec, "attitude/heading-true-rad");
   double p      = get_value(FDMExec, "velocities/p-rad_sec");
   double q      = get_value(FDMExec, "velocities/q-rad_sec");
+  double r      = get_value(FDMExec, "velocities/r-rad_sec");
 
   // copy to AHRS
-  provide_attitude_and_rates(roll, pitch, yaw, p, q);
+  provide_attitude_and_rates(roll, pitch, yaw, p, q, r);
 
   // copy IR
   set_ir(roll, pitch);

sw/simulator/sitl.ml

@@ -194,9 +194,11 @@ 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 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 provide_attitude : float -> float -> float -> unit = "provide_attitude"
+  external provide_rates : float -> float -> float -> unit = "provide_rates"
+  let attitude_and_rates = fun phi theta psi p q r ->
+    provide_attitude phi theta psi;
+    provide_rates p q r
 
   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 ->