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
lisa_test_itg3200.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 The Paparazzi Team
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
22 /* lisa/L lisa/M
23  * gyro-drdy PC14
24  *
25  *
26  */
27 
28 
29 #include <libopencm3/stm32/f1/gpio.h>
30 #include <libopencm3/stm32/exti.h>
31 #include <libopencm3/stm32/spi.h>
32 
33 #include BOARD_CONFIG
34 #include "mcu.h"
35 #include "mcu_periph/uart.h"
36 #include "mcu_periph/i2c.h"
37 #include "mcu_periph/sys_time.h"
39 #include "std.h"
40 #include "math/pprz_algebra_int.h"
41 
43 #include "my_debug_servo.h"
44 #include "led.h"
45 
46 static inline void main_init( void );
47 static inline void main_periodic_task( void );
48 static inline void main_event_task( void );
49 
50 static inline void main_init_hw(void);
51 
52 static struct i2c_transaction i2c_trans;
53 #define INITIALIZED 6
54 static uint8_t gyro_state = 0;
55 static volatile uint8_t gyro_ready_for_read = FALSE;
57 
58 int main(void) {
59  main_init();
60 
61  while(1) {
65  }
66 
67  return 0;
68 }
69 
70 static inline void main_init( void ) {
71  mcu_init();
73  main_init_hw();
74 }
75 
76 static inline void main_periodic_task( void ) {
77  // LED_TOGGLE(6);
78  RunOnceEvery(10,
79  {
80  DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);
81  LED_PERIODIC();
82  });
83  RunOnceEvery(256, {
84  uint16_t i2c2_ack_fail_cnt = i2c2.errors->ack_fail_cnt;
85  uint16_t i2c2_miss_start_stop_cnt = i2c2.errors->miss_start_stop_cnt;
86  uint16_t i2c2_arb_lost_cnt = i2c2.errors->arb_lost_cnt;
87  uint16_t i2c2_over_under_cnt = i2c2.errors->over_under_cnt;
88  uint16_t i2c2_pec_recep_cnt = i2c2.errors->pec_recep_cnt;
89  uint16_t i2c2_timeout_tlow_cnt = i2c2.errors->timeout_tlow_cnt;
90  uint16_t i2c2_smbus_alert_cnt = i2c2.errors->smbus_alert_cnt;
91  uint16_t i2c2_unexpected_event_cnt = i2c2.errors->unexpected_event_cnt;
92  uint32_t i2c2_last_unexpected_event = i2c2.errors->last_unexpected_event;
93  const uint8_t _bus2 = 2;
94  DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice,
95  &i2c2_ack_fail_cnt,
96  &i2c2_miss_start_stop_cnt,
97  &i2c2_arb_lost_cnt,
98  &i2c2_over_under_cnt,
99  &i2c2_pec_recep_cnt,
100  &i2c2_timeout_tlow_cnt,
101  &i2c2_smbus_alert_cnt,
102  &i2c2_unexpected_event_cnt,
103  &i2c2_last_unexpected_event,
104  &_bus2);
105  });
106 
107  switch (gyro_state) {
108 
109  case 1:
110  /* dummy one byte write for testing */
114  i2c_trans.len_w = 1;
115  i2c_submit(&i2c2,&i2c_trans);
116  break;
117  case 2:
118  /* set gyro range to 2000deg/s and low pass at 256Hz */
122  i2c_trans.buf[1] = (0x03<<3);
123  i2c_trans.len_w = 2;
124  i2c_submit(&i2c2,&i2c_trans);
125  break;
126  case 3:
127  /* set sample rate to 533Hz */
131  i2c_trans.buf[1] = 0x0E;
132  i2c_trans.len_w = 2;
133  i2c_submit(&i2c2,&i2c_trans);
134  break;
135  case 4:
136  /* switch to gyroX clock */
140  i2c_trans.buf[1] = 0x01;
141  i2c_trans.len_w = 2;
142  i2c_submit(&i2c2,&i2c_trans);
143  break;
144  case 5:
145  /* enable interrupt on data ready, idle hight */
149  i2c_trans.buf[1] = (0x01 | 0x01<<7);
150  i2c_trans.len_w = 2;
151  i2c_submit(&i2c2,&i2c_trans);
152  break;
153  case INITIALIZED:
154  /* reads 8 bytes from address 0x1b */
155  // i2c2.buf[0] = ITG3200_REG_TEMP_OUT_H;
156  // i2c2_transceive(ITG3200_ADDR,1, 8, &i2c_done);
157  // reading_gyro = TRUE;
158  default:
159  break;
160  }
161 
162  // if (gyro_state == 1) gyro_state = 0;
164 
165 }
166 
167 
168 #if 0
169 
170 #endif
171 
172 static inline void main_event_task( void ) {
173 
176  /* reads 8 bytes from address 0x1b */
179  i2c_trans.len_w = 1;
180  i2c_trans.len_r = 8;
181  i2c_submit(&i2c2,&i2c_trans);
182  // i2c2.buf[0] = ITG3200_REG_GYRO_XOUT_H;
183  // i2c2_transceive(ITG3200_ADDR,1, 6, &i2c_done);
185  reading_gyro = TRUE;
186  }
187 
188  if (reading_gyro &&
190  // DEBUG_S5_ON();
192  int16_t tgp, tgq, tgr;
193 
194  int16_t ttemp = i2c_trans.buf[0]<<8 | i2c_trans.buf[1];
195 #if 1
196  tgp = i2c_trans.buf[2]<<8 | i2c_trans.buf[3];
197  tgq = i2c_trans.buf[4]<<8 | i2c_trans.buf[5];
198  tgr = i2c_trans.buf[6]<<8 | i2c_trans.buf[7];
199 #endif
200 #if 0
201  tgp = __REVSH(*(int16_t*)(i2c_trans.buf+2));
202  tgq = __REVSH(*(int16_t*)(i2c_trans.buf+4));
203  tgr = __REVSH(*(int16_t*)(i2c_trans.buf+6));
204 #endif
205 #if 0
206  MyByteSwap16(*(int16_t*)(i2c_trans.buf+2), tgp);
207  MyByteSwap16(*(int16_t*)(i2c_trans.buf+4), tgq);
208  MyByteSwap16(*(int16_t*)(i2c_trans.buf+6), tgr);
209 #endif
210  struct Int32Rates g;
211  RATES_ASSIGN(g, tgp, tgq, tgr);
212  RunOnceEvery(10,
213  {
214  DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, &g.p, &g.q, &g.r);
215 
216  uint8_t tmp[8];
217  memcpy(tmp, i2c_trans.buf, 8);
218  DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice, 8, tmp);
219 
220 
221  });
222  // DEBUG_S5_OFF();
223  }
224 }
225 
226 static inline void main_init_hw( void ) {
227 #warning "Needs to be ported to libopencm3 or use the real driver!"
228 
229 #if 0
230  /* set mag ss as floating input (on PC12) = shorted to sda ------------------------------*/
231  /* set mag reset as floating input (on PC13) = shorted to scl ------------------------------*/
232  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
233  GPIO_InitTypeDef GPIO_InitStructure;
234  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12|GPIO_Pin_13;
235  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
236  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
237  GPIO_Init(GPIOC, &GPIO_InitStructure);
238 
239 
240  /* set "eeprom ss" as floating input (on PC14) = gyro int ------------------------------*/
241  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14;
242  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
243  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
244  GPIO_Init(GPIOC, &GPIO_InitStructure);
245 
246  /* configure external interrupt exti15_10 on PC14( gyro int ) */
247  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO, ENABLE);
248  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14;
249  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
250  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
251  GPIO_Init(GPIOC, &GPIO_InitStructure);
252 
253  GPIO_EXTILineConfig(GPIO_PortSourceGPIOC, GPIO_PinSource14);
254  EXTI_InitTypeDef EXTI_InitStructure;
255  EXTI_InitStructure.EXTI_Line = EXTI_Line14;
256  EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
257  EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
258  EXTI_InitStructure.EXTI_LineCmd = ENABLE;
259  EXTI_Init(&EXTI_InitStructure);
260 
261  NVIC_InitTypeDef NVIC_InitStructure;
262  NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn;
263  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
264  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
265  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
266  NVIC_Init(&NVIC_InitStructure);
267 
270 #endif
271 
272 }
273 
274 
276 
277  // DEBUG_S4_ON();
278 
279  /* clear EXTI */
280  exti_reset_request(EXTI14);
281 
282  // DEBUG_S4_TOGGLE();
283 
285 
286  // DEBUG_S4_OFF();
287 
288 }
unsigned short uint16_t
Definition: types.h:16
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
void mcu_init(void)
Microcontroller peripherals initialization.
Definition: mcu.c:61
bool_t i2c_submit(struct i2c_periph *p, struct i2c_transaction *t)
Definition: i2c_arch.c:333
#define ITG3200_REG_DLPF_FS
Definition: itg3200_regs.h:37
int32_t p
in rad/s^2 with INT32_RATE_FRAC
static void main_init_hw(void)
static struct i2c_transaction i2c_trans
static uint8_t gyro_state
uint8_t slave_addr
Definition: i2c.h:79
#define FALSE
Definition: imu_chimu.h:141
void exti15_10_irq_handler(void)
static void main_init(void)
#define DEBUG_SERVO1_INIT()
Architecture independent timing functions.
Register defs for ITG3200 and IMU3000 gyros.
unsigned long uint32_t
Definition: types.h:18
int main(void)
signed short int16_t
Definition: types.h:17
static void main_periodic_task(void)
#define INITIALIZED
#define DEBUG_SERVO2_INIT()
enum I2CTransactionStatus status
Definition: i2c.h:83
int sys_time_register_timer(float duration, sys_time_cb cb)
Register a new system timer.
Definition: sys_time.c:37
angular rates
static bool_t sys_time_check_and_ack_timer(tid_t id)
Check if timer has elapsed.
Definition: sys_time.h:111
#define ITG3200_ADDR
Definition: itg3200_regs.h:31
#define TRUE
Definition: imu_chimu.h:144
#define ITG3200_REG_PWR_MGM
Definition: itg3200_regs.h:48
volatile uint8_t buf[I2C_BUF_LEN]
Definition: i2c.h:82
static void main_event_task(void)
uint8_t len_w
Definition: i2c.h:81
uint16_t len_r
Definition: i2c.h:80
#define LED_PERIODIC()
Definition: led_hw.h:8
Arch independent mcu ( Micro Controller Unit ) utilities.
unsigned char uint8_t
Definition: types.h:14
#define MyByteSwap16(in, out)
Definition: mcu_arch.h:39
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:294
#define ITG3200_REG_INT_CFG
Definition: itg3200_regs.h:38
static volatile uint8_t gyro_ready_for_read
Definition: i2c.h:45
#define ITG3200_REG_TEMP_OUT_H
Definition: itg3200_regs.h:40
enum I2CTransactionType type
Definition: i2c.h:78
int32_t q
in rad/s^2 with INT32_RATE_FRAC
#define PERIODIC_FREQUENCY
Definition: imu_aspirin2.c:54
arch independent LED (Light Emitting Diodes) API
int32_t r
in rad/s^2 with INT32_RATE_FRAC
static uint8_t reading_gyro
#define ITG3200_REG_SMPLRT_DIV
Definition: itg3200_regs.h:36
Paparazzi fixed point algebra.
Architecture independent I2C (Inter-Integrated Circuit Bus) API.