Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
ins_arduimu_basic.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2011 Gautier Hattenberger
3  * based on ArduIMU driver:
4  * Autoren@ZHAW: schmiemi
5  * chaneren
6  *
7  * This file is part of paparazzi.
8  *
9  * paparazzi is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2, or (at your option)
12  * any later version.
13  *
14  * paparazzi is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License
20  * along with paparazzi; see the file COPYING. If not, write to
21  * the Free Software Foundation, 59 Temple Place - Suite 330,
22  * Boston, MA 02111-1307, USA.
23  *
24  */
25 
26 #include <stdbool.h>
28 #include "mcu_periph/i2c.h"
29 
30 // Estimator interface
31 #include "state.h"
32 
33 // GPS data for ArduIMU
34 #include "modules/gps/gps.h"
35 
36 // Command vector for thrust
37 #include "generated/airframe.h"
38 #include "modules/core/commands.h"
39 
40 #define NB_DATA 9
41 
42 #ifndef ARDUIMU_I2C_DEV
43 #define ARDUIMU_I2C_DEV i2c0
44 #endif
45 
46 // Adresse des I2C Slaves: 0001 0110 letztes Bit ist für Read/Write
47 // einzugebende Adresse im ArduIMU ist 0000 1011
48 // da ArduIMU das Read/Write Bit selber anfügt.
49 #define ArduIMU_SLAVE_ADDR 0x22
50 
51 #ifdef ARDUIMU_SYNC_SEND
52 #include "mcu_periph/uart.h"
53 #include "pprzlink/messages.h"
55 #endif
56 
59 
61 
65 
68 
69 // Ask the arduimu to recalibrate the gyros and accels neutrals
70 // After calibration, values are store in the arduimu eeprom
72 
73 // High Accel Flag
74 #define HIGH_ACCEL_LOW_SPEED 15.0
75 #define HIGH_ACCEL_LOW_SPEED_RESUME 4.0 // Hysteresis
76 #define HIGH_ACCEL_HIGH_THRUST (0.8*MAX_PPRZ)
77 #define HIGH_ACCEL_HIGH_THRUST_RESUME (0.1*MAX_PPRZ) // Hysteresis
80 
81 void ArduIMU_init(void)
82 {
86 
89 
93 
94  high_accel_done = false;
95  high_accel_flag = false;
96 }
97 
98 #define FillBufWith32bit(_buf, _index, _value) { \
99  _buf[_index] = (uint8_t) (_value); \
100  _buf[_index+1] = (uint8_t) ((_value) >> 8); \
101  _buf[_index+2] = (uint8_t) ((_value) >> 16); \
102  _buf[_index+3] = (uint8_t) ((_value) >> 24); \
103  }
104 
106 {
107 
108  if (ardu_gps_trans.status != I2CTransDone) { return; }
109 
110 #if USE_HIGH_ACCEL_FLAG
111  // Test for high acceleration:
112  // - low speed
113  // - high thrust
114  float speed = stateGetHorizontalSpeedNorm_f();
115  pprz_t cmd = command_get(COMMAND_THROTTLE);
116  if (speed < HIGH_ACCEL_LOW_SPEED && cmd > HIGH_ACCEL_HIGH_THRUST && !high_accel_done) {
117  high_accel_flag = true;
118  } else {
119  high_accel_flag = false;
120  if (speed > HIGH_ACCEL_LOW_SPEED && !high_accel_done) {
121  high_accel_done = true; // After takeoff, don't use high accel before landing (GS small, Throttle small)
122  }
124  high_accel_done = false; // Activate high accel after landing
125  }
126  }
127 #endif
128 
130  FillBufWith32bit(ardu_gps_trans.buf, 4, (int32_t)gps.gspeed); // ground speed
132  ardu_gps_trans.buf[12] = gps.fix; // status gps fix
133  ardu_gps_trans.buf[13] = (uint8_t)arduimu_calibrate_neutrals; // calibration flag
134  ardu_gps_trans.buf[14] = (uint8_t)
135  high_accel_flag; // high acceleration flag (disable accelerometers in the arduimu filter)
137 
138  // Reset calibration flag
140 }
141 
143 {
144  //Frequence defined in conf/modules/ins_arduimu.xml
145 
148  }
149 
150 }
151 
152 #include "math/pprz_algebra_int.h"
153 /*
154  Buffer O: Roll
155  Buffer 1: Pitch
156  Buffer 2: Yaw
157  Buffer 3: Gyro X
158  Buffer 4: Gyro Y
159  Buffer 5: Gyro Z
160  Buffer 6: Accel X
161  Buffer 7: Accel Y
162  Buffer 8: Accel Z
163  */
164 
165 void ArduIMU_event(void)
166 {
167  // Handle INS I2C event
169  // received data from I2C transaction
170  recievedData[0] = (ardu_ins_trans.buf[1] << 8) | ardu_ins_trans.buf[0];
171  recievedData[1] = (ardu_ins_trans.buf[3] << 8) | ardu_ins_trans.buf[2];
172  recievedData[2] = (ardu_ins_trans.buf[5] << 8) | ardu_ins_trans.buf[4];
173  recievedData[3] = (ardu_ins_trans.buf[7] << 8) | ardu_ins_trans.buf[6];
174  recievedData[4] = (ardu_ins_trans.buf[9] << 8) | ardu_ins_trans.buf[8];
175  recievedData[5] = (ardu_ins_trans.buf[11] << 8) | ardu_ins_trans.buf[10];
176  recievedData[6] = (ardu_ins_trans.buf[13] << 8) | ardu_ins_trans.buf[12];
177  recievedData[7] = (ardu_ins_trans.buf[15] << 8) | ardu_ins_trans.buf[14];
178  recievedData[8] = (ardu_ins_trans.buf[17] << 8) | ardu_ins_trans.buf[16];
179 
180  // Update ArduIMU data
190 
191  // Update estimator
192  stateSetNedToBodyEulers_f(MODULE_INS_ARDUIMU_BASIC_ID, &arduimu_eulers);
193  stateSetBodyRates_f(MODULE_INS_ARDUIMU_BASIC_ID, &arduimu_rates);
194  stateSetAccelNed_f(MODULE_INS_ARDUIMU_BASIC_ID, &((struct NedCoor_f)arduimu_accel));
196 
197 #ifdef ARDUIMU_SYNC_SEND
198  // uint8_t arduimu_id = 102;
199  //RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi, &arduimu_id));
200  RunOnceEvery(15, DOWNLINK_SEND_IMU_GYRO(DefaultChannel, DefaultDevice, &arduimu_rates.p, &arduimu_rates.q,
201  &arduimu_rates.r));
202  RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, DefaultDevice, &arduimu_accel.x, &arduimu_accel.y,
203  &arduimu_accel.z));
204 #endif
205  } else if (ardu_ins_trans.status == I2CTransFailed) {
207  }
208  // Handle GPS I2C event
211  }
212 }
213 
214 void ahrs_update_gps(void)
215 {
216 
217 }
#define INS_PITCH_NEUTRAL_DEFAULT
Definition: ahrs_sim.c:46
#define INS_ROLL_NEUTRAL_DEFAULT
Definition: ahrs_sim.c:43
struct FloatRates arduimu_rates
void ArduIMU_event(void)
float ins_roll_neutral
void ArduIMU_periodicGPS(void)
struct FloatVect3 arduimu_accel
struct FloatEulers arduimu_eulers
ArduIMU simulation.
void ArduIMU_periodic(void)
void ArduIMU_init(void)
float ins_pitch_neutral
bool arduimu_calibrate_neutrals
Hardware independent code for commands handling.
struct GpsState gps
global GPS state
Definition: gps.c:74
Device independent GPS code (interface)
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:99
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:97
uint16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:98
uint8_t fix
status of fix
Definition: gps.h:107
volatile uint8_t buf[I2C_BUF_LEN]
Transaction buffer With I2C_BUF_LEN number of bytes.
Definition: i2c.h:122
enum I2CTransactionStatus status
Transaction status.
Definition: i2c.h:126
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:324
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:334
@ I2CTransSuccess
transaction successfully finished by I2C driver
Definition: i2c.h:57
@ I2CTransFailed
transaction failed
Definition: i2c.h:58
@ I2CTransDone
transaction set to done by user level
Definition: i2c.h:59
I2C transaction structure.
Definition: i2c.h:93
float q
in rad/s
float phi
in radians
float p
in rad/s
float r
in rad/s
float theta
in radians
float psi
in radians
#define FLOAT_EULERS_ZERO(_e)
#define FLOAT_VECT3_ZERO(_v)
#define FLOAT_RATES_ZERO(_r)
euler angles
angular rates
#define RATE_FLOAT_OF_BFP(_ai)
#define ANGLE_FLOAT_OF_BFP(_ai)
#define ACCEL_FLOAT_OF_BFP(_ai)
static void stateSetAccelNed_f(uint16_t id, struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition: state.h:1147
static void stateSetNedToBodyEulers_f(uint16_t id, struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
Definition: state.h:1267
static void stateSetBodyRates_f(uint16_t id, struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1346
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:1076
Architecture independent I2C (Inter-Integrated Circuit Bus) API.
void ahrs_update_gps(void)
#define FillBufWith32bit(_buf, _index, _value)
#define ARDUIMU_I2C_DEV
#define HIGH_ACCEL_LOW_SPEED
static int16_t recievedData[NB_DATA]
bool high_accel_flag
#define HIGH_ACCEL_HIGH_THRUST_RESUME
bool high_accel_done
#define HIGH_ACCEL_HIGH_THRUST
#define ArduIMU_SLAVE_ADDR
struct i2c_transaction ardu_gps_trans
struct i2c_transaction ardu_ins_trans
#define NB_DATA
int16_t pprz_t
Definition: paparazzi.h:6
Paparazzi fixed point algebra.
vector in North East Down coordinates Units: meters
API to get/set the generic vehicle states.
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98