Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-21 16:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_ChibiOS: update to match plane 3.9.0beta6

Browse Source
This commit is contained in:
Andrew Tridgell 2018-08-01 18:54:36 +10:00 committed by Randy Mackay
parent 9384bc9b03
commit 97dae1a03d
16 changed files with 292 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_HAL_ChibiOS/Device.cpp
View File

@ -31,8 +31,8 @@ static const AP_HAL::HAL &hal = AP_HAL::get_HAL();
DeviceBus::DeviceBus(uint8_t _thread_priority) :
thread_priority(_thread_priority)
{
bouncebuffer_init(&bounce_buffer_tx);
bouncebuffer_init(&bounce_buffer_rx);
bouncebuffer_init(&bounce_buffer_tx, 10);
bouncebuffer_init(&bounce_buffer_rx, 10);
}
/*

5
libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp
View File

@ -25,6 +25,7 @@
#include "shared_dma.h"
#include "sdcard.h"
#include "hwdef/common/usbcfg.h"
#include "hwdef/common/stm32_util.h"
#include <hwdef.h>
@ -152,7 +153,9 @@ static THD_FUNCTION(main_loop,arg)
#endif
ChibiOS::Shared_DMA::init();
peripheral_power_enable();
hal.uartA->begin(115200);
#ifdef HAL_SPI_CHECK_CLOCK_FREQ

4
libraries/AP_HAL_ChibiOS/I2CDevice.cpp
View File

@ -340,7 +340,7 @@ uint32_t I2CDeviceManager::get_bus_mask(void) const
uint32_t I2CDeviceManager::get_bus_mask_internal(void) const
{
// assume first bus is internal
return 1U;
return get_bus_mask() & HAL_I2C_INTERNAL_MASK;
}
/*
@ -349,7 +349,7 @@ uint32_t I2CDeviceManager::get_bus_mask_internal(void) const
uint32_t I2CDeviceManager::get_bus_mask_external(void) const
{
// assume first bus is internal
return get_bus_mask() & 0xFE;
return get_bus_mask() & ~HAL_I2C_INTERNAL_MASK;
}
#endif // HAL_USE_I2C

65
libraries/AP_HAL_ChibiOS/Scheduler.cpp
View File

@ -176,16 +176,14 @@ bool Scheduler::check_called_boost(void)
void Scheduler::delay(uint16_t ms)
{
if (!in_main_thread()) {
//chprintf("ERROR: delay() from timer process\n");
return;
}
uint64_t start = AP_HAL::micros64();
while ((AP_HAL::micros64() - start)/1000 < ms) {
delay_microseconds(1000);
if (_min_delay_cb_ms <= ms) {
call_delay_cb();
if (in_main_thread()) {
call_delay_cb();
}
}
}
}
@ -435,4 +433,61 @@ void Scheduler::restore_interrupts(void *state)
chSysRestoreStatusX((syssts_t)(uintptr_t)state);
}
/*
trampoline for thread create
*/
void Scheduler::thread_create_trampoline(void *ctx)
{
AP_HAL::MemberProc *t = (AP_HAL::MemberProc *)ctx;
(*t)();
free(t);
}
/*
create a new thread
*/
bool Scheduler::thread_create(AP_HAL::MemberProc proc, const char *name, uint32_t stack_size, priority_base base, int8_t priority)
{
// take a copy of the MemberProc, it is freed after thread exits
AP_HAL::MemberProc *tproc = (AP_HAL::MemberProc *)malloc(sizeof(proc));
if (!tproc) {
return false;
}
*tproc = proc;
uint8_t thread_priority = APM_IO_PRIORITY;
static const struct {
priority_base base;
uint8_t p;
} priority_map[] = {
{ PRIORITY_BOOST, APM_MAIN_PRIORITY_BOOST},
{ PRIORITY_MAIN, APM_MAIN_PRIORITY},
{ PRIORITY_SPI, APM_SPI_PRIORITY},
{ PRIORITY_I2C, APM_I2C_PRIORITY},
{ PRIORITY_CAN, APM_CAN_PRIORITY},
{ PRIORITY_TIMER, APM_TIMER_PRIORITY},
{ PRIORITY_RCIN, APM_RCIN_PRIORITY},
{ PRIORITY_IO, APM_IO_PRIORITY},
{ PRIORITY_UART, APM_UART_PRIORITY},
{ PRIORITY_STORAGE, APM_STORAGE_PRIORITY},
};
for (uint8_t i=0; i<ARRAY_SIZE(priority_map); i++) {
if (priority_map[i].base == base) {
thread_priority = constrain_int16(priority_map[i].p + priority, LOWPRIO, HIGHPRIO);
break;
}
}
thread_t *thread_ctx = chThdCreateFromHeap(NULL,
THD_WORKING_AREA_SIZE(stack_size),
name,
thread_priority,
thread_create_trampoline,
tproc);
if (thread_ctx == nullptr) {
free(tproc);
return false;
}
return true;
}
#endif // CH_CFG_USE_DYNAMIC

6
libraries/AP_HAL_ChibiOS/Scheduler.h
View File

@ -92,6 +92,11 @@ public:
restore interrupt state from disable_interrupts_save()
*/
void restore_interrupts(void *) override;
/*
create a new thread
*/
bool thread_create(AP_HAL::MemberProc, const char *name, uint32_t stack_size, priority_base base, int8_t priority) override;
private:
bool _initialized;
@ -132,5 +137,6 @@ private:
#endif
void _run_timers();
void _run_io(void);
static void thread_create_trampoline(void *ctx);
};
#endif

102
libraries/AP_HAL_ChibiOS/Storage.cpp
View File

@ -19,6 +19,8 @@
#include "Storage.h"
#include "hwdef/common/flash.h"
#include "hwdef/common/posix.h"
#include "sdcard.h"
using namespace ChibiOS;
@ -26,12 +28,30 @@ using namespace ChibiOS;
extern const AP_HAL::HAL& hal;
#ifndef HAL_STORAGE_FILE
// using SKETCHNAME allows the one microSD to be used
// for multiple vehicle types
#define HAL_STORAGE_FILE "/APM/" SKETCHNAME ".stg"
#endif
#ifndef HAL_STORAGE_BACKUP_FILE
// location of backup file
#define HAL_STORAGE_BACKUP_FILE "/APM/" SKETCHNAME ".bak"
#endif
void Storage::_storage_open(void)
{
if (_initialised) {
return;
}
#ifdef USE_POSIX
// if we have failed filesystem init don't try again
if (log_fd == -1) {
return;
}
#endif
_dirty_mask.clearall();
#if HAL_WITH_RAMTRON
@ -40,18 +60,65 @@ void Storage::_storage_open(void)
if (!fram.read(0, _buffer, CH_STORAGE_SIZE)) {
return;
}
_save_backup();
_initialised = true;
return;
}
// allow for FMUv3 with no FRAM chip, fall through to flash storage
#endif
#ifdef STORAGE_FLASH_PAGE
// load from storage backend
_flash_load();
#elif defined(USE_POSIX)
// allow for fallback to microSD based storage
sdcard_init();
log_fd = open(HAL_STORAGE_FILE, O_RDWR|O_CREAT);
if (log_fd == -1) {
hal.console->printf("open failed of " HAL_STORAGE_FILE "\n");
return;
}
int ret = read(log_fd, _buffer, CH_STORAGE_SIZE);
if (ret < 0) {
hal.console->printf("read failed for " HAL_STORAGE_FILE "\n");
close(log_fd);
log_fd = -1;
return;
}
// pre-fill to full size
if (lseek(log_fd, ret, SEEK_SET) != ret ||
write(log_fd, &_buffer[ret], CH_STORAGE_SIZE-ret) != CH_STORAGE_SIZE-ret) {
hal.console->printf("setup failed for " HAL_STORAGE_FILE "\n");
close(log_fd);
log_fd = -1;
return;
}
using_filesystem = true;
#endif
_save_backup();
_initialised = true;
}
/*
save a backup of storage file if we have microSD available. This is
very handy for diagnostics, and for moving a copy of storage into
SITL for testing
*/
void Storage::_save_backup(void)
{
#ifdef USE_POSIX
// allow for fallback to microSD based storage
sdcard_init();
int fd = open(HAL_STORAGE_BACKUP_FILE, O_WRONLY|O_CREAT|O_TRUNC);
if (fd != -1) {
write(fd, _buffer, CH_STORAGE_SIZE);
close(fd);
}
#endif
}
/*
mark some lines as dirty. Note that there is no attempt to avoid
the race condition between this code and the _timer_tick() code
@ -92,10 +159,13 @@ void Storage::write_block(uint16_t loc, const void *src, size_t n)
void Storage::_timer_tick(void)
{
if (!_initialised || _dirty_mask.empty()) {
if (!_initialised) {
return;
}
if (_dirty_mask.empty()) {
_last_empty_ms = AP_HAL::millis();
return;
}
// write out the first dirty line. We don't write more
// than one to keep the latency of this call to a minimum
@ -119,8 +189,27 @@ void Storage::_timer_tick(void)
}
#endif
#ifdef USE_POSIX
if (using_filesystem && log_fd != -1) {
uint32_t offset = CH_STORAGE_LINE_SIZE*i;
if (lseek(log_fd, offset, SEEK_SET) != offset) {
return;
}
if (write(log_fd, &_buffer[offset], CH_STORAGE_LINE_SIZE) != CH_STORAGE_LINE_SIZE) {
return;
}
if (fsync(log_fd) != 0) {
return;
}
_dirty_mask.clear(i);
return;
}
#endif
#ifdef STORAGE_FLASH_PAGE
// save to storage backend
_flash_write(i);
#endif
}
/*
@ -207,4 +296,13 @@ bool Storage::_flash_erase_ok(void)
return !hal.util->get_soft_armed();
}
/*
consider storage healthy if we have nothing to write sometime in the
last 2 seconds
*/
bool Storage::healthy(void)
{
return _initialised && AP_HAL::millis() - _last_empty_ms < 2000;
}
#endif // HAL_USE_EMPTY_STORAGE

7
libraries/AP_HAL_ChibiOS/Storage.h
View File

@ -41,11 +41,13 @@ public:
void write_block(uint16_t dst, const void* src, size_t n);
void _timer_tick(void) override;
bool healthy(void) override;
private:
volatile bool _initialised;
void _storage_create(void);
void _storage_open(void);
void _save_backup(void);
void _mark_dirty(uint16_t loc, uint16_t length);
uint8_t _buffer[CH_STORAGE_SIZE] __attribute__((aligned(4)));
Bitmask _dirty_mask{CH_STORAGE_NUM_LINES};
@ -57,6 +59,7 @@ private:
uint8_t _flash_page;
bool _flash_failed;
uint32_t _last_re_init_ms;
uint32_t _last_empty_ms;
#ifdef STORAGE_FLASH_PAGE
AP_FlashStorage _flash{_buffer,
@ -74,6 +77,10 @@ private:
AP_RAMTRON fram;
bool using_fram;
#endif
#ifdef USE_POSIX
bool using_filesystem;
int log_fd;
#endif
};
#endif // HAL_USE_EMPTY_STORAGE

48
libraries/AP_HAL_ChibiOS/UARTDriver.cpp
View File

@ -652,6 +652,7 @@ void UARTDriver::write_pending_bytes_DMA(uint32_t n)
}
/* TX DMA channel preparation.*/
_total_written += tx_len;
_writebuf.advance(tx_len);
tx_len = _writebuf.peekbytes(tx_bounce_buf, MIN(n, TX_BOUNCE_BUFSIZE));
if (tx_len == 0) {
@ -712,6 +713,7 @@ void UARTDriver::write_pending_bytes_NODMA(uint32_t n)
}
if (ret > 0) {
_last_write_completed_us = AP_HAL::micros();
_total_written += ret;
}
_writebuf.advance(ret);
@ -751,9 +753,27 @@ void UARTDriver::write_pending_bytes(void)
if (_first_write_started_us == 0) {
_first_write_started_us = AP_HAL::micros();
}
if (_last_write_completed_us != 0) {
_flow_control = FLOW_CONTROL_ENABLE;
} else if (AP_HAL::micros() - _first_write_started_us > 500*1000UL) {
if (sdef.dma_tx) {
// when we are using DMA we have a reliable indication that a write
// has completed from the DMA completion interrupt
if (_last_write_completed_us != 0) {
_flow_control = FLOW_CONTROL_ENABLE;
return;
}
} else {
// without DMA we need to look at the number of bytes written into the queue versus the
// remaining queue space
uint32_t space = qSpaceI(&((SerialDriver*)sdef.serial)->oqueue);
uint32_t used = SERIAL_BUFFERS_SIZE - space;
// threshold is 8 for the GCS_Common code to unstick SiK radios, which
// sends 6 bytes with flow control disabled
const uint8_t threshold = 8;
if (_total_written > used && _total_written - used > threshold) {
_flow_control = FLOW_CONTROL_ENABLE;
return;
}
}
if (AP_HAL::micros() - _first_write_started_us > 500*1000UL) {
// it doesn't look like hw flow control is working
hal.console->printf("disabling flow control on serial %u\n", sdef.get_index());
set_flow_control(FLOW_CONTROL_DISABLE);
@ -868,8 +888,9 @@ void UARTDriver::set_flow_control(enum flow_control flowcontrol)
if (sdef.rts_line == 0 || sdef.is_usb) {
// no hw flow control available
return;
}
}
#if HAL_USE_SERIAL == TRUE
SerialDriver *sd = (SerialDriver*)(sdef.serial);
_flow_control = flowcontrol;
if (!_initialised) {
// not ready yet, we just set variable for when we call begin
@ -883,7 +904,13 @@ void UARTDriver::set_flow_control(enum flow_control flowcontrol)
palClearLine(sdef.rts_line);
_rts_is_active = true;
// disable hardware CTS support
((SerialDriver*)(sdef.serial))->usart->CR3 &= ~(USART_CR3_CTSE | USART_CR3_RTSE);
chSysLock();
if ((sd->usart->CR3 & (USART_CR3_CTSE | USART_CR3_RTSE)) != 0) {
sd->usart->CR1 &= ~USART_CR1_UE;
sd->usart->CR3 &= ~(USART_CR3_CTSE | USART_CR3_RTSE);
sd->usart->CR1 |= USART_CR1_UE;
}
chSysUnlock();
break;
case FLOW_CONTROL_AUTO:
@ -899,8 +926,15 @@ void UARTDriver::set_flow_control(enum flow_control flowcontrol)
palClearLine(sdef.rts_line);
_rts_is_active = true;
// enable hardware CTS support, disable RTS support as we do that in software
((SerialDriver*)(sdef.serial))->usart->CR3 |= USART_CR3_CTSE;
((SerialDriver*)(sdef.serial))->usart->CR3 &= ~USART_CR3_RTSE;
chSysLock();
if ((sd->usart->CR3 & (USART_CR3_CTSE | USART_CR3_RTSE)) != USART_CR3_CTSE) {
// CTSE and RTSE can only be written when uart is disabled
sd->usart->CR1 &= ~USART_CR1_UE;
sd->usart->CR3 |= USART_CR3_CTSE;
sd->usart->CR3 &= ~USART_CR3_RTSE;
sd->usart->CR1 |= USART_CR1_UE;
}
chSysUnlock();
break;
}
#endif // HAL_USE_SERIAL

1
libraries/AP_HAL_ChibiOS/UARTDriver.h
View File

@ -154,6 +154,7 @@ private:
bool _rts_is_active;
uint32_t _last_write_completed_us;
uint32_t _first_write_started_us;
uint32_t _total_written;
// set to true for unbuffered writes (low latency writes)
bool unbuffered_writes;

6
libraries/AP_HAL_ChibiOS/Util.cpp
View File

@ -202,7 +202,7 @@ bool Util::flash_bootloader()
uint32_t fw_size;
const char *fw_name = "bootloader.bin";
const uint8_t *fw = AP_ROMFS::find_file(fw_name, fw_size);
uint8_t *fw = AP_ROMFS::find_decompress(fw_name, fw_size);
if (!fw) {
hal.console->printf("failed to find %s\n", fw_name);
return false;
@ -211,12 +211,14 @@ bool Util::flash_bootloader()
const uint32_t addr = stm32_flash_getpageaddr(0);
if (!memcmp(fw, (const void*)addr, fw_size)) {
hal.console->printf("Bootloader up-to-date\n");
free(fw);
return true;
}
hal.console->printf("Erasing\n");
if (!stm32_flash_erasepage(0)) {
hal.console->printf("Erase failed\n");
free(fw);
return false;
}
hal.console->printf("Flashing %s @%08x\n", fw_name, (unsigned int)addr);
@ -233,10 +235,12 @@ bool Util::flash_bootloader()
continue;
}
hal.console->printf("Flash OK\n");
free(fw);
return true;
}
hal.console->printf("Flash failed after %u attempts\n", max_attempts);
free(fw);
return false;
}

7
libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.c
View File

@ -32,10 +32,15 @@
/*
initialise a bouncebuffer
*/
void bouncebuffer_init(struct bouncebuffer_t **bouncebuffer)
void bouncebuffer_init(struct bouncebuffer_t **bouncebuffer, uint32_t prealloc_bytes)
{
(*bouncebuffer) = calloc(1, sizeof(struct bouncebuffer_t));
osalDbgAssert(((*bouncebuffer) != NULL), "bouncebuffer init");
if (prealloc_bytes) {
(*bouncebuffer)->dma_buf = malloc_dma(prealloc_bytes);
osalDbgAssert(((*bouncebuffer)->dma_buf != NULL), "bouncebuffer preallocate");
(*bouncebuffer)->size = prealloc_bytes;
}
}
/*

2
libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.h
View File

@ -21,7 +21,7 @@ struct bouncebuffer_t {
/*
initialise a bouncebuffer
*/
void bouncebuffer_init(struct bouncebuffer_t **bouncebuffer);
void bouncebuffer_init(struct bouncebuffer_t **bouncebuffer, uint32_t prealloc_bytes);
/*
setup for reading from a device into memory, allocating a bouncebuffer if needed

25
libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
View File

@ -251,3 +251,28 @@ void set_fast_reboot(enum rtc_boot_magic v)
{
set_rtc_backup0(v);
}
/*
enable peripheral power if needed This is done late to prevent
problems with CTS causing SiK radios to stay in the bootloader. A
SiK radio will stay in the bootloader if CTS is held to GND on boot
*/
void peripheral_power_enable(void)
{
#if defined(HAL_GPIO_PIN_nVDD_5V_PERIPH_EN) || defined(HAL_GPIO_PIN_nVDD_5V_HIPOWER_EN)
// we don't know what state the bootloader had the CTS pin in, so
// wait here with it pulled up from the PAL table for enough time
// for the radio to be definately powered down
uint8_t i;
for (i=0; i<100; i++) {
// use a loop as this may be a 16 bit timer
chThdSleep(MS2ST(1));
}
#ifdef HAL_GPIO_PIN_nVDD_5V_PERIPH_EN
palWriteLine(HAL_GPIO_PIN_nVDD_5V_PERIPH_EN, 0);
#endif
#ifdef HAL_GPIO_PIN_nVDD_5V_HIPOWER_EN
palWriteLine(HAL_GPIO_PIN_nVDD_5V_HIPOWER_EN, 0);
#endif
#endif
}

3
libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h
View File

@ -64,6 +64,9 @@ enum rtc_boot_magic check_fast_reboot(void);
// set RTC register for a fast reboot
void set_fast_reboot(enum rtc_boot_magic v);
// enable peripheral power if needed
void peripheral_power_enable(void);
#ifdef __cplusplus
}

32
libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py
View File

@ -245,12 +245,8 @@ class generic_pin(object):
self.type.startswith('UART')) and (
(self.label.endswith('_TX') or
self.label.endswith('_RX') or
self.label.endswith('_CTS'))):
# default RX/TX lines to pullup, to prevent spurious bytes
# on disconnected ports. CTS is the exception, which is pulldown
if self.label.endswith("CTS"):
v = "PULLDOWN"
else:
self.label.endswith('_CTS') or
self.label.endswith('_RTS'))):
v = "PULLUP"
for e in self.extra:
if e in values:
@ -934,10 +930,14 @@ def write_GPIO_config(f):
# and write #defines for use by config code
f.write('}\n\n')
f.write('// full pin define list\n')
for l in sorted(bylabel.keys()):
last_label = None
for l in sorted(list(set(bylabel.keys()))):
p = bylabel[l]
label = p.label
label = label.replace('-', '_')
if label == last_label:
continue
last_label = label
f.write('#define HAL_GPIO_PIN_%-20s PAL_LINE(GPIO%s,%uU)\n' %
(label, p.port, p.pin))
f.write('\n')
@ -1153,7 +1153,7 @@ def write_env_py(filename):
# see if board has a defaults.parm file
defaults_filename = os.path.join(os.path.dirname(args.hwdef), 'defaults.parm')
if os.path.exists(defaults_filename):
if os.path.exists(defaults_filename) and not args.bootloader:
print("Adding defaults.parm")
env_vars['DEFAULT_PARAMETERS'] = os.path.abspath(defaults_filename)
@ -1161,6 +1161,18 @@ def write_env_py(filename):
env_vars['CHIBIOS_BUILD_FLAGS'] = ' '.join(build_flags)
pickle.dump(env_vars, open(filename, "wb"))
def romfs_add(romfs_filename, filename):
'''add a file to ROMFS'''
romfs.append((romfs_filename, filename))
def romfs_wildcard(pattern):
'''add a set of files to ROMFS by wildcard'''
base_path = os.path.join(os.path.dirname(__file__), '..', '..', '..', '..')
(pattern_dir, pattern) = os.path.split(pattern)
for f in os.listdir(os.path.join(base_path, pattern_dir)):
if fnmatch.fnmatch(f, pattern):
romfs.append((f, os.path.join(pattern_dir, f)))
def process_line(line):
'''process one line of pin definition file'''
global allpins
@ -1201,7 +1213,9 @@ def process_line(line):
if a[0] == 'SPIDEV':
spidev.append(a[1:])
if a[0] == 'ROMFS':
romfs.append((a[1],a[2]))
romfs_add(a[1],a[2])
if a[0] == 'ROMFS_WILDCARD':
romfs_wildcard(a[1])
if a[0] == 'undef':
print("Removing %s" % a[1])
config.pop(a[1], '')

9
libraries/AP_HAL_ChibiOS/sdcard.cpp
View File

@ -22,6 +22,7 @@ extern const AP_HAL::HAL& hal;
#ifdef USE_POSIX
static FATFS SDC_FS; // FATFS object
static bool sdcard_init_done;
#endif
#if HAL_USE_MMC_SPI
@ -39,11 +40,13 @@ static bool sdcard_running;
void sdcard_init()
{
#ifdef USE_POSIX
if (sdcard_init_done) {
return;
}
sdcard_init_done = true;
#if HAL_USE_SDC
#if defined(STM32_SDC_USE_SDMMC1) && STM32_SDC_USE_SDMMC1 == TRUE
bouncebuffer_init(&SDCD1.bouncebuffer);
#endif
bouncebuffer_init(&SDCD1.bouncebuffer, 512);
sdcStart(&SDCD1, NULL);