mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-05 15:38:29 -04:00
158 lines
3.6 KiB
C++
158 lines
3.6 KiB
C++
#include <AP_HAL.h>
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_ERLE
|
|
#include "SPIDriver.h"
|
|
#include <sys/types.h>
|
|
#include <sys/stat.h>
|
|
#include <fcntl.h>
|
|
#include <unistd.h>
|
|
#include <stdio.h>
|
|
#include <stdint.h>
|
|
#include <sys/ioctl.h>
|
|
#include <linux/spi/spidev.h>
|
|
#include "GPIO.h"
|
|
|
|
using namespace Linux;
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
#define SPI0_BUS_SPEED 1000000
|
|
#define SPI1_BUS_SPEED 2600000
|
|
|
|
LinuxSPIDeviceDriver::LinuxSPIDeviceDriver(const char *spipath, uint8_t mode, uint8_t bitsPerWord, uint8_t cs_pin, uint32_t speed):
|
|
_spipath(spipath),
|
|
_fd(-1),
|
|
_mode(mode),
|
|
_bitsPerWord(bitsPerWord),
|
|
_speed(speed),
|
|
_cs_pin(cs_pin)
|
|
{
|
|
}
|
|
|
|
void LinuxSPIDeviceDriver::init()
|
|
{
|
|
_fd = open(_spipath, O_RDWR);
|
|
if (_fd == -1) {
|
|
return;
|
|
}
|
|
int ret;
|
|
ret = ioctl(_fd, SPI_IOC_WR_MODE, &_mode);
|
|
if (ret == -1) {
|
|
goto failed;
|
|
}
|
|
|
|
ret = ioctl(_fd, SPI_IOC_RD_MODE, &_mode);
|
|
if (ret == -1) {
|
|
goto failed;
|
|
}
|
|
|
|
ret = ioctl(_fd, SPI_IOC_WR_BITS_PER_WORD, &_bitsPerWord);
|
|
if (ret == -1) {
|
|
goto failed;
|
|
}
|
|
|
|
ret = ioctl(_fd, SPI_IOC_RD_BITS_PER_WORD, &_bitsPerWord);
|
|
if (ret == -1) {
|
|
goto failed;
|
|
}
|
|
|
|
ret = ioctl(_fd, SPI_IOC_WR_MAX_SPEED_HZ, &_speed);
|
|
if (ret == -1) {
|
|
goto failed;
|
|
}
|
|
|
|
ret = ioctl(_fd, SPI_IOC_RD_MAX_SPEED_HZ, &_speed);
|
|
if (ret == -1) {
|
|
goto failed;
|
|
}
|
|
|
|
// Init the CS
|
|
_cs = hal.gpio->channel(_cs_pin);
|
|
_cs->mode(GPIO_OUT);
|
|
_cs->write(HIGH); // do not hold the SPI bus initially
|
|
return;
|
|
|
|
failed:
|
|
close(_fd);
|
|
_fd = -1;
|
|
}
|
|
|
|
AP_HAL::Semaphore* LinuxSPIDeviceDriver::get_semaphore()
|
|
{
|
|
return &_semaphore;
|
|
}
|
|
|
|
void LinuxSPIDeviceDriver::transaction(const uint8_t *tx, uint8_t *rx, uint16_t len)
|
|
{
|
|
cs_assert();
|
|
struct spi_ioc_transfer spi[1];
|
|
spi[0].tx_buf = (uint64_t)tx;
|
|
spi[0].rx_buf = (uint64_t)rx;
|
|
spi[0].len = len;
|
|
spi[0].delay_usecs = 0;
|
|
spi[0].speed_hz = _speed;
|
|
spi[0].bits_per_word = _bitsPerWord;
|
|
spi[0].cs_change = 0;
|
|
|
|
ioctl(_fd, SPI_IOC_MESSAGE(1), &spi);
|
|
cs_release();
|
|
}
|
|
|
|
|
|
void LinuxSPIDeviceDriver::cs_assert()
|
|
{
|
|
_cs->write(0);
|
|
}
|
|
|
|
void LinuxSPIDeviceDriver::cs_release()
|
|
{
|
|
_cs->write(1);
|
|
}
|
|
|
|
uint8_t LinuxSPIDeviceDriver::transfer(uint8_t data)
|
|
{
|
|
uint8_t v = 0;
|
|
transaction(&data, &v, 1);
|
|
return v;
|
|
}
|
|
|
|
void LinuxSPIDeviceDriver::transfer(const uint8_t *data, uint16_t len)
|
|
{
|
|
transaction(data, NULL, len);
|
|
}
|
|
|
|
LinuxSPIDeviceManager::LinuxSPIDeviceManager() :
|
|
_device_cs0("/dev/spidev2.0", SPI_MODE_0, 8, 7, SPI1_BUS_SPEED), /* SPIDevice_MS5611 */
|
|
_device_cs1("/dev/spidev2.0", SPI_MODE_0, 8, 113, SPI1_BUS_SPEED), /* SPIDevice_MPU6000 */
|
|
_device_cs2("/dev/spidev2.0", SPI_MODE_0, 8, 49, SPI1_BUS_SPEED), /* SPIDevice_MPU9250 */
|
|
_device_cs3("/dev/spidev1.0", SPI_MODE_0, 8, 5,SPI0_BUS_SPEED) /* SPIDevice_LSM9DS0 */
|
|
{}
|
|
|
|
void LinuxSPIDeviceManager::init(void *)
|
|
{
|
|
_device_cs0.init();
|
|
_device_cs1.init();
|
|
_device_cs2.init();
|
|
_device_cs3.init();
|
|
}
|
|
|
|
/*
|
|
return a SPIDeviceDriver for a particular device
|
|
*/
|
|
AP_HAL::SPIDeviceDriver* LinuxSPIDeviceManager::device(enum AP_HAL::SPIDevice dev)
|
|
{
|
|
switch (dev) {
|
|
case AP_HAL::SPIDevice_MPU6000:
|
|
return &_device_cs1;
|
|
case AP_HAL::SPIDevice_MPU9250:
|
|
return &_device_cs2;
|
|
case AP_HAL::SPIDevice_MS5611:
|
|
return &_device_cs0;
|
|
case AP_HAL::SPIDevice_LSM9DS0:
|
|
return &_device_cs3;
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
#endif // CONFIG_HAL_BOARD
|