You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
128 lines
3.2 KiB
128 lines
3.2 KiB
|
|
#include <avr/interrupt.h> |
|
#include <avr/io.h> |
|
|
|
#include "pins_arduino_mega.h" |
|
|
|
#include "GPIO.h" |
|
using namespace AP_HAL_AVR; |
|
|
|
|
|
ArduinoGPIO* ArduinoDigitalSource::parent; |
|
|
|
AP_HAL::Proc ArduinoGPIO::_interrupt_6 = NULL; |
|
|
|
SIGNAL(INT6_vect) { |
|
if (ArduinoGPIO::_interrupt_6) { |
|
ArduinoGPIO::_interrupt_6(); |
|
} |
|
} |
|
|
|
// Get the bit location within the hardware port of the given virtual pin. |
|
// This comes from the pins_*.c file for the active board configuration. |
|
|
|
#define analogInPinToBit(P) (P) |
|
|
|
|
|
// Get the bit location within the hardware port of the given virtual pin. |
|
// This comes from the pins_*.c file for the active board configuration. |
|
// |
|
// These perform slightly better as macros compared to inline functions |
|
// |
|
#define digitalPinToPort(P) ( pgm_read_byte( digital_pin_to_port_PGM + (P) ) ) |
|
#define digitalPinToBitMask(P) ( pgm_read_byte( digital_pin_to_bit_mask_PGM + (P) ) ) |
|
#define digitalPinToTimer(P) ( pgm_read_byte( digital_pin_to_timer_PGM + (P) ) ) |
|
#define analogInPinToBit(P) (P) |
|
#define portOutputRegister(P) ( (volatile uint8_t *)( pgm_read_word( port_to_output_PGM + (P))) ) |
|
#define portInputRegister(P) ( (volatile uint8_t *)( pgm_read_word( port_to_input_PGM + (P))) ) |
|
#define portModeRegister(P) ( (volatile uint8_t *)( pgm_read_word( port_to_mode_PGM + (P))) ) |
|
|
|
|
|
void ArduinoGPIO::pinMode(uint8_t pin, uint8_t mode) { |
|
uint8_t bit = digitalPinToBitMask(pin); |
|
uint8_t port = digitalPinToPort(pin); |
|
volatile uint8_t *reg; |
|
|
|
if (port == NOT_A_PIN) return; |
|
|
|
// JWS: can I let the optimizer do this? |
|
reg = portModeRegister(port); |
|
|
|
if (mode == GPIO_INPUT) { |
|
uint8_t oldSREG = SREG; |
|
cli(); |
|
*reg &= ~bit; |
|
SREG = oldSREG; |
|
} else { |
|
uint8_t oldSREG = SREG; |
|
cli(); |
|
*reg |= bit; |
|
SREG = oldSREG; |
|
} |
|
} |
|
|
|
uint8_t ArduinoGPIO::read(uint8_t pin) { |
|
uint8_t bit = digitalPinToBitMask(pin); |
|
uint8_t port = digitalPinToPort(pin); |
|
|
|
if (port == NOT_A_PIN) return 0; |
|
|
|
if (*portInputRegister(port) & bit) return 1; |
|
return 0; |
|
} |
|
|
|
void ArduinoGPIO::write(uint8_t pin, uint8_t value) { |
|
uint8_t bit = digitalPinToBitMask(pin); |
|
uint8_t port = digitalPinToPort(pin); |
|
volatile uint8_t *out; |
|
|
|
if (port == NOT_A_PIN) return; |
|
|
|
out = portOutputRegister(port); |
|
|
|
uint8_t oldSREG = SREG; |
|
cli(); |
|
|
|
if (value == 0) { |
|
*out &= ~bit; |
|
} else { |
|
*out |= bit; |
|
} |
|
|
|
SREG = oldSREG; |
|
} |
|
|
|
/* Implement GPIO Interrupt 6, used for MPU6000 data ready on APM2. */ |
|
bool ArduinoGPIO::attach_interrupt( |
|
int interrupt_num, AP_HAL::Proc proc, int mode) { |
|
if (interrupt_num == 6) { |
|
_interrupt_6 = proc; |
|
EICRB = (EICRB & ~((1 << ISC60) | (1 << ISC61))) | (mode << ISC60); |
|
EIMSK |= (1 << INT6); |
|
return true; |
|
} else { |
|
return false; |
|
} |
|
} |
|
|
|
|
|
AP_HAL::DigitalSource* ArduinoGPIO::channel(int n) { |
|
if (ArduinoDigitalSource::parent == NULL) { |
|
ArduinoDigitalSource::parent = this; |
|
} |
|
return new ArduinoDigitalSource(n); |
|
} |
|
|
|
void ArduinoDigitalSource::mode(uint8_t output) { |
|
parent->pinMode(_pin, output); |
|
} |
|
|
|
uint8_t ArduinoDigitalSource::read() { |
|
return parent->read(_pin); |
|
} |
|
|
|
void ArduinoDigitalSource::write(uint8_t value) { |
|
parent->write(_pin, value); |
|
} |
|
|
|
|
|
|