Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
ins_arduimu.c
Go to the documentation of this file.
1 /*
2 C Datei für die Einbindung eines ArduIMU's
3 Autoren@ZHAW: schmiemi
4  chaneren
5 */
6 
7 
8 #include <stdbool.h>
11 #include "mcu_periph/i2c.h"
12 
13 // test
14 #include "estimator.h"
15 
16 // für das Senden von GPS-Daten an den ArduIMU
17 #ifndef UBX
18 #error "currently only compatible with uBlox GPS modules"
19 #endif
20 #include "subsystems/gps.h"
22 
23 #ifndef ARDUIMU_I2C_DEV
24 #define ARDUIMU_I2C_DEV i2c0
25 #endif
26 
27 // Adresse des I2C Slaves: 0001 0110 letztes Bit ist für Read/Write
28 // einzugebende Adresse im ArduIMU ist 0000 1011
29 //da ArduIMU das Read/Write Bit selber anfügt.
30 #define ArduIMU_SLAVE_ADDR 0x22
31 
32 #ifndef DOWNLINK_DEVICE
33 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
34 #endif
35 
36 #include "mcu_periph/uart.h"
37 #include "messages.h"
39 
42 
50 
53 //float pitch_of_throttle_gain;
55 
56 void ArduIMU_init( void ) {
57  ardu_gps_trans.buf[0] = 0;
58  ardu_gps_trans.buf[1] = 0;
59  messageNr = 0;
64  ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
65  ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
66 // pitch_of_throttle_gain = PITCH_OF_THROTTLE_GAIN;
68 }
69 
70 
71 void ArduIMU_periodicGPS( void ) {
72 
75  }
76 
77  if( imu_daten_angefordert == TRUE ){
79  }
80 
81  if ( gps_daten_abgespeichert == FALSE ) {
82  //posllh
83  GPS_Data [0] = gps.tow;
84  GPS_Data [1] = gps.lla_pos.lon;
85  GPS_Data [2] = gps.lla_pos.lat;
86  GPS_Data [3] = gps.lla_pos.alt; //höhe über elipsoid
87  GPS_Data [4] = gps.hmsl; //höhe über sea level
88  //velend
89  GPS_Data [5] = gps.speed_3d; //speed 3D
90  GPS_Data [6] = gps.gspeed; //ground speed
91  GPS_Data [7] = DegOfRad(gps.course / 1e6); //Kurs
92  //status
93  GPS_Data [8] = gps.fix; //fix
94  GPS_Data [9] = gps_ubx.status_flags; //flags
95  //sol
96  GPS_Data [10] = gps.fix; //fix
97  //GPS_Data [11] = gps_ubx.sol_flags; //flags
98  GPS_Data [12] = -gps.ned_vel.z;
99  GPS_Data [13] = gps.num_sv;
100 
102  }
103 
104  if(messageNr == 0) {
105 
106  //test für 32bit in byte packete abzupacken:
107  //GPS_Data [0] = -1550138773;
108 
109  ardu_gps_trans.buf[0] = 0; //message Nr = 0 --> itow bis ground speed
110  ardu_gps_trans.buf[1] = (uint8_t) GPS_Data[0]; //itow
111  ardu_gps_trans.buf[2] = (uint8_t) (GPS_Data[0] >>8);
112  ardu_gps_trans.buf[3] = (uint8_t) (GPS_Data[0] >>16);
113  ardu_gps_trans.buf[4] = (uint8_t) (GPS_Data[0] >>24);
114  ardu_gps_trans.buf[5] = (uint8_t) GPS_Data[1]; //lon
115  ardu_gps_trans.buf[6] = (uint8_t) (GPS_Data[1] >>8);
116  ardu_gps_trans.buf[7] = (uint8_t) (GPS_Data[1] >>16);
117  ardu_gps_trans.buf[8] = (uint8_t) (GPS_Data[1] >>24);
118  ardu_gps_trans.buf[9] = (uint8_t) GPS_Data[2]; //lat
119  ardu_gps_trans.buf[10] = (uint8_t) (GPS_Data[2] >>8);
120  ardu_gps_trans.buf[11] = (uint8_t) (GPS_Data[2] >>16);
121  ardu_gps_trans.buf[12] = (uint8_t) (GPS_Data[2] >>24);
122  ardu_gps_trans.buf[13] = (uint8_t) GPS_Data[3]; //height
123  ardu_gps_trans.buf[14] = (uint8_t) (GPS_Data[3] >>8);
124  ardu_gps_trans.buf[15] = (uint8_t) (GPS_Data[3] >>16);
125  ardu_gps_trans.buf[16] = (uint8_t) (GPS_Data[3] >>24);
126  ardu_gps_trans.buf[17] = (uint8_t) GPS_Data[4]; //hmsl
127  ardu_gps_trans.buf[18] = (uint8_t) (GPS_Data[4] >>8);
128  ardu_gps_trans.buf[19] = (uint8_t) (GPS_Data[4] >>16);
129  ardu_gps_trans.buf[20] = (uint8_t) (GPS_Data[4] >>24);
130  ardu_gps_trans.buf[21] = (uint8_t) GPS_Data[5]; //speed
131  ardu_gps_trans.buf[22] = (uint8_t) (GPS_Data[5] >>8);
132  ardu_gps_trans.buf[23] = (uint8_t) (GPS_Data[5] >>16);
133  ardu_gps_trans.buf[24] = (uint8_t) (GPS_Data[5] >>24);
134  ardu_gps_trans.buf[25] = (uint8_t) GPS_Data[6]; //gspeed
135  ardu_gps_trans.buf[26] = (uint8_t) (GPS_Data[6] >>8);
136  ardu_gps_trans.buf[27] = (uint8_t) (GPS_Data[6] >>16);
137  ardu_gps_trans.buf[28] = (uint8_t) (GPS_Data[6] >>24);
139 
141  messageNr =1;
142  }
143  else {
144 
145  ardu_gps_trans.buf[0] = 1; //message Nr = 1 --> ground course, ecefVZ, numSV, Fix, flags, fix, flags
146  ardu_gps_trans.buf[1] = GPS_Data[7]; //ground course
147  ardu_gps_trans.buf[2] = (GPS_Data[7] >>8);
148  ardu_gps_trans.buf[3] = (GPS_Data[7] >>16);
149  ardu_gps_trans.buf[4] = (GPS_Data[7] >>24);
150  ardu_gps_trans.buf[5] = GPS_Data[12]; //ecefVZ
151  ardu_gps_trans.buf[6] = (GPS_Data[12] >>8);
152  ardu_gps_trans.buf[7] = (GPS_Data[12] >>16);
153  ardu_gps_trans.buf[8] = (GPS_Data[12] >>24);
154  ardu_gps_trans.buf[9] = GPS_Data[13]; //numSV
155  ardu_gps_trans.buf[10] = GPS_Data[8]; //status gps fix
156  ardu_gps_trans.buf[11] = GPS_Data[9]; //status flags
157  ardu_gps_trans.buf[12] = GPS_Data[10]; //sol gps fix
158  ardu_gps_trans.buf[13] = GPS_Data[11]; //sol flags
160 
162  messageNr = 0;
163  }
164 
165  //DOWNLINK_SEND_DEBUG_ZHAW(DefaultChannel, DefaultDevice, &gps_mode , &gps_numSV ,&gps_alt , &gps_hmsl , &gps.tow, &gps_speed_3d);
166 }
167 
168 void ArduIMU_periodic( void ) {
169 //Frequenz des Aufrufs wird in conf/modules/ArduIMU.xml festgelegt.
170 
171  //I2C-Nachricht anfordern an Slave Adresse, erwartete Anzahl Byte, Status
172  if (imu_daten_angefordert == TRUE) {
174  }
176 
178  /*
179  Buffer O: Roll
180  Buffer 1: Pitch
181  Buffer 2: Yaw
182  Buffer 3: Beschleunigung X-Achse
183  Buffer 4: Beschleunigung Y-Achse
184  Buffer 5: Beschleunigung Z-Achse
185  */
186 
187 
188  //Nachricht zum GCS senden
189  // DOWNLINK_SEND_ArduIMU(DefaultChannel, DefaultDevice, &ArduIMU_data[0], &ArduIMU_data[1], &ArduIMU_data[2], &ArduIMU_data[3], &ArduIMU_data[4], &ArduIMU_data[5]);
190 
191  // DOWNLINK_SEND_DEBUG_ZHAW(DefaultChannel, DefaultDevice, &airspeed_mode , &altitude_mode ,&amsys_baro, &amsys_baro, &amsys_airspeed_scaliert, &amsys_baro_scaliert);
192 }
193 
194 void IMU_Daten_verarbeiten( void ) {
195  //Empfangene Byts zusammenfügen und bereitstellen
201  recievedData[5] = (ardu_ins_trans.buf[11]<<8) | ardu_ins_trans.buf[10];
202 
203  //Floats zurück transformieren. Transformation ist auf ArduIMU programmiert.
204  ArduIMU_data[0] = (float) (recievedData[0] / (float)100);
205  ArduIMU_data[1] = (float) (recievedData[1] / (float)100);
206  ArduIMU_data[2] = (float) (recievedData[2] / (float)100);
207  ArduIMU_data[3] = (float) (recievedData[3] / (float)100);
208  ArduIMU_data[4] = (float) (recievedData[4] / (float)100);
209  ArduIMU_data[5] = (float) (recievedData[5] / (float)100);
210 
211  // test
212  estimator_phi = (float)ArduIMU_data[0]*0.01745329252 - ins_roll_neutral;
213  estimator_theta = (float)ArduIMU_data[1]*0.01745329252 - ins_pitch_neutral;
215 
216  {
217  float psi=0;
218  RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &estimator_phi, &estimator_theta, &psi));
219  }
220 }
void ArduIMU_periodicGPS(void)
Definition: ins_arduimu.c:31
FBW ( FlyByWire ) process API.
int8_t messageNr
Definition: ins_arduimu.c:45
struct LlaCoor_i lla_pos
position in LLA (lat,lon: rad*1e7; alt: mm over ellipsoid)
Definition: gps.h:64
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:68
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
int32_t lat
in radians*1e7
#define I2CReceive(_p, _t, _s_addr, _len)
Definition: i2c.h:140
float estimator_theta
pitch angle in rad, + = up
Definition: estimator.c:51
int32_t course
GPS heading in rad*1e7 (CW/north)
Definition: gps.h:71
int8_t gps_daten_abgespeichert
Definition: ins_arduimu.c:47
static int16_t recievedData[NB_DATA]
Definition: ins_arduimu.c:43
#define ArduIMU_SLAVE_ADDR
Definition: ins_arduimu.c:30
void ArduIMU_init(void)
Definition: ins_arduimu.c:25
struct i2c_transaction ardu_ins_trans
Definition: ins_arduimu.c:41
#define I2CTransmit(_p, _t, _s_addr, _len)
Definition: i2c.h:148
float estimator_phi
roll angle in rad, + = right
Definition: estimator.c:49
struct GpsUbx gps_ubx
Definition: gps_ubx.c:81
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:66
uint8_t fix
status of fix
Definition: gps.h:77
float ArduIMU_data[NB_DATA]
ArduIMU simulation.
Definition: ins_arduimu.c:12
#define V_CTL_THROTTLE_SLEW
Definition: energy_ctrl.c:405
#define FALSE
Definition: imu_chimu.h:141
struct i2c_transaction ardu_gps_trans
Definition: ins_arduimu.c:40
uint32_t tow
GPS time of week in ms.
Definition: gps.h:79
uint8_t status_flags
Definition: gps_ubx.h:56
int8_t gps_daten_versendet_msg1
Definition: ins_arduimu.c:48
int16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:69
Device independent GPS code (interface)
int8_t gps_daten_versendet_msg2
Definition: ins_arduimu.c:49
signed short int16_t
Definition: types.h:17
void IMU_Daten_verarbeiten(void)
Definition: ins_arduimu.c:32
int32_t alt
in millimeters above WGS84 reference ellipsoid
int8_t imu_daten_angefordert
Definition: ins_arduimu.c:46
float ins_pitch_neutral
Definition: ins_arduimu.c:15
signed long int32_t
Definition: types.h:19
#define TRUE
Definition: imu_chimu.h:144
volatile uint8_t buf[I2C_BUF_LEN]
Definition: i2c.h:46
int16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:70
void ArduIMU_periodic(void)
Definition: ins_arduimu.c:26
unsigned char uint8_t
Definition: types.h:14
int32_t lon
in radians*1e7
#define NB_DATA
Definition: generic_com.c:38
State estimation, fusioning sensors.
float ins_roll_neutral
Definition: ins_arduimu.c:14
int32_t GPS_Data[14]
Definition: ins_arduimu.c:21
signed char int8_t
Definition: types.h:15
struct GpsState gps
global GPS state
Definition: gps.c:31
#define ARDUIMU_I2C_DEV
Definition: ins_arduimu.c:24
float throttle_slew
Definition: ins_arduimu.c:19
uint8_t num_sv
number of sat in fix
Definition: gps.h:76