Revision 4:698a3c538482, committed 2012-11-14
- Comitter:
- xiaxia686
- Date:
- Wed Nov 14 15:51:56 2012 +0000
- Parent:
- 3:8c78c15a92e3
- Child:
- 8:a8693e2daa00
- Commit message:
- removed varicance from sonar callback;
Changed in this revision
--- a/Kalman/Sonar/RF12B/RF12B.cpp Wed Nov 14 15:48:20 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,400 +0,0 @@
-#include "RF12B.h"
-
-#include "RF_defs.h"
-#include <algorithm>
-#include "system.h"
-#include "globals.h"
-
-
-RF12B::RF12B(PinName _SDI,
- PinName _SDO,
- PinName _SCK,
- PinName _NCS,
- PinName _NIRQ):spi(_SDI, _SDO, _SCK),
- NCS(_NCS), NIRQ(_NIRQ), NIRQ_in(_NIRQ) {// rfled(LED3) {
-
- // SPI frequency, word lenght, polarity and phase */
- spi.format(16,0);
- spi.frequency(2000000);
-
- // Set ~CS high
- NCS = 1;
-
- // Initialise RF Module
- init();
-
- // Setup interrupt to happen on falling edge of NIRQ
- NIRQ.fall(this, &RF12B::rxISR);
-}
-
-// Returns the packet length if data is available in the receive buffer, 0 otherwise
-//unsigned int RF12B::available() {
-// return fifo.size();
-//}
-
-// Reads a packet of data, with length "size" Returns false if read failed. TODO: make a metafifo to isolate packets
-/*bool RF12B::read(unsigned char* data, unsigned int size) {
- if (fifo.size() == 0) {
- return false;
- } else {
- unsigned int i = 0;
- while (fifo.size() > 0 && i < size) {
- data[i++] = fifo.front();
- fifo.pop();
- }
- return true;
- }
-}
-*/
-
-// Reads a byte of data from the receive buffer
-/*
-unsigned char RF12B::read() {
- if (available()) {
- unsigned char data = fifo.front();
- fifo.pop();
- return data;
- } else {
- return 0xFF; // Error val although could also be data...
- }
-}
-*/
-
-// Sends a packet of data to the RF module for transmission TODO: Make asych
-void RF12B::write(unsigned char *data, unsigned char length) {
- unsigned char crc = 0;
-
- // Transmitter mode
- changeMode(TX);
-
- writeCmd(0x0000);
- send(0xAA); // PREAMBLE
- send(0xAA);
- send(0xAA);
- send(0x2D); // SYNC
- send(0xD4);
- // Packet Length
- send(length);
- crc = crc8(crc, length);
- send(crc);
- crc = crc8(crc, crc);
- // Packet Data
- for (unsigned char i=0; i<length; i++) {
- send(data[i]);
- crc = crc8(crc, data[i]);
- }
- send(crc);
- send(0xAA); // DUMMY BYTES
- send(0xAA);
- send(0xAA);
-
- // Back to receiver mode
- changeMode(RX);
- status();
-
-
-}
-
-// Transmit a 1-byte data packet
-void RF12B::write(unsigned char data) {
- write(&data, 1);
-}
-/*
-void RF12B::write(queue<char> &data, int length) {
- char crc = 0;
- char length_byte = 0;
-
- // -1 means try to transmit everything in the queue
- if (length == -1) {
- length = data.size();
- }
-
- // max length of packet is 255
- length_byte = min(length, 255);
-
- // Transmitter mode
- changeMode(TX);
-
- writeCmd(0x0000);
- send(0xAA); // PREAMBLE
- send(0xAA);
- send(0xAA);
- send(0x2D); // SYNC
- send(0xD4);
- // Packet Length
- send(length_byte);
- crc = crc8(crc, length_byte);
- send(crc);
- crc = crc8(crc, crc);
- // Packet Data
- for (char i=0; i<length_byte; i++) {
- send(data.front());
- crc = crc8(crc, data.front());
- data.pop();
- }
- send(crc);
- send(0xAA); // DUMMY BYTES
- send(0xAA);
- send(0xAA);
-
- // Back to receiver mode
- changeMode(RX);
- status();
-}
-*/
-/**********************************************************************
- * PRIVATE FUNCTIONS
- *********************************************************************/
-
-// Initialises the RF12B module
-void RF12B::init() {
- // writeCmd(0x80E7); //EL,EF,868band,12.0pF
- changeMode(RX);
- writeCmd(0xA640); //frequency select
- writeCmd(0xC647); //4.8kbps
- writeCmd(0x94A0); //VDI,FAST,134kHz,0dBm,-103dBm
- writeCmd(0xC2AC); //AL,!ml,DIG,DQD4
- writeCmd(0xCA81); //FIFO8,SYNC,!ff,DR
- writeCmd(0xCED4); //SYNC=2DD4
- writeCmd(0xC483); //@PWR,NO RSTRIC,!st,!fi,OE,EN
- writeCmd(0x9850); //!mp,90kHz,MAX OUT
- writeCmd(0xCC17); //OB1, COB0, LPX, Iddy, CDDIT�CBW0
- writeCmd(0xE000); //NOT USED
- writeCmd(0xC800); //NOT USED
- writeCmd(0xC040); //1.66MHz,2.2V
-
- writeCmd(
- RFM_CONFIG_EL |
- RFM_CONFIG_EF |
- RFM_CONFIG_BAND_433 //|
- //RFM_CONFIG_X_11_0pf // meh, using default
- );
-
- // 2. Power Management Command
- // leave everything switched off for now
- /*
- writeCmd(
- RFM_POWER_MANAGEMENT // switch all off
- );
- */
-
- // 3. Frequency Setting Command
- writeCmd(
- RFM_FREQUENCY |
- RFM_FREQ_433Band(435.7) //I totally made this value up... if someone knows where the sweetspots are in this band, tell me!
- );
-
-
- // 4. Data Rate Command
- //writeCmd(RFM_DATA_RATE_9600);
- writeCmd(RFM_DATA_RATE_57600);
-
-
- // 5. Receiver Control Command
- writeCmd(
- RFM_RX_CONTROL_P20_VDI |
- RFM_RX_CONTROL_VDI_FAST |
- //RFM_RX_CONTROL_BW(RFM_BAUD_RATE) |
- RFM_RX_CONTROL_BW_134 | // CHANGE THIS TO 67 TO IMPROVE RANGE! (though the bitrate must then be below 8kbaud, and fsk modulation changed)
- RFM_RX_CONTROL_GAIN_0 |
- RFM_RX_CONTROL_RSSI_103 // Might need adjustment. Datasheet says around 10^-5 bit error rate at this level and baudrate.
- );
-
- // 6. Data Filter Command
- writeCmd(
- RFM_DATA_FILTER_AL |
- RFM_DATA_FILTER_ML |
- RFM_DATA_FILTER_DIG //|
- //RFM_DATA_FILTER_DQD(4)
- );
-
- // 7. FIFO and Reset Mode Command
- writeCmd(
- RFM_FIFO_IT(8) |
- RFM_FIFO_DR |
- 0x8 //turn on 16bit sync word
- );
-
- // 8. FIFO Syncword
- // Leave as default: 0xD4
-
- // 9. Receiver FIFO Read
- // when the interupt goes high, (and if we can assume that it was a fifo fill interrupt) we can read a byte using:
- // result = RFM_READ_FIFO();
-
- // 10. AFC Command
- writeCmd(
- //RFM_AFC_AUTO_VDI | //Note this might be changed to improve range. Refer to datasheet.
- RFM_AFC_AUTO_INDEPENDENT |
- RFM_AFC_RANGE_LIMIT_7_8 |
- RFM_AFC_EN |
- RFM_AFC_OE |
- RFM_AFC_FI
- );
-
- // 11. TX Configuration Control Command
- writeCmd(
- RFM_TX_CONTROL_MOD_60 |
- RFM_TX_CONTROL_POW_0
- );
-
-
- // 12. PLL Setting Command
- writeCmd(
- 0xCC77 & ~0x01 // Setting the PLL bandwith, less noise, but max bitrate capped at 86.2
- // I think this will slow down the pll's reaction time. Not sure, check with someone!
- );
-
- changeMode(RX);
- resetRX();
- status();
-}
-
-/* Write a command to the RF Module */
-unsigned int RF12B::writeCmd(unsigned int cmd) {
- NCS = 0;
- unsigned int recv = spi.write(cmd);
- NCS = 1;
- return recv;
-}
-
-/* Sends a byte of data across RF */
-void RF12B::send(unsigned char data) {
- while (NIRQ);
- writeCmd(0xB800 + data);
-}
-
-/* Change the mode of the RF module to Transmitting or Receiving */
-void RF12B::changeMode(rfmode_t _mode) {
- mode = _mode;
- if (_mode == TX) {
- writeCmd(0x8239); //!er,!ebb,ET,ES,EX,!eb,!ew,DC
- } else { /* mode == RX */
- writeCmd(0x8299); //er,!ebb,ET,ES,EX,!eb,!ew,DC
- }
-}
-
-// Interrupt routine for data reception */
-void RF12B::rxISR() {
-
- unsigned int data = 0;
- static int i = -2;
- static unsigned char packet_length = 0;
- static unsigned char crc = 0;
-// #ifdef ROBOT_SECONDARY
- static unsigned char temp;
-// #endif
-
- //Loop while interrupt is asserted
- while (!NIRQ_in && mode == RX) {
-
- // Grab the packet's length byte
- if (i == -2) {
- data = writeCmd(0x0000);
- if ( (data&0x8000) ) {
- data = writeCmd(0xB000);
- packet_length = (data&0x00FF);
- crc = crc8(crc, packet_length);
- i++;
- }
- }
-
- //If we exhaust the interrupt, exit
- if (NIRQ_in)
- break;
-
- // Check that packet length was correct
- if (i == -1) {
- data = writeCmd(0x0000);
- if ( (data&0x8000) ) {
- data = writeCmd(0xB000);
- unsigned char crcofsize = (data&0x00FF);
- if (crcofsize != crc) {
- //It was wrong, start over
- i = -2;
- packet_length = 0;
- crc = 0;
- //temp = queue<unsigned char>();
- resetRX();
- } else {
- crc = crc8(crc, crcofsize);
- i++;
- }
- }
- }
-
- //If we exhaust the interrupt, exit
- if (NIRQ_in)
- break;
-
- // Grab the packet's data
- if (i >= 0 && i < packet_length) {
- data = writeCmd(0x0000);
- if ( (data&0x8000) ) {
- data = writeCmd(0xB000);
- // #ifdef ROBOT_SECONDARY
- temp = data&0x00FF;
- // #endif
- //temp.push(data&0x00FF);
- crc = crc8(crc, (unsigned char)(data&0x00FF));
- i++;
- }
- }
-
- //If we exhaust the interrupt, exit
- if (NIRQ_in)
- break;
-
- if (i >= packet_length) {
- data = writeCmd(0x0000);
- if ( (data&0x8000) ) {
- data = writeCmd(0xB000);
- if ((unsigned char)(data & 0x00FF) == crc) {
- //If the checksum is correct, add our data to the end of the output buffer
- //while (!temp.empty()) {
- //fifo.push(temp);
- // temp.pop();
-//#ifdef ROBOT_SECONDARY
- if (callbackfunc)
- (*callbackfunc)(temp);
-
- if (callbackobj && mcallbackfunc)
- (callbackobj->*mcallbackfunc)(temp);
-//#endif
- // }
- }
-
- // Tell RF Module we are finished, and clean up
- i = -2;
- packet_length = 0;
- crc = 0;
- //temp = queue<unsigned char>();
- resetRX();
- }
- }
- }
-
-}
-
-unsigned int RF12B::status() {
- return writeCmd(0x0000);
-}
-
-// Tell the RF Module this packet is received and wait for the next */
-void RF12B::resetRX() {
- writeCmd(0xCA81);
- writeCmd(0xCA83);
-};
-
-// Calculate CRC8 */
-unsigned char RF12B::crc8(unsigned char crc, unsigned char data) {
- crc = crc ^ data;
- for (int i = 0; i < 8; i++) {
- if (crc & 0x01) {
- crc = (crc >> 1) ^ 0x8C;
- } else {
- crc >>= 1;
- }
- }
- return crc;
-}
\ No newline at end of file
--- a/Kalman/Sonar/RF12B/RF12B.h Wed Nov 14 15:48:20 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,83 +0,0 @@
-#ifndef _RF12B_H
-#define _RF12B_H
-
-#include "mbed.h"
-//#include <queue>
-
-enum rfmode_t{RX, TX};
-
-class DummyCT;
-
-class RF12B {
-public:
- /* Constructor */
- RF12B(PinName SDI,
- PinName SDO,
- PinName SCK,
- PinName NCS,
- PinName NIRQ);
-
-
-
- /* Reads a packet of data. Returns false if read failed. Use available() to check how much space to allocate for buffer */
- bool read(unsigned char* data, unsigned int size);
-
- /* Reads a byte of data from the receive buffer
- Returns 0xFF if there is no data */
- unsigned char read();
-
- /* Transmits a packet of data */
- void write(unsigned char* data, unsigned char length);
- void write(unsigned char data); /* 1-byte packet */
-// void write(std::queue<char> &data, int length = -1); /* sends a whole queue */
-
- /* Returns the packet length if data is available in the receive buffer, 0 otherwise*/
- unsigned int available();
-
- /** A assigns a callback function when a new reading is available **/
- void (*callbackfunc)(unsigned char rx_code);
- DummyCT* callbackobj;
- void (DummyCT::*mcallbackfunc)(unsigned char rx_code);
-
-protected:
- /* Receive FIFO buffer */
-// std::queue<unsigned char> fifo;
-// std::queue<unsigned char> temp; //for storing stuff mid-packet
-
- /* SPI module */
- SPI spi;
-
- /* Other digital pins */
- DigitalOut NCS;
- InterruptIn NIRQ;
- DigitalIn NIRQ_in;
- //DigitalOut rfled;
-
- rfmode_t mode;
-
- /* Initialises the RF12B module */
- void init();
-
- /* Write a command to the RF Module */
- unsigned int writeCmd(unsigned int cmd);
-
- /* Sends a byte of data across RF */
- void send(unsigned char data);
-
- /* Switch module between receive and transmit modes */
- void changeMode(rfmode_t mode);
-
- /* Interrupt routine for data reception */
- void rxISR();
-
- /* Tell the RF Module this packet is received and wait for the next */
- void resetRX();
-
- /* Return the RF Module Status word */
- unsigned int status();
-
- /* Calculate CRC8 */
- unsigned char crc8(unsigned char crc, unsigned char data);
-};
-
-#endif /* _RF12B_H */
\ No newline at end of file
--- a/Kalman/Sonar/RF12B/RF_defs.h Wed Nov 14 15:48:20 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,478 +0,0 @@
-/*
- * Open HR20
- *
- * target: ATmega169 @ 4 MHz in Honnywell Rondostat HR20E
- *
- * compiler: WinAVR-20071221
- * avr-libc 1.6.0
- * GCC 4.2.2
- *
- * copyright: 2008 Dario Carluccio (hr20-at-carluccio-dot-de)
- * 2008 Jiri Dobry (jdobry-at-centrum-dot-cz)
- * 2008 Mario Fischer (MarioFischer-at-gmx-dot-net)
- * 2007 Michael Smola (Michael-dot-Smola-at-gmx-dot-net)
- *
- * license: This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU Library General Public
- * License as published by the Free Software Foundation; either
- * version 2 of the License, or (at your option) any later version.
- *
- * This program 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 this program. If not, see http:*www.gnu.org/licenses
- */
-
-/*
- * \file rfm.h
- * \brief functions to control the RFM12 Radio Transceiver Module
- * \author Mario Fischer <MarioFischer-at-gmx-dot-net>; Michael Smola <Michael-dot-Smola-at-gmx-dot-net>
- * \date $Date: 2010/04/17 17:57:02 $
- * $Rev: 260 $
- */
-
-
-//#pragma once // multi-iclude prevention. gcc knows this pragma
-#ifndef rfm_H
-#define rfm_H
-
-
-#define RFM_SPI_16(OUTVAL) rfm_spi16(OUTVAL) //<! a function that gets a uint16_t (clocked out value) and returns a uint16_t (clocked in value)
-
-#define RFM_CLK_OUTPUT 0
-
-/*
-#define RFM_TESTPIN_INIT
-#define RFM_TESTPIN_ON
-#define RFM_TESTPIN_OFF
-#define RFM_TESTPIN_TOG
-
-#define RFM_CONFIG_DISABLE 0x00 //<! RFM_CONFIG_*** are combinable flags, what the RFM shold do
-#define RFM_CONFIG_BROADCASTSTATUS 0x01 //<! Flag that enables the HR20's status broadcast every minute
-
-#define RFM_CONFIG_ENABLEALL 0xff
-*/
-
-
-///////////////////////////////////////////////////////////////////////////////
-//
-// RFM status bits
-//
-///////////////////////////////////////////////////////////////////////////////
-
-// Interrupt bits, latched ////////////////////////////////////////////////////
-
-#define RFM_STATUS_FFIT 0x8000 // RX FIFO reached the progr. number of bits
- // Cleared by any FIFO read method
-
-#define RFM_STATUS_RGIT 0x8000 // TX register is ready to receive
- // Cleared by TX write
-
-#define RFM_STATUS_POR 0x4000 // Power On reset
- // Cleared by read status
-
-#define RFM_STATUS_RGUR 0x2000 // TX register underrun, register over write
- // Cleared by read status
-
-#define RFM_STATUS_FFOV 0x2000 // RX FIFO overflow
- // Cleared by read status
-
-#define RFM_STATUS_WKUP 0x1000 // Wake up timer overflow
- // Cleared by read status
-
-#define RFM_STATUS_EXT 0x0800 // Interupt changed to low
- // Cleared by read status
-
-#define RFM_STATUS_LBD 0x0400 // Low battery detect
-
-// Status bits ////////////////////////////////////////////////////////////////
-
-#define RFM_STATUS_FFEM 0x0200 // FIFO is empty
-#define RFM_STATUS_ATS 0x0100 // TX mode: Strong enough RF signal
-#define RFM_STATUS_RSSI 0x0100 // RX mode: signal strength above programmed limit
-#define RFM_STATUS_DQD 0x0080 // Data Quality detector output
-#define RFM_STATUS_CRL 0x0040 // Clock recovery lock
-#define RFM_STATUS_ATGL 0x0020 // Toggling in each AFC cycle
-
-///////////////////////////////////////////////////////////////////////////////
-//
-// 1. Configuration Setting Command
-//
-///////////////////////////////////////////////////////////////////////////////
-
-#define RFM_CONFIG 0x8000
-
-#define RFM_CONFIG_EL 0x8080 // Enable TX Register
-#define RFM_CONFIG_EF 0x8040 // Enable RX FIFO buffer
-#define RFM_CONFIG_BAND_315 0x8000 // Frequency band
-#define RFM_CONFIG_BAND_433 0x8010
-#define RFM_CONFIG_BAND_868 0x8020
-#define RFM_CONFIG_BAND_915 0x8030
-#define RFM_CONFIG_X_8_5pf 0x8000 // Crystal Load Capacitor
-#define RFM_CONFIG_X_9_0pf 0x8001
-#define RFM_CONFIG_X_9_5pf 0x8002
-#define RFM_CONFIG_X_10_0pf 0x8003
-#define RFM_CONFIG_X_10_5pf 0x8004
-#define RFM_CONFIG_X_11_0pf 0x8005
-#define RFM_CONFIG_X_11_5pf 0x8006
-#define RFM_CONFIG_X_12_0pf 0x8007
-#define RFM_CONFIG_X_12_5pf 0x8008
-#define RFM_CONFIG_X_13_0pf 0x8009
-#define RFM_CONFIG_X_13_5pf 0x800A
-#define RFM_CONFIG_X_14_0pf 0x800B
-#define RFM_CONFIG_X_14_5pf 0x800C
-#define RFM_CONFIG_X_15_0pf 0x800D
-#define RFM_CONFIG_X_15_5pf 0x800E
-#define RFM_CONFIG_X_16_0pf 0x800F
-
-///////////////////////////////////////////////////////////////////////////////
-//
-// 2. Power Management Command
-//
-///////////////////////////////////////////////////////////////////////////////
-
-#define RFM_POWER_MANAGEMENT 0x8200
-
-#define RFM_POWER_MANAGEMENT_ER 0x8280 // Enable receiver
-#define RFM_POWER_MANAGEMENT_EBB 0x8240 // Enable base band block
-#define RFM_POWER_MANAGEMENT_ET 0x8220 // Enable transmitter
-#define RFM_POWER_MANAGEMENT_ES 0x8210 // Enable synthesizer
-#define RFM_POWER_MANAGEMENT_EX 0x8208 // Enable crystal oscillator
-#define RFM_POWER_MANAGEMENT_EB 0x8204 // Enable low battery detector
-#define RFM_POWER_MANAGEMENT_EW 0x8202 // Enable wake-up timer
-#define RFM_POWER_MANAGEMENT_DC 0x8201 // Disable clock output of CLK pin
-
-#ifndef RFM_CLK_OUTPUT
- #error RFM_CLK_OUTPUT must be defined to 0 or 1
-#endif
-#if RFM_CLK_OUTPUT
- #define RFM_TX_ON_PRE() RFM_SPI_16( \
- RFM_POWER_MANAGEMENT_ES | \
- RFM_POWER_MANAGEMENT_EX )
- #define RFM_TX_ON() RFM_SPI_16( \
- RFM_POWER_MANAGEMENT_ET | \
- RFM_POWER_MANAGEMENT_ES | \
- RFM_POWER_MANAGEMENT_EX )
- #define RFM_RX_ON() RFM_SPI_16( \
- RFM_POWER_MANAGEMENT_ER | \
- RFM_POWER_MANAGEMENT_EBB | \
- RFM_POWER_MANAGEMENT_ES | \
- RFM_POWER_MANAGEMENT_EX )
- #define RFM_OFF() RFM_SPI_16( \
- RFM_POWER_MANAGEMENT_EX )
-#else
- #define RFM_TX_ON_PRE() RFM_SPI_16( \
- RFM_POWER_MANAGEMENT_DC | \
- RFM_POWER_MANAGEMENT_ES | \
- RFM_POWER_MANAGEMENT_EX )
- #define RFM_TX_ON() RFM_SPI_16( \
- RFM_POWER_MANAGEMENT_DC | \
- RFM_POWER_MANAGEMENT_ET | \
- RFM_POWER_MANAGEMENT_ES | \
- RFM_POWER_MANAGEMENT_EX )
- #define RFM_RX_ON() RFM_SPI_16( \
- RFM_POWER_MANAGEMENT_DC | \
- RFM_POWER_MANAGEMENT_ER | \
- RFM_POWER_MANAGEMENT_EBB | \
- RFM_POWER_MANAGEMENT_ES | \
- RFM_POWER_MANAGEMENT_EX )
- #define RFM_OFF() RFM_SPI_16(RFM_POWER_MANAGEMENT_DC)
-#endif
-///////////////////////////////////////////////////////////////////////////////
-//
-// 3. Frequency Setting Command
-//
-///////////////////////////////////////////////////////////////////////////////
-
-#define RFM_FREQUENCY 0xA000
-
-#define RFM_FREQ_315Band(v) (uint16_t)((v/10.0-31)*4000)
-#define RFM_FREQ_433Band(v) (uint16_t)((v/10.0-43)*4000)
-#define RFM_FREQ_868Band(v) (uint16_t)((v/20.0-43)*4000)
-#define RFM_FREQ_915Band(v) (uint16_t)((v/30.0-30)*4000)
-
-///////////////////////////////////////////////////////////////////////////////
-//
-// 4. Data Rate Command
-//
-/////////////////////////////////////////////////////////////////////////////////
-
-#define RFM_BAUD_RATE 9600
-
-#define RFM_DATA_RATE 0xC600
-
-#define RFM_DATA_RATE_CS 0xC680
-#define RFM_DATA_RATE_4800 0xC647
-#define RFM_DATA_RATE_9600 0xC623
-#define RFM_DATA_RATE_19200 0xC611
-#define RFM_DATA_RATE_38400 0xC608
-#define RFM_DATA_RATE_57600 0xC605
-
-#define RFM_SET_DATARATE(baud) ( ((baud)<5400) ? (RFM_DATA_RATE_CS|((43104/(baud))-1)) : (RFM_DATA_RATE|((344828UL/(baud))-1)) )
-
-///////////////////////////////////////////////////////////////////////////////
-//
-// 5. Receiver Control Command
-//
-///////////////////////////////////////////////////////////////////////////////
-
-#define RFM_RX_CONTROL 0x9000
-
-#define RFM_RX_CONTROL_P20_INT 0x9000 // Pin20 = ExternalInt
-#define RFM_RX_CONTROL_P20_VDI 0x9400 // Pin20 = VDI out
-
-#define RFM_RX_CONTROL_VDI_FAST 0x9000 // fast VDI Response time
-#define RFM_RX_CONTROL_VDI_MED 0x9100 // medium
-#define RFM_RX_CONTROL_VDI_SLOW 0x9200 // slow
-#define RFM_RX_CONTROL_VDI_ON 0x9300 // Always on
-
-#define RFM_RX_CONTROL_BW_400 0x9020 // bandwidth 400kHz
-#define RFM_RX_CONTROL_BW_340 0x9040 // bandwidth 340kHz
-#define RFM_RX_CONTROL_BW_270 0x9060 // bandwidth 270kHz
-#define RFM_RX_CONTROL_BW_200 0x9080 // bandwidth 200kHz
-#define RFM_RX_CONTROL_BW_134 0x90A0 // bandwidth 134kHz
-#define RFM_RX_CONTROL_BW_67 0x90C0 // bandwidth 67kHz
-
-#define RFM_RX_CONTROL_GAIN_0 0x9000 // LNA gain 0db
-#define RFM_RX_CONTROL_GAIN_6 0x9008 // LNA gain -6db
-#define RFM_RX_CONTROL_GAIN_14 0x9010 // LNA gain -14db
-#define RFM_RX_CONTROL_GAIN_20 0x9018 // LNA gain -20db
-
-#define RFM_RX_CONTROL_RSSI_103 0x9000 // DRSSI threshold -103dbm
-#define RFM_RX_CONTROL_RSSI_97 0x9001 // DRSSI threshold -97dbm
-#define RFM_RX_CONTROL_RSSI_91 0x9002 // DRSSI threshold -91dbm
-#define RFM_RX_CONTROL_RSSI_85 0x9003 // DRSSI threshold -85dbm
-#define RFM_RX_CONTROL_RSSI_79 0x9004 // DRSSI threshold -79dbm
-#define RFM_RX_CONTROL_RSSI_73 0x9005 // DRSSI threshold -73dbm
-//#define RFM_RX_CONTROL_RSSI_67 0x9006 // DRSSI threshold -67dbm // RF12B reserved
-//#define RFM_RX_CONTROL_RSSI_61 0x9007 // DRSSI threshold -61dbm // RF12B reserved
-
-#define RFM_RX_CONTROL_BW(baud) (((baud)<8000) ? \
- RFM_RX_CONTROL_BW_67 : \
- ( \
- ((baud)<30000) ? \
- RFM_RX_CONTROL_BW_134 : \
- RFM_RX_CONTROL_BW_200 \
- ))
-
-///////////////////////////////////////////////////////////////////////////////
-//
-// 6. Data Filter Command
-//
-///////////////////////////////////////////////////////////////////////////////
-
-#define RFM_DATA_FILTER 0xC228
-
-#define RFM_DATA_FILTER_AL 0xC2A8 // clock recovery auto-lock
-#define RFM_DATA_FILTER_ML 0xC268 // clock recovery fast mode
-#define RFM_DATA_FILTER_DIG 0xC228 // data filter type digital
-#define RFM_DATA_FILTER_ANALOG 0xC238 // data filter type analog
-#define RFM_DATA_FILTER_DQD(level) (RFM_DATA_FILTER | (level & 0x7))
-
-///////////////////////////////////////////////////////////////////////////////
-//
-// 7. FIFO and Reset Mode Command
-//
-///////////////////////////////////////////////////////////////////////////////
-
-#define RFM_FIFO 0xCA00
-
-#define RFM_FIFO_AL 0xCA04 // FIFO Start condition sync-word/always
-#define RFM_FIFO_FF 0xCA02 // Enable FIFO fill
-#define RFM_FIFO_DR 0xCA01 // Disable hi sens reset mode
-#define RFM_FIFO_IT(level) (RFM_FIFO | (( (level) & 0xF)<<4))
-
-#define RFM_FIFO_OFF() RFM_SPI_16(RFM_FIFO_IT(8) | RFM_FIFO_DR)
-#define RFM_FIFO_ON() RFM_SPI_16(RFM_FIFO_IT(8) | RFM_FIFO_FF | RFM_FIFO_DR)
-
-/////////////////////////////////////////////////////////////////////////////
-//
-// 8. Receiver FIFO Read
-//
-/////////////////////////////////////////////////////////////////////////////
-
-#define RFM_READ_FIFO() (RFM_SPI_16(0xB000) & 0xFF)
-
-/////////////////////////////////////////////////////////////////////////////
-//
-// 9. AFC Command
-//
-/////////////////////////////////////////////////////////////////////////////
-
-#define RFM_AFC 0xC400
-
-#define RFM_AFC_EN 0xC401
-#define RFM_AFC_OE 0xC402
-#define RFM_AFC_FI 0xC404
-#define RFM_AFC_ST 0xC408
-
-// Limits the value of the frequency offset register to the next values:
-
-#define RFM_AFC_RANGE_LIMIT_NO 0xC400 // 0: No restriction
-#define RFM_AFC_RANGE_LIMIT_15_16 0xC410 // 1: +15 fres to -16 fres
-#define RFM_AFC_RANGE_LIMIT_7_8 0xC420 // 2: +7 fres to -8 fres
-#define RFM_AFC_RANGE_LIMIT_3_4 0xC430 // 3: +3 fres to -4 fres
-
-// fres=2.5 kHz in 315MHz and 433MHz Bands
-// fres=5.0 kHz in 868MHz Band
-// fres=7.5 kHz in 915MHz Band
-
-#define RFM_AFC_AUTO_OFF 0xC400 // 0: Auto mode off (Strobe is controlled by microcontroller)
-#define RFM_AFC_AUTO_ONCE 0xC440 // 1: Runs only once after each power-up
-#define RFM_AFC_AUTO_VDI 0xC480 // 2: Keep the foffset only during receiving(VDI=high)
-#define RFM_AFC_AUTO_INDEPENDENT 0xC4C0 // 3: Keep the foffset value independently trom the state of the VDI signal
-
-///////////////////////////////////////////////////////////////////////////////
-//
-// 10. TX Configuration Control Command
-//
-///////////////////////////////////////////////////////////////////////////////
-
-#define RFM_TX_CONTROL 0x9800
-
-#define RFM_TX_CONTROL_POW_0 0x9800
-#define RFM_TX_CONTROL_POW_3 0x9801
-#define RFM_TX_CONTROL_POW_6 0x9802
-#define RFM_TX_CONTROL_POW_9 0x9803
-#define RFM_TX_CONTROL_POW_12 0x9804
-#define RFM_TX_CONTROL_POW_15 0x9805
-#define RFM_TX_CONTROL_POW_18 0x9806
-#define RFM_TX_CONTROL_POW_21 0x9807
-#define RFM_TX_CONTROL_MOD_15 0x9800
-#define RFM_TX_CONTROL_MOD_30 0x9810
-#define RFM_TX_CONTROL_MOD_45 0x9820
-#define RFM_TX_CONTROL_MOD_60 0x9830
-#define RFM_TX_CONTROL_MOD_75 0x9840
-#define RFM_TX_CONTROL_MOD_90 0x9850
-#define RFM_TX_CONTROL_MOD_105 0x9860
-#define RFM_TX_CONTROL_MOD_120 0x9870
-#define RFM_TX_CONTROL_MOD_135 0x9880
-#define RFM_TX_CONTROL_MOD_150 0x9890
-#define RFM_TX_CONTROL_MOD_165 0x98A0
-#define RFM_TX_CONTROL_MOD_180 0x98B0
-#define RFM_TX_CONTROL_MOD_195 0x98C0
-#define RFM_TX_CONTROL_MOD_210 0x98D0
-#define RFM_TX_CONTROL_MOD_225 0x98E0
-#define RFM_TX_CONTROL_MOD_240 0x98F0
-#define RFM_TX_CONTROL_MP 0x9900
-
-#define RFM_TX_CONTROL_MOD(baud) (((baud)<8000) ? \
- RFM_TX_CONTROL_MOD_45 : \
- ( \
- ((baud)<20000) ? \
- RFM_TX_CONTROL_MOD_60 : \
- ( \
- ((baud)<30000) ? \
- RFM_TX_CONTROL_MOD_75 : \
- ( \
- ((baud)<40000) ? \
- RFM_TX_CONTROL_MOD_90 : \
- RFM_TX_CONTROL_MOD_120 \
- ) \
- ) \
- ))
-
-/////////////////////////////////////////////////////////////////////////////
-//
-// 11. Transmitter Register Write Command
-//
-/////////////////////////////////////////////////////////////////////////////
-
-//#define RFM_WRITE(byte) RFM_SPI_16(0xB800 | ((byte) & 0xFF))
-#define RFM_WRITE(byte) RFM_SPI_16(0xB800 | (byte) )
-
-///////////////////////////////////////////////////////////////////////////////
-//
-// 12. Wake-up Timer Command
-//
-///////////////////////////////////////////////////////////////////////////////
-
-#define RFM_WAKEUP_TIMER 0xE000
-#define RFM_WAKEUP_SET(time) RFM_SPI_16(RFM_WAKEUP_TIMER | (time))
-
-#define RFM_WAKEUP_480s (RFM_WAKEUP_TIMER |(11 << 8)| 234)
-#define RFM_WAKEUP_240s (RFM_WAKEUP_TIMER |(10 << 8)| 234)
-#define RFM_WAKEUP_120s (RFM_WAKEUP_TIMER |(9 << 8)| 234)
-#define RFM_WAKEUP_119s (RFM_WAKEUP_TIMER |(9 << 8)| 232)
-
-#define RFM_WAKEUP_60s (RFM_WAKEUP_TIMER |(8 << 8) | 235)
-#define RFM_WAKEUP_59s (RFM_WAKEUP_TIMER |(8 << 8) | 230)
-
-#define RFM_WAKEUP_30s (RFM_WAKEUP_TIMER |(7 << 8) | 235)
-#define RFM_WAKEUP_29s (RFM_WAKEUP_TIMER |(7 << 8) | 227)
-
-#define RFM_WAKEUP_8s (RFM_WAKEUP_TIMER |(5 << 8) | 250)
-#define RFM_WAKEUP_7s (RFM_WAKEUP_TIMER |(5 << 8) | 219)
-#define RFM_WAKEUP_6s (RFM_WAKEUP_TIMER |(6 << 8) | 94)
-#define RFM_WAKEUP_5s (RFM_WAKEUP_TIMER |(5 << 8) | 156)
-#define RFM_WAKEUP_4s (RFM_WAKEUP_TIMER |(5 << 8) | 125)
-#define RFM_WAKEUP_1s (RFM_WAKEUP_TIMER |(2 << 8) | 250)
-#define RFM_WAKEUP_900ms (RFM_WAKEUP_TIMER |(2 << 8) | 225)
-#define RFM_WAKEUP_800ms (RFM_WAKEUP_TIMER |(2 << 8) | 200)
-#define RFM_WAKEUP_700ms (RFM_WAKEUP_TIMER |(2 << 8) | 175)
-#define RFM_WAKEUP_600ms (RFM_WAKEUP_TIMER |(2 << 8) | 150)
-#define RFM_WAKEUP_500ms (RFM_WAKEUP_TIMER |(2 << 8) | 125)
-#define RFM_WAKEUP_400ms (RFM_WAKEUP_TIMER |(2 << 8) | 100)
-#define RFM_WAKEUP_300ms (RFM_WAKEUP_TIMER |(2 << 8) | 75)
-#define RFM_WAKEUP_200ms (RFM_WAKEUP_TIMER |(2 << 8) | 50)
-#define RFM_WAKEUP_100ms (RFM_WAKEUP_TIMER |(2 << 8) | 25)
-
-///////////////////////////////////////////////////////////////////////////////
-//
-// 13. Low Duty-Cycle Command
-//
-///////////////////////////////////////////////////////////////////////////////
-
-#define RFM_LOW_DUTY_CYCLE 0xC800
-
-///////////////////////////////////////////////////////////////////////////////
-//
-// 14. Low Battery Detector Command
-//
-///////////////////////////////////////////////////////////////////////////////
-
-#define RFM_LOW_BATT_DETECT 0xC000
-#define RFM_LOW_BATT_DETECT_D_1MHZ 0xC000
-#define RFM_LOW_BATT_DETECT_D_1_25MHZ 0xC020
-#define RFM_LOW_BATT_DETECT_D_1_66MHZ 0xC040
-#define RFM_LOW_BATT_DETECT_D_2MHZ 0xC060
-#define RFM_LOW_BATT_DETECT_D_2_5MHZ 0xC080
-#define RFM_LOW_BATT_DETECT_D_3_33MHZ 0xC0A0
-#define RFM_LOW_BATT_DETECT_D_5MHZ 0xC0C0
-#define RFM_LOW_BATT_DETECT_D_10MHZ 0xC0E0
-
-///////////////////////////////////////////////////////////////////////////////
-//
-// 15. Status Read Command
-//
-///////////////////////////////////////////////////////////////////////////////
-
-#define RFM_READ_STATUS() RFM_SPI_16(0x0000)
-#define RFM_READ_STATUS_FFIT() SPI_1 (0x00)
-#define RFM_READ_STATUS_RGIT RFM_READ_STATUS_FFIT
-
-///////////////////////////////////////////////////////////////////////////////
-
-// RFM air protocol flags:
-
-#define RFMPROTO_FLAGS_BITASK_PACKETTYPE 0b11000000 //!< the uppermost 2 bits of the flags field encode the packettype
-#define RFMPROTO_FLAGS_PACKETTYPE_BROADCAST 0b00000000 //!< broadcast packettype (message from hr20, protocol; step 1)
-#define RFMPROTO_FLAGS_PACKETTYPE_COMMAND 0b01000000 //!< command packettype (message to hr20, protocol; step 2)
-#define RFMPROTO_FLAGS_PACKETTYPE_REPLY 0b10000000 //!< reply packettype (message from hr20, protocol; step 3)
-#define RFMPROTO_FLAGS_PACKETTYPE_SPECIAL 0b11000000 //!< currently unused packettype
-
-#define RFMPROTO_FLAGS_BITASK_DEVICETYPE 0b00011111 //!< the lowermost 5 bytes denote the device type. this way other sensors and actors may coexist
-#define RFMPROTO_FLAGS_DEVICETYPE_OPENHR20 0b00010100 //!< topen HR20 device type. 10100 is for decimal 20
-
-#define RFMPROTO_IS_PACKETTYPE_BROADCAST(FLAGS) ( RFMPROTO_FLAGS_PACKETTYPE_BROADCAST == ((FLAGS) & RFMPROTO_FLAGS_BITASK_PACKETTYPE) )
-#define RFMPROTO_IS_PACKETTYPE_COMMAND(FLAGS) ( RFMPROTO_FLAGS_PACKETTYPE_COMMAND == ((FLAGS) & RFMPROTO_FLAGS_BITASK_PACKETTYPE) )
-#define RFMPROTO_IS_PACKETTYPE_REPLY(FLAGS) ( RFMPROTO_FLAGS_PACKETTYPE_REPLY == ((FLAGS) & RFMPROTO_FLAGS_BITASK_PACKETTYPE) )
-#define RFMPROTO_IS_PACKETTYPE_SPECIAL(FLAGS) ( RFMPROTO_FLAGS_PACKETTYPE_SPECIAL == ((FLAGS) & RFMPROTO_FLAGS_BITASK_PACKETTYPE) )
-#define RFMPROTO_IS_DEVICETYPE_OPENHR20(FLAGS) ( RFMPROTO_FLAGS_DEVICETYPE_OPENHR20 == ((FLAGS) & RFMPROTO_FLAGS_BITASK_DEVICETYPE) )
-
-///////////////////////////////////////////////////////////////////////////////
-
-#endif
\ No newline at end of file
--- a/Kalman/Sonar/RFSRF05.cpp Wed Nov 14 15:48:20 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,183 +0,0 @@
-
-#include "RFSRF05.h"
-#include "mbed.h"
-#include "globals.h"
-#include "system.h"
-
-
-RFSRF05::RFSRF05(PinName trigger,
- PinName echo0,
- PinName echo1,
- PinName echo2,
- PinName echo3,
- PinName echo4,
- PinName echo5,
- PinName SDI,
- PinName SDO,
- PinName SCK,
- PinName NCS,
- PinName NIRQ)
- : _rf(SDI,SDO,SCK,NCS,NIRQ),
- _trigger(trigger),
- _echo0(echo0),
- _echo1(echo1),
- _echo2(echo2),
- _echo3(echo3),
- _echo4(echo4),
- _echo5(echo5) {
-
- // initialises codes
- codes[0] = CODE0;
- codes[1] = CODE1;
- codes[2] = CODE2;
-
- //set callback execute to true
- ValidPulse = false;
-
- // Attach interrupts
-#ifdef SONAR_ECHO_INV
- // inverted sonar inputs
- _echo5.fall(this, &RFSRF05::_rising);
- _echo0.rise(this, &RFSRF05::_falling);
- _echo1.rise(this, &RFSRF05::_falling);
- _echo2.rise(this, &RFSRF05::_falling);
- _echo3.rise(this, &RFSRF05::_falling);
- _echo4.rise(this, &RFSRF05::_falling);
- _echo5.rise(this, &RFSRF05::_falling);
-#else
- _echo5.rise(this, &RFSRF05::_rising);
- _echo0.fall(this, &RFSRF05::_falling);
- _echo1.fall(this, &RFSRF05::_falling);
- _echo2.fall(this, &RFSRF05::_falling);
- _echo3.fall(this, &RFSRF05::_falling);
- _echo4.fall(this, &RFSRF05::_falling);
- _echo5.fall(this, &RFSRF05::_falling);
-#endif
-
-
- //init callabck function
- callbackfunc = NULL;
- callbackobj = NULL;
- mcallbackfunc = NULL;
-
- // innitialises beacon counter
- _beacon_counter = 0;
-
-#ifdef ROBOT_PRIMARY
- //Interrupts every 50ms for primary robot
- _ticker.attach(this, &RFSRF05::_startRange, 0.05);
-#else
- //attach callback
- _rf.callbackobj = (DummyCT*)this;
- _rf.mcallbackfunc = (void (DummyCT::*)(unsigned char rx_data)) &RFSRF05::startRange;
-#endif
-
-}
-
-#ifdef ROBOT_PRIMARY
-void RFSRF05::_startRange() {
-
- //printf("Srange\r\r");
-
- // increments counter
- _beacon_counter = (_beacon_counter + 1) % 3;
-
-
- // set flags
- ValidPulse = false;
- expValidPulse = true;
-
- // writes code to RF port
- _rf.write(codes[_beacon_counter]);
-
- // send a trigger pulse, 10uS long
- _trigger = 1;
- wait_us (10);
- _trigger = 0;
-
-}
-#else
-
-void RFSRF05::startRange(unsigned char rx_code) {
- for (int i = 0; i < 3; i++) {
- if (rx_code == codes[i]) {
-
- // assign beacon_counter
- _beacon_counter = i;
-
- // set flags
- ValidPulse = false;
- expValidPulse = true;
-
- // send a trigger pulse, 10uS long
- _trigger = 1;
- wait_us (10);
- _trigger = 0;
- break;
- }
- }
-}
-#endif
-
-// Clear and start the timer at the begining of the echo pulse
-void RFSRF05::_rising(void) {
-
- _timer.reset();
- _timer.start();
-
- //Set callback execute to ture
- if (expValidPulse) {
- ValidPulse = true;
- expValidPulse = false;
- }
-}
-
-// Stop and read the timer at the end of the pulse
-void RFSRF05::_falling(void) {
- _timer.stop();
-
- if (ValidPulse) {
- //printf("Validpulse trig!\r\n");
- ValidPulse = false;
-
- //Calucate distance
- //true offset is about 100, we put 300 so circles overlap
- _dist[_beacon_counter] = _timer.read_us()/2.9 + 300;
-
- if (callbackfunc)
- (*callbackfunc)(_beacon_counter, _dist[_beacon_counter]);
-
- if (callbackobj && mcallbackfunc)
- (callbackobj->*mcallbackfunc)(_beacon_counter, _dist[_beacon_counter], sonarvariance);
-
- }
-
-}
-
-float RFSRF05::read0() {
- // returns distance
- return (_dist[0]);
-}
-
-float RFSRF05::read1() {
- // returns distance
- return (_dist[1]);
-}
-
-float RFSRF05::read2() {
- // returns distance
- return (_dist[2]);
-}
-
-float RFSRF05::read(unsigned int beaconnum) {
- // returns distance
- return (_dist[beaconnum]);
-}
-
-void RFSRF05::setCode(int code_index, unsigned char code) {
- codes[code_index] = code;
-}
-
-//SRF05::operator float() {
-// return read();
-//}
--- a/Kalman/Sonar/RFSRF05.h Wed Nov 14 15:48:20 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,102 +0,0 @@
-
-#ifndef MBED_RFSRF05_H
-#define MBED_RFSRF05_H
-
-
-
-#include "mbed.h"
-#include "RF12B.h"
-#include "globals.h"
-
-
-#define CODE0 0x22
-#define CODE1 0x44
-#define CODE2 0x88
-
-/* SAMPLE IMPLEMENTATION!
-RFSRF05 my_srf(p13,p21,p22,p23,p24,p25,p26,p5,p6,p7,p8,p9);
-
-
-void callbinmain(int num, float dist) {
- //Here is where you deal with your brand new reading ;D
-}
-
-int main() {
- pc.printf("Hello World of RobotSonar!\r\n");
- my_srf.callbackfunc = callbinmain;
-
- while (1);
-}
-
- */
-
-class DummyCT;
-
-class RFSRF05 {
-public:
-
- RFSRF05(
- PinName trigger,
- PinName echo0,
- PinName echo1,
- PinName echo2,
- PinName echo3,
- PinName echo4,
- PinName echo5,
- PinName SDI,
- PinName SDO,
- PinName SCK,
- PinName NCS,
- PinName NIRQ);
-
- /** A non-blocking function that will return the last measurement
- *
- * @returns floating point representation of distance in mm
- */
- float read0();
- float read1();
- float read2();
- float read(unsigned int beaconnum);
-
-
- /** A assigns a callback function when a new reading is available **/
- void (*callbackfunc)(int beaconnum, float distance);
- DummyCT* callbackobj;
- void (DummyCT::*mcallbackfunc)(int beaconnum, float distance, float variance);
-
- //triggers a read
- #ifndef ROBOT_PRIMARY
- void startRange(unsigned char rx_code);
- #endif
-
- //set codes
- void setCode(int code_index, unsigned char code);
- unsigned char codes[3];
-
- /** A short hand way of using the read function */
- //operator float();
-
-private :
- RF12B _rf;
- DigitalOut _trigger;
- InterruptIn _echo0;
- InterruptIn _echo1;
- InterruptIn _echo2;
- InterruptIn _echo3;
- InterruptIn _echo4;
- InterruptIn _echo5;
- Timer _timer;
- Ticker _ticker;
- #ifdef ROBOT_PRIMARY
- void _startRange(void);
- #endif
- void _rising (void);
- void _falling (void);
- float _dist[3];
- int _beacon_counter;
- bool ValidPulse;
- bool expValidPulse;
-
-};
-
-#endif
Oskar Weigl
