From 99b17f0d8945991e9f472ccd19eeb5d7dceff0a6 Mon Sep 17 00:00:00 2001 From: Antoine Drouin Date: Sun, 22 Aug 2010 01:11:10 +0000 Subject: [PATCH] lisa_l and booz now have different baros implementations --- sw/airborne/boards/booz/baro_board.c | 80 ++++++++++++++++++ sw/airborne/boards/booz/baro_board.h | 46 +++++++++++ sw/airborne/boards/booz/test_baro.c | 89 ++++++++++++++++++++ sw/airborne/boards/lisa_l/baro_board.c | 97 ++++++++++++++++++++++ sw/airborne/boards/lisa_l/baro_board.h | 50 ++++++++++++ sw/airborne/boards/lisa_l/brao_board.c | 4 + sw/airborne/boards/lisa_l/test_baro.c | 107 +++++++++++++++++++++++++ 7 files changed, 473 insertions(+) create mode 100644 sw/airborne/boards/booz/baro_board.c create mode 100644 sw/airborne/boards/booz/baro_board.h create mode 100644 sw/airborne/boards/booz/test_baro.c create mode 100644 sw/airborne/boards/lisa_l/baro_board.c create mode 100644 sw/airborne/boards/lisa_l/baro_board.h create mode 100644 sw/airborne/boards/lisa_l/brao_board.c create mode 100644 sw/airborne/boards/lisa_l/test_baro.c diff --git a/sw/airborne/boards/booz/baro_board.c b/sw/airborne/boards/booz/baro_board.c new file mode 100644 index 0000000000..5e067e3cdb --- /dev/null +++ b/sw/airborne/boards/booz/baro_board.c @@ -0,0 +1,80 @@ +/* $Id$ + * + * Copyright (C) 2010 The Paparazzi Team + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + + +#include "rotorcraft/baro.h" + +#include "airframe.h" +#include "led.h" + +/* threshold >0 && <1023 */ +#ifndef BOOZ_ANALOG_BARO_THRESHOLD +#define BOOZ_ANALOG_BARO_THRESHOLD 850 +#endif + +// pressure on AD0.1 on P0.28 +// offset on DAC on P0.25 + +struct Baro baro; +struct BaroBoard baro_board; + +void baro_init( void ) { + + baro.status = BS_UNINITIALIZED; + baro.absolute = 0; + baro.differential = 0; /* not handled on this board */ + + baro_board.offset = 1023; + Booz2AnalogSetDAC(baro_board.offset); + + baro_board.value_filtered = 0; + baro_board.data_available = FALSE; +#ifdef ROTORCRAFT_BARO_LED + LED_OFF(ROTORCRAFT_BARO_LED); +#endif +} + +void baro_periodic(void) {} + +/* decrement offset until adc reading is over a threshold */ +void baro_board_calibrate(void) { + if (baro_board.value_filtered < BOOZ_ANALOG_BARO_THRESHOLD && baro_board.offset >= 1) { + if (baro_board.value_filtered == 0 && baro_board.offset > 15) + baro_board.offset -= 15; + else + baro_board.offset--; + Booz2AnalogSetDAC(baro_board.offset); +#ifdef ROTORCRAFT_BARO_LED + LED_TOGGLE(ROTORCRAFT_BARO_LED); +#endif + } + else { + baro.status = BS_RUNNING; +#ifdef ROTORCRAFT_BARO_LED + LED_ON(ROTORCRAFT_BARO_LED); +#endif + } +} + + + + diff --git a/sw/airborne/boards/booz/baro_board.h b/sw/airborne/boards/booz/baro_board.h new file mode 100644 index 0000000000..1eb465d744 --- /dev/null +++ b/sw/airborne/boards/booz/baro_board.h @@ -0,0 +1,46 @@ +#ifndef BOARDS_BOOZ_BARO_H +#define BOARDS_BOOZ_BARO_H + +#include "std.h" + +#include "rotorcraft/baro.h" +#include "booz/booz2_analog.h" + +/* we don't need that on this board */ + +struct BaroBoard { + uint16_t offset; + uint16_t value_filtered; + bool_t data_available; +}; + +extern struct BaroBoard baro_board; + +extern void baro_board_calibrate(void); + +#define BaroEvent(_b_abs_handler, _b_diff_handler) { \ + if (baro_board.data_available) { \ + _b_abs_handler(); \ + baro_board.data_available = FALSE; \ + } \ + } + +static inline void baro_board_SetOffset(uint16_t _o) { + baro_board.offset = _o; + Booz2AnalogSetDAC(_o); +} + +static inline void BoozBaroISRHandler(uint16_t _val) { + baro.absolute = _val; + baro_board.value_filtered = (3*baro_board.value_filtered + baro.absolute)/4; + if (baro.status == BS_UNINITIALIZED) { + RunOnceEvery(10, { baro_board_calibrate();}); + } + /* else */ + baro_board.data_available = TRUE; +} + + + + +#endif /* BOARDS_BOOZ_BARO_H */ diff --git a/sw/airborne/boards/booz/test_baro.c b/sw/airborne/boards/booz/test_baro.c new file mode 100644 index 0000000000..bd6e4b9332 --- /dev/null +++ b/sw/airborne/boards/booz/test_baro.c @@ -0,0 +1,89 @@ +/* + * $Id$ + * + * Copyright (C) 2009 Antoine Drouin + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/* + * + * test baro using interrupts + * + */ + +#include BOARD_CONFIG + +#include "init_hw.h" +#include "sys_time.h" + +#include "downlink.h" + +#include "rotorcraft/baro.h" +//#include "my_debug_servo.h" + +static inline void main_init( void ); +static inline void main_periodic_task( void ); +static inline void main_event_task( void ); + +static inline void main_on_baro_diff(void); +static inline void main_on_baro_abs(void); + + +int main(void) { + main_init(); + + while(1) { + if (sys_time_periodic()) + main_periodic_task(); + main_event_task(); + } + + return 0; +} + +static inline void main_init( void ) { + hw_init(); + sys_time_init(); + baro_init(); +} + + + +static inline void main_periodic_task( void ) { + + RunOnceEvery(2, {baro_periodic();}); + LED_PERIODIC(); + RunOnceEvery(256, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);}); +} + + + +static inline void main_event_task( void ) { + BaroEvent(main_on_baro_abs, main_on_baro_diff); +} + + + +static inline void main_on_baro_diff(void) { + +} + +static inline void main_on_baro_abs(void) { + RunOnceEvery(5,{DOWNLINK_SEND_BARO_RAW(DefaultChannel, &baro.absolute, &baro.differential);}); +} diff --git a/sw/airborne/boards/lisa_l/baro_board.c b/sw/airborne/boards/lisa_l/baro_board.c new file mode 100644 index 0000000000..4a959453ed --- /dev/null +++ b/sw/airborne/boards/lisa_l/baro_board.c @@ -0,0 +1,97 @@ + +#include "rotorcraft/baro.h" + +struct Baro baro; +struct BaroBoard baro_board; + +static inline void baro_board_write_to_register(uint8_t baro_addr, uint8_t reg_addr, uint8_t val_msb, uint8_t val_lsb); +static inline void baro_board_read_from_register(uint8_t baro_addr, uint8_t reg_addr); +static inline void baro_board_set_current_register(uint8_t baro_addr, uint8_t reg_addr); +static inline void baro_board_read_from_current_register(uint8_t baro_addr); + +// absolute +#define BARO_ABS_ADDR 0x90 +// differential +#define BARO_DIFF_ADDR 0x92 + +void baro_init(void) { + baro.status = BS_UNINITIALIZED; + baro.absolute = 0; + baro.differential = 0; + baro_board.status = LBS_UNINITIALIZED; +} + + +void baro_periodic(void) { + // check i2c_done + switch (baro_board.status) { + case LBS_UNINITIALIZED: + baro_board_send_reset(); + baro_board.status = LBS_RESETED; + break; + case LBS_RESETED: + baro_board_send_config_abs(); + baro_board.status = LBS_INITIALIZING_ABS; + break; + case LBS_INITIALIZING_ABS: + baro_board_set_current_register(BARO_ABS_ADDR, 0x00); + baro_board.status = LBS_INITIALIZING_ABS_1; + break; + case LBS_INITIALIZING_ABS_1: + baro_board_send_config_diff(); + baro_board.status = LBS_INITIALIZING_DIFF; + break; + case LBS_INITIALIZING_DIFF: + baro_board_set_current_register(BARO_DIFF_ADDR, 0x00); + baro_board.status = LBS_INITIALIZING_DIFF_1; + // baro_board.status = LBS_UNINITIALIZED; + break; + case LBS_INITIALIZING_DIFF_1: + case LBS_READ_DIFF: + baro_board_read_from_current_register(BARO_ABS_ADDR); + baro_board.status = LBS_READING_ABS; + break; + case LBS_READ_ABS: + baro_board_read_from_current_register(BARO_DIFF_ADDR); + baro_board.status = LBS_READING_DIFF; + break; + default: + break; + } + +} + + +void baro_board_send_reset(void) { + i2c2.buf[0] = 0x06; + i2c2_transmit(0x00, 1, &baro_board.i2c_done); +} + +void baro_board_send_config_abs(void) { + baro_board_write_to_register(BARO_ABS_ADDR, 0x01, 0x86, 0x83); +} + +void baro_board_send_config_diff(void) { + baro_board_write_to_register(BARO_DIFF_ADDR, 0x01, 0x84, 0x83); +} + +static inline void baro_board_write_to_register(uint8_t baro_addr, uint8_t reg_addr, uint8_t val_msb, uint8_t val_lsb) { + i2c2.buf[0] = reg_addr; + i2c2.buf[1] = val_msb; + i2c2.buf[2] = val_lsb; + i2c2_transmit(baro_addr, 3, &baro_board.i2c_done); +} + +static inline void baro_board_read_from_register(uint8_t baro_addr, uint8_t reg_addr) { + i2c2.buf[0] = reg_addr; + i2c2_transceive(baro_addr, 1, 2, &baro_board.i2c_done); +} + +static inline void baro_board_set_current_register(uint8_t baro_addr, uint8_t reg_addr) { + i2c2.buf[0] = reg_addr; + i2c2_transmit(baro_addr, 1, &baro_board.i2c_done); +} + +static inline void baro_board_read_from_current_register(uint8_t baro_addr) { + i2c2_receive(baro_addr, 2, &baro_board.i2c_done); +} diff --git a/sw/airborne/boards/lisa_l/baro_board.h b/sw/airborne/boards/lisa_l/baro_board.h new file mode 100644 index 0000000000..785dd6173d --- /dev/null +++ b/sw/airborne/boards/lisa_l/baro_board.h @@ -0,0 +1,50 @@ +#ifndef BOARDS_LISA_L_BARO_H +#define BOARDS_LISA_L_BARO_H + +#include "std.h" +#include "i2c.h" + +enum LisaBaroStatus { + LBS_UNINITIALIZED, + LBS_RESETED, + LBS_INITIALIZING_ABS, + LBS_INITIALIZING_ABS_1, + LBS_INITIALIZING_DIFF, + LBS_INITIALIZING_DIFF_1, + LBS_IDLE, + LBS_READING_ABS, + LBS_READ_ABS, + LBS_READING_DIFF, + LBS_READ_DIFF +}; + +struct BaroBoard { + enum LisaBaroStatus status; + bool_t i2c_done; +}; + +extern struct BaroBoard baro_board; + +extern void baro_board_send_reset(void); +extern void baro_board_send_config_abs(void); +extern void baro_board_send_config_diff(void); + +#define BaroEvent(_b_abs_handler, _b_diff_handler) { \ + if (baro_board.status == LBS_READING_ABS && baro_board.i2c_done) { \ + int16_t tmp = i2c2.buf[0]<<8 | i2c2.buf[1]; \ + baro.absolute = tmp; \ + baro_board.status = LBS_READ_ABS; \ + _b_abs_handler(); \ + } \ + else if (baro_board.status == LBS_READING_DIFF && baro_board.i2c_done) { \ + int16_t tmp = i2c2.buf[0]<<8 | i2c2.buf[1]; \ + baro.differential = tmp; \ + baro_board.status = LBS_READ_DIFF; \ + _b_diff_handler(); \ + } \ + } + + + + +#endif /* BOARDS_LISA_L_BARO_H */ diff --git a/sw/airborne/boards/lisa_l/brao_board.c b/sw/airborne/boards/lisa_l/brao_board.c new file mode 100644 index 0000000000..ddea3d5035 --- /dev/null +++ b/sw/airborne/boards/lisa_l/brao_board.c @@ -0,0 +1,4 @@ + +#include "rotorcraft/baro.h" + + diff --git a/sw/airborne/boards/lisa_l/test_baro.c b/sw/airborne/boards/lisa_l/test_baro.c new file mode 100644 index 0000000000..fa2bf931e1 --- /dev/null +++ b/sw/airborne/boards/lisa_l/test_baro.c @@ -0,0 +1,107 @@ +/* + * $Id$ + * + * Copyright (C) 2009 Antoine Drouin + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/* + * + * test baro using interrupts + * + */ + +#include BOARD_CONFIG + +#include "init_hw.h" +#include "sys_time.h" + +#include "downlink.h" + +#include "rotorcraft/baro.h" +//#include "my_debug_servo.h" + +static inline void main_init( void ); +static inline void main_periodic_task( void ); +static inline void main_event_task( void ); + +static inline void main_on_baro_diff(void); +static inline void main_on_baro_abs(void); + + +int main(void) { + main_init(); + + while(1) { + if (sys_time_periodic()) + main_periodic_task(); + main_event_task(); + } + + return 0; +} + +static inline void main_init( void ) { + hw_init(); + sys_time_init(); + baro_init(); + + // DEBUG_SERVO1_INIT(); + // DEBUG_SERVO2_INIT(); + + +} + + + +static inline void main_periodic_task( void ) { + + RunOnceEvery(2, {baro_periodic();}); + LED_PERIODIC(); + RunOnceEvery(256, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);}); + RunOnceEvery(256, + { + DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, + &i2c2_errors.ack_fail_cnt, + &i2c2_errors.miss_start_stop_cnt, + &i2c2_errors.arb_lost_cnt, + &i2c2_errors.over_under_cnt, + &i2c2_errors.pec_recep_cnt, + &i2c2_errors.timeout_tlow_cnt, + &i2c2_errors.smbus_alert_cnt, + &i2c2_errors.unexpected_event_cnt, + &i2c2_errors.last_unexpected_event); + }); +} + + + +static inline void main_event_task( void ) { + BaroEvent(main_on_baro_abs, main_on_baro_diff); +} + + + +static inline void main_on_baro_diff(void) { + +} + +static inline void main_on_baro_abs(void) { + RunOnceEvery(5,{DOWNLINK_SEND_BARO_RAW(DefaultChannel, &baro.absolute, &baro.differential);}); +}