diff --git a/conf/airframes/mm/fixed-wing/fw_ins_arduimu.xml b/conf/airframes/mm/fixed-wing/fw_ins_arduimu.xml new file mode 100644 index 0000000000..ba646479fb --- /dev/null +++ b/conf/airframes/mm/fixed-wing/fw_ins_arduimu.xml @@ -0,0 +1,207 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + +
+ + + + + + + + + + +
+ + +
+ +
+ + + + +
+ +
+ + +
+ +
+ + + + + + + +
+ +
+ + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + +
+ +
+ + +
+ +
+ + + + + + + + +
+ +
+ + + + + +
+ + diff --git a/conf/modules/ArduIMU.xml b/conf/modules/ins_arduimu.xml similarity index 79% rename from conf/modules/ArduIMU.xml rename to conf/modules/ins_arduimu.xml index 0212d67411..f3e4074b1d 100644 --- a/conf/modules/ArduIMU.xml +++ b/conf/modules/ins_arduimu.xml @@ -1,14 +1,14 @@ - +
- +
- + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sw/airborne/gps.h b/sw/airborne/gps.h index 9ae48bb59c..5f4a8d0e66 100644 --- a/sw/airborne/gps.h +++ b/sw/airborne/gps.h @@ -48,17 +48,22 @@ extern uint8_t gps_mode; /* Receiver status */ +extern uint8_t gps_status_flags; +extern uint8_t gps_sol_flags; extern uint16_t gps_week; /* weeks */ extern uint32_t gps_itow; /* ms */ extern int32_t gps_alt; /* cm */ +extern uint16_t gps_speed_3d; /* cm/s */ extern uint16_t gps_gspeed; /* cm/s */ extern int16_t gps_climb; /* m/s */ extern int16_t gps_course; /* decideg */ extern int32_t gps_utm_east, gps_utm_north; /** cm */ extern uint8_t gps_utm_zone; extern int32_t gps_lat, gps_lon; /* 1e7 deg */ +extern int32_t gps_hmsl; extern uint16_t gps_PDOP; extern uint32_t gps_Pacc, gps_Sacc; +extern int32_t gps_ecefVZ; extern uint8_t gps_numSV; extern uint8_t gps_configuring; diff --git a/sw/airborne/gps_ubx.c b/sw/airborne/gps_ubx.c index eb2599ad71..68543b421a 100644 --- a/sw/airborne/gps_ubx.c +++ b/sw/airborne/gps_ubx.c @@ -81,21 +81,26 @@ uint32_t gps_t0_itow; uint32_t gps_t0_frac; #endif int32_t gps_alt; +uint16_t gps_speed_3d; uint16_t gps_gspeed; int16_t gps_climb; int16_t gps_course; int32_t gps_utm_east, gps_utm_north; uint8_t gps_utm_zone; uint8_t gps_mode; +uint8_t gps_status_flags; +uint8_t gps_sol_flags; volatile bool_t gps_msg_received; bool_t gps_pos_available; uint8_t ubx_id, ubx_class; uint16_t ubx_len; int32_t gps_lat, gps_lon; +int32_t gps_hmsl; uint16_t gps_reset; uint16_t gps_PDOP; uint32_t gps_Pacc, gps_Sacc; +int32_t gps_ecefVZ; uint8_t gps_numSV; #define UTM_HEM_NORTH 0 @@ -250,11 +255,14 @@ void parse_gps_msg( void ) { if (ubx_class == UBX_NAV_ID) { if (ubx_id == UBX_NAV_STATUS_ID) { gps_mode = UBX_NAV_STATUS_GPSfix(ubx_msg_buf); + gps_status_flags = UBX_NAV_STATUS_Flags(ubx_msg_buf); + gps_sol_flags = UBX_NAV_SOL_Flags(ubx_msg_buf); #ifdef GPS_USE_LATLONG /* Computes from (lat, long) in the referenced UTM zone */ } else if (ubx_id == UBX_NAV_POSLLH_ID) { gps_lat = UBX_NAV_POSLLH_LAT(ubx_msg_buf); gps_lon = UBX_NAV_POSLLH_LON(ubx_msg_buf); + gps_hmsl = UBX_NAV_POSLLH_HMSL(ubx_msg_buf); latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7), nav_utm_zone0); @@ -273,6 +281,7 @@ void parse_gps_msg( void ) { gps_utm_zone = UBX_NAV_POSUTM_ZONE(ubx_msg_buf); #endif } else if (ubx_id == UBX_NAV_VELNED_ID) { + gps_speed_3d = UBX_NAV_VELNED_Speed(ubx_msg_buf); gps_gspeed = UBX_NAV_VELNED_GSpeed(ubx_msg_buf); gps_climb = - UBX_NAV_VELNED_VEL_D(ubx_msg_buf); gps_course = UBX_NAV_VELNED_Heading(ubx_msg_buf) / 10000; @@ -290,6 +299,7 @@ void parse_gps_msg( void ) { gps_mode = UBX_NAV_SOL_GPSfix(ubx_msg_buf); gps_PDOP = UBX_NAV_SOL_PDOP(ubx_msg_buf); gps_Pacc = UBX_NAV_SOL_Pacc(ubx_msg_buf); + gps_ecefVZ = UBX_NAV_SOL_ECEFVZ(ubx_msg_buf); gps_Sacc = UBX_NAV_SOL_Sacc(ubx_msg_buf); gps_numSV = UBX_NAV_SOL_numSV(ubx_msg_buf); gps_week = UBX_NAV_SOL_week(ubx_msg_buf); diff --git a/sw/airborne/modules/ins/ArduIMU.c b/sw/airborne/modules/ins/ArduIMU.c deleted file mode 100644 index 4a05de0215..0000000000 --- a/sw/airborne/modules/ins/ArduIMU.c +++ /dev/null @@ -1,211 +0,0 @@ -/* -C Datei für die Einbindung eines ArduIMU's -Autoren@ZHAW: schmiemi - chaneren -*/ - - -#include -#include "ArduIMU.h" -#include "main_fbw.h" -#include "i2c.h" - -// test -#include "estimator.h" - -// für das Senden von GPS-Daten an den ArduIMU -#include "gps.h" -int32_t GPS_Data[13]; -static volatile bool_t gps_i2c_done; - - -// 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. -#define ArduIMU_SLAVE_ADDR 0x22 - -#ifndef DOWNLINK_DEVICE -#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE -#endif - -#include "uart.h" -#include "messages.h" -#include "downlink.h" - -static volatile bool_t ArduIMU_i2c_done; -static int16_t recievedData[NB_DATA]; -float ArduIMU_data[NB_DATA]; -int8_t messageNr; -int8_t imu_daten_angefordert; -int8_t gps_daten_abgespeichert; -int8_t gps_daten_versendet_msg1; -int8_t gps_daten_versendet_msg2; - -float arduimu_roll_neutral; -float arduimu_pitch_neutral; -float pitch_of_throttle_gain; -float throttle_slew; - -void ArduIMU_init( void ) { - ArduIMU_i2c_done = TRUE; - gps_i2c_done = TRUE; - i2c0_buf[0] = 0; - i2c0_buf[1] = 0; - messageNr = 0; - imu_daten_angefordert = FALSE; - gps_daten_abgespeichert = FALSE; - gps_daten_versendet_msg1 = FALSE; - gps_daten_versendet_msg2 = FALSE; - arduimu_roll_neutral = ARDUIMU_ROLL_NEUTRAL; - arduimu_pitch_neutral = ARDUIMU_PITCH_NEUTRAL; - pitch_of_throttle_gain = PITCH_OF_THROTTLE_GAIN; - throttle_slew = V_CTL_THROTTLE_SLEW; -} - - -void ArduIMU_periodicGPS( void ) { - - if ( gps_daten_versendet_msg1 == TRUE && gps_daten_versendet_msg2 == TRUE ) { - gps_daten_abgespeichert = FALSE; - } - - if( imu_daten_angefordert == TRUE ){ - IMU_Daten_verarbeiten(); - } - - if ( gps_daten_abgespeichert == FALSE ) { - //posllh - GPS_Data [0] = gps_itow; - GPS_Data [1] = gps_lon; - GPS_Data [2] = gps_lat; - GPS_Data [3] = gps_alt; //höhe über elipsoid - GPS_Data [4] = gps_hmsl; //höhe über sea level - //velend - GPS_Data [5] = gps_speed; //speed 3D - GPS_Data [6] = gps_gspeed; //ground speed - GPS_Data [7] = gps_course * 100000; //Kurs - //status - GPS_Data [8] = gps_mode; //fix - GPS_Data [9] = gps_stauts_flag; //flags - //sol - GPS_Data [10] = gps_mode; //fix - GPS_Data [11] = gps_sol_flags; //flags - GPS_Data [12] = gps_ecefVZ; //ecefVZ - GPS_Data [13] = gps_numSV; - - gps_daten_abgespeichert = TRUE; - } - - if(messageNr == 0) { - - //test für 32bit in byte packete abzupacken: - //GPS_Data [0] = -1550138773; - - i2c0_buf[0] = 0; //message Nr = 0 --> itow bis ground speed - i2c0_buf[1] = (uint8_t) GPS_Data[0]; //itow - i2c0_buf[2] = (uint8_t) (GPS_Data[0] >>8); - i2c0_buf[3] = (uint8_t) (GPS_Data[0] >>16); - i2c0_buf[4] = (uint8_t) (GPS_Data[0] >>24); - i2c0_buf[5] = (uint8_t) GPS_Data[1]; //lon - i2c0_buf[6] = (uint8_t) (GPS_Data[1] >>8); - i2c0_buf[7] = (uint8_t) (GPS_Data[1] >>16); - i2c0_buf[8] = (uint8_t) (GPS_Data[1] >>24); - i2c0_buf[9] = (uint8_t) GPS_Data[2]; //lat - i2c0_buf[10] = (uint8_t) (GPS_Data[2] >>8); - i2c0_buf[11] = (uint8_t) (GPS_Data[2] >>16); - i2c0_buf[12] = (uint8_t) (GPS_Data[2] >>24); - i2c0_buf[13] = (uint8_t) GPS_Data[3]; //height - i2c0_buf[14] = (uint8_t) (GPS_Data[3] >>8); - i2c0_buf[15] = (uint8_t) (GPS_Data[3] >>16); - i2c0_buf[16] = (uint8_t) (GPS_Data[3] >>24); - i2c0_buf[17] = (uint8_t) GPS_Data[4]; //hmsl - i2c0_buf[18] = (uint8_t) (GPS_Data[4] >>8); - i2c0_buf[19] = (uint8_t) (GPS_Data[4] >>16); - i2c0_buf[20] = (uint8_t) (GPS_Data[4] >>24); - i2c0_buf[21] = (uint8_t) GPS_Data[5]; //speed - i2c0_buf[22] = (uint8_t) (GPS_Data[5] >>8); - i2c0_buf[23] = (uint8_t) (GPS_Data[5] >>16); - i2c0_buf[24] = (uint8_t) (GPS_Data[5] >>24); - i2c0_buf[25] = (uint8_t) GPS_Data[6]; //gspeed - i2c0_buf[26] = (uint8_t) (GPS_Data[6] >>8); - i2c0_buf[27] = (uint8_t) (GPS_Data[6] >>16); - i2c0_buf[28] = (uint8_t) (GPS_Data[6] >>24); - i2c0_transmit(ArduIMU_SLAVE_ADDR, 28, &gps_i2c_done); - - gps_daten_versendet_msg1 = TRUE; - messageNr =1; - } - else { - - i2c0_buf[0] = 1; //message Nr = 1 --> ground course, ecefVZ, numSV, Fix, flags, fix, flags - i2c0_buf[1] = GPS_Data[7]; //ground course - i2c0_buf[2] = (GPS_Data[7] >>8); - i2c0_buf[3] = (GPS_Data[7] >>16); - i2c0_buf[4] = (GPS_Data[7] >>24); - i2c0_buf[5] = GPS_Data[12]; //ecefVZ - i2c0_buf[6] = (GPS_Data[12] >>8); - i2c0_buf[7] = (GPS_Data[12] >>16); - i2c0_buf[8] = (GPS_Data[12] >>24); - i2c0_buf[9] = GPS_Data[13]; //numSV - i2c0_buf[10] = GPS_Data[8]; //status gps fix - i2c0_buf[11] = GPS_Data[9]; //status flags - i2c0_buf[12] = GPS_Data[10]; //sol gps fix - i2c0_buf[13] = GPS_Data[11]; //sol flags - i2c0_transmit(ArduIMU_SLAVE_ADDR, 13, &gps_i2c_done); - - gps_daten_versendet_msg2 = TRUE; - messageNr = 0; - } - - //DOWNLINK_SEND_DEBUG_ZHAW(DefaultChannel, &gps_mode , &gps_numSV ,&gps_alt , &gps_hmsl , &gps_itow, &gps_speed); -} - -void ArduIMU_periodic( void ) { -//Frequenz des Aufrufs wird in conf/modules/ArduIMU.xml festgelegt. - - //I2C-Nachricht anfordern an Slave Adresse, erwartete Anzahl Byte, Status - if (imu_daten_angefordert == TRUE) { - IMU_Daten_verarbeiten(); - } - i2c0_receive(ArduIMU_SLAVE_ADDR, 12, &ArduIMU_i2c_done); - imu_daten_angefordert = TRUE; - /* - Buffer O: Roll - Buffer 1: Pitch - Buffer 2: Yaw - Buffer 3: Beschleunigung X-Achse - Buffer 4: Beschleunigung Y-Achse - Buffer 5: Beschleunigung Z-Achse - */ - - - //Nachricht zum GCS senden - // DOWNLINK_SEND_ArduIMU(DefaultChannel, &ArduIMU_data[0], &ArduIMU_data[1], &ArduIMU_data[2], &ArduIMU_data[3], &ArduIMU_data[4], &ArduIMU_data[5]); - - // DOWNLINK_SEND_DEBUG_ZHAW(DefaultChannel, &airspeed_mode , &altitude_mode ,&amsys_baro, &amsys_baro, &amsys_airspeed_scaliert, &amsys_baro_scaliert); -} - -void IMU_Daten_verarbeiten( void ) { - //Empfangene Byts zusammenfügen und bereitstellen - recievedData[0] = (i2c0_buf[1]<<8) | i2c0_buf[0]; - recievedData[1] = (i2c0_buf[3]<<8) | i2c0_buf[2]; - recievedData[2] = (i2c0_buf[5]<<8) | i2c0_buf[4]; - recievedData[3] = (i2c0_buf[7]<<8) | i2c0_buf[6]; - recievedData[4] = (i2c0_buf[9]<<8) | i2c0_buf[8]; - recievedData[5] = (i2c0_buf[11]<<8) | i2c0_buf[10]; - - //Floats zurück transformieren. Transformation ist auf ArduIMU programmiert. - ArduIMU_data[0] = (float) (recievedData[0] / (float)100); - ArduIMU_data[1] = (float) (recievedData[1] / (float)100); - ArduIMU_data[2] = (float) (recievedData[2] / (float)100); - ArduIMU_data[3] = (float) (recievedData[3] / (float)100); - ArduIMU_data[4] = (float) (recievedData[4] / (float)100); - ArduIMU_data[5] = (float) (recievedData[5] / (float)100); - - // test - estimator_phi = (float)ArduIMU_data[0]*0.01745329252 - arduimu_roll_neutral; - estimator_theta = (float)ArduIMU_data[1]*0.01745329252 - arduimu_pitch_neutral; - imu_daten_angefordert = FALSE; -} - - diff --git a/sw/airborne/modules/ins/ins_arduimu.c b/sw/airborne/modules/ins/ins_arduimu.c new file mode 100644 index 0000000000..4750777485 --- /dev/null +++ b/sw/airborne/modules/ins/ins_arduimu.c @@ -0,0 +1,218 @@ +/* +C Datei für die Einbindung eines ArduIMU's +Autoren@ZHAW: schmiemi + chaneren +*/ + + +#include +#include "ins_arduimu.h" +#include "main_fbw.h" +#include "i2c.h" + +// test +#include "estimator.h" + +// für das Senden von GPS-Daten an den ArduIMU +#include "gps.h" +int32_t GPS_Data[13]; + +#ifndef ARDUIMU_I2C_DEV +#define ARDUIMU_I2C_DEV i2c0 +#endif + +// 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. +#define ArduIMU_SLAVE_ADDR 0x22 + +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif + +#include "uart.h" +#include "messages.h" +#include "downlink.h" + +struct i2c_transaction ardu_gps_trans; +struct i2c_transaction ardu_ins_trans; + +static int16_t recievedData[NB_DATA]; +float ArduIMU_data[NB_DATA]; +int8_t messageNr; +int8_t imu_daten_angefordert; +int8_t gps_daten_abgespeichert; +int8_t gps_daten_versendet_msg1; +int8_t gps_daten_versendet_msg2; + +float ins_roll_neutral; +float ins_pitch_neutral; +//float pitch_of_throttle_gain; +float throttle_slew; + +void ArduIMU_init( void ) { + ardu_gps_trans.buf[0] = 0; + ardu_gps_trans.buf[1] = 0; + messageNr = 0; + imu_daten_angefordert = FALSE; + gps_daten_abgespeichert = FALSE; + gps_daten_versendet_msg1 = FALSE; + gps_daten_versendet_msg2 = FALSE; + ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; + ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; +// pitch_of_throttle_gain = PITCH_OF_THROTTLE_GAIN; + throttle_slew = V_CTL_THROTTLE_SLEW; +} + + +void ArduIMU_periodicGPS( void ) { + + if ( gps_daten_versendet_msg1 == TRUE && gps_daten_versendet_msg2 == TRUE ) { + gps_daten_abgespeichert = FALSE; + } + + if( imu_daten_angefordert == TRUE ){ + IMU_Daten_verarbeiten(); + } + + if ( gps_daten_abgespeichert == FALSE ) { + //posllh + GPS_Data [0] = gps_itow; + GPS_Data [1] = gps_lon; + GPS_Data [2] = gps_lat; + GPS_Data [3] = gps_alt; //höhe über elipsoid + GPS_Data [4] = gps_hmsl; //höhe über sea level + //velend + GPS_Data [5] = gps_speed_3d; //speed 3D + GPS_Data [6] = gps_gspeed; //ground speed + GPS_Data [7] = gps_course * 100000; //Kurs + //status + GPS_Data [8] = gps_mode; //fix + GPS_Data [9] = gps_status_flags; //flags + //sol + GPS_Data [10] = gps_mode; //fix + GPS_Data [11] = gps_sol_flags; //flags + GPS_Data [12] = gps_ecefVZ; //ecefVZ + GPS_Data [13] = gps_numSV; + + gps_daten_abgespeichert = TRUE; + } + + if(messageNr == 0) { + + //test für 32bit in byte packete abzupacken: + //GPS_Data [0] = -1550138773; + + ardu_gps_trans.buf[0] = 0; //message Nr = 0 --> itow bis ground speed + ardu_gps_trans.buf[1] = (uint8_t) GPS_Data[0]; //itow + ardu_gps_trans.buf[2] = (uint8_t) (GPS_Data[0] >>8); + ardu_gps_trans.buf[3] = (uint8_t) (GPS_Data[0] >>16); + ardu_gps_trans.buf[4] = (uint8_t) (GPS_Data[0] >>24); + ardu_gps_trans.buf[5] = (uint8_t) GPS_Data[1]; //lon + ardu_gps_trans.buf[6] = (uint8_t) (GPS_Data[1] >>8); + ardu_gps_trans.buf[7] = (uint8_t) (GPS_Data[1] >>16); + ardu_gps_trans.buf[8] = (uint8_t) (GPS_Data[1] >>24); + ardu_gps_trans.buf[9] = (uint8_t) GPS_Data[2]; //lat + ardu_gps_trans.buf[10] = (uint8_t) (GPS_Data[2] >>8); + ardu_gps_trans.buf[11] = (uint8_t) (GPS_Data[2] >>16); + ardu_gps_trans.buf[12] = (uint8_t) (GPS_Data[2] >>24); + ardu_gps_trans.buf[13] = (uint8_t) GPS_Data[3]; //height + ardu_gps_trans.buf[14] = (uint8_t) (GPS_Data[3] >>8); + ardu_gps_trans.buf[15] = (uint8_t) (GPS_Data[3] >>16); + ardu_gps_trans.buf[16] = (uint8_t) (GPS_Data[3] >>24); + ardu_gps_trans.buf[17] = (uint8_t) GPS_Data[4]; //hmsl + ardu_gps_trans.buf[18] = (uint8_t) (GPS_Data[4] >>8); + ardu_gps_trans.buf[19] = (uint8_t) (GPS_Data[4] >>16); + ardu_gps_trans.buf[20] = (uint8_t) (GPS_Data[4] >>24); + ardu_gps_trans.buf[21] = (uint8_t) GPS_Data[5]; //speed + ardu_gps_trans.buf[22] = (uint8_t) (GPS_Data[5] >>8); + ardu_gps_trans.buf[23] = (uint8_t) (GPS_Data[5] >>16); + ardu_gps_trans.buf[24] = (uint8_t) (GPS_Data[5] >>24); + ardu_gps_trans.buf[25] = (uint8_t) GPS_Data[6]; //gspeed + ardu_gps_trans.buf[26] = (uint8_t) (GPS_Data[6] >>8); + ardu_gps_trans.buf[27] = (uint8_t) (GPS_Data[6] >>16); + ardu_gps_trans.buf[28] = (uint8_t) (GPS_Data[6] >>24); + I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 28); + + gps_daten_versendet_msg1 = TRUE; + messageNr =1; + } + else { + + ardu_gps_trans.buf[0] = 1; //message Nr = 1 --> ground course, ecefVZ, numSV, Fix, flags, fix, flags + ardu_gps_trans.buf[1] = GPS_Data[7]; //ground course + ardu_gps_trans.buf[2] = (GPS_Data[7] >>8); + ardu_gps_trans.buf[3] = (GPS_Data[7] >>16); + ardu_gps_trans.buf[4] = (GPS_Data[7] >>24); + ardu_gps_trans.buf[5] = GPS_Data[12]; //ecefVZ + ardu_gps_trans.buf[6] = (GPS_Data[12] >>8); + ardu_gps_trans.buf[7] = (GPS_Data[12] >>16); + ardu_gps_trans.buf[8] = (GPS_Data[12] >>24); + ardu_gps_trans.buf[9] = GPS_Data[13]; //numSV + ardu_gps_trans.buf[10] = GPS_Data[8]; //status gps fix + ardu_gps_trans.buf[11] = GPS_Data[9]; //status flags + ardu_gps_trans.buf[12] = GPS_Data[10]; //sol gps fix + ardu_gps_trans.buf[13] = GPS_Data[11]; //sol flags + I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 13); + + gps_daten_versendet_msg2 = TRUE; + messageNr = 0; + } + + //DOWNLINK_SEND_DEBUG_ZHAW(DefaultChannel, &gps_mode , &gps_numSV ,&gps_alt , &gps_hmsl , &gps_itow, &gps_speed_3d); +} + +void ArduIMU_periodic( void ) { +//Frequenz des Aufrufs wird in conf/modules/ArduIMU.xml festgelegt. + + //I2C-Nachricht anfordern an Slave Adresse, erwartete Anzahl Byte, Status + if (imu_daten_angefordert == TRUE) { + IMU_Daten_verarbeiten(); + } + I2CReceive(ARDUIMU_I2C_DEV, ardu_ins_trans, ArduIMU_SLAVE_ADDR, 12); + + imu_daten_angefordert = TRUE; + /* + Buffer O: Roll + Buffer 1: Pitch + Buffer 2: Yaw + Buffer 3: Beschleunigung X-Achse + Buffer 4: Beschleunigung Y-Achse + Buffer 5: Beschleunigung Z-Achse + */ + + + //Nachricht zum GCS senden + // DOWNLINK_SEND_ArduIMU(DefaultChannel, &ArduIMU_data[0], &ArduIMU_data[1], &ArduIMU_data[2], &ArduIMU_data[3], &ArduIMU_data[4], &ArduIMU_data[5]); + + // DOWNLINK_SEND_DEBUG_ZHAW(DefaultChannel, &airspeed_mode , &altitude_mode ,&amsys_baro, &amsys_baro, &amsys_airspeed_scaliert, &amsys_baro_scaliert); +} + +void IMU_Daten_verarbeiten( void ) { + //Empfangene Byts zusammenfügen und bereitstellen + recievedData[0] = (ardu_ins_trans.buf[1]<<8) | ardu_ins_trans.buf[0]; + recievedData[1] = (ardu_ins_trans.buf[3]<<8) | ardu_ins_trans.buf[2]; + recievedData[2] = (ardu_ins_trans.buf[5]<<8) | ardu_ins_trans.buf[4]; + recievedData[3] = (ardu_ins_trans.buf[7]<<8) | ardu_ins_trans.buf[6]; + recievedData[4] = (ardu_ins_trans.buf[9]<<8) | ardu_ins_trans.buf[8]; + recievedData[5] = (ardu_ins_trans.buf[11]<<8) | ardu_ins_trans.buf[10]; + + //Floats zurück transformieren. Transformation ist auf ArduIMU programmiert. + ArduIMU_data[0] = (float) (recievedData[0] / (float)100); + ArduIMU_data[1] = (float) (recievedData[1] / (float)100); + ArduIMU_data[2] = (float) (recievedData[2] / (float)100); + ArduIMU_data[3] = (float) (recievedData[3] / (float)100); + ArduIMU_data[4] = (float) (recievedData[4] / (float)100); + ArduIMU_data[5] = (float) (recievedData[5] / (float)100); + + // test + estimator_phi = (float)ArduIMU_data[0]*0.01745329252 - ins_roll_neutral; + estimator_theta = (float)ArduIMU_data[1]*0.01745329252 - ins_pitch_neutral; + imu_daten_angefordert = FALSE; + + { + float psi=0; + RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &estimator_phi, &estimator_theta, &psi)); + } +} + diff --git a/sw/airborne/modules/ins/ArduIMU.h b/sw/airborne/modules/ins/ins_arduimu.h similarity index 82% rename from sw/airborne/modules/ins/ArduIMU.h rename to sw/airborne/modules/ins/ins_arduimu.h index 403b992fc5..7e2c31170e 100644 --- a/sw/airborne/modules/ins/ArduIMU.h +++ b/sw/airborne/modules/ins/ins_arduimu.h @@ -9,8 +9,8 @@ extern float ArduIMU_data[NB_DATA]; -extern float arduimu_roll_neutral; -extern float arduimu_pitch_neutral; +extern float ins_roll_neutral; +extern float ins_pitch_neutral; //mixer extern float pitch_of_throttle_gain;