diff --git a/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h b/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h index a391cbd77c..562b47e17f 100644 --- a/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h +++ b/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h @@ -1,22 +1,23 @@ #pragma once namespace ChibiOS { - class ChibiAnalogIn; - class ChibiAnalogSource; - class ChibiDigitalSource; - class ChibiGPIO; - class ChibiI2CDevice; + class AnalogIn; + class AnalogSource; + class DigitalSource; + class GPIO; + class I2CDevice; class I2CDeviceManager; class OpticalFlow; class PrivateMember; - class ChibiRCInput; - class ChibiRCOutput; - class ChibiScheduler; + class RCInput; + class RCOutput; + class Scheduler; class Semaphore; class SPIDevice; class SPIDeviceDriver; class SPIDeviceManager; - class ChibiStorage; - class ChibiUARTDriver; - class ChibiUtil; + class Storage; + class UARTDriver; + class Util; + class Shared_DMA; } diff --git a/libraries/AP_HAL_ChibiOS/AnalogIn.cpp b/libraries/AP_HAL_ChibiOS/AnalogIn.cpp index 23b15f6aaa..c5eaa94032 100644 --- a/libraries/AP_HAL_ChibiOS/AnalogIn.cpp +++ b/libraries/AP_HAL_ChibiOS/AnalogIn.cpp @@ -45,7 +45,7 @@ using namespace ChibiOS; scaling table between ADC count and actual input voltage, to account for voltage dividers on the board. */ -const ChibiAnalogIn::pin_info ChibiAnalogIn::pin_config[] = { +const AnalogIn::pin_info AnalogIn::pin_config[] = { #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 { ANALOG_VCC_5V_PIN, 0.007734 }, // VCC 5V rail sense #else @@ -58,17 +58,17 @@ const ChibiAnalogIn::pin_info ChibiAnalogIn::pin_config[] = { { 15, VOLTAGE_SCALING*2 }, // analog airspeed sensor, 2:1 scaling }; -#define ADC_GRP1_NUM_CHANNELS ARRAY_SIZE_SIMPLE(ChibiAnalogIn::pin_config) +#define ADC_GRP1_NUM_CHANNELS ARRAY_SIZE_SIMPLE(AnalogIn::pin_config) // samples filled in by ADC DMA engine -adcsample_t ChibiAnalogIn::samples[ADC_DMA_BUF_DEPTH*ADC_GRP1_NUM_CHANNELS]; -uint32_t ChibiAnalogIn::sample_sum[ADC_GRP1_NUM_CHANNELS]; -uint32_t ChibiAnalogIn::sample_count; +adcsample_t AnalogIn::samples[ADC_DMA_BUF_DEPTH*ADC_GRP1_NUM_CHANNELS]; +uint32_t AnalogIn::sample_sum[ADC_GRP1_NUM_CHANNELS]; +uint32_t AnalogIn::sample_count; // special pin numbers #define ANALOG_VCC_5V_PIN 4 -ChibiAnalogSource::ChibiAnalogSource(int16_t pin, float initial_value) : +AnalogSource::AnalogSource(int16_t pin, float initial_value) : _pin(pin), _value(initial_value), _value_ratiometric(initial_value), @@ -80,7 +80,7 @@ ChibiAnalogSource::ChibiAnalogSource(int16_t pin, float initial_value) : } -float ChibiAnalogSource::read_average() +float AnalogSource::read_average() { if (_sum_count == 0) { return _value; @@ -93,7 +93,7 @@ float ChibiAnalogSource::read_average() return _value; } -float ChibiAnalogSource::read_latest() +float AnalogSource::read_latest() { return _latest_value; } @@ -101,12 +101,12 @@ float ChibiAnalogSource::read_latest() /* return scaling from ADC count to Volts */ -float ChibiAnalogSource::_pin_scaler(void) +float AnalogSource::_pin_scaler(void) { float scaling = VOLTAGE_SCALING; for (uint8_t i=0; i_pin) { // add a value c->_add_value(buf_adc[i], _board_voltage); @@ -291,11 +291,11 @@ void ChibiAnalogIn::_timer_tick(void) #endif } -AP_HAL::AnalogSource* ChibiAnalogIn::channel(int16_t pin) +AP_HAL::AnalogSource* AnalogIn::channel(int16_t pin) { for (uint8_t j=0; jbegin(115200); hal.uartB->begin(38400); diff --git a/libraries/AP_HAL_ChibiOS/RCInput.cpp b/libraries/AP_HAL_ChibiOS/RCInput.cpp index aa82f57ced..f37dd86088 100644 --- a/libraries/AP_HAL_ChibiOS/RCInput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCInput.cpp @@ -28,7 +28,7 @@ extern AP_IOMCU iomcu; using namespace ChibiOS; extern const AP_HAL::HAL& hal; -void ChibiRCInput::init() +void RCInput::init() { #if HAL_USE_ICU == TRUE ppm_init(1000000, true); @@ -37,7 +37,7 @@ void ChibiRCInput::init() _init = true; } -bool ChibiRCInput::new_input() +bool RCInput::new_input() { if (!_init) { return false; @@ -65,7 +65,7 @@ bool ChibiRCInput::new_input() return valid; } -uint8_t ChibiRCInput::num_channels() +uint8_t RCInput::num_channels() { if (!_init) { return 0; @@ -76,7 +76,7 @@ uint8_t ChibiRCInput::num_channels() return n; } -uint16_t ChibiRCInput::read(uint8_t channel) +uint16_t RCInput::read(uint8_t channel) { if (!_init) { return 0; @@ -105,7 +105,7 @@ uint16_t ChibiRCInput::read(uint8_t channel) return v; } -uint8_t ChibiRCInput::read(uint16_t* periods, uint8_t len) +uint8_t RCInput::read(uint16_t* periods, uint8_t len) { if (!_init) { return false; @@ -120,7 +120,7 @@ uint8_t ChibiRCInput::read(uint16_t* periods, uint8_t len) return len; } -bool ChibiRCInput::set_overrides(int16_t *overrides, uint8_t len) +bool RCInput::set_overrides(int16_t *overrides, uint8_t len) { if (!_init) { return false; @@ -133,7 +133,7 @@ bool ChibiRCInput::set_overrides(int16_t *overrides, uint8_t len) return res; } -bool ChibiRCInput::set_override(uint8_t channel, int16_t override) +bool RCInput::set_override(uint8_t channel, int16_t override) { if (!_init) { return false; @@ -153,7 +153,7 @@ bool ChibiRCInput::set_override(uint8_t channel, int16_t override) return false; } -void ChibiRCInput::clear_overrides() +void RCInput::clear_overrides() { for (uint8_t i = 0; i < RC_INPUT_MAX_CHANNELS; i++) { set_override(i, 0); @@ -161,7 +161,7 @@ void ChibiRCInput::clear_overrides() } -void ChibiRCInput::_timer_tick(void) +void RCInput::_timer_tick(void) { if (!_init) { return; @@ -202,7 +202,7 @@ void ChibiRCInput::_timer_tick(void) // and a timeout for the last valid input to handle failsafe } -bool ChibiRCInput::rc_bind(int dsmMode) +bool RCInput::rc_bind(int dsmMode) { #if HAL_RCINPUT_WITH_AP_RADIO if (radio) { diff --git a/libraries/AP_HAL_ChibiOS/RCInput.h b/libraries/AP_HAL_ChibiOS/RCInput.h index 17ecbfaed5..d22963cf13 100644 --- a/libraries/AP_HAL_ChibiOS/RCInput.h +++ b/libraries/AP_HAL_ChibiOS/RCInput.h @@ -25,7 +25,7 @@ #define RC_INPUT_MAX_CHANNELS 18 #endif -class ChibiOS::ChibiRCInput : public AP_HAL::RCInput { +class ChibiOS::RCInput : public AP_HAL::RCInput { public: void init() override; bool new_input() override; diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 24ebec1287..50e78429b6 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -27,13 +27,13 @@ extern const AP_HAL::HAL& hal; extern AP_IOMCU iomcu; #endif -struct ChibiRCOutput::pwm_group ChibiRCOutput::pwm_group_list[] = { HAL_PWM_GROUPS }; +struct RCOutput::pwm_group RCOutput::pwm_group_list[] = { HAL_PWM_GROUPS }; #define NUM_GROUPS ARRAY_SIZE_SIMPLE(pwm_group_list) #define CHAN_DISABLED 255 -void ChibiRCOutput::init() +void RCOutput::init() { for (uint8_t i = 0; i < NUM_GROUPS; i++ ) { //Start Pwm groups @@ -54,7 +54,7 @@ void ChibiRCOutput::init() #endif } -void ChibiRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) +void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) { //check if the request spans accross any of the channel groups uint8_t update_mask = 0; @@ -123,7 +123,7 @@ void ChibiRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) /* set default output rate */ -void ChibiRCOutput::set_default_rate(uint16_t freq_hz) +void RCOutput::set_default_rate(uint16_t freq_hz) { #if HAL_WITH_IO_MCU if (AP_BoardConfig::io_enabled()) { @@ -146,7 +146,7 @@ void ChibiRCOutput::set_default_rate(uint16_t freq_hz) } } -uint16_t ChibiRCOutput::get_freq(uint8_t chan) +uint16_t RCOutput::get_freq(uint8_t chan) { if (chan >= total_channels) { return 0; @@ -169,7 +169,7 @@ uint16_t ChibiRCOutput::get_freq(uint8_t chan) return 50; } -void ChibiRCOutput::enable_ch(uint8_t chan) +void RCOutput::enable_ch(uint8_t chan) { if (chan >= total_channels) { return; @@ -188,7 +188,7 @@ void ChibiRCOutput::enable_ch(uint8_t chan) } } -void ChibiRCOutput::disable_ch(uint8_t chan) +void RCOutput::disable_ch(uint8_t chan) { if (chan >= total_channels) { return; @@ -208,7 +208,7 @@ void ChibiRCOutput::disable_ch(uint8_t chan) } } -void ChibiRCOutput::write(uint8_t chan, uint16_t period_us) +void RCOutput::write(uint8_t chan, uint16_t period_us) { if (chan >= total_channels) { return; @@ -236,7 +236,7 @@ void ChibiRCOutput::write(uint8_t chan, uint16_t period_us) /* push values to local channels from period[] array */ -void ChibiRCOutput::push_local(void) +void RCOutput::push_local(void) { if (num_channels == 0) { return; @@ -274,7 +274,7 @@ void ChibiRCOutput::push_local(void) } } -uint16_t ChibiRCOutput::read(uint8_t chan) +uint16_t RCOutput::read(uint8_t chan) { if (chan >= total_channels) { return 0; @@ -288,7 +288,7 @@ uint16_t ChibiRCOutput::read(uint8_t chan) return period[chan]; } -void ChibiRCOutput::read(uint16_t* period_us, uint8_t len) +void RCOutput::read(uint16_t* period_us, uint8_t len) { if (len > total_channels) { len = total_channels; @@ -307,7 +307,7 @@ void ChibiRCOutput::read(uint16_t* period_us, uint8_t len) memcpy(period_us, period, len*sizeof(uint16_t)); } -uint16_t ChibiRCOutput::read_last_sent(uint8_t chan) +uint16_t RCOutput::read_last_sent(uint8_t chan) { if (chan >= total_channels) { return 0; @@ -315,7 +315,7 @@ uint16_t ChibiRCOutput::read_last_sent(uint8_t chan) return last_sent[chan]; } -void ChibiRCOutput::read_last_sent(uint16_t* period_us, uint8_t len) +void RCOutput::read_last_sent(uint16_t* period_us, uint8_t len) { if (len > total_channels) { len = total_channels; @@ -327,7 +327,7 @@ void ChibiRCOutput::read_last_sent(uint16_t* period_us, uint8_t len) /* setup output mode */ -void ChibiRCOutput::set_output_mode(enum output_mode mode) +void RCOutput::set_output_mode(enum output_mode mode) { _output_mode = mode; if (_output_mode == MODE_PWM_BRUSHED) { @@ -341,7 +341,7 @@ void ChibiRCOutput::set_output_mode(enum output_mode mode) /* force the safety switch on, disabling PWM output from the IO board */ -bool ChibiRCOutput::force_safety_on(void) +bool RCOutput::force_safety_on(void) { #if HAL_WITH_IO_MCU if (AP_BoardConfig::io_enabled()) { @@ -354,7 +354,7 @@ bool ChibiRCOutput::force_safety_on(void) /* force the safety switch off, enabling PWM output from the IO board */ -void ChibiRCOutput::force_safety_off(void) +void RCOutput::force_safety_off(void) { #if HAL_WITH_IO_MCU if (AP_BoardConfig::io_enabled()) { @@ -366,7 +366,7 @@ void ChibiRCOutput::force_safety_off(void) /* start corking output */ -void ChibiRCOutput::cork(void) +void RCOutput::cork(void) { corked = true; #if HAL_WITH_IO_MCU @@ -379,7 +379,7 @@ void ChibiRCOutput::cork(void) /* stop corking output */ -void ChibiRCOutput::push(void) +void RCOutput::push(void) { corked = false; push_local(); @@ -393,7 +393,7 @@ void ChibiRCOutput::push(void) /* enable sbus output */ -bool ChibiRCOutput::enable_px4io_sbus_out(uint16_t rate_hz) +bool RCOutput::enable_px4io_sbus_out(uint16_t rate_hz) { #if HAL_WITH_IO_MCU if (AP_BoardConfig::io_enabled()) { diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h index 6f40ac9dbe..263dbabb49 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.h +++ b/libraries/AP_HAL_ChibiOS/RCOutput.h @@ -20,7 +20,7 @@ #include "ch.h" #include "hal.h" -class ChibiOS::ChibiRCOutput : public AP_HAL::RCOutput { +class ChibiOS::RCOutput : public AP_HAL::RCOutput { public: void init(); void set_freq(uint32_t chmask, uint16_t freq_hz); diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp index 65c0ac52c7..99a1a6b030 100644 --- a/libraries/AP_HAL_ChibiOS/Scheduler.cpp +++ b/libraries/AP_HAL_ChibiOS/Scheduler.cpp @@ -38,13 +38,13 @@ THD_WORKING_AREA(_storage_thread_wa, 2048); THD_WORKING_AREA(_uart_thread_wa, 2048); #if HAL_WITH_IO_MCU -extern ChibiOS::ChibiUARTDriver uart_io; +extern ChibiOS::UARTDriver uart_io; #endif -ChibiScheduler::ChibiScheduler() +Scheduler::Scheduler() {} -void ChibiScheduler::init() +void Scheduler::init() { // setup the timer thread - this will call tasks at 1kHz _timer_thread_ctx = chThdCreateStatic(_timer_thread_wa, @@ -75,7 +75,7 @@ void ChibiScheduler::init() this); /* Thread parameter. */ } -void ChibiScheduler::delay_microseconds(uint16_t usec) +void Scheduler::delay_microseconds(uint16_t usec) { if (usec == 0) { //chibios faults with 0us sleep return; @@ -109,7 +109,7 @@ static void set_normal_priority() microseconds when the time completes. This significantly improves the regularity of timing of the main loop as it takes */ -void ChibiScheduler::delay_microseconds_boost(uint16_t usec) +void Scheduler::delay_microseconds_boost(uint16_t usec) { delay_microseconds(usec); //Suspends Thread for desired microseconds set_high_priority(); @@ -117,7 +117,7 @@ void ChibiScheduler::delay_microseconds_boost(uint16_t usec) set_normal_priority(); } -void ChibiScheduler::delay(uint16_t ms) +void Scheduler::delay(uint16_t ms) { if (!in_main_thread()) { //chprintf("ERROR: delay() from timer process\n"); @@ -135,14 +135,14 @@ void ChibiScheduler::delay(uint16_t ms) } } -void ChibiScheduler::register_delay_callback(AP_HAL::Proc proc, +void Scheduler::register_delay_callback(AP_HAL::Proc proc, uint16_t min_time_ms) { _delay_cb = proc; _min_delay_cb_ms = min_time_ms; } -void ChibiScheduler::register_timer_process(AP_HAL::MemberProc proc) +void Scheduler::register_timer_process(AP_HAL::MemberProc proc) { for (uint8_t i = 0; i < _num_timer_procs; i++) { if (_timer_proc[i] == proc) { @@ -158,7 +158,7 @@ void ChibiScheduler::register_timer_process(AP_HAL::MemberProc proc) } } -void ChibiScheduler::register_io_process(AP_HAL::MemberProc proc) +void Scheduler::register_io_process(AP_HAL::MemberProc proc) { for (uint8_t i = 0; i < _num_io_procs; i++) { if (_io_proc[i] == proc) { @@ -174,17 +174,17 @@ void ChibiScheduler::register_io_process(AP_HAL::MemberProc proc) } } -void ChibiScheduler::register_timer_failsafe(AP_HAL::Proc failsafe, uint32_t period_us) +void Scheduler::register_timer_failsafe(AP_HAL::Proc failsafe, uint32_t period_us) { _failsafe = failsafe; } -void ChibiScheduler::suspend_timer_procs() +void Scheduler::suspend_timer_procs() { _timer_suspended = true; } -void ChibiScheduler::resume_timer_procs() +void Scheduler::resume_timer_procs() { _timer_suspended = false; if (_timer_event_missed == true) { @@ -193,7 +193,7 @@ void ChibiScheduler::resume_timer_procs() } } extern void Reset_Handler(); -void ChibiScheduler::reboot(bool hold_in_bootloader) +void Scheduler::reboot(bool hold_in_bootloader) { // disarm motors to ensure they are off during a bootloader upload hal.rcout->force_safety_on(); @@ -209,7 +209,7 @@ void ChibiScheduler::reboot(bool hold_in_bootloader) NVIC_SystemReset(); } -void ChibiScheduler::_run_timers(bool called_from_timer_thread) +void Scheduler::_run_timers(bool called_from_timer_thread) { if (_in_timer_proc) { return; @@ -233,14 +233,14 @@ void ChibiScheduler::_run_timers(bool called_from_timer_thread) } // process analog input - ((ChibiAnalogIn *)hal.analogin)->_timer_tick(); + ((AnalogIn *)hal.analogin)->_timer_tick(); _in_timer_proc = false; } -void ChibiScheduler::_timer_thread(void *arg) +void Scheduler::_timer_thread(void *arg) { - ChibiScheduler *sched = (ChibiScheduler *)arg; + Scheduler *sched = (Scheduler *)arg; sched->_timer_thread_ctx->name = "apm_timer"; while (!sched->_hal_initialized) { @@ -256,11 +256,11 @@ void ChibiScheduler::_timer_thread(void *arg) //hal.rcout->timer_tick(); // process any pending RC input requests - ((ChibiRCInput *)hal.rcin)->_timer_tick(); + ((RCInput *)hal.rcin)->_timer_tick(); } } -void ChibiScheduler::_run_io(void) +void Scheduler::_run_io(void) { if (_in_io_proc) { return; @@ -279,9 +279,9 @@ void ChibiScheduler::_run_io(void) _in_io_proc = false; } -void ChibiScheduler::_uart_thread(void* arg) +void Scheduler::_uart_thread(void* arg) { - ChibiScheduler *sched = (ChibiScheduler *)arg; + Scheduler *sched = (Scheduler *)arg; sched->_uart_thread_ctx->name = "apm_uart"; while (!sched->_hal_initialized) { sched->delay_microseconds(1000); @@ -290,21 +290,21 @@ void ChibiScheduler::_uart_thread(void* arg) sched->delay_microseconds(1000); // process any pending serial bytes - ((ChibiUARTDriver *)hal.uartA)->_timer_tick(); - ((ChibiUARTDriver *)hal.uartB)->_timer_tick(); - ((ChibiUARTDriver *)hal.uartC)->_timer_tick(); - /*((ChibiUARTDriver *)hal.uartD)->_timer_tick(); - ((ChibiUARTDriver *)hal.uartE)->_timer_tick(); - ((ChibiUARTDriver *)hal.uartF)->_timer_tick();*/ + ((UARTDriver *)hal.uartA)->_timer_tick(); + ((UARTDriver *)hal.uartB)->_timer_tick(); + ((UARTDriver *)hal.uartC)->_timer_tick(); + /*((UARTDriver *)hal.uartD)->_timer_tick(); + ((UARTDriver *)hal.uartE)->_timer_tick(); + ((UARTDriver *)hal.uartF)->_timer_tick();*/ #if HAL_WITH_IO_MCU uart_io._timer_tick(); #endif } } -void ChibiScheduler::_io_thread(void* arg) +void Scheduler::_io_thread(void* arg) { - ChibiScheduler *sched = (ChibiScheduler *)arg; + Scheduler *sched = (Scheduler *)arg; sched->_io_thread_ctx->name = "apm_io"; while (!sched->_hal_initialized) { sched->delay_microseconds(1000); @@ -317,9 +317,9 @@ void ChibiScheduler::_io_thread(void* arg) } } -void ChibiScheduler::_storage_thread(void* arg) +void Scheduler::_storage_thread(void* arg) { - ChibiScheduler *sched = (ChibiScheduler *)arg; + Scheduler *sched = (Scheduler *)arg; sched->_storage_thread_ctx->name = "apm_storage"; while (!sched->_hal_initialized) { sched->delay_microseconds(10000); @@ -328,16 +328,16 @@ void ChibiScheduler::_storage_thread(void* arg) sched->delay_microseconds(10000); // process any pending storage writes - ((ChibiStorage *)hal.storage)->_timer_tick(); + ((Storage *)hal.storage)->_timer_tick(); } } -bool ChibiScheduler::in_main_thread() const +bool Scheduler::in_main_thread() const { return get_main_thread() == chThdGetSelfX(); } -void ChibiScheduler::system_initialized() +void Scheduler::system_initialized() { if (_initialized) { AP_HAL::panic("PANIC: scheduler::system_initialized called" diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.h b/libraries/AP_HAL_ChibiOS/Scheduler.h index 0be5caf279..3c78fac7d8 100644 --- a/libraries/AP_HAL_ChibiOS/Scheduler.h +++ b/libraries/AP_HAL_ChibiOS/Scheduler.h @@ -52,9 +52,9 @@ #define APM_MAIN_THREAD_STACK_SIZE 8192 /* Scheduler implementation: */ -class ChibiOS::ChibiScheduler : public AP_HAL::Scheduler { +class ChibiOS::Scheduler : public AP_HAL::Scheduler { public: - ChibiScheduler(); + Scheduler(); /* AP_HAL::Scheduler methods */ diff --git a/libraries/AP_HAL_ChibiOS/Storage.cpp b/libraries/AP_HAL_ChibiOS/Storage.cpp index d06e025900..7dbf348f3a 100644 --- a/libraries/AP_HAL_ChibiOS/Storage.cpp +++ b/libraries/AP_HAL_ChibiOS/Storage.cpp @@ -27,7 +27,7 @@ using namespace ChibiOS; extern const AP_HAL::HAL& hal; -void ChibiStorage::_storage_open(void) +void Storage::_storage_open(void) { if (_initialised) { return; @@ -60,7 +60,7 @@ void ChibiStorage::_storage_open(void) result is that a line is written more than once, but it won't result in a line not being written. */ -void ChibiStorage::_mark_dirty(uint16_t loc, uint16_t length) +void Storage::_mark_dirty(uint16_t loc, uint16_t length) { uint16_t end = loc + length; for (uint16_t line=loc>>CH_STORAGE_LINE_SHIFT; @@ -70,7 +70,7 @@ void ChibiStorage::_mark_dirty(uint16_t loc, uint16_t length) } } -void ChibiStorage::read_block(void *dst, uint16_t loc, size_t n) +void Storage::read_block(void *dst, uint16_t loc, size_t n) { if (loc >= sizeof(_buffer)-(n-1)) { return; @@ -79,7 +79,7 @@ void ChibiStorage::read_block(void *dst, uint16_t loc, size_t n) memcpy(dst, &_buffer[loc], n); } -void ChibiStorage::write_block(uint16_t loc, const void *src, size_t n) +void Storage::write_block(uint16_t loc, const void *src, size_t n) { if (loc >= sizeof(_buffer)-(n-1)) { return; @@ -91,7 +91,7 @@ void ChibiStorage::write_block(uint16_t loc, const void *src, size_t n) } } -void ChibiStorage::_timer_tick(void) +void Storage::_timer_tick(void) { if (!_initialised || _dirty_mask.empty()) { return; @@ -127,7 +127,7 @@ void ChibiStorage::_timer_tick(void) /* load all data from flash */ -void ChibiStorage::_flash_load(void) +void Storage::_flash_load(void) { _flash_page = STORAGE_FLASH_PAGE; @@ -141,7 +141,7 @@ void ChibiStorage::_flash_load(void) /* write one storage line. This also updates _dirty_mask. */ -void ChibiStorage::_flash_write(uint16_t line) +void Storage::_flash_write(uint16_t line) { if (_flash.write(line*CH_STORAGE_LINE_SIZE, CH_STORAGE_LINE_SIZE)) { // mark the line clean @@ -152,7 +152,7 @@ void ChibiStorage::_flash_write(uint16_t line) /* callback to write data to flash */ -bool ChibiStorage::_flash_write_data(uint8_t sector, uint32_t offset, const uint8_t *data, uint16_t length) +bool Storage::_flash_write_data(uint8_t sector, uint32_t offset, const uint8_t *data, uint16_t length) { size_t base_address = stm32_flash_getpageaddr(_flash_page+sector); bool ret = stm32_flash_write(base_address+offset, data, length) == length; @@ -173,7 +173,7 @@ bool ChibiStorage::_flash_write_data(uint8_t sector, uint32_t offset, const uint /* callback to read data from flash */ -bool ChibiStorage::_flash_read_data(uint8_t sector, uint32_t offset, uint8_t *data, uint16_t length) +bool Storage::_flash_read_data(uint8_t sector, uint32_t offset, uint8_t *data, uint16_t length) { size_t base_address = stm32_flash_getpageaddr(_flash_page+sector); const uint8_t *b = ((const uint8_t *)base_address)+offset; @@ -184,7 +184,7 @@ bool ChibiStorage::_flash_read_data(uint8_t sector, uint32_t offset, uint8_t *da /* callback to erase flash sector */ -bool ChibiStorage::_flash_erase_sector(uint8_t sector) +bool Storage::_flash_erase_sector(uint8_t sector) { return stm32_flash_erasepage(_flash_page+sector); } @@ -192,7 +192,7 @@ bool ChibiStorage::_flash_erase_sector(uint8_t sector) /* callback to check if erase is allowed */ -bool ChibiStorage::_flash_erase_ok(void) +bool Storage::_flash_erase_ok(void) { // only allow erase while disarmed return !hal.util->get_soft_armed(); diff --git a/libraries/AP_HAL_ChibiOS/Storage.h b/libraries/AP_HAL_ChibiOS/Storage.h index bb1209ccb7..f988132dfb 100644 --- a/libraries/AP_HAL_ChibiOS/Storage.h +++ b/libraries/AP_HAL_ChibiOS/Storage.h @@ -32,7 +32,7 @@ #define CH_STORAGE_LINE_SIZE (1<pinMode(2, HAL_GPIO_OUTPUT); hal.gpio->pinMode(3, HAL_GPIO_OUTPUT); @@ -135,8 +135,8 @@ void ChibiUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) // cannot be shared dma_handle = new Shared_DMA(sdef.dma_tx_stream_id, SHARED_DMA_NONE, - FUNCTOR_BIND_MEMBER(&ChibiUARTDriver::dma_tx_allocate, void), - FUNCTOR_BIND_MEMBER(&ChibiUARTDriver::dma_tx_deallocate, void)); + FUNCTOR_BIND_MEMBER(&UARTDriver::dma_tx_allocate, void), + FUNCTOR_BIND_MEMBER(&UARTDriver::dma_tx_deallocate, void)); } } sercfg.speed = _baudrate; @@ -186,7 +186,7 @@ void ChibiUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) set_flow_control(_flow_control); } -void ChibiUARTDriver::dma_tx_allocate(void) +void UARTDriver::dma_tx_allocate(void) { osalDbgAssert(txdma == nullptr, "double DMA allocation"); txdma = STM32_DMA_STREAM(sdef.dma_tx_stream_id); @@ -198,7 +198,7 @@ void ChibiUARTDriver::dma_tx_allocate(void) dmaStreamSetPeripheral(txdma, &((SerialDriver*)sdef.serial)->usart->DR); } -void ChibiUARTDriver::dma_tx_deallocate(void) +void UARTDriver::dma_tx_deallocate(void) { chSysLock(); dmaStreamRelease(txdma); @@ -206,18 +206,18 @@ void ChibiUARTDriver::dma_tx_deallocate(void) chSysUnlock(); } -void ChibiUARTDriver::tx_complete(void* self, uint32_t flags) +void UARTDriver::tx_complete(void* self, uint32_t flags) { - ChibiUARTDriver* uart_drv = (ChibiUARTDriver*)self; + UARTDriver* uart_drv = (UARTDriver*)self; uart_drv->dma_handle->unlock_from_IRQ(); uart_drv->_last_write_completed_us = AP_HAL::micros(); uart_drv->tx_bounce_buf_ready = true; } -void ChibiUARTDriver::rx_irq_cb(void* self) +void UARTDriver::rx_irq_cb(void* self) { - ChibiUARTDriver* uart_drv = (ChibiUARTDriver*)self; + UARTDriver* uart_drv = (UARTDriver*)self; if (!uart_drv->sdef.dma_rx) { return; } @@ -230,9 +230,9 @@ void ChibiUARTDriver::rx_irq_cb(void* self) } } -void ChibiUARTDriver::rxbuff_full_irq(void* self, uint32_t flags) +void UARTDriver::rxbuff_full_irq(void* self, uint32_t flags) { - ChibiUARTDriver* uart_drv = (ChibiUARTDriver*)self; + UARTDriver* uart_drv = (UARTDriver*)self; if (uart_drv->_lock_rx_in_timer_tick) { return; } @@ -258,12 +258,12 @@ void ChibiUARTDriver::rxbuff_full_irq(void* self, uint32_t flags) } } -void ChibiUARTDriver::begin(uint32_t b) +void UARTDriver::begin(uint32_t b) { begin(b, 0, 0); } -void ChibiUARTDriver::end() +void UARTDriver::end() { _initialised = false; while (_in_timer) hal.scheduler->delay(1); @@ -280,7 +280,7 @@ void ChibiUARTDriver::end() _writebuf.set_size(0); } -void ChibiUARTDriver::flush() +void UARTDriver::flush() { if (sdef.is_usb) { #ifdef HAVE_USB_SERIAL @@ -292,20 +292,20 @@ void ChibiUARTDriver::flush() } } -bool ChibiUARTDriver::is_initialized() +bool UARTDriver::is_initialized() { return _initialised; } -void ChibiUARTDriver::set_blocking_writes(bool blocking) +void UARTDriver::set_blocking_writes(bool blocking) { _nonblocking_writes = !blocking; } -bool ChibiUARTDriver::tx_pending() { return false; } +bool UARTDriver::tx_pending() { return false; } /* Empty implementations of Stream virtual methods */ -uint32_t ChibiUARTDriver::available() { +uint32_t UARTDriver::available() { if (!_initialised) { return 0; } @@ -320,7 +320,7 @@ uint32_t ChibiUARTDriver::available() { return _readbuf.available(); } -uint32_t ChibiUARTDriver::txspace() +uint32_t UARTDriver::txspace() { if (!_initialised) { return 0; @@ -328,7 +328,7 @@ uint32_t ChibiUARTDriver::txspace() return _writebuf.space(); } -int16_t ChibiUARTDriver::read() +int16_t UARTDriver::read() { if (_uart_owner_thd != chThdGetSelfX()){ return -1; @@ -349,7 +349,7 @@ int16_t ChibiUARTDriver::read() } /* Empty implementations of Print virtual methods */ -size_t ChibiUARTDriver::write(uint8_t c) +size_t UARTDriver::write(uint8_t c) { if (!chMtxTryLock(&_write_mutex)) { return -1; @@ -372,7 +372,7 @@ size_t ChibiUARTDriver::write(uint8_t c) return ret; } -size_t ChibiUARTDriver::write(const uint8_t *buffer, size_t size) +size_t UARTDriver::write(const uint8_t *buffer, size_t size) { if (!_initialised) { return 0; @@ -404,7 +404,7 @@ size_t ChibiUARTDriver::write(const uint8_t *buffer, size_t size) wait for data to arrive, or a timeout. Return true if data has arrived, false on timeout */ -bool ChibiUARTDriver::wait_timeout(uint16_t n, uint32_t timeout_ms) +bool UARTDriver::wait_timeout(uint16_t n, uint32_t timeout_ms) { chEvtGetAndClearEvents(EVT_DATA); if (available() >= n) { @@ -421,7 +421,7 @@ bool ChibiUARTDriver::wait_timeout(uint16_t n, uint32_t timeout_ms) 1kHz in the timer thread. Doing it this way reduces the system call overhead in the main task enormously. */ -void ChibiUARTDriver::_timer_tick(void) +void UARTDriver::_timer_tick(void) { int ret; uint32_t n; @@ -463,7 +463,7 @@ void ChibiUARTDriver::_timer_tick(void) } if(sdef.is_usb) { #ifdef HAVE_USB_SERIAL - ((ChibiGPIO *)hal.gpio)->set_usb_connected(); + ((GPIO *)hal.gpio)->set_usb_connected(); #endif } _in_timer = true; @@ -578,7 +578,7 @@ end: /* change flow control mode for port */ -void ChibiUARTDriver::set_flow_control(enum flow_control flow_control) +void UARTDriver::set_flow_control(enum flow_control flow_control) { if (sdef.rts_line == 0 || sdef.is_usb) { // no hw flow control available @@ -623,7 +623,7 @@ void ChibiUARTDriver::set_flow_control(enum flow_control flow_control) software update of rts line. We don't use the HW support for RTS as it has no hysteresis, so it ends up toggling RTS on every byte */ -void ChibiUARTDriver::update_rts_line(void) +void UARTDriver::update_rts_line(void) { if (sdef.rts_line == 0 || _flow_control == FLOW_CONTROL_DISABLE) { return; diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.h b/libraries/AP_HAL_ChibiOS/UARTDriver.h index 1f8553b6bf..eab482ffbb 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.h +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.h @@ -24,9 +24,9 @@ #define RX_BOUNCE_BUFSIZE 128 #define TX_BOUNCE_BUFSIZE 64 -class ChibiOS::ChibiUARTDriver : public AP_HAL::UARTDriver { +class ChibiOS::UARTDriver : public AP_HAL::UARTDriver { public: - ChibiUARTDriver(uint8_t serial_num); + UARTDriver(uint8_t serial_num); void begin(uint32_t b); void begin(uint32_t b, uint16_t rxS, uint16_t txS); diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp index a3022790bb..ec3e43fa5a 100644 --- a/libraries/AP_HAL_ChibiOS/Util.cpp +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -37,7 +37,7 @@ extern "C" { /** how much free memory do we have in bytes. */ -uint32_t ChibiUtil::available_memory(void) +uint32_t Util::available_memory(void) { // from malloc.c in hwdef return mem_available(); @@ -47,7 +47,7 @@ uint32_t ChibiUtil::available_memory(void) Special Allocation Routines */ -void* ChibiUtil::malloc_type(size_t size, AP_HAL::Util::Memory_Type mem_type) +void* Util::malloc_type(size_t size, AP_HAL::Util::Memory_Type mem_type) { if (mem_type == AP_HAL::Util::MEM_FAST) { return try_alloc_from_ccm_ram(size); @@ -56,7 +56,7 @@ void* ChibiUtil::malloc_type(size_t size, AP_HAL::Util::Memory_Type mem_type) } } -void ChibiUtil::free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type) +void Util::free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type) { if (ptr != NULL) { chHeapFree(ptr); @@ -64,7 +64,7 @@ void ChibiUtil::free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_ } -void* ChibiUtil::try_alloc_from_ccm_ram(size_t size) +void* Util::try_alloc_from_ccm_ram(size_t size) { void *ret = malloc_ccm(size); if (ret == nullptr) { @@ -77,7 +77,7 @@ void* ChibiUtil::try_alloc_from_ccm_ram(size_t size) /* get safety switch state */ -ChibiUtil::safety_state ChibiUtil::safety_switch_state(void) +Util::safety_state Util::safety_switch_state(void) { #if HAL_WITH_IO_MCU if (AP_BoardConfig::io_enabled()) { @@ -87,7 +87,7 @@ ChibiUtil::safety_state ChibiUtil::safety_switch_state(void) return SAFETY_NONE; } -void ChibiUtil::set_imu_temp(float current) +void Util::set_imu_temp(float current) { #if HAL_WITH_IO_MCU && HAL_HAVE_IMU_HEATER if (!heater.target || *heater.target == -1 || !AP_BoardConfig::io_enabled()) { @@ -130,7 +130,7 @@ void ChibiUtil::set_imu_temp(float current) #endif // HAL_WITH_IO_MCU && HAL_HAVE_IMU_HEATER } -void ChibiUtil::set_imu_target_temp(int8_t *target) +void Util::set_imu_target_temp(int8_t *target) { #if HAL_WITH_IO_MCU && HAL_HAVE_IMU_HEATER heater.target = target; diff --git a/libraries/AP_HAL_ChibiOS/Util.h b/libraries/AP_HAL_ChibiOS/Util.h index a6c98f90cc..14a72569d5 100644 --- a/libraries/AP_HAL_ChibiOS/Util.h +++ b/libraries/AP_HAL_ChibiOS/Util.h @@ -23,7 +23,7 @@ // this checks an address is in main memory and 16 bit aligned #define IS_DMA_SAFE(addr) ((uint32_t(addr) & 0xF0000001) == 0x20000000) -class ChibiOS::ChibiUtil : public AP_HAL::Util { +class ChibiOS::Util : public AP_HAL::Util { public: bool run_debug_shell(AP_HAL::BetterStream *stream) { return false; } AP_HAL::Semaphore *new_semaphore(void) override { return new ChibiOS::Semaphore; } diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index 1d5e294adb..339e29fdc4 100755 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -419,7 +419,7 @@ def write_UART_config(f): devnames = "ABCDEFGH" for dev in uart_list: idx = uart_list.index(dev) - f.write('#define HAL_UART%s_DRIVER ChibiOS::ChibiUARTDriver uart%sDriver(%u)\n' % (devnames[idx], devnames[idx], idx)) + f.write('#define HAL_UART%s_DRIVER ChibiOS::UARTDriver uart%sDriver(%u)\n' % (devnames[idx], devnames[idx], idx)) for idx in range(len(uart_list), 6): f.write('#define HAL_UART%s_DRIVER Empty::UARTDriver uart%sDriver\n' % (devnames[idx], devnames[idx])) @@ -427,7 +427,7 @@ def write_UART_config(f): f.write('#define HAL_WITH_IO_MCU 1\n') idx = len(uart_list) f.write('#define HAL_UART_IOMCU_IDX %u\n' % idx) - f.write('#define HAL_UART_IO_DRIVER ChibiOS::ChibiUARTDriver uart_io(HAL_UART_IOMCU_IDX)\n') + f.write('#define HAL_UART_IO_DRIVER ChibiOS::UARTDriver uart_io(HAL_UART_IOMCU_IDX)\n') uart_list.append(config['IOMCU_UART'][0]) else: f.write('#define HAL_WITH_IO_MCU 0\n') diff --git a/libraries/AP_HAL_ChibiOS/shared_dma.cpp b/libraries/AP_HAL_ChibiOS/shared_dma.cpp index 670520dbb6..dbaa748774 100644 --- a/libraries/AP_HAL_ChibiOS/shared_dma.cpp +++ b/libraries/AP_HAL_ChibiOS/shared_dma.cpp @@ -20,6 +20,8 @@ code to handle sharing of DMA channels between peripherals */ +using namespace ChibiOS; + Shared_DMA::dma_lock Shared_DMA::locks[SHARED_DMA_MAX_STREAM_ID]; void Shared_DMA::init(void) diff --git a/libraries/AP_HAL_ChibiOS/shared_dma.h b/libraries/AP_HAL_ChibiOS/shared_dma.h index c39ddba5a5..0d2effd0af 100644 --- a/libraries/AP_HAL_ChibiOS/shared_dma.h +++ b/libraries/AP_HAL_ChibiOS/shared_dma.h @@ -23,7 +23,7 @@ // DMA stream ID for stream_id2 when only one is needed #define SHARED_DMA_NONE 255 -class Shared_DMA +class ChibiOS::Shared_DMA { public: FUNCTOR_TYPEDEF(dma_allocate_fn_t, void);