parent
161b2ce88f
commit
5f9aaa0c1b
@ -0,0 +1,169 @@ |
||||
#include <avr/io.h> |
||||
#include <util/delay.h> |
||||
#include <stdint.h> |
||||
#include <stdbool.h> |
||||
|
||||
#include "pins.h" |
||||
#include "sonar.h" |
||||
|
||||
// Currently measured sonar
|
||||
volatile sonar_t *_sonar_active_so; |
||||
|
||||
// Flag that measurement is in progress
|
||||
volatile bool _busy; |
||||
|
||||
// Result of last measurement, in millimeters
|
||||
volatile int16_t _result = 0xFFFF; |
||||
|
||||
|
||||
void _sonar_init_do(sonar_t* so, PORT_P port, uint8_t ntx, PORT_P pin, uint8_t nrx) |
||||
{ |
||||
so->port = port; |
||||
so->ntx = ntx; |
||||
so->pin = pin; |
||||
so->nrx = nrx; |
||||
|
||||
switch((const uint16_t) pin) { |
||||
case (const uint16_t)&PINB: |
||||
so->bank = 0; |
||||
break; |
||||
case (const uint16_t)&PINC: |
||||
so->bank = 1; |
||||
break; |
||||
case (const uint16_t)&PIND: |
||||
so->bank = 2; |
||||
break; |
||||
} |
||||
} |
||||
|
||||
|
||||
/**
|
||||
* Start sonar measurement |
||||
* Interrupts must be enabled |
||||
* TIMER 1 will be used for the async measurement |
||||
* Timer 1 overflow and Pin Change interrupts must invoke Sonar handlers. |
||||
*/ |
||||
bool sonar_start(sonar_t* so) |
||||
{ |
||||
if (_busy) return false; |
||||
|
||||
_sonar_active_so = so; |
||||
|
||||
_busy = true; |
||||
|
||||
// make sure the timer is stopped (set clock to NONE)
|
||||
TCCR1B = 0; |
||||
|
||||
// Timer overflow interrupt enable
|
||||
// We'll stop measuring on overflow
|
||||
TIMSK1 |= (1 << TOIE1); |
||||
|
||||
// Clear the timer value
|
||||
TCNT1 = 0; |
||||
|
||||
// Set up pin change interrupt mask for the RX pin
|
||||
switch(so->bank) { |
||||
case 0: |
||||
PCMSK0 |= (1 << (so->nrx)); |
||||
break; |
||||
case 1: |
||||
PCMSK1 |= (1 << (so->nrx)); |
||||
break; |
||||
case 2: |
||||
PCMSK2 |= (1 << (so->nrx)); |
||||
break; |
||||
} |
||||
|
||||
// send positive pulse
|
||||
*(so->port) |= (1 << so->ntx); |
||||
_delay_us(_SNR_TRIG_TIME); |
||||
*(so->port) &= ~(1 << so->ntx); |
||||
|
||||
// Wait for start of response
|
||||
while ( (*(so->pin) & (1 << so->nrx)) == 0 ); |
||||
|
||||
// Set timer clock source: F_CPU / 8 (0.5 us resolution)
|
||||
TCCR1B = (0b010 << CS10); |
||||
|
||||
// Enable pin change interrupt
|
||||
PCICR |= (1 << (so->bank)); |
||||
|
||||
return true; |
||||
} |
||||
|
||||
|
||||
/** Stop the timer */ |
||||
void _sonar_stop() |
||||
{ |
||||
// stop timer
|
||||
TCCR1B = 0; |
||||
|
||||
// Disable RX pin interrupt mask
|
||||
switch(_sonar_active_so->bank) { |
||||
case 0: |
||||
PCMSK0 &= ~(1 << (_sonar_active_so->nrx)); |
||||
break; |
||||
case 1: |
||||
PCMSK1 &= ~(1 << (_sonar_active_so->nrx)); |
||||
break; |
||||
case 2: |
||||
PCMSK2 &= ~(1 << (_sonar_active_so->nrx)); |
||||
break; |
||||
} |
||||
|
||||
// Disable timer1 overflow interrupt
|
||||
TIMSK1 &= ~(1 << TOIE1); |
||||
|
||||
_busy = false; |
||||
} |
||||
|
||||
|
||||
/** Handle TIMER1_OVF (returns true if consumed) */ |
||||
bool sonar_handle_t1ovf() |
||||
{ |
||||
if (!_busy) return false; // nothing
|
||||
|
||||
_result = -1; |
||||
_sonar_stop(); |
||||
|
||||
return true; |
||||
} |
||||
|
||||
|
||||
/** Handle pin change interrupt (returns true if consumed) */ |
||||
bool sonar_handle_pci() |
||||
{ |
||||
if (!_busy) { |
||||
return false; // nothing
|
||||
} |
||||
|
||||
if (*(_sonar_active_so->pin) & (1 << _sonar_active_so->nrx)) { |
||||
// rx is high, not our pin change event
|
||||
return false; |
||||
} |
||||
|
||||
uint64_t x = TCNT1; |
||||
x /= _SNR_DIV_CONST; |
||||
x *= 100000000L; |
||||
x /= F_CPU; |
||||
_result = (int16_t) x; |
||||
|
||||
// no obstacle
|
||||
if (_result > _SNR_MAX_DIST) _result = -1; |
||||
|
||||
_sonar_stop(); |
||||
|
||||
return true; |
||||
} |
||||
|
||||
|
||||
bool sonar_busy() |
||||
{ |
||||
return _busy; |
||||
} |
||||
|
||||
|
||||
int16_t sonar_result() |
||||
{ |
||||
return _result; |
||||
} |
||||
@ -0,0 +1,70 @@ |
||||
#pragma once |
||||
|
||||
//
|
||||
// Utilities for working with the HC-SR04 ultrasonic sensor
|
||||
// Can be easily modified to work with other similar modules
|
||||
//
|
||||
// It's required that you call the sonar_handle_* functions from your ISRs
|
||||
// See example program for more info.
|
||||
//
|
||||
|
||||
#include <stdint.h> |
||||
#include <stdbool.h> |
||||
|
||||
#include "lib/pins.h" |
||||
|
||||
// Calib constant for the module
|
||||
// CM = uS / _DIV_CONST
|
||||
#define _SNR_DIV_CONST 58 |
||||
|
||||
// Max module distance in MM
|
||||
#define _SNR_MAX_DIST 4000 |
||||
|
||||
// Trigger time in uS
|
||||
#define _SNR_TRIG_TIME 10 |
||||
|
||||
|
||||
// Sonar data object
|
||||
typedef struct { |
||||
PORT_P port; // Tx PORT
|
||||
uint8_t ntx; // Tx bit number
|
||||
PORT_P pin; // Rx PIN
|
||||
uint8_t nrx; // Rx bit number
|
||||
uint8_t bank; // Rx PCINT bank
|
||||
} sonar_t; |
||||
|
||||
|
||||
// Create a Sonar port
|
||||
// Args: sonar_t* so, Trig pin, Echo pin
|
||||
#define sonar_init(so, trig, echo) do { \ |
||||
as_output(io_pack(trig)); \
|
||||
as_input_pu(io_pack(echo)); \
|
||||
_sonar_init_do(so, &io2port(io_pack(trig)), io2n(io_pack(trig)), &io2pin(io_pack(echo)), io2n(io_pack(echo))); \
|
||||
} while(0) |
||||
|
||||
// private, in header because of the macro.
|
||||
void _sonar_init_do(sonar_t* so, PORT_P port, uint8_t ntx, PORT_P pin, uint8_t nrx); |
||||
|
||||
|
||||
/** Check if sonar is busy */ |
||||
bool sonar_busy(); |
||||
|
||||
|
||||
/** Get result of last measurement, in millimeters. Returns -1 if no obstacle detected */ |
||||
int16_t sonar_result(); |
||||
|
||||
|
||||
/**
|
||||
* Start sonar measurement |
||||
* Interrupts must be enabled |
||||
* TIMER 1 will be used for the async measurement |
||||
*/ |
||||
bool sonar_start(sonar_t* so); |
||||
|
||||
|
||||
/** Handle TIMER1_OVF (returns true if consumed) */ |
||||
bool sonar_handle_t1ovf(); |
||||
|
||||
|
||||
/** Handle pin change interrupt (returns true if consumed) */ |
||||
bool sonar_handle_pci(); |
||||
Loading…
Reference in new issue