mirror of https://github.com/ArduPilot/ardupilot
280 lines
7.0 KiB
C++
280 lines
7.0 KiB
C++
/*
|
|
(c) 2017 night_ghost@ykoctpa.ru
|
|
|
|
|
|
a low-level interface for SD card driver
|
|
|
|
*/
|
|
#pragma GCC optimize ("O2")
|
|
|
|
#include "Sd2Card.h"
|
|
#include "SdFatFs.h"
|
|
#include <stdio.h>
|
|
#include <util.h>
|
|
#include <utility>
|
|
|
|
#include <systick.h>
|
|
#include <hal.h>
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include <GCS_MAVLink/GCS.h>
|
|
|
|
#if defined(BOARD_SDCARD_CS_PIN) || defined(BOARD_DATAFLASH_FATFS)
|
|
|
|
#include <AP_HAL_F4Light/AP_HAL_F4Light.h>
|
|
#include <AP_HAL_F4Light/SPIDevice.h>
|
|
#include <AP_HAL_F4Light/Scheduler.h>
|
|
|
|
|
|
using namespace F4Light;
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
static AP_HAL::OwnPtr<F4Light::SPIDevice> _spi;
|
|
AP_HAL::Semaphore *_spi_sem;
|
|
static AP_HAL::Device::Speed _speed;
|
|
|
|
|
|
/** Send a byte to the card */
|
|
void spi_spiSend(uint8_t b) {
|
|
return _spi->send(b);
|
|
}
|
|
|
|
/** Receive a byte from the card */
|
|
uint8_t spi_spiRecv(void) {
|
|
return _spi->transfer(0xFF);
|
|
}
|
|
|
|
uint8_t spi_spiXchg(uint8_t b) {
|
|
return _spi->transfer(b);
|
|
}
|
|
|
|
uint8_t spi_waitFor(uint8_t out, spi_WaitFunc cb, uint32_t dly) {
|
|
return _spi->wait_for(out, cb, dly);
|
|
}
|
|
|
|
void spi_spiTransfer(const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len) {
|
|
_spi->transfer(send, send_len, recv, recv_len);
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
|
|
|
|
void spi_yield(){
|
|
hal_yield(0); // while we wait - let others work
|
|
}
|
|
|
|
|
|
uint8_t spi_detect(){
|
|
#ifdef BOARD_SDCARD_DET_PIN
|
|
const stm32_pin_info &pp = PIN_MAP[BOARD_SDCARD_DET_PIN];
|
|
return gpio_read_bit(pp.gpio_device, pp.gpio_bit) == LOW;
|
|
#else
|
|
return 1;
|
|
#endif
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
uint32_t get_fattime()
|
|
{
|
|
uint64_t now = hal.util->get_system_clock_ms(); //millis + gps_time_shift
|
|
|
|
uint16_t year = 1970;
|
|
uint8_t month;
|
|
|
|
uint64_t seconds = now / 1000;
|
|
uint32_t sys_days = seconds / (24*60*60uL);
|
|
uint16_t day_seconds = seconds % (24*60*60uL);
|
|
|
|
uint32_t days = sys_days;
|
|
uint16_t y_day;
|
|
|
|
while(1) {
|
|
bool leapYear = (year % 4 == 0 && (year % 100 != 0 || year % 400 == 0));
|
|
uint16_t daysInYear = leapYear ? 366 : 365;
|
|
|
|
if (days >= daysInYear) {
|
|
days -= daysInYear;
|
|
++year;
|
|
} else {
|
|
y_day = days;
|
|
|
|
/* calculate the month and day */
|
|
static const uint8_t daysInMonth[12] = {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};
|
|
for(month = 0; month < 12; ++month) {
|
|
uint8_t dim = daysInMonth[month];
|
|
|
|
/* add a day to feburary if this is a leap year */
|
|
if (month == 1 && leapYear) ++dim;
|
|
|
|
if (days >= dim)
|
|
days -= dim;
|
|
else
|
|
break;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
uint16_t min = day_seconds / 60;
|
|
uint16_t hour = min /60;
|
|
|
|
uint16_t sec = day_seconds % 60;
|
|
|
|
|
|
/* Pack date and time into a uint32_t variable */
|
|
return ((uint32_t)(year - 1980) << 25)
|
|
| ((uint32_t)month << 21)
|
|
| ((uint32_t)days << 16)
|
|
| ((uint32_t)hour << 11)
|
|
| ((uint32_t)min << 5)
|
|
| ((uint32_t)sec >> 1);
|
|
}
|
|
|
|
#endif
|
|
|
|
|
|
#if defined(BOARD_SDCARD_CS_PIN)
|
|
|
|
void spi_chipSelectHigh(void) {
|
|
_spi->wait_busy();
|
|
const stm32_pin_info &pp = PIN_MAP[BOARD_SDCARD_CS_PIN];
|
|
gpio_write_bit(pp.gpio_device, pp.gpio_bit, HIGH);
|
|
_spi_sem->give();
|
|
}
|
|
|
|
bool spi_chipSelectLow(bool take_sem) {
|
|
if(take_sem){
|
|
if(!_spi_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) return false;
|
|
|
|
_spi->set_speed(_speed);
|
|
|
|
}
|
|
const stm32_pin_info &pp = PIN_MAP[BOARD_SDCARD_CS_PIN];
|
|
gpio_write_bit(pp.gpio_device, pp.gpio_bit, LOW);
|
|
return true;
|
|
}
|
|
|
|
|
|
uint8_t Sd2Card::init(AP_HAL::OwnPtr<F4Light::SPIDevice> spi) {
|
|
|
|
_spi = std::move(spi);
|
|
|
|
|
|
// set pin modes
|
|
{
|
|
const stm32_pin_info &pp = PIN_MAP[BOARD_SDCARD_CS_PIN];
|
|
gpio_set_mode(pp.gpio_device, pp.gpio_bit, GPIO_OUTPUT_PP);
|
|
gpio_set_speed(pp.gpio_device, pp.gpio_bit, GPIO_speed_100MHz);
|
|
gpio_write_bit(pp.gpio_device, pp.gpio_bit, HIGH);
|
|
}
|
|
|
|
#ifdef BOARD_SDCARD_DET_PIN
|
|
{
|
|
const stm32_pin_info &pp = PIN_MAP[BOARD_SDCARD_DET_PIN];
|
|
gpio_set_mode(pp.gpio_device, pp.gpio_bit, GPIO_INPUT_PU);
|
|
gpio_write_bit(pp.gpio_device, pp.gpio_bit, HIGH);
|
|
}
|
|
#endif
|
|
|
|
_spi_sem = _spi->get_semaphore();
|
|
|
|
if(!_spi_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) return false;
|
|
|
|
//_spi->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&Sd2Card::_timer, void));
|
|
Revo_handler h = { .mp = FUNCTOR_BIND_MEMBER(&Sd2Card::_timer, void) };
|
|
systick_attach_callback(h.h);
|
|
|
|
_speed = AP_HAL::Device::SPEED_LOW; // all initialization at low speed
|
|
_spi->set_speed(_speed);
|
|
|
|
// must supply min of 74 clock cycles with CS high.
|
|
for (uint8_t i = 0; i < 10; i++) spi_spiSend(0XFF);
|
|
|
|
_spi_sem->give();
|
|
|
|
uint8_t n_try=3;
|
|
|
|
DSTATUS ret;
|
|
do {
|
|
ret = disk_initialize(0);
|
|
} while(ret!=RES_OK && n_try-- != 0);
|
|
|
|
printf("\nSD initialize: status %d size %ldMb\n", ret, sectorCount()/2048UL);
|
|
gcs().send_text(MAV_SEVERITY_INFO, "\nSD initialize: status %d size %ldMb\n", ret, sectorCount()/2048UL);
|
|
|
|
_speed = AP_HAL::Device::SPEED_HIGH;
|
|
|
|
return ret == RES_OK;
|
|
}
|
|
|
|
#elif defined(BOARD_DATAFLASH_FATFS)
|
|
|
|
#define DF_RESET BOARD_DATAFLASH_CS_PIN
|
|
|
|
void spi_chipSelectHigh(void) {
|
|
_spi->wait_busy();
|
|
const stm32_pin_info &pp = PIN_MAP[BOARD_DATAFLASH_CS_PIN];
|
|
gpio_write_bit(pp.gpio_device, pp.gpio_bit, HIGH);
|
|
_spi_sem->give();
|
|
}
|
|
|
|
bool spi_chipSelectLow(bool take_sem) {
|
|
if(take_sem){
|
|
if(!_spi_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) return false;
|
|
|
|
_spi->set_speed(_speed);
|
|
|
|
}
|
|
const stm32_pin_info &pp = PIN_MAP[BOARD_DATAFLASH_CS_PIN];
|
|
gpio_write_bit(pp.gpio_device, pp.gpio_bit, LOW);
|
|
return true;
|
|
}
|
|
|
|
uint8_t Sd2Card::init(AP_HAL::OwnPtr<F4Light::SPIDevice> spi) {
|
|
|
|
_spi = std::move(spi);
|
|
|
|
GPIO::_pinMode(DF_RESET,OUTPUT);
|
|
GPIO::_setSpeed(DF_RESET, GPIO_speed_100MHz);
|
|
// Reset the chip. We don't need a semaphore because no SPI activity
|
|
GPIO::_write(DF_RESET,0);
|
|
Scheduler::_delay(1);
|
|
GPIO::_write(DF_RESET,1);
|
|
|
|
if (!_spi) {
|
|
printf("DataFlash SPIDeviceDriver not found\n");
|
|
return false;
|
|
}
|
|
|
|
_spi_sem = _spi->get_semaphore();
|
|
if (!_spi_sem) {
|
|
printf("DataFlash SPIDeviceDriver semaphore is null\n");
|
|
return false;
|
|
}
|
|
|
|
if(!_spi_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) return false; // just for check
|
|
|
|
_spi_sem->give();
|
|
|
|
Revo_handler h = { .mp = FUNCTOR_BIND_MEMBER(&Sd2Card::_timer, void) };
|
|
systick_attach_callback(h.h); // not at common interrupt level as tasks because it can be called at USB interrupt level.
|
|
// Systick has own interrupt level above all IRQ
|
|
|
|
_speed = AP_HAL::Device::SPEED_HIGH;
|
|
|
|
DSTATUS ret;
|
|
ret = disk_initialize(0);
|
|
|
|
printf("\nDataFlash initialize: status %d size %ldMb\n", ret, sectorCount()/2048UL);
|
|
gcs().send_text(MAV_SEVERITY_INFO, "\nDataFlash initialize: status %d size %ldMb\n", ret, sectorCount()/2048UL);
|
|
|
|
return ret == RES_OK;
|
|
}
|
|
|
|
#endif
|