Paparazzi UAS  v5.15_devel-81-gd13dafb
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces 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 "pprzlink/messages.h"
36 
39 
47 
50 //float pitch_of_throttle_gain;
52 
53 void ArduIMU_init(void)
54 {
55  ardu_gps_trans.buf[0] = 0;
56  ardu_gps_trans.buf[1] = 0;
57  messageNr = 0;
58  imu_daten_angefordert = false;
64 // pitch_of_throttle_gain = PITCH_OF_THROTTLE_GAIN;
66 }
67 
68 
70 {
71 
74  }
75 
76  if (imu_daten_angefordert == TRUE) {
78  }
79 
81  //posllh
82  GPS_Data [0] = gps.tow;
83  GPS_Data [1] = gps.lla_pos.lon;
84  GPS_Data [2] = gps.lla_pos.lat;
85  GPS_Data [3] = gps.lla_pos.alt; //höhe über elipsoid
86  GPS_Data [4] = gps.hmsl; //höhe über sea level
87  //velend
88  GPS_Data [5] = gps.speed_3d; //speed 3D
89  GPS_Data [6] = gps.gspeed; //ground speed
90  GPS_Data [7] = DegOfRad(gps.course / 1e6); //Kurs
91  //status
92  GPS_Data [8] = gps.fix; //fix
93  GPS_Data [9] = gps_ubx.status_flags; //flags
94  //sol
95  GPS_Data [10] = gps.fix; //fix
96  //GPS_Data [11] = gps_ubx.sol_flags; //flags
97  GPS_Data [12] = -gps.ned_vel.z;
98  GPS_Data [13] = gps.num_sv;
99 
101  }
102 
103  if (messageNr == 0) {
104 
105  //test für 32bit in byte packete abzupacken:
106  //GPS_Data [0] = -1550138773;
107 
108  ardu_gps_trans.buf[0] = 0; //message Nr = 0 --> itow bis ground speed
109  ardu_gps_trans.buf[1] = (uint8_t) GPS_Data[0]; //itow
110  ardu_gps_trans.buf[2] = (uint8_t)(GPS_Data[0] >> 8);
111  ardu_gps_trans.buf[3] = (uint8_t)(GPS_Data[0] >> 16);
112  ardu_gps_trans.buf[4] = (uint8_t)(GPS_Data[0] >> 24);
113  ardu_gps_trans.buf[5] = (uint8_t) GPS_Data[1]; //lon
114  ardu_gps_trans.buf[6] = (uint8_t)(GPS_Data[1] >> 8);
115  ardu_gps_trans.buf[7] = (uint8_t)(GPS_Data[1] >> 16);
116  ardu_gps_trans.buf[8] = (uint8_t)(GPS_Data[1] >> 24);
117  ardu_gps_trans.buf[9] = (uint8_t) GPS_Data[2]; //lat
118  ardu_gps_trans.buf[10] = (uint8_t)(GPS_Data[2] >> 8);
119  ardu_gps_trans.buf[11] = (uint8_t)(GPS_Data[2] >> 16);
120  ardu_gps_trans.buf[12] = (uint8_t)(GPS_Data[2] >> 24);
121  ardu_gps_trans.buf[13] = (uint8_t) GPS_Data[3]; //height
122  ardu_gps_trans.buf[14] = (uint8_t)(GPS_Data[3] >> 8);
123  ardu_gps_trans.buf[15] = (uint8_t)(GPS_Data[3] >> 16);
124  ardu_gps_trans.buf[16] = (uint8_t)(GPS_Data[3] >> 24);
125  ardu_gps_trans.buf[17] = (uint8_t) GPS_Data[4]; //hmsl
126  ardu_gps_trans.buf[18] = (uint8_t)(GPS_Data[4] >> 8);
127  ardu_gps_trans.buf[19] = (uint8_t)(GPS_Data[4] >> 16);
128  ardu_gps_trans.buf[20] = (uint8_t)(GPS_Data[4] >> 24);
129  ardu_gps_trans.buf[21] = (uint8_t) GPS_Data[5]; //speed
130  ardu_gps_trans.buf[22] = (uint8_t)(GPS_Data[5] >> 8);
131  ardu_gps_trans.buf[23] = (uint8_t)(GPS_Data[5] >> 16);
132  ardu_gps_trans.buf[24] = (uint8_t)(GPS_Data[5] >> 24);
133  ardu_gps_trans.buf[25] = (uint8_t) GPS_Data[6]; //gspeed
134  ardu_gps_trans.buf[26] = (uint8_t)(GPS_Data[6] >> 8);
135  ardu_gps_trans.buf[27] = (uint8_t)(GPS_Data[6] >> 16);
136  ardu_gps_trans.buf[28] = (uint8_t)(GPS_Data[6] >> 24);
138 
140  messageNr = 1;
141  } else {
142 
143  ardu_gps_trans.buf[0] = 1; //message Nr = 1 --> ground course, ecefVZ, numSV, Fix, flags, fix, flags
144  ardu_gps_trans.buf[1] = GPS_Data[7]; //ground course
145  ardu_gps_trans.buf[2] = (GPS_Data[7] >> 8);
146  ardu_gps_trans.buf[3] = (GPS_Data[7] >> 16);
147  ardu_gps_trans.buf[4] = (GPS_Data[7] >> 24);
148  ardu_gps_trans.buf[5] = GPS_Data[12]; //ecefVZ
149  ardu_gps_trans.buf[6] = (GPS_Data[12] >> 8);
150  ardu_gps_trans.buf[7] = (GPS_Data[12] >> 16);
151  ardu_gps_trans.buf[8] = (GPS_Data[12] >> 24);
152  ardu_gps_trans.buf[9] = GPS_Data[13]; //numSV
153  ardu_gps_trans.buf[10] = GPS_Data[8]; //status gps fix
154  ardu_gps_trans.buf[11] = GPS_Data[9]; //status flags
155  ardu_gps_trans.buf[12] = GPS_Data[10]; //sol gps fix
156  ardu_gps_trans.buf[13] = GPS_Data[11]; //sol flags
158 
160  messageNr = 0;
161  }
162 
163  //DOWNLINK_SEND_DEBUG_ZHAW(DefaultChannel, DefaultDevice, &gps_mode , &gps_numSV ,&gps_alt , &gps_hmsl , &gps.tow, &gps_speed_3d);
164 }
165 
167 {
168 //Frequenz des Aufrufs wird in conf/modules/ArduIMU.xml festgelegt.
169 
170  //I2C-Nachricht anfordern an Slave Adresse, erwartete Anzahl Byte, Status
171  if (imu_daten_angefordert == TRUE) {
173  }
175 
176  imu_daten_angefordert = true;
177  /*
178  Buffer O: Roll
179  Buffer 1: Pitch
180  Buffer 2: Yaw
181  Buffer 3: Beschleunigung X-Achse
182  Buffer 4: Beschleunigung Y-Achse
183  Buffer 5: Beschleunigung Z-Achse
184  */
185 
186 
187  //Nachricht zum GCS senden
188  // DOWNLINK_SEND_ArduIMU(DefaultChannel, DefaultDevice, &ArduIMU_data[0], &ArduIMU_data[1], &ArduIMU_data[2], &ArduIMU_data[3], &ArduIMU_data[4], &ArduIMU_data[5]);
189 
190  // DOWNLINK_SEND_DEBUG_ZHAW(DefaultChannel, DefaultDevice, &airspeed_mode , &altitude_mode ,&amsys_baro, &amsys_baro, &amsys_airspeed_scaliert, &amsys_baro_scaliert);
191 }
192 
194 {
195  //Empfangene Byts zusammenfügen und bereitstellen
196  recievedData[0] = (ardu_ins_trans.buf[1] << 8) | ardu_ins_trans.buf[0];
197  recievedData[1] = (ardu_ins_trans.buf[3] << 8) | ardu_ins_trans.buf[2];
198  recievedData[2] = (ardu_ins_trans.buf[5] << 8) | ardu_ins_trans.buf[4];
199  recievedData[3] = (ardu_ins_trans.buf[7] << 8) | ardu_ins_trans.buf[6];
200  recievedData[4] = (ardu_ins_trans.buf[9] << 8) | ardu_ins_trans.buf[8];
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  struct FloatEulers att;
213  att.phi = (float)ArduIMU_data[0] * 0.01745329252 - ins_roll_neutral;
214  att.theta = (float)ArduIMU_data[1] * 0.01745329252 - ins_pitch_neutral;
215  att.psi = 0.;
216  imu_daten_angefordert = false;
218  uint8_t arduimu_id = 102;
219 
220  RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &att->phi, &att->theta, &att->psi, &arduimu_id));
221 }
void ArduIMU_periodicGPS(void)
Definition: ins_arduimu.c:36
int8_t messageNr
Definition: ins_arduimu.c:42
static void stateSetNedToBodyEulers_f(struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
Definition: state.h:1105
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
int8_t gps_daten_abgespeichert
Definition: ins_arduimu.c:44
float phi
in radians
volatile uint8_t buf[I2C_BUF_LEN]
Transaction buffer With I2C_BUF_LEN number of bytes.
Definition: i2c.h:122
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
float psi
in radians
struct GpsUbx gps_ubx
Definition: gps_ubx.c:73
uint16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:98
#define INS_ROLL_NEUTRAL_DEFAULT
Definition: ahrs_sim.c:43
int32_t z
Down.
float ArduIMU_data[NB_DATA]
ArduIMU simulation.
Definition: ins_arduimu.c:12
#define V_CTL_THROTTLE_SLEW
Definition: energy_ctrl.c:447
#define FALSE
Definition: std.h:5
int32_t alt
in millimeters above WGS84 reference ellipsoid
struct i2c_transaction ardu_gps_trans
Definition: ins_arduimu.c:37
euler angles
float theta
in radians
#define TRUE
Definition: std.h:4
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:94
int8_t gps_daten_versendet_msg1
Definition: ins_arduimu.c:45
uint32_t tow
GPS time of week in ms.
Definition: gps.h:109
Device independent GPS code (interface)
int8_t gps_daten_versendet_msg2
Definition: ins_arduimu.c:46
signed short int16_t
Definition: types.h:17
void IMU_Daten_verarbeiten(void)
Definition: ins_arduimu.c:37
int32_t lon
in degrees*1e7
int8_t imu_daten_angefordert
Definition: ins_arduimu.c:43
#define INS_PITCH_NEUTRAL_DEFAULT
Definition: ahrs_sim.c:46
I2C transaction structure.
Definition: i2c.h:93
float ins_pitch_neutral
Definition: ins_arduimu.c:15
bool 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:308
signed long int32_t
Definition: types.h:19
uint8_t status_flags
Definition: gps_ubx.h:68
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 course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:99
bool 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:318
#define NB_DATA
Definition: generic_com.c:38
uint8_t num_sv
number of sat in fix
Definition: gps.h:106
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:97
float ins_roll_neutral
Definition: ins_arduimu.c:14
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:92
FBW ( FlyByWire ) process API.
int32_t GPS_Data[14]
Definition: ins_arduimu.c:21
signed char int8_t
Definition: types.h:15
int32_t lat
in degrees*1e7
uint8_t fix
status of fix
Definition: gps.h:107
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:96
struct GpsState gps
global GPS state
Definition: gps.c:69
#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.