Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules 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 "state.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 
33 #include "mcu_periph/uart.h"
34 #include "messages.h"
36 
39 
47 
50 //float pitch_of_throttle_gain;
52 
53 void ArduIMU_init( void ) {
54  ardu_gps_trans.buf[0] = 0;
55  ardu_gps_trans.buf[1] = 0;
56  messageNr = 0;
63 // pitch_of_throttle_gain = PITCH_OF_THROTTLE_GAIN;
65 }
66 
67 
68 void ArduIMU_periodicGPS( void ) {
69 
72  }
73 
74  if( imu_daten_angefordert == TRUE ){
76  }
77 
78  if ( gps_daten_abgespeichert == FALSE ) {
79  //posllh
80  GPS_Data [0] = gps.tow;
81  GPS_Data [1] = gps.lla_pos.lon;
82  GPS_Data [2] = gps.lla_pos.lat;
83  GPS_Data [3] = gps.lla_pos.alt; //höhe über elipsoid
84  GPS_Data [4] = gps.hmsl; //höhe über sea level
85  //velend
86  GPS_Data [5] = gps.speed_3d; //speed 3D
87  GPS_Data [6] = gps.gspeed; //ground speed
88  GPS_Data [7] = DegOfRad(gps.course / 1e6); //Kurs
89  //status
90  GPS_Data [8] = gps.fix; //fix
91  GPS_Data [9] = gps_ubx.status_flags; //flags
92  //sol
93  GPS_Data [10] = gps.fix; //fix
94  //GPS_Data [11] = gps_ubx.sol_flags; //flags
95  GPS_Data [12] = -gps.ned_vel.z;
96  GPS_Data [13] = gps.num_sv;
97 
99  }
100 
101  if(messageNr == 0) {
102 
103  //test für 32bit in byte packete abzupacken:
104  //GPS_Data [0] = -1550138773;
105 
106  ardu_gps_trans.buf[0] = 0; //message Nr = 0 --> itow bis ground speed
107  ardu_gps_trans.buf[1] = (uint8_t) GPS_Data[0]; //itow
108  ardu_gps_trans.buf[2] = (uint8_t) (GPS_Data[0] >>8);
109  ardu_gps_trans.buf[3] = (uint8_t) (GPS_Data[0] >>16);
110  ardu_gps_trans.buf[4] = (uint8_t) (GPS_Data[0] >>24);
111  ardu_gps_trans.buf[5] = (uint8_t) GPS_Data[1]; //lon
112  ardu_gps_trans.buf[6] = (uint8_t) (GPS_Data[1] >>8);
113  ardu_gps_trans.buf[7] = (uint8_t) (GPS_Data[1] >>16);
114  ardu_gps_trans.buf[8] = (uint8_t) (GPS_Data[1] >>24);
115  ardu_gps_trans.buf[9] = (uint8_t) GPS_Data[2]; //lat
116  ardu_gps_trans.buf[10] = (uint8_t) (GPS_Data[2] >>8);
117  ardu_gps_trans.buf[11] = (uint8_t) (GPS_Data[2] >>16);
118  ardu_gps_trans.buf[12] = (uint8_t) (GPS_Data[2] >>24);
119  ardu_gps_trans.buf[13] = (uint8_t) GPS_Data[3]; //height
120  ardu_gps_trans.buf[14] = (uint8_t) (GPS_Data[3] >>8);
121  ardu_gps_trans.buf[15] = (uint8_t) (GPS_Data[3] >>16);
122  ardu_gps_trans.buf[16] = (uint8_t) (GPS_Data[3] >>24);
123  ardu_gps_trans.buf[17] = (uint8_t) GPS_Data[4]; //hmsl
124  ardu_gps_trans.buf[18] = (uint8_t) (GPS_Data[4] >>8);
125  ardu_gps_trans.buf[19] = (uint8_t) (GPS_Data[4] >>16);
126  ardu_gps_trans.buf[20] = (uint8_t) (GPS_Data[4] >>24);
127  ardu_gps_trans.buf[21] = (uint8_t) GPS_Data[5]; //speed
128  ardu_gps_trans.buf[22] = (uint8_t) (GPS_Data[5] >>8);
129  ardu_gps_trans.buf[23] = (uint8_t) (GPS_Data[5] >>16);
130  ardu_gps_trans.buf[24] = (uint8_t) (GPS_Data[5] >>24);
131  ardu_gps_trans.buf[25] = (uint8_t) GPS_Data[6]; //gspeed
132  ardu_gps_trans.buf[26] = (uint8_t) (GPS_Data[6] >>8);
133  ardu_gps_trans.buf[27] = (uint8_t) (GPS_Data[6] >>16);
134  ardu_gps_trans.buf[28] = (uint8_t) (GPS_Data[6] >>24);
136 
138  messageNr =1;
139  }
140  else {
141 
142  ardu_gps_trans.buf[0] = 1; //message Nr = 1 --> ground course, ecefVZ, numSV, Fix, flags, fix, flags
143  ardu_gps_trans.buf[1] = GPS_Data[7]; //ground course
144  ardu_gps_trans.buf[2] = (GPS_Data[7] >>8);
145  ardu_gps_trans.buf[3] = (GPS_Data[7] >>16);
146  ardu_gps_trans.buf[4] = (GPS_Data[7] >>24);
147  ardu_gps_trans.buf[5] = GPS_Data[12]; //ecefVZ
148  ardu_gps_trans.buf[6] = (GPS_Data[12] >>8);
149  ardu_gps_trans.buf[7] = (GPS_Data[12] >>16);
150  ardu_gps_trans.buf[8] = (GPS_Data[12] >>24);
151  ardu_gps_trans.buf[9] = GPS_Data[13]; //numSV
152  ardu_gps_trans.buf[10] = GPS_Data[8]; //status gps fix
153  ardu_gps_trans.buf[11] = GPS_Data[9]; //status flags
154  ardu_gps_trans.buf[12] = GPS_Data[10]; //sol gps fix
155  ardu_gps_trans.buf[13] = GPS_Data[11]; //sol flags
157 
159  messageNr = 0;
160  }
161 
162  //DOWNLINK_SEND_DEBUG_ZHAW(DefaultChannel, DefaultDevice, &gps_mode , &gps_numSV ,&gps_alt , &gps_hmsl , &gps.tow, &gps_speed_3d);
163 }
164 
165 void ArduIMU_periodic( void ) {
166 //Frequenz des Aufrufs wird in conf/modules/ArduIMU.xml festgelegt.
167 
168  //I2C-Nachricht anfordern an Slave Adresse, erwartete Anzahl Byte, Status
169  if (imu_daten_angefordert == TRUE) {
171  }
173 
175  /*
176  Buffer O: Roll
177  Buffer 1: Pitch
178  Buffer 2: Yaw
179  Buffer 3: Beschleunigung X-Achse
180  Buffer 4: Beschleunigung Y-Achse
181  Buffer 5: Beschleunigung Z-Achse
182  */
183 
184 
185  //Nachricht zum GCS senden
186  // DOWNLINK_SEND_ArduIMU(DefaultChannel, DefaultDevice, &ArduIMU_data[0], &ArduIMU_data[1], &ArduIMU_data[2], &ArduIMU_data[3], &ArduIMU_data[4], &ArduIMU_data[5]);
187 
188  // DOWNLINK_SEND_DEBUG_ZHAW(DefaultChannel, DefaultDevice, &airspeed_mode , &altitude_mode ,&amsys_baro, &amsys_baro, &amsys_airspeed_scaliert, &amsys_baro_scaliert);
189 }
190 
191 void IMU_Daten_verarbeiten( void ) {
192  //Empfangene Byts zusammenfügen und bereitstellen
198  recievedData[5] = (ardu_ins_trans.buf[11]<<8) | ardu_ins_trans.buf[10];
199 
200  //Floats zurück transformieren. Transformation ist auf ArduIMU programmiert.
201  ArduIMU_data[0] = (float) (recievedData[0] / (float)100);
202  ArduIMU_data[1] = (float) (recievedData[1] / (float)100);
203  ArduIMU_data[2] = (float) (recievedData[2] / (float)100);
204  ArduIMU_data[3] = (float) (recievedData[3] / (float)100);
205  ArduIMU_data[4] = (float) (recievedData[4] / (float)100);
206  ArduIMU_data[5] = (float) (recievedData[5] / (float)100);
207 
208  // test
209  struct FloatEulers att;
210  att.phi = (float)ArduIMU_data[0]*0.01745329252 - ins_roll_neutral;
211  att.theta = (float)ArduIMU_data[1]*0.01745329252 - ins_pitch_neutral;
212  att.psi = 0.;
215 
216  RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &att->phi, &att->theta, &att->psi));
217 }
void ArduIMU_periodicGPS(void)
Definition: ins_arduimu.c:35
FBW ( FlyByWire ) process API.
int8_t messageNr
Definition: ins_arduimu.c:42
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:65
static void stateSetNedToBodyEulers_f(struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
Definition: state.h:995
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:69
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
int32_t lat
in degrees*1e7
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:72
int8_t gps_daten_abgespeichert
Definition: ins_arduimu.c:44
static int16_t recievedData[NB_DATA]
Definition: ins_arduimu.c:40
#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:38
I2C transaction structure.
Definition: i2c.h:93
float psi
in radians
#define INS_ROLL_NEUTRAL_DEFAULT
struct GpsUbx gps_ubx
Definition: gps_ubx.c:58
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:67
float theta
in radians
uint8_t fix
status of fix
Definition: gps.h:78
euler angles
float ArduIMU_data[NB_DATA]
ArduIMU simulation.
Definition: ins_arduimu.c:12
#define V_CTL_THROTTLE_SLEW
Definition: energy_ctrl.c:423
#define FALSE
Definition: imu_chimu.h:141
struct i2c_transaction ardu_gps_trans
Definition: ins_arduimu.c:37
uint32_t tow
GPS time of week in ms.
Definition: gps.h:80
int32_t z
Down.
uint8_t status_flags
Definition: gps_ubx.h:56
int8_t gps_daten_versendet_msg1
Definition: ins_arduimu.c:45
int16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:70
float phi
in radians
Device independent GPS code (interface)
bool_t i2c_transmit(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint8_t len)
Submit a write only transaction.
Definition: i2c.c:238
int8_t gps_daten_versendet_msg2
Definition: ins_arduimu.c:46
#define INS_PITCH_NEUTRAL_DEFAULT
signed short int16_t
Definition: types.h:17
void IMU_Daten_verarbeiten(void)
Definition: ins_arduimu.c:36
int32_t alt
in millimeters above WGS84 reference ellipsoid
int8_t imu_daten_angefordert
Definition: ins_arduimu.c:43
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]
Transaction buffer With I2C_BUF_LEN number of bytes.
Definition: i2c.h:122
int16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:71
void ArduIMU_periodic(void)
Definition: ins_arduimu.c:26
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
int32_t lon
in degrees*1e7
#define NB_DATA
Definition: generic_com.c:38
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:41
bool_t i2c_receive(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint16_t len)
Submit a read only transaction.
Definition: i2c.c:248
#define ARDUIMU_I2C_DEV
Definition: ins_arduimu.c:24
float throttle_slew
Definition: ins_arduimu.c:19
Architecture independent I2C (Inter-Integrated Circuit Bus) API.
uint8_t num_sv
number of sat in fix
Definition: gps.h:77