From 38120e1b4edaf2deb6f20eedcf2e10a3faaf8259 Mon Sep 17 00:00:00 2001 From: Martin Mueller Date: Thu, 21 Oct 2010 17:51:40 +0000 Subject: [PATCH] srf08 telemeter, a good example for delay="" --- conf/airframes/mm/fixed-wing/funjetmm.xml | 8 +- conf/modules/alt_srf08.xml | 17 +++ sw/airborne/main_ap.c | 33 ----- sw/airborne/modules/sensors/alt_srf08.c | 151 ++++++++++++++++++++++ sw/airborne/modules/sensors/alt_srf08.h | 119 +++++++++++++++++ 5 files changed, 292 insertions(+), 36 deletions(-) create mode 100644 conf/modules/alt_srf08.xml create mode 100644 sw/airborne/modules/sensors/alt_srf08.c create mode 100644 sw/airborne/modules/sensors/alt_srf08.h diff --git a/conf/airframes/mm/fixed-wing/funjetmm.xml b/conf/airframes/mm/fixed-wing/funjetmm.xml index e1a98b1bc5..14964566c4 100755 --- a/conf/airframes/mm/fixed-wing/funjetmm.xml +++ b/conf/airframes/mm/fixed-wing/funjetmm.xml @@ -24,6 +24,7 @@ + @@ -41,7 +42,7 @@ - + @@ -54,8 +55,9 @@ - - + + + + + + + diff --git a/sw/airborne/main_ap.c b/sw/airborne/main_ap.c index fc67c70da8..ecc96ba699 100644 --- a/sw/airborne/main_ap.c +++ b/sw/airborne/main_ap.c @@ -75,10 +75,6 @@ #include "airspeed.h" #endif -#ifdef TELEMETER -#include "srf08.h" -#endif - #if defined USE_I2C0 || USE_I2C1 #include "i2c.h" #endif @@ -87,10 +83,6 @@ #include "spi.h" #endif -#ifdef TELEMETER -#include "srf08.h" -#endif - #ifdef TRAFFIC_INFO #include "traffic_info.h" #endif @@ -446,15 +438,6 @@ void periodic_task_ap( void ) { kill_throttle |= launch && (dist2_to_home > Square(KILL_MODE_DISTANCE)); } switch (_1Hz) { -#ifdef TELEMETER - case 1: - srf08_initiate_ranging(); - break; - case 5: - /** 65ms since initiate_ranging() (the spec ask for 65ms) */ - srf08_receive(); - break; -#endif #ifdef TCAS case 6: @@ -612,9 +595,6 @@ void init_ap( void ) { #ifdef GPS gps_init(); #endif -#ifdef TELEMETER - srf08_init(); -#endif #ifdef USE_UART0 Uart0Init(); #endif @@ -809,19 +789,6 @@ void event_task_ap( void ) { } #endif /** DATALINK */ -#ifdef TELEMETER - /** Handling of data sent by the device (initiated by srf08_receive() */ - if (srf08_received) { - srf08_received = FALSE; - srf08_read(); - } - if (srf08_got) { - srf08_got = FALSE; - srf08_copy(); - DOWNLINK_SEND_RANGEFINDER(DefaultChannel, &srf08_range); - } -#endif - #ifdef MCU_SPI_LINK if (spi_message_received) { /* Got a message on SPI. */ diff --git a/sw/airborne/modules/sensors/alt_srf08.c b/sw/airborne/modules/sensors/alt_srf08.c new file mode 100644 index 0000000000..e660b46457 --- /dev/null +++ b/sw/airborne/modules/sensors/alt_srf08.c @@ -0,0 +1,151 @@ +/* + * $Id$ + * + * Copyright (C) 2005 Pascal Brisset, Antoine Drouin + * Copyright (C) 2002 Chris efstathiou hendrix@otenet.gr + * + * 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. + * + */ +/** \file srf08.c + * \brief Basic library for SRF08 telemeter + * + */ + +#include "i2c.h" +#include "alt_srf08.h" +#include "uart.h" +#include "messages.h" +#include "downlink.h" +#include "led.h" + +#ifndef SRF08_I2C_DEV +#define SRF08_I2C_DEV i2c0 +#endif + +/* Global Variables */ +bool_t srf08_received, srf08_got; +struct i2c_transaction srf_trans; +uint16_t srf08_range; + + +/*###########################################################################*/ + +void srf08_init(void) +{ + srf08_received = FALSE; + srf08_got = FALSE; + + srf_trans.buf[0] = 0x00; + srf_trans.buf[1] = 0x51; + I2CTransmit(SRF08_I2C_DEV, srf_trans, SRF08_UNIT_0, 2); + + /** Setting the gain to the minimun value (to avoid echos ?) */ + srf_trans.buf[0] = SRF08_SET_GAIN; + srf_trans.buf[1] = SRF08_MIN_GAIN; + I2CTransmit(SRF08_I2C_DEV, srf_trans, SRF08_UNIT_0, 2); + + return; +} +/*###########################################################################*/ + +void srf08_initiate_ranging(void) { +LED_ON(2); + srf_trans.buf[0] = SRF08_COMMAND; + srf_trans.buf[1] = SRF08_CENTIMETERS; + I2CTransmit(SRF08_I2C_DEV, srf_trans, SRF08_UNIT_0, 2); +} + +/** Ask the value to the device */ +void srf08_receive(void) { +LED_OFF(2); + srf_trans.buf[0] = SRF08_ECHO_1; + srf08_received = TRUE; + I2CTransmit(SRF08_I2C_DEV, srf_trans, SRF08_UNIT_0, 1); +} + +/** Read values on the bus */ +void srf08_read(void) { + srf08_got = TRUE; + I2CReceive(SRF08_I2C_DEV, srf_trans, SRF08_UNIT_0, 2); +} + +/** Copy the I2C buffer */ +void srf08_copy(void) { + srf08_range = srf_trans.buf[0] << 8 | srf_trans.buf[1]; +} + +void srf08_ping() +{ + srf08_initiate_ranging(); + while (srf_trans.status != I2CTransSuccess); /* blocking */ + + srf08_receive(); +} +/*###########################################################################*/ + +uint32_t srf08_read_register(uint8_t srf08_register) +{ + uint8_t cnt; + + union i2c_union { + uint32_t rx_word; + uint8_t rx_byte[2]; + } i2c; + + + srf_trans.buf[0] = srf08_register; + + /* get high byte msb first */ + if (srf08_register>=2) + cnt = 2; + else + cnt = 1; + + I2CTransceive(SRF08_I2C_DEV, srf_trans, SRF08_UNIT_0, 1, cnt); + + /* get high byte msb first */ + if(srf08_register>=2) { + i2c.rx_byte[1]=srf_trans.buf[1]; + } + + /* get low byte msb first */ + i2c.rx_byte[0]=srf_trans.buf[0]; + + return(i2c.rx_word); +} + +void srf08_event(void) +{ + float f=0; + uint8_t i=0; + + /** Handling of data sent by the device (initiated by srf08_receive() */ + if (srf_trans.status == I2CTransSuccess) { + if (srf08_received) { + srf08_received = FALSE; + srf08_read(); + } + else if (srf08_got) { + srf08_got = FALSE; + srf08_copy(); + DOWNLINK_SEND_RANGEFINDER(DefaultChannel, &srf08_range, &f, &f, &f, &f, &f, &i); + } + } +} + diff --git a/sw/airborne/modules/sensors/alt_srf08.h b/sw/airborne/modules/sensors/alt_srf08.h new file mode 100644 index 0000000000..5d816bfe15 --- /dev/null +++ b/sw/airborne/modules/sensors/alt_srf08.h @@ -0,0 +1,119 @@ +/* + * $Id$ + * + * Copyright (C) 2005 Pascal Brisset, Antoine Drouin + * Copyright (C) 2002 Chris efstathiou hendrix@otenet.gr + * + * 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. + * + */ +/** \file srf08.h + * \brief Basic library for SRF08 telemeter + * + */ + + +#ifndef SRF08_H +#define SRF08_H 1 + +/*###########################################################################*/ +/* START OF CONFIGURATION BLOCK */ +/*###########################################################################*/ +/* Normally you shouldn't need to change anything */ + +#define SRF08_UNIT_0 0xE0 /* the SRF08 MODULE I2C address */ +#define SRF08_UNIT_1 0xE2 /* the SRF08 MODULE I2C address */ +#define SRF08_UNIT_2 0xE4 /* the SRF08 MODULE I2C address */ +#define SRF08_UNIT_3 0xE6 /* the SRF08 MODULE I2C address */ +#define SRF08_UNIT_4 0xE8 /* the SRF08 MODULE I2C address */ +#define SRF08_UNIT_5 0xEA /* the SRF08 MODULE I2C address */ +#define SRF08_UNIT_6 0xEC /* the SRF08 MODULE I2C address */ +#define SRF08_UNIT_7 0xEE /* the SRF08 MODULE I2C address */ +#define SRF08_UNIT_8 0xF0 /* the SRF08 MODULE I2C address */ +#define SRF08_UNIT_9 0xF2 /* the SRF08 MODULE I2C address */ +#define SRF08_UNIT_10 0xF4 /* the SRF08 MODULE I2C address */ +#define SRF08_UNIT_11 0xF6 /* the SRF08 MODULE I2C address */ +#define SRF08_UNIT_12 0xF8 /* the SRF08 MODULE I2C address */ +#define SRF08_UNIT_13 0xFA /* the SRF08 MODULE I2C address */ +#define SRF08_UNIT_14 0xFC /* the SRF08 MODULE I2C address */ +#define SRF08_UNIT_15 0xFE /* the SRF08 MODULE I2C address */ + +/*###########################################################################*/ +/* END OF CONFIGURATION BLOCK */ +/*###########################################################################*/ + +#define SRF08_I2C_BROADCAST_ADDRESS 0X00 + +#define SRF08_MIN_GAIN 0 /* sets gain to 94 */ +#define SRF08_MAX_GAIN 31 /* sets gain to 1025 */ +#define SRF08_MIN_RANGE 0 /* in millimeters */ +#define SRF08_MAX_RANGE 11008 /* in millimeters */ + +#define SRF08_INCHES 0X50 +#define SRF08_CENTIMETERS 0X51 +#define SRF08_MICROSECONDS 0X52 + +/* register positions */ +#define SRF08_COMMAND 0 +#define SRF08_SET_GAIN 1 +#define SRF08_LIGHT 1 +#define SRF08_ECHO_1 2 +#define SRF08_ECHO_2 4 +#define SRF08_ECHO_3 6 +#define SRF08_ECHO_4 8 +#define SRF08_ECHO_5 10 +#define SRF08_ECHO_6 12 +#define SRF08_ECHO_7 14 +#define SRF08_ECHO_8 16 +#define SRF08_ECHO_9 18 +#define SRF08_ECHO_10 20 +#define SRF08_ECHO_11 22 +#define SRF08_ECHO_12 24 +#define SRF08_ECHO_13 26 +#define SRF08_ECHO_14 28 +#define SRF08_ECHO_15 30 +#define SRF08_ECHO_16 32 +#define SRF08_ECHO_17 34 + +/* Function Declaration */ + +extern void srf08_select_unit(uint8_t srf08_address); +extern void srf08_init(void); + +extern void srf08_set_gain(uint8_t gain); +extern void srf08_set_range(uint32_t millimeters); + +extern void srf08_ping(void); +extern uint32_t srf08_read_register(uint8_t srf08_register); + +extern void srf08_change_i2c_address(uint8_t new_i2c_address); + +extern void srf08_initiate_ranging(void); +extern void srf08_receive(void); + +extern uint16_t srf08_range; +extern bool_t srf08_received, srf08_got; +/** Read values on the bus */ +extern void srf08_read(void); +/** Copy the I2C buffer */ +extern void srf08_copy(void); + +extern void srf08_event(void); + +#endif /* #ifndef SRF08_H */ +