diff --git a/sw/airborne/arch/sim/modules/ins/ins_arduimu.c b/sw/airborne/arch/sim/modules/ins/ins_arduimu.c index 05d1f1c9c0..0fd1d8bace 100644 --- a/sw/airborne/arch/sim/modules/ins/ins_arduimu.c +++ b/sw/airborne/arch/sim/modules/ins/ins_arduimu.c @@ -1,19 +1,11 @@ -/** ArduIMU simulation. OCaml binding. - * Sim provides IR sensor reading and airspeed +/** ArduIMU simulation. + * Sim provides attitude. */ -#include #include "generated/airframe.h" - -#include - #include "estimator.h" -// Prevent undefined reference (from estimator.c) -#include "subsystems/sensors/infrared.h" -struct Infrared infrared; - // Arduimu empty implementation #include "modules/ins/ins_arduimu.h" @@ -26,24 +18,16 @@ float ins_pitch_neutral; float pitch_of_throttle_gain; float throttle_slew; +// Updates from Ocaml sim +extern float sim_phi; +extern float sim_theta; + void ArduIMU_init( void ) {} -void ArduIMU_periodic( void ) {} +void ArduIMU_periodic( void ) { + // Feed directly the estimator + estimator_phi = sim_phi - ins_roll_neutral; + estimator_theta = sim_theta - ins_pitch_neutral; +} void ArduIMU_periodicGPS( void ) {} void IMU_Daten_verarbeiten( void ) {} -// Updates from Ocaml sim -float sim_air_speed; - -value set_ir_and_airspeed( - value roll __attribute__ ((unused)), - value front __attribute__ ((unused)), - value top __attribute__ ((unused)), - value air_speed - ) { - // Feed directly the estimator - estimator_phi = atan2(Int_val(roll), Int_val(top)) - ins_roll_neutral; - estimator_theta = atan2(Int_val(front), Int_val(top)) - ins_pitch_neutral; - sim_air_speed = Double_val(air_speed); - return Val_unit; -} - diff --git a/sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c b/sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c index 4cde8cfdb8..ed5dcce9e3 100644 --- a/sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c +++ b/sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c @@ -1,19 +1,11 @@ -/** ArduIMU simulation. OCaml binding. - * Sim provides IR sensor reading and airspeed +/** ArduIMU simulation. + * Sim provides attitude and rates. */ -#include #include "generated/airframe.h" - -#include - #include "estimator.h" -// Prevent undefined reference (from estimator.c) -#include "subsystems/sensors/infrared.h" -struct Infrared infrared; - // Arduimu empty implementation #include "modules/ins/ins_arduimu_basic.h" @@ -25,24 +17,19 @@ float ins_roll_neutral; float ins_pitch_neutral; bool_t arduimu_calibrate_neutrals; +// Updates from Ocaml sim +extern float sim_phi; +extern float sim_theta; +extern float sim_p; +extern float sim_q; + void ArduIMU_init( void ) {} -void ArduIMU_periodic( void ) {} +void ArduIMU_periodic( void ) { + // Feed directly the estimator + estimator_phi = sim_phi - ins_roll_neutral; + estimator_theta = sim_theta - ins_pitch_neutral; + estimator_p = sim_p; + estimator_q = sim_q; +} void ArduIMU_periodicGPS( void ) {} void ArduIMU_event( void ) {} - -// Updates from Ocaml sim -float sim_air_speed; - -value set_ir_and_airspeed( - value roll __attribute__ ((unused)), - value front __attribute__ ((unused)), - value top __attribute__ ((unused)), - value air_speed - ) { - // Feed directly the estimator - estimator_phi = atan2(Int_val(roll), Int_val(top)) - ins_roll_neutral; - estimator_theta = atan2(Int_val(front), Int_val(top)) - ins_pitch_neutral; - sim_air_speed = Double_val(air_speed); - return Val_unit; -} -