Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
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 #ifndef DOWNLINK_DEVICE
53 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
54 #endif
55 #include "mcu_periph/uart.h"
56 #include "messages.h"
58 #endif
59 
62 
64 
68 
71 
72 // Ask the arduimu to recalibrate the gyros and accels neutrals
73 // After calibration, values are store in the arduimu eeprom
75 
76 // High Accel Flag
77 #define HIGH_ACCEL_LOW_SPEED 15.0
78 #define HIGH_ACCEL_LOW_SPEED_RESUME 4.0 // Hysteresis
79 #define HIGH_ACCEL_HIGH_THRUST (0.8*MAX_PPRZ)
80 #define HIGH_ACCEL_HIGH_THRUST_RESUME (0.1*MAX_PPRZ) // Hysteresis
83 
84 void ArduIMU_init( void ) {
88 
91 
95 
98 }
99 
100 #define FillBufWith32bit(_buf, _index, _value) { \
101  _buf[_index] = (uint8_t) (_value); \
102  _buf[_index+1] = (uint8_t) ((_value) >> 8); \
103  _buf[_index+2] = (uint8_t) ((_value) >> 16); \
104  _buf[_index+3] = (uint8_t) ((_value) >> 24); \
105 }
106 
107 void ArduIMU_periodicGPS( void ) {
108 
109  if (ardu_gps_trans.status != I2CTransDone) { return; }
110 
111 #if USE_HIGH_ACCEL_FLAG
112  // Test for high acceleration:
113  // - low speed
114  // - high thrust
115  float speed = *stateGetHorizontalSpeedNorm_f();
116  if (speed < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE] > HIGH_ACCEL_HIGH_THRUST && !high_accel_done) {
118  } else {
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  }
123  if (speed < HIGH_ACCEL_HIGH_THRUST_RESUME && ap_state->commands[COMMAND_THROTTLE] < HIGH_ACCEL_HIGH_THRUST_RESUME) {
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)high_accel_flag; // high acceleration flag (disable accelerometers in the arduimu filter)
136 
137  // Reset calibration flag
139 }
140 
141 void ArduIMU_periodic( void ) {
142  //Frequence defined in conf/modules/ins_arduimu.xml
143 
146  }
147 
148 }
149 
150 #include "math/pprz_algebra_int.h"
151 /*
152  Buffer O: Roll
153  Buffer 1: Pitch
154  Buffer 2: Yaw
155  Buffer 3: Gyro X
156  Buffer 4: Gyro Y
157  Buffer 5: Gyro Z
158  Buffer 6: Accel X
159  Buffer 7: Accel Y
160  Buffer 8: Accel Z
161  */
162 
163 void ArduIMU_event( void ) {
164  // Handle INS I2C event
166  // received data from I2C transaction
172  recievedData[5] = (ardu_ins_trans.buf[11]<<8) | ardu_ins_trans.buf[10];
173  recievedData[6] = (ardu_ins_trans.buf[13]<<8) | ardu_ins_trans.buf[12];
174  recievedData[7] = (ardu_ins_trans.buf[15]<<8) | ardu_ins_trans.buf[14];
175  recievedData[8] = (ardu_ins_trans.buf[17]<<8) | ardu_ins_trans.buf[16];
176 
177  // Update ArduIMU data
187 
188  // Update estimator
193 
194 #ifdef ARDUIMU_SYNC_SEND
195  //RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi));
196  RunOnceEvery(15, DOWNLINK_SEND_IMU_GYRO(DefaultChannel, DefaultDevice, &arduimu_rates.p, &arduimu_rates.q, &arduimu_rates.r));
197  RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, DefaultDevice, &arduimu_accel.x, &arduimu_accel.y, &arduimu_accel.z));
198 #endif
199  }
200  else if (ardu_ins_trans.status == I2CTransFailed) {
202  }
203  // Handle GPS I2C event
206  }
207 }
208 
209 void ahrs_update_gps( void )
210 {
211 
212 }
#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
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^2
float x
in meters
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)
Definition: i2c.c:85
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
#define HIGH_ACCEL_HIGH_THRUST
enum I2CTransactionStatus status
Definition: i2c.h:83
#define ACCEL_FLOAT_OF_BFP(_ai)
float r
in rad/s^2
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]
Definition: i2c.h:82
#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^2
#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:33
bool_t i2c_receive(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint16_t len)
Definition: i2c.c:95
bool_t arduimu_calibrate_neutrals
Paparazzi fixed point algebra.
Architecture independent I2C (Inter-Integrated Circuit Bus) API.