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 25 Packages Projects Releases Wiki Activity

update simulation of ins_arduimu

Browse Source
This commit is contained in:
Felix Ruess
2011-10-05 22:04:37 +02:00
parent e4d624cae0
commit 9dba6dcca1
2 changed files with 26 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/arch/sim/modules/ins/ins_arduimu.c
+11 -27
View File
@@ -1,19 +1,11 @@
/** ArduIMU simulation. OCaml binding. /** ArduIMU simulation.
* Sim provides IR sensor reading and airspeed * Sim provides attitude.
*/ */
#include <inttypes.h>
#include "generated/airframe.h" #include "generated/airframe.h"
#include <caml/mlvalues.h>
#include "estimator.h" #include "estimator.h"
// Prevent undefined reference (from estimator.c)
#include "subsystems/sensors/infrared.h"
struct Infrared infrared;
// Arduimu empty implementation // Arduimu empty implementation
#include "modules/ins/ins_arduimu.h" #include "modules/ins/ins_arduimu.h"
@@ -26,24 +18,16 @@ float ins_pitch_neutral;
float pitch_of_throttle_gain; float pitch_of_throttle_gain;
float throttle_slew; float throttle_slew;
// Updates from Ocaml sim
extern float sim_phi;
extern float sim_theta;
void ArduIMU_init( void ) {} 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 ArduIMU_periodicGPS( void ) {}
void IMU_Daten_verarbeiten( 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;
}
sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c
+15 -28
View File
@@ -1,19 +1,11 @@
/** ArduIMU simulation. OCaml binding. /** ArduIMU simulation.
* Sim provides IR sensor reading and airspeed * Sim provides attitude and rates.
*/ */
#include <inttypes.h>
#include "generated/airframe.h" #include "generated/airframe.h"
#include <caml/mlvalues.h>
#include "estimator.h" #include "estimator.h"
// Prevent undefined reference (from estimator.c)
#include "subsystems/sensors/infrared.h"
struct Infrared infrared;
// Arduimu empty implementation // Arduimu empty implementation
#include "modules/ins/ins_arduimu_basic.h" #include "modules/ins/ins_arduimu_basic.h"
@@ -25,24 +17,19 @@ float ins_roll_neutral;
float ins_pitch_neutral; float ins_pitch_neutral;
bool_t arduimu_calibrate_neutrals; 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_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_periodicGPS( void ) {}
void ArduIMU_event( 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;
}