Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-01 21:07:40 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:paparazzi/paparazzi into dev

Browse Source
This commit is contained in:
Christophe De Wagter
2011-04-22 12:58:04 +02:00
parent b45a6dc949 bcf40abd95
commit f6f68da076
9 changed files with 101 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/ins_arduimu_basic.xml
+1 -1
View File
@@ -12,7 +12,7 @@
<file name="ins_arduimu_basic.c"/>
</makefile>
<makefile target="sim">
<file_arch name="ins_arduimu.c"/>
<file_arch name="ins_arduimu_basic.c"/>
</makefile>
</module>
sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c
+47
View File
@@ -0,0 +1,47 @@
/** ArduIMU simulation. OCaml binding.
* Sim provides IR sensor reading and airspeed
*/
#include <inttypes.h>
#include "generated/airframe.h"
#include <caml/mlvalues.h>
#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"
struct FloatEulers arduimu_eulers;
struct FloatRates arduimu_rates;
struct FloatVect3 arduimu_accel;
float ins_roll_neutral;
float ins_pitch_neutral;
void ArduIMU_init( void ) {}
void ArduIMU_periodic( void ) {}
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;
}
sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/DCM.pde
+2
View File
@@ -97,6 +97,8 @@ void Drift_correction(void)
// Dynamic weighting of accelerometer info (reliability filter)
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); //
// Set to 0 if high_accel_flag is true
if (high_accel_flag) Accel_weight = 0.;
#if PERFORMANCE_REPORTING == 1
tempfloat = ((Accel_weight - 0.5) * 256.0f); //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction
sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_pprz.pde
+1
View File
@@ -25,6 +25,7 @@ void parse_pprz_gps() {
ground_course = (float)join_4_bytes(&Paparazzi_GPS_buffer[8])/100000.0; // Heading 2D 8,9,10,11
stGpsFix = Paparazzi_GPS_buffer[12];
stFlags = Paparazzi_GPS_buffer[13];
high_accel_flag = Paparazzi_GPS_buffer[14];
if((stGpsFix >= 0x03) && (stFlags&0x01)) {
gpsFix = 0; //valid position
sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde
+1 -5
View File
@@ -104,11 +104,7 @@ float Omega_P[3]= {0,0,0};//Omega Proportional correction
float Omega_I[3]= {0,0,0};//Omega Integrator
float Omega[3]= {0,0,0};
//Magnetometer variables
//int magnetom_x;
//int magnetom_y;
//int magnetom_z;
//float MAG_Heading;
boolean high_accel_flag = false; // disable update on accelerometers if true
// Euler angles
float roll;
sw/airborne/modules/ins/ins_arduimu_basic.c
+24 -3
View File
@@ -13,9 +13,13 @@ Autoren@ZHAW: schmiemi
// test
#include "estimator.h"
// für das Senden von GPS-Daten an den ArduIMU
// GPS data for ArduIMU
#include "gps.h"
// Command vector for thrust
#include "generated/airframe.h"
#include "inter_mcu.h"
#define NB_DATA 9
#ifndef ARDUIMU_I2C_DEV
@@ -24,7 +28,7 @@ Autoren@ZHAW: schmiemi
// Adresse des I2C Slaves: 0001 0110 letztes Bit ist für Read/Write
// einzugebende Adresse im ArduIMU ist 0000 1011
//da ArduIMU das Read/Write Bit selber anfügt.
// da ArduIMU das Read/Write Bit selber anfügt.
#define ArduIMU_SLAVE_ADDR 0x22
#ifndef DOWNLINK_DEVICE
@@ -47,6 +51,11 @@ struct FloatVect3 arduimu_accel;
float ins_roll_neutral;
float ins_pitch_neutral;
// High Accel Flag
#define HIGH_ACCEL_LOW_SPEED 2.0
#define HIGH_ACCEL_HIGH_THRUST (0.8*MAX_PPRZ)
bool_t high_accel_flag;
void ArduIMU_init( void ) {
FLOAT_EULERS_ZERO(arduimu_eulers);
FLOAT_RATES_ZERO(arduimu_rates);
@@ -57,6 +66,8 @@ void ArduIMU_init( void ) {
ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
high_accel_flag = FALSE;
}
#define FillBufWith32bit(_buf, _index, _value) { \
@@ -70,12 +81,22 @@ void ArduIMU_periodicGPS( void ) {
if (ardu_gps_trans.status != I2CTransDone) { return; }
// Test for high acceleration:
// - low speed
// - high thrust
if (gps_speed_3d < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE]) {
high_accel_flag = TRUE;
} else {
high_accel_flag = FALSE;
}
FillBufWith32bit(ardu_gps_trans.buf, 0, (int32_t)gps_speed_3d); // speed 3D
FillBufWith32bit(ardu_gps_trans.buf, 4, (int32_t)gps_gspeed); // ground speed
FillBufWith32bit(ardu_gps_trans.buf, 8, (int32_t)gps_course); // course
ardu_gps_trans.buf[12] = gps_mode; // status gps fix
ardu_gps_trans.buf[13] = gps_status_flags; // status flags
I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 14);
ardu_gps_trans.buf[14] = (uint8_t)high_accel_flag; // high acceleration flag (disable accelerometers in the arduimu filter)
I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 15);
}
sw/airborne/modules/ins/ins_chimu_spi.c
+4 -25
View File
@@ -27,7 +27,6 @@ C code to connect a CHIMU using uart
#include "ins_module.h"
#include "imu_chimu.h"
CHIMU_PARSER_DATA CHIMU_DATA;
INS_FORMAT ins_roll_neutral;
@@ -37,15 +36,13 @@ volatile uint8_t new_ins_attitude;
void ins_init( void )
{
// uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI
uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI
uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI
// uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI
// uint8_t euler[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x00, 0xaf }; // 25Hz attitude only + SPI
uint8_t quaternions[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x01, 0x39 }; // 25Hz attitude only + SPI
CHIMU_Checksum(rate,12);
new_ins_attitude = 0;
ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
@@ -58,6 +55,7 @@ void ins_init( void )
{
InsSend1(quaternions[i]);
}
// Wait a bit (SPI send zero)
InsSend1(0);
InsSend1(0);
@@ -73,9 +71,6 @@ void ins_init( void )
}
//float tempang = 0;
void parse_ins_msg( void )
{
while (InsLink(ChAvailable()))
@@ -87,28 +82,12 @@ void parse_ins_msg( void )
if(CHIMU_DATA.m_MsgID==0x03)
{
new_ins_attitude = 1;
// RunOnceEvery(25, LED_TOGGLE(3) );
// LED_TOGGLE(3);
RunOnceEvery(25, LED_TOGGLE(3) );
if (CHIMU_DATA.m_attitude.euler.phi > M_PI)
{
CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
}
if (CHIMU_DATA.m_attrates.euler.phi > M_PI)
{
CHIMU_DATA.m_attrates.euler.phi -= 2 * M_PI;
}
LED_TOGGLE(3);
/* if (CHIMU_DATA.m_attitude.euler.phi == tempang)
{
LED_ON(3);
}
else
{
LED_OFF(3);
}
tempang = CHIMU_DATA.m_attitude.euler.phi;
*/
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);
}
sw/airborne/modules/ins/ins_chimu_uart.c
+17 -17
View File
@@ -4,8 +4,11 @@ C code to connect a CHIMU using uart
#include <stdbool.h>
//#include "modules/ins/ins_chimu_uart.h"
// Output
#include "estimator.h"
@@ -33,11 +36,13 @@ volatile uint8_t new_ins_attitude;
void ins_init( void )
{
uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0xab, 0x76 }; // 50Hz attitude only + SPI
// uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0xab, 0xd3 }; // 25Hz attitude only + SPI
uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI
// uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI
// uint8_t euler[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x00, 0xaf }; // 25Hz attitude only + SPI
uint8_t quaternions[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x01, 0x39 }; // 25Hz attitude only + SPI
CHIMU_Checksum(rate,12);
new_ins_attitude = 0;
ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
@@ -50,6 +55,14 @@ void ins_init( void )
{
InsUartSend1(quaternions[i]);
}
// 50Hz
for (int i=0;i<12;i++)
{
@@ -58,8 +71,6 @@ void ins_init( void )
}
float tempang = 0;
void parse_ins_msg( void )
{
while (InsLink(ChAvailable()))
@@ -71,23 +82,12 @@ void parse_ins_msg( void )
if(CHIMU_DATA.m_MsgID==0x03)
{
new_ins_attitude = 1;
// RunOnceEvery(25, LED_TOGGLE(3) );
// LED_TOGGLE(3);
RunOnceEvery(25, LED_TOGGLE(3) );
if (CHIMU_DATA.m_attitude.euler.phi > M_PI)
{
CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
}
if (CHIMU_DATA.m_attitude.euler.phi == tempang)
{
LED_ON(3);
}
else
{
LED_OFF(3);
}
tempang = CHIMU_DATA.m_attitude.euler.phi;
EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta);
//EstimatorSetRate(ins_p,ins_q);
sw/ground_segment/joystick/input2ivy.ml
+4 -4
View File
@@ -588,13 +588,13 @@ let () =
Ivy.init "Paparazzi joystick" "READY" (fun _ _ -> ());
Ivy.start !ivy_bus;
(** setup stdin *)
let tstatus = (Unix.tcgetattr Unix.stdin) in
(** setup stdin *) (* TODO find a better way to change trim, use a GUI ? *)
(*let tstatus = (Unix.tcgetattr Unix.stdin) in
tstatus.c_icanon <- false;
Unix.tcsetattr Unix.stdin Unix.TCSANOW tstatus;
Unix.tcsetattr Unix.stdin Unix.TCSANOW tstatus;*)
ignore (Glib.Timeout.add actions.period_ms (fun () -> execute_actions actions ac_id; true));
ignore (Glib.Io.add_watch ~cond:[`IN] ~callback:(fun x -> execute_kb_action actions x) (Glib.Io.channel_of_descr Unix.stdin));
(*ignore (Glib.Io.add_watch ~cond:[`IN] ~callback:(fun x -> execute_kb_action actions x) (Glib.Io.channel_of_descr Unix.stdin));*)
(** Start the main loop *)
let loop = Glib.Main.create true in