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_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 "subsystems/gps.h"
35 
36 // Command vector for thrust
37 #include "generated/airframe.h"
38 #include "inter_mcu.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 "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 ) {
85 
88 
92 
95 }
96 
97 #define FillBufWith32bit(_buf, _index, _value) { \
98  _buf[_index] = (uint8_t) (_value); \
99  _buf[_index+1] = (uint8_t) ((_value) >> 8); \
100  _buf[_index+2] = (uint8_t) ((_value) >> 16); \
101  _buf[_index+3] = (uint8_t) ((_value) >> 24); \
102 }
103 
104 void ArduIMU_periodicGPS( void ) {
105 
106  if (ardu_gps_trans.status != I2CTransDone) { return; }
107 
108 #if USE_HIGH_ACCEL_FLAG
109  // Test for high acceleration:
110  // - low speed
111  // - high thrust
112  float speed = *stateGetHorizontalSpeedNorm_f();
113  if (speed < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE] > HIGH_ACCEL_HIGH_THRUST && !high_accel_done) {
115  } else {
117  if (speed > HIGH_ACCEL_LOW_SPEED && !high_accel_done) {
118  high_accel_done = TRUE; // After takeoff, don't use high accel before landing (GS small, Throttle small)
119  }
120  if (speed < HIGH_ACCEL_HIGH_THRUST_RESUME && ap_state->commands[COMMAND_THROTTLE] < HIGH_ACCEL_HIGH_THRUST_RESUME) {
121  high_accel_done = FALSE; // Activate high accel after landing
122  }
123  }
124 #endif
125 
127  FillBufWith32bit(ardu_gps_trans.buf, 4, (int32_t)gps.gspeed); // ground speed
129  ardu_gps_trans.buf[12] = gps.fix; // status gps fix
130  ardu_gps_trans.buf[13] = (uint8_t)arduimu_calibrate_neutrals; // calibration flag
131  ardu_gps_trans.buf[14] = (uint8_t)high_accel_flag; // high acceleration flag (disable accelerometers in the arduimu filter)
133 
134  // Reset calibration flag
136 }
137 
138 void ArduIMU_periodic( void ) {
139  //Frequence defined in conf/modules/ins_arduimu.xml
140 
143  }
144 
145 }
146 
147 #include "math/pprz_algebra_int.h"
148 /*
149  Buffer O: Roll
150  Buffer 1: Pitch
151  Buffer 2: Yaw
152  Buffer 3: Gyro X
153  Buffer 4: Gyro Y
154  Buffer 5: Gyro Z
155  Buffer 6: Accel X
156  Buffer 7: Accel Y
157  Buffer 8: Accel Z
158  */
159 
160 void ArduIMU_event( void ) {
161  // Handle INS I2C event
163  // received data from I2C transaction
169  recievedData[5] = (ardu_ins_trans.buf[11]<<8) | ardu_ins_trans.buf[10];
170  recievedData[6] = (ardu_ins_trans.buf[13]<<8) | ardu_ins_trans.buf[12];
171  recievedData[7] = (ardu_ins_trans.buf[15]<<8) | ardu_ins_trans.buf[14];
172  recievedData[8] = (ardu_ins_trans.buf[17]<<8) | ardu_ins_trans.buf[16];
173 
174  // Update ArduIMU data
184 
185  // Update estimator
190 
191 #ifdef ARDUIMU_SYNC_SEND
192  //RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi));
193  RunOnceEvery(15, DOWNLINK_SEND_IMU_GYRO(DefaultChannel, DefaultDevice, &arduimu_rates.p, &arduimu_rates.q, &arduimu_rates.r));
194  RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, DefaultDevice, &arduimu_accel.x, &arduimu_accel.y, &arduimu_accel.z));
195 #endif
196  }
197  else if (ardu_ins_trans.status == I2CTransFailed) {
199  }
200  // Handle GPS I2C event
203  }
204 }
205 
206 void ahrs_update_gps( void )
207 {
208 
209 }
#define FLOAT_RATES_ZERO(_r)
void ahrs_update_gps(void)
Update AHRS state with GPS measurements.
static int16_t recievedData[NB_DATA]
#define ARDUIMU_I2C_DEV
static void stateSetNedToBodyEulers_f(struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
Definition: state.h:995
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:72
#define ANGLE_FLOAT_OF_BFP(_ai)
struct i2c_transaction ardu_gps_trans
transaction successfully finished by I2C driver
Definition: i2c.h:57
I2C transaction structure.
Definition: i2c.h:93
angular rates
bool_t high_accel_done
#define FillBufWith32bit(_buf, _index, _value)
float psi
in radians
#define RATE_FLOAT_OF_BFP(_ai)
float y
in meters
static float * stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:854
#define INS_ROLL_NEUTRAL_DEFAULT
float theta
in radians
uint8_t fix
status of fix
Definition: gps.h:78
euler angles
struct FloatVect3 arduimu_accel
#define FALSE
Definition: imu_chimu.h:141
struct FloatEulers arduimu_eulers
ArduIMU simulation.
void ArduIMU_init(void)
float p
in rad/s
float x
in meters
transaction set to done by user level
Definition: i2c.h:59
int16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:70
vector in North East Down coordinates Units: meters
float phi
in radians
Device independent GPS code (interface)
struct FloatRates arduimu_rates
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
float z
in meters
#define ArduIMU_SLAVE_ADDR
#define INS_PITCH_NEUTRAL_DEFAULT
signed short int16_t
Definition: types.h:17
#define HIGH_ACCEL_HIGH_THRUST_RESUME
transaction failed
Definition: i2c.h:58
#define HIGH_ACCEL_HIGH_THRUST
enum I2CTransactionStatus status
Transaction status.
Definition: i2c.h:126
#define ACCEL_FLOAT_OF_BFP(_ai)
float r
in rad/s
signed long int32_t
Definition: types.h:19
float ins_roll_neutral
#define TRUE
Definition: imu_chimu.h:144
void ArduIMU_periodic(void)
volatile uint8_t buf[I2C_BUF_LEN]
Transaction buffer With I2C_BUF_LEN number of bytes.
Definition: i2c.h:122
#define FLOAT_EULERS_ZERO(_e)
void ArduIMU_periodicGPS(void)
int16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:71
unsigned char uint8_t
Definition: types.h:14
pprz_t commands[COMMANDS_NB]
Storage of intermediate command values.
Definition: commands.c:30
API to get/set the generic vehicle states.
bool_t high_accel_flag
struct i2c_transaction ardu_ins_trans
float ins_pitch_neutral
float q
in rad/s
#define NB_DATA
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1062
void ArduIMU_event(void)
#define FLOAT_VECT3_ZERO(_v)
static void stateSetAccelNed_f(struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition: state.h:908
#define HIGH_ACCEL_LOW_SPEED
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
bool_t arduimu_calibrate_neutrals
Paparazzi fixed point algebra.
Architecture independent I2C (Inter-Integrated Circuit Bus) API.