diff --git a/libraries/AP_HAL_ChibiOS/AnalogIn.cpp b/libraries/AP_HAL_ChibiOS/AnalogIn.cpp index dd1597a55d..6b4e805d90 100644 --- a/libraries/AP_HAL_ChibiOS/AnalogIn.cpp +++ b/libraries/AP_HAL_ChibiOS/AnalogIn.cpp @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #include @@ -58,7 +58,7 @@ using namespace ChibiOS; /* scaling table between ADC count and actual input voltage, to account - for voltage dividers on the board. + for voltage dividers on the board. */ const AnalogIn::pin_info AnalogIn::pin_config[] = HAL_ANALOG_PINS; @@ -81,7 +81,7 @@ AnalogSource::AnalogSource(int16_t pin, float initial_value) : } -float AnalogSource::read_average() +float AnalogSource::read_average() { WITH_SEMAPHORE(_semaphore); @@ -97,7 +97,7 @@ float AnalogSource::read_average() return _value; } -float AnalogSource::read_latest() +float AnalogSource::read_latest() { return _latest_value; } @@ -192,7 +192,7 @@ void AnalogIn::adccallback(ADCDriver *adcp) stm32_cacheBufferInvalidate(buffer, sizeof(adcsample_t)*ADC_DMA_BUF_DEPTH*ADC_GRP1_NUM_CHANNELS); for (uint8_t i = 0; i < ADC_DMA_BUF_DEPTH; i++) { - for (uint8_t j = 0; j < ADC_GRP1_NUM_CHANNELS; j++) { + for (uint8_t j = 0; j < ADC_GRP1_NUM_CHANNELS; j++) { sample_sum[j] += *buffer++; } } @@ -290,7 +290,7 @@ void AnalogIn::_timer_tick(void) // update power status flags update_power_flags(); - + // match the incoming channels to the currently active pins for (uint8_t i=0; i < ADC_GRP1_NUM_CHANNELS; i++) { #ifdef ANALOG_VCC_5V_PIN @@ -312,7 +312,7 @@ void AnalogIn::_timer_tick(void) _servorail_voltage = iomcu.get_vservo(); _rssi_voltage = iomcu.get_vrssi(); #endif - + for (uint8_t i=0; iget_soft_armed()) { // the power status has changed while armed flags |= MAV_POWER_STATUS_CHANGED; @@ -413,4 +413,3 @@ void AnalogIn::update_power_flags(void) _power_flags = flags; } #endif // HAL_USE_ADC - diff --git a/libraries/AP_HAL_ChibiOS/AnalogIn.h b/libraries/AP_HAL_ChibiOS/AnalogIn.h index d218f1f5ea..6f6471dbf2 100644 --- a/libraries/AP_HAL_ChibiOS/AnalogIn.h +++ b/libraries/AP_HAL_ChibiOS/AnalogIn.h @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ @@ -54,7 +54,7 @@ private: class ChibiOS::AnalogIn : public AP_HAL::AnalogIn { public: friend class AnalogSource; - + void init() override; AP_HAL::AnalogSource* channel(int16_t pin) override; void _timer_tick(void); @@ -66,7 +66,7 @@ public: private: void read_adc(uint32_t *val); void update_power_flags(void); - + int _battery_handle; int _servorail_handle; int _system_power_handle; @@ -84,9 +84,9 @@ private: struct pin_info { uint8_t channel; float scaling; - }; + }; static const pin_info pin_config[]; - + static adcsample_t *samples; static uint32_t sample_sum[]; static uint32_t sample_count; diff --git a/libraries/AP_HAL_ChibiOS/CAN.h b/libraries/AP_HAL_ChibiOS/CAN.h index 1f11192a70..6717b1057f 100644 --- a/libraries/AP_HAL_ChibiOS/CAN.h +++ b/libraries/AP_HAL_ChibiOS/CAN.h @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ diff --git a/libraries/AP_HAL_ChibiOS/CANClock.cpp b/libraries/AP_HAL_ChibiOS/CANClock.cpp index 4507e827f9..f7748d0e23 100644 --- a/libraries/AP_HAL_ChibiOS/CANClock.cpp +++ b/libraries/AP_HAL_ChibiOS/CANClock.cpp @@ -409,4 +409,4 @@ UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler) } #endif -#endif //HAL_WITH_UAVCAN \ No newline at end of file +#endif //HAL_WITH_UAVCAN diff --git a/libraries/AP_HAL_ChibiOS/CANSerialRouter.h b/libraries/AP_HAL_ChibiOS/CANSerialRouter.h index 0c39113ecf..841a4f5580 100644 --- a/libraries/AP_HAL_ChibiOS/CANSerialRouter.h +++ b/libraries/AP_HAL_ChibiOS/CANSerialRouter.h @@ -66,4 +66,3 @@ public: }; #endif // AP_UAVCAN_SLCAN_ENABLED - diff --git a/libraries/AP_HAL_ChibiOS/CanIface.cpp b/libraries/AP_HAL_ChibiOS/CanIface.cpp index 15ed933ceb..925ccb4a5a 100644 --- a/libraries/AP_HAL_ChibiOS/CanIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CanIface.cpp @@ -1,18 +1,18 @@ /* * The MIT License (MIT) - * + * * Copyright (c) 2014 Pavel Kirienko - * + * * Permission is hereby granted, free of charge, to any person obtaining a copy of * this software and associated documentation files (the "Software"), to deal in * the Software without restriction, including without limitation the rights to * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of * the Software, and to permit persons to whom the Software is furnished to do so, * subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in all * copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR @@ -34,7 +34,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Siddharth Bharat Purohit */ diff --git a/libraries/AP_HAL_ChibiOS/Device.h b/libraries/AP_HAL_ChibiOS/Device.h index 5978d0f482..febd7ff663 100644 --- a/libraries/AP_HAL_ChibiOS/Device.h +++ b/libraries/AP_HAL_ChibiOS/Device.h @@ -42,7 +42,7 @@ public: void bouncebuffer_setup(const uint8_t *&buf_tx, uint16_t tx_len, uint8_t *&buf_rx, uint16_t rx_len); void bouncebuffer_finish(const uint8_t *buf_tx, uint8_t *buf_rx, uint16_t rx_len); - + private: struct callback_info { struct callback_info *next; @@ -63,4 +63,3 @@ private: } #endif // I2C or SPI - diff --git a/libraries/AP_HAL_ChibiOS/GPIO.cpp b/libraries/AP_HAL_ChibiOS/GPIO.cpp index a10ea98d18..554982db6c 100644 --- a/libraries/AP_HAL_ChibiOS/GPIO.cpp +++ b/libraries/AP_HAL_ChibiOS/GPIO.cpp @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #include "GPIO.h" @@ -130,7 +130,7 @@ AP_HAL::DigitalSource* GPIO::channel(uint16_t pin) extern const AP_HAL::HAL& hal; -/* +/* Attach an interrupt handler to a GPIO pin number. The pin number must be one specified with a GPIO() marker in hwdef.dat */ @@ -188,7 +188,7 @@ bool GPIO::_attach_interrupt(ioline_t line, palcallback_t cb, void *p, uint8_t m return false; } break; - } + } osalSysLock(); palevent_t *pep = pal_lld_get_line_event(line); @@ -260,4 +260,3 @@ void pal_interrupt_cb_functor(void *arg) } (g->fn)(g->pin_num, palReadLine(g->pal_line), now); } - diff --git a/libraries/AP_HAL_ChibiOS/GPIO.h b/libraries/AP_HAL_ChibiOS/GPIO.h index 459e303b28..3aa5f941da 100644 --- a/libraries/AP_HAL_ChibiOS/GPIO.h +++ b/libraries/AP_HAL_ChibiOS/GPIO.h @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #pragma once @@ -55,7 +55,7 @@ public: /* attach interrupt via ioline_t */ bool _attach_interrupt(ioline_t line, AP_HAL::Proc p, uint8_t mode); - + private: bool _usb_connected; bool _ext_started; diff --git a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp index 3054aa917a..c7f64dc7ff 100644 --- a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp +++ b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #include @@ -178,13 +178,13 @@ static void main_loop() ChibiOS::Shared_DMA::init(); peripheral_power_enable(); - + hal.uartA->begin(115200); #ifdef HAL_SPI_CHECK_CLOCK_FREQ // optional test of SPI clock frequencies ChibiOS::SPIDevice::test_clock_freq(); -#endif +#endif hal.uartB->begin(38400); hal.uartC->begin(57600); @@ -242,7 +242,7 @@ static void main_loop() thread_running = true; chRegSetThreadName(SKETCHNAME); - + /* switch to high priority for main loop */ @@ -282,7 +282,7 @@ void HAL_ChibiOS::run(int argc, char * const argv[], Callbacks* callbacks) const #ifdef HAL_USB_PRODUCT_ID setup_usb_strings(); #endif - + #ifdef HAL_STDOUT_SERIAL //STDOUT Initialistion SerialConfig stdoutcfg = diff --git a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.h b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.h index af9660b6ff..b73d47124c 100644 --- a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.h +++ b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.h @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #pragma once diff --git a/libraries/AP_HAL_ChibiOS/I2CDevice.cpp b/libraries/AP_HAL_ChibiOS/I2CDevice.cpp index f3ca77aade..01c5e38f40 100644 --- a/libraries/AP_HAL_ChibiOS/I2CDevice.cpp +++ b/libraries/AP_HAL_ChibiOS/I2CDevice.cpp @@ -65,9 +65,9 @@ I2CBus I2CDeviceManager::businfo[ARRAY_SIZE(I2CD)]; void I2CBus::dma_init(void) { chMtxObjectInit(&dma_lock); - dma_handle = new Shared_DMA(I2CD[busnum].dma_channel_tx, I2CD[busnum].dma_channel_rx, + dma_handle = new Shared_DMA(I2CD[busnum].dma_channel_tx, I2CD[busnum].dma_channel_rx, FUNCTOR_BIND_MEMBER(&I2CBus::dma_allocate, void, Shared_DMA *), - FUNCTOR_BIND_MEMBER(&I2CBus::dma_deallocate, void, Shared_DMA *)); + FUNCTOR_BIND_MEMBER(&I2CBus::dma_deallocate, void, Shared_DMA *)); } // Clear Bus to avoid bus lockup @@ -174,7 +174,7 @@ I2CDevice::I2CDevice(uint8_t busnum, uint8_t address, uint32_t bus_clock, bool u I2CDevice::~I2CDevice() { #if 0 - printf("I2C device bus %u address 0x%02x closed\n", + printf("I2C device bus %u address 0x%02x closed\n", (unsigned)bus.busnum, (unsigned)_address); #endif free(pname); @@ -201,7 +201,7 @@ bool I2CDevice::transfer(const uint8_t *send, uint32_t send_len, hal.console->printf("I2C: not owner of 0x%x for addr 0x%02x\n", (unsigned)get_bus_id(), _address); return false; } - + #if defined(STM32F7) || defined(STM32H7) if (_use_smbus) { bus.i2ccfg.cr1 |= I2C_CR1_SMBHEN; @@ -248,7 +248,7 @@ bool I2CDevice::_transfer(const uint8_t *send, uint32_t send_len, i2cAcquireBus(I2CD[bus.busnum].i2c); bus.bouncebuffer_setup(send, send_len, recv, recv_len); - + for(uint8_t i=0 ; i <= _retries; i++) { int ret; // calculate a timeout as twice the expected transfer time, and set as min of 4ms @@ -262,7 +262,7 @@ bool I2CDevice::_transfer(const uint8_t *send, uint32_t send_len, i2cStart(I2CD[bus.busnum].i2c, &bus.i2ccfg); osalDbgAssert(I2CD[bus.busnum].i2c->state == I2C_READY, "i2cStart state"); - + osalSysLock(); hal.util->persistent_data.i2c_count++; osalSysUnlock(); @@ -278,7 +278,7 @@ bool I2CDevice::_transfer(const uint8_t *send, uint32_t send_len, osalDbgAssert(I2CD[bus.busnum].i2c->state == I2C_STOP, "i2cStart state"); bus.dma_handle->unlock(); - + if (I2CD[bus.busnum].i2c->errors & I2C_ISR_LIMIT) { AP::internalerror().error(AP_InternalError::error_t::i2c_isr); break; @@ -311,7 +311,7 @@ bool I2CDevice::read_registers_multiple(uint8_t first_reg, uint8_t *recv, return false; } - + /* register a periodic callback */ @@ -319,7 +319,7 @@ AP_HAL::Device::PeriodicHandle I2CDevice::register_periodic_callback(uint32_t pe { return bus.register_periodic_callback(period_usec, cb, this); } - + /* adjust a periodic callback diff --git a/libraries/AP_HAL_ChibiOS/I2CDevice.h b/libraries/AP_HAL_ChibiOS/I2CDevice.h index 5a7448b4a7..72580d7a88 100644 --- a/libraries/AP_HAL_ChibiOS/I2CDevice.h +++ b/libraries/AP_HAL_ChibiOS/I2CDevice.h @@ -13,7 +13,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Modified for use in AP_HAL_ChibiOS by Andrew Tridgell and Siddharth Bharat Purohit */ @@ -45,7 +45,7 @@ public: // have two DMA channels that we are handling with the shared_dma // code mutex_t dma_lock; - + void dma_allocate(Shared_DMA *); void dma_deallocate(Shared_DMA *); void dma_init(void); @@ -53,7 +53,7 @@ public: static void clear_bus(uint8_t busidx); static uint8_t read_sda(uint8_t busidx); }; - + class I2CDevice : public AP_HAL::I2CDevice { public: static I2CDevice *from(AP_HAL::I2CDevice *dev) @@ -95,7 +95,7 @@ public: void set_split_transfers(bool set) override { _split_transfers = set; } - + private: I2CBus &bus; bool _transfer(const uint8_t *send, uint32_t send_len, @@ -115,10 +115,10 @@ public: friend class I2CDevice; static I2CBus businfo[]; - + // constructor I2CDeviceManager(); - + static I2CDeviceManager *from(AP_HAL::I2CDeviceManager *i2c_mgr) { return static_cast(i2c_mgr); @@ -147,4 +147,3 @@ public: } #endif // HAL_USE_I2C - diff --git a/libraries/AP_HAL_ChibiOS/RCInput.cpp b/libraries/AP_HAL_ChibiOS/RCInput.cpp index d672fe3c88..ced9e78aa0 100644 --- a/libraries/AP_HAL_ChibiOS/RCInput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCInput.cpp @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #include "RCInput.h" @@ -73,7 +73,7 @@ bool RCInput::new_input() radio->init(); } } -#endif +#endif return valid; } @@ -107,7 +107,7 @@ uint8_t RCInput::read(uint16_t* periods, uint8_t len) if (!_init) { return false; } - + if (len > RC_INPUT_MAX_CHANNELS) { len = RC_INPUT_MAX_CHANNELS; } diff --git a/libraries/AP_HAL_ChibiOS/RCInput.h b/libraries/AP_HAL_ChibiOS/RCInput.h index 47ed354f89..a841279205 100644 --- a/libraries/AP_HAL_ChibiOS/RCInput.h +++ b/libraries/AP_HAL_ChibiOS/RCInput.h @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #pragma once diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 914a5fa7e7..806b80a76d 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -98,7 +98,7 @@ void RCOutput::set_freq_group(pwm_group &group) // speed setup in DMA handler return; } - + uint16_t freq_set = group.rc_frequency; uint32_t old_clock = group.pwm_cfg.frequency; uint32_t old_period = group.pwm_cfg.period; @@ -132,7 +132,7 @@ void RCOutput::set_freq_group(pwm_group &group) } else { group.pwm_cfg.period = group.pwm_cfg.frequency/freq_set; } - + bool force_reconfig = false; for (uint8_t j=0; j<4; j++) { if (group.pwm_cfg.channels[j].mode == PWM_OUTPUT_ACTIVE_LOW) { @@ -323,7 +323,7 @@ void RCOutput::write(uint8_t chan, uint16_t period_us) if (chan < chan_offset) { return; } - + if (safety_state == AP_HAL::Util::SAFETY_DISARMED && !(safety_mask & (1U<lock(); - + // configure timer driver for DMAR at requested rate if (group.pwm_started) { pwmStop(group.pwm_drv); @@ -561,10 +561,10 @@ bool RCOutput::setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_ } } } - + pwmStart(group.pwm_drv, &group.pwm_cfg); group.pwm_started = true; - + for (uint8_t j=0; j<4; j++) { if (group.chan[j] != CHAN_DISABLED) { pwmEnableChannel(group.pwm_drv, j, 0); @@ -579,7 +579,7 @@ bool RCOutput::setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_ /* - setup output mode for a group, using group.current_mode. Used to restore output + setup output mode for a group, using group.current_mode. Used to restore output after serial operations */ void RCOutput::set_group_mode(pwm_group &group) @@ -588,7 +588,7 @@ void RCOutput::set_group_mode(pwm_group &group) pwmStop(group.pwm_drv); group.pwm_started = false; } - + switch (group.current_mode) { case MODE_PWM_BRUSHED: // force zero output initially @@ -637,7 +637,7 @@ void RCOutput::set_group_mode(pwm_group &group) dshot_pulse_time_us = 1000000UL * dshot_bit_length / rate; break; } - + case MODE_PWM_ONESHOT: case MODE_PWM_ONESHOT125: // for oneshot we set a period of 0, which results in no pulses till we trigger @@ -655,7 +655,7 @@ void RCOutput::set_group_mode(pwm_group &group) } set_freq_group(group); - + if (group.current_mode != MODE_PWM_NONE && !group.pwm_started) { pwmStart(group.pwm_drv, &group.pwm_cfg); @@ -788,7 +788,7 @@ void RCOutput::trigger_groups(void) } } } - + /* calculate time that we are allowed to trigger next pulse to guarantee at least a 50us gap between pulses @@ -805,7 +805,7 @@ void RCOutput::trigger_groups(void) void RCOutput::timer_tick(void) { safety_update(); - + uint64_t now = AP_HAL::micros64(); for (uint8_t i = 0; i < NUM_GROUPS; i++ ) { pwm_group &group = pwm_group_list[i]; @@ -933,7 +933,7 @@ void RCOutput::dshot_send(pwm_group &group, bool blocking) // doing serial output, don't send DShot pulses return; } - + if (blocking) { group.dma_handle->lock(); } else { @@ -941,11 +941,11 @@ void RCOutput::dshot_send(pwm_group &group, bool blocking) return; } } - + bool safety_on = hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED; memset((uint8_t *)group.dma_buffer, 0, dshot_buffer_length); - + for (uint8_t i=0; i<4; i++) { uint8_t chan = group.chan[i]; if (chan != CHAN_DISABLED) { @@ -955,7 +955,7 @@ void RCOutput::dshot_send(pwm_group &group, bool blocking) // safety is on, overwride pwm pwm = safe_pwm[chan+chan_offset]; } - + const uint16_t chan_mask = (1U<>= chan_offset; pwm_group *new_serial_group = nullptr; - + // find the channel group for (uint8_t i = 0; i < NUM_GROUPS; i++ ) { pwm_group &group = pwm_group_list[i]; @@ -1149,10 +1149,10 @@ bool RCOutput::serial_setup_output(uint8_t chan, uint32_t baudrate, uint16_t cha serial_thread = chThdGetSelfX(); serial_priority = chThdGetSelfX()->realprio; chThdSetPriority(HIGHPRIO); - + // remember the bit period for serial_read_byte() serial_group->serial.bit_time_us = 1000000UL / baudrate; - + // remember the thread that set things up. This is also used to // mark the group as doing serial output, so normal output is // suspended @@ -1160,7 +1160,7 @@ bool RCOutput::serial_setup_output(uint8_t chan, uint32_t baudrate, uint16_t cha return true; } - + /* fill in a DMA buffer for a serial byte, assuming 1 start bit and 1 stop bit @@ -1190,11 +1190,11 @@ void RCOutput::fill_DMA_buffer_byte(uint32_t *buffer, uint8_t stride, uint8_t b, bool RCOutput::serial_write_byte(uint8_t b) { chEvtGetAndClearEvents(serial_event_mask); - + fill_DMA_buffer_byte(serial_group->dma_buffer+serial_group->serial.chan, 4, b, serial_group->bit_width_mul*10); serial_group->in_serial_dma = true; - + // start sending the pulses out send_pulses_DMAR(*serial_group, 10*4*sizeof(uint32_t)); @@ -1202,7 +1202,7 @@ bool RCOutput::serial_write_byte(uint8_t b) eventmask_t mask = chEvtWaitAnyTimeout(serial_event_mask, chTimeMS2I(2)); serial_group->in_serial_dma = false; - + return (mask & serial_event_mask) != 0; } @@ -1227,7 +1227,7 @@ bool RCOutput::serial_write_bytes(const uint8_t *bytes, uint16_t len) // add a small delay for last word of output to have completely // finished hal.scheduler->delay_microseconds(25); - + serial_group->dma_handle->unlock(); return true; #else @@ -1248,7 +1248,7 @@ void RCOutput::serial_bit_irq(void) #if RCOU_SERIAL_TIMING_DEBUG palWriteLine(HAL_GPIO_LINE_GPIO55, bit); #endif - + if (irq.nbits == 0 || bit == irq.last_bit) { // start of byte, should be low if (bit != 0) { @@ -1286,7 +1286,7 @@ void RCOutput::serial_bit_irq(void) } } irq.last_bit = bit; - + if (send_signal) { chSysLockFromISR(); chVTResetI(&irq.serial_timeout); @@ -1325,7 +1325,7 @@ bool RCOutput::serial_read_byte(uint8_t &b) } byteval = irq.bitmask | 0x200; } - + if ((byteval & 0x201) != 0x200) { // wrong start/stop bits return false; @@ -1354,7 +1354,7 @@ uint16_t RCOutput::serial_read_bytes(uint8_t *buf, uint16_t len) hal.gpio->pinMode(54, 1); hal.gpio->pinMode(55, 1); #endif - + // assume GPIO mappings for PWM outputs start at 50 palSetLineMode(line, gpio_mode); @@ -1372,14 +1372,14 @@ uint16_t RCOutput::serial_read_bytes(uint8_t *buf, uint16_t len) #if RCOU_SERIAL_TIMING_DEBUG palWriteLine(HAL_GPIO_LINE_GPIO54, 1); #endif - + if (!((GPIO *)hal.gpio)->_attach_interrupt(line, serial_bit_irq, AP_HAL::GPIO::INTERRUPT_BOTH)) { #if RCOU_SERIAL_TIMING_DEBUG palWriteLine(HAL_GPIO_LINE_GPIO54, 0); #endif return false; } - + for (i=0; i_attach_interrupt(line, nullptr, 0); irq.waiter = nullptr; - + palSetLineMode(line, restore_mode); #if RCOU_SERIAL_TIMING_DEBUG palWriteLine(HAL_GPIO_LINE_GPIO54, 0); @@ -1509,7 +1509,7 @@ void RCOutput::safety_update(void) // remember mask of channels to allow with safety on safety_mask = boardconfig->get_safety_mask(); } - + #ifdef HAL_GPIO_PIN_SAFETY_IN // handle safety button bool safety_pressed = palReadLine(HAL_GPIO_PIN_SAFETY_IN); diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h index 3e1089b525..e343508029 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.h +++ b/libraries/AP_HAL_ChibiOS/RCOutput.h @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #pragma once @@ -53,7 +53,7 @@ public: float scale_esc_to_unity(uint16_t pwm) override { return 2.0 * ((float) pwm - _esc_pwm_min) / (_esc_pwm_max - _esc_pwm_min) - 1.0; } - + void cork(void) override; void push(void) override; @@ -72,7 +72,7 @@ public: in the safe state */ void set_safety_pwm(uint32_t chmask, uint16_t period_us) override; - + bool enable_px4io_sbus_out(uint16_t rate_hz) override; /* @@ -117,7 +117,7 @@ public: return the number of bytes read */ uint16_t serial_read_bytes(uint8_t *buf, uint16_t len) override; - + /* stop serial output. This restores the previous output mode for the channel and any other channels that were stopped by @@ -171,7 +171,7 @@ public: trigger send of neopixel data */ void neopixel_send(void) override; - + private: struct pwm_group { // only advanced timers can do high clocks needed for more than 400Hz @@ -201,17 +201,17 @@ private: uint64_t last_dmar_send_us; virtual_timer_t dma_timeout; uint8_t neopixel_nleds; - + // serial output struct { // expected time per bit uint32_t bit_time_us; - + // channel to output to within group (0 to 3) uint8_t chan; // thread waiting for byte to be written - thread_t *waiter; + thread_t *waiter; } serial; }; @@ -231,7 +231,7 @@ private: // bitmask of bits so far (includes start and stop bits) uint16_t bitmask; - // value of completed byte (includes start and stop bits) + // value of completed byte (includes start and stop bits) uint16_t byteval; // expected time per bit in system ticks @@ -239,7 +239,7 @@ private: // the bit value of the last bit received uint8_t last_bit; - + // thread waiting for byte to be read thread_t *waiter; @@ -248,12 +248,12 @@ private: bool timed_out; } irq; - + // the group being used for serial output struct pwm_group *serial_group; thread_t *serial_thread; tprio_t serial_priority; - + static pwm_group pwm_group_list[]; uint16_t _esc_pwm_min; uint16_t _esc_pwm_max; @@ -266,12 +266,12 @@ private: // number of active fmu channels uint8_t active_fmu_channels; - + static const uint8_t max_channels = 16; - + // last sent values are for all channels uint16_t last_sent[max_channels]; - + // these values are for the local channels. Non-local channels are handled by IOMCU uint32_t en_mask; uint16_t period[max_channels]; @@ -321,7 +321,7 @@ private: // update safety switch and LED void safety_update(void); - + /* DShot handling */ @@ -341,7 +341,7 @@ private: bool neopixel_pending; void dma_allocate(Shared_DMA *ctx); - void dma_deallocate(Shared_DMA *ctx); + void dma_deallocate(Shared_DMA *ctx); uint16_t create_dshot_packet(const uint16_t value, bool telem_request); void fill_DMA_buffer_dshot(uint32_t *buffer, uint8_t stride, uint16_t packet, uint16_t clockmul); void dshot_send(pwm_group &group, bool blocking); diff --git a/libraries/AP_HAL_ChibiOS/SPIDevice.cpp b/libraries/AP_HAL_ChibiOS/SPIDevice.cpp index 95347986e6..1404078be7 100644 --- a/libraries/AP_HAL_ChibiOS/SPIDevice.cpp +++ b/libraries/AP_HAL_ChibiOS/SPIDevice.cpp @@ -62,7 +62,7 @@ static const uint32_t bus_clocks[6] = { SPI1_CLOCK, SPI2_CLOCK, SPI3_CLOCK, SPI4_CLOCK, SPI5_CLOCK, SPI6_CLOCK }; -static const struct SPIDriverInfo { +static const struct SPIDriverInfo { SPIDriver *driver; uint8_t busid; // used for device IDs in parameters uint8_t dma_channel_rx; @@ -79,13 +79,13 @@ SPIBus::SPIBus(uint8_t _bus) : bus(_bus) { chMtxObjectInit(&dma_lock); - + // allow for sharing of DMA channels with other peripherals dma_handle = new Shared_DMA(spi_devices[bus].dma_channel_rx, spi_devices[bus].dma_channel_tx, FUNCTOR_BIND_MEMBER(&SPIBus::dma_allocate, void, Shared_DMA *), FUNCTOR_BIND_MEMBER(&SPIBus::dma_deallocate, void, Shared_DMA *)); - + } /* @@ -101,13 +101,13 @@ void SPIBus::dma_allocate(Shared_DMA *ctx) */ void SPIBus::dma_deallocate(Shared_DMA *ctx) { - chMtxLock(&dma_lock); + chMtxLock(&dma_lock); // another non-SPI peripheral wants one of our DMA channels if (spi_started) { spiStop(spi_devices[bus].driver); spi_started = false; } - chMtxUnlock(&dma_lock); + chMtxUnlock(&dma_lock); } @@ -242,7 +242,7 @@ uint32_t SPIDevice::derive_freq_flag_bus(uint8_t busid, uint32_t _frequency) spi_clock_freq >>= 1; i++; } - + // assuming the bitrate bits are consecutive in the CR1 register, // we can just multiply by BR_0 to get the right bits for the desired // scaling diff --git a/libraries/AP_HAL_ChibiOS/SPIDevice.h b/libraries/AP_HAL_ChibiOS/SPIDevice.h index a3acf9d5e3..d3963cacde 100644 --- a/libraries/AP_HAL_ChibiOS/SPIDevice.h +++ b/libraries/AP_HAL_ChibiOS/SPIDevice.h @@ -37,7 +37,7 @@ public: void dma_deallocate(Shared_DMA *ctx); bool spi_started; uint8_t slowdown; - + // we need an additional lock in the dma_allocate and // dma_deallocate functions to cope with 3-way contention as we // have two DMA channels that we are handling with the shared_dma @@ -82,12 +82,12 @@ public: bool transfer_fullduplex(const uint8_t *send, uint8_t *recv, uint32_t len) override; - /* + /* * send N bytes of clock pulses without taking CS. This is used * when initialising microSD interfaces over SPI */ bool clock_pulse(uint32_t len) override; - + /* See AP_HAL::Device::get_semaphore() */ AP_HAL::Semaphore *get_semaphore() override; diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp index 1af6aac2bb..2468cccaa0 100644 --- a/libraries/AP_HAL_ChibiOS/Scheduler.cpp +++ b/libraries/AP_HAL_ChibiOS/Scheduler.cpp @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #include @@ -225,7 +225,7 @@ void Scheduler::register_io_process(AP_HAL::MemberProc proc) return; } } - + if (_num_io_procs < CHIBIOS_SCHEDULER_MAX_TIMER_PROCS) { _io_proc[_num_io_procs] = proc; _num_io_procs++; @@ -268,7 +268,7 @@ void Scheduler::reboot(bool hold_in_bootloader) // disable all interrupt sources port_disable(); - + // reboot NVIC_SystemReset(); } diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.h b/libraries/AP_HAL_ChibiOS/Scheduler.h index 6371be7033..f09f2e761a 100644 --- a/libraries/AP_HAL_ChibiOS/Scheduler.h +++ b/libraries/AP_HAL_ChibiOS/Scheduler.h @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #pragma once @@ -104,7 +104,7 @@ public: A value of zero cancels the previous expected delay */ void expect_delay_ms(uint32_t ms) override; - + /* disable interrupts and return a context that can be used to restore the interrupt state. This can be used to protect @@ -163,6 +163,6 @@ private: void _run_timers(); void _run_io(void); - static void thread_create_trampoline(void *ctx); + static void thread_create_trampoline(void *ctx); }; #endif diff --git a/libraries/AP_HAL_ChibiOS/Semaphores.cpp b/libraries/AP_HAL_ChibiOS/Semaphores.cpp index 708c266167..06218b6e16 100644 --- a/libraries/AP_HAL_ChibiOS/Semaphores.cpp +++ b/libraries/AP_HAL_ChibiOS/Semaphores.cpp @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #include @@ -150,4 +150,3 @@ bool Semaphore_Recursive::take_nonblocking(void) } #endif // CH_CFG_USE_MUTEXES - diff --git a/libraries/AP_HAL_ChibiOS/Semaphores.h b/libraries/AP_HAL_ChibiOS/Semaphores.h index b2f27f6663..9ea8f93d0c 100644 --- a/libraries/AP_HAL_ChibiOS/Semaphores.h +++ b/libraries/AP_HAL_ChibiOS/Semaphores.h @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #pragma once diff --git a/libraries/AP_HAL_ChibiOS/SoftSigReader.cpp b/libraries/AP_HAL_ChibiOS/SoftSigReader.cpp index adfa0a8db3..d13a41f0af 100644 --- a/libraries/AP_HAL_ChibiOS/SoftSigReader.cpp +++ b/libraries/AP_HAL_ChibiOS/SoftSigReader.cpp @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ diff --git a/libraries/AP_HAL_ChibiOS/SoftSigReader.h b/libraries/AP_HAL_ChibiOS/SoftSigReader.h index 61a4bc3a5c..3727ae0683 100644 --- a/libraries/AP_HAL_ChibiOS/SoftSigReader.h +++ b/libraries/AP_HAL_ChibiOS/SoftSigReader.h @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #pragma once @@ -57,4 +57,3 @@ private: }; #endif // HAL_USE_ICU - diff --git a/libraries/AP_HAL_ChibiOS/SoftSigReaderInt.cpp b/libraries/AP_HAL_ChibiOS/SoftSigReaderInt.cpp index 72bd169d61..3343766937 100644 --- a/libraries/AP_HAL_ChibiOS/SoftSigReaderInt.cpp +++ b/libraries/AP_HAL_ChibiOS/SoftSigReaderInt.cpp @@ -63,7 +63,7 @@ void SoftSigReaderInt::init(EICUDriver* icu_drv, eicuchannel_t chan) for (int i=0; i< EICU_CHANNEL_ENUM_END; i++) { icucfg.iccfgp[i]=nullptr; } - + //configure main channel icucfg.iccfgp[chan] = &channel_config; #ifdef HAL_RCIN_IS_INVERTED @@ -72,7 +72,7 @@ void SoftSigReaderInt::init(EICUDriver* icu_drv, eicuchannel_t chan) channel_config.alvl = EICU_INPUT_ACTIVE_LOW; #endif channel_config.capture_cb = nullptr; - + //configure aux channel icucfg.iccfgp[aux_chan] = &aux_channel_config; #ifdef HAL_RCIN_IS_INVERTED @@ -85,7 +85,7 @@ void SoftSigReaderInt::init(EICUDriver* icu_drv, eicuchannel_t chan) eicuStart(_icu_drv, &icucfg); //sets input filtering to 4 timer clock stm32_timer_set_input_filter(_icu_drv->tim, chan, 2); - //sets input for aux_chan + //sets input for aux_chan stm32_timer_set_channel_input(_icu_drv->tim, aux_chan, 2); eicuEnable(_icu_drv); } @@ -96,10 +96,10 @@ void SoftSigReaderInt::_irq_handler(EICUDriver *eicup, eicuchannel_t aux_channel pulse_t pulse; pulse.w0 = eicup->tim->CCR[channel]; pulse.w1 = eicup->tim->CCR[aux_channel]; - + _singleton->sigbuf.push(pulse); - //check for missed interrupt + //check for missed interrupt uint32_t mask = (STM32_TIM_SR_CC1OF << channel) | (STM32_TIM_SR_CC1OF << aux_channel); if ((eicup->tim->SR & mask) != 0) { //we have missed some pulses diff --git a/libraries/AP_HAL_ChibiOS/SoftSigReaderInt.h b/libraries/AP_HAL_ChibiOS/SoftSigReaderInt.h index 9b344f9a9a..7f982a49d3 100644 --- a/libraries/AP_HAL_ChibiOS/SoftSigReaderInt.h +++ b/libraries/AP_HAL_ChibiOS/SoftSigReaderInt.h @@ -47,7 +47,7 @@ private: static SoftSigReaderInt *_singleton; static void _irq_handler(EICUDriver *eicup, eicuchannel_t channel); - + static eicuchannel_t get_pair_channel(eicuchannel_t channel); typedef struct PACKED { uint16_t w0; @@ -62,4 +62,3 @@ private: }; #endif // HAL_USE_EICU - diff --git a/libraries/AP_HAL_ChibiOS/Storage.cpp b/libraries/AP_HAL_ChibiOS/Storage.cpp index 9c8c37e0e8..b9c7ae5882 100644 --- a/libraries/AP_HAL_ChibiOS/Storage.cpp +++ b/libraries/AP_HAL_ChibiOS/Storage.cpp @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #include @@ -53,7 +53,7 @@ void Storage::_storage_open(void) return; } #endif - + _dirty_mask.clearall(); #if HAL_WITH_RAMTRON @@ -75,7 +75,7 @@ void Storage::_storage_open(void) #elif defined(USE_POSIX) // allow for fallback to microSD based storage sdcard_retry(); - + log_fd = AP::FS().open(HAL_STORAGE_FILE, O_RDWR|O_CREAT); if (log_fd == -1) { hal.console->printf("open failed of " HAL_STORAGE_FILE "\n"); @@ -94,7 +94,7 @@ void Storage::_storage_open(void) hal.console->printf("setup failed for " HAL_STORAGE_FILE "\n"); AP::FS().close(log_fd); log_fd = -1; - return; + return; } using_filesystem = true; #endif @@ -188,7 +188,7 @@ void Storage::_timer_tick(void) _dirty_mask.clear(i); } return; - } + } #endif #ifdef USE_POSIX @@ -205,9 +205,9 @@ void Storage::_timer_tick(void) } _dirty_mask.clear(i); return; - } + } #endif - + #ifdef STORAGE_FLASH_PAGE // save to storage backend _flash_write(i); @@ -223,7 +223,7 @@ void Storage::_flash_load(void) _flash_page = STORAGE_FLASH_PAGE; hal.console->printf("Storage: Using flash pages %u and %u\n", _flash_page, _flash_page+1); - + if (!_flash.init()) { AP_HAL::panic("unable to init flash storage"); } @@ -233,7 +233,7 @@ void Storage::_flash_load(void) } /* - write one storage line. This also updates _dirty_mask. + write one storage line. This also updates _dirty_mask. */ void Storage::_flash_write(uint16_t line) { diff --git a/libraries/AP_HAL_ChibiOS/Storage.h b/libraries/AP_HAL_ChibiOS/Storage.h index c56244d84a..03e4e5b55d 100644 --- a/libraries/AP_HAL_ChibiOS/Storage.h +++ b/libraries/AP_HAL_ChibiOS/Storage.h @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #pragma once @@ -69,7 +69,7 @@ private: FUNCTOR_BIND_MEMBER(&Storage::_flash_erase_sector, bool, uint8_t), FUNCTOR_BIND_MEMBER(&Storage::_flash_erase_ok, bool)}; #endif - + void _flash_load(void); void _flash_write(uint16_t line); diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp index a35172d3a5..0c0d460148 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #include @@ -139,7 +139,7 @@ static int hal_console_vprintf(const char *fmt, va_list arg) void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) { thread_init(); - + if (sdef.serial == nullptr) { return; } @@ -150,7 +150,7 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) // give more buffer space for log download on USB min_tx_buffer *= 4; } - + // on PX4 we have enough memory to have a larger transmit and // receive buffer for all ports. This means we don't get delays // while waiting to write GPS config packets @@ -184,7 +184,7 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) } _baudrate = b; } - + if (clear_buffers) { _readbuf.clear(); } @@ -382,9 +382,9 @@ void UARTDriver::tx_complete(void* self, uint32_t flags) UARTDriver* uart_drv = (UARTDriver*)self; chSysLockFromISR(); if (!uart_drv->tx_bounce_buf_ready) { - // reset timeout + // reset timeout chVTResetI(&uart_drv->tx_timeout); - + uart_drv->_last_write_completed_us = AP_HAL::micros(); uart_drv->tx_bounce_buf_ready = true; if (uart_drv->unbuffered_writes && uart_drv->_writebuf.available()) { @@ -444,14 +444,14 @@ void UARTDriver::rxbuff_full_irq(void* self, uint32_t flags) uart_drv->receive_timestamp_update(); } - + //restart the DMA transfers dmaStreamSetMemory0(uart_drv->rxdma, uart_drv->rx_bounce_buf); dmaStreamSetTransactionSize(uart_drv->rxdma, RX_BOUNCE_BUFSIZE); dmaStreamEnable(uart_drv->rxdma); if (uart_drv->_wait.thread_ctx && uart_drv->_readbuf.available() >= uart_drv->_wait.n) { chSysLockFromISR(); - chEvtSignalI(uart_drv->_wait.thread_ctx, EVT_DATA); + chEvtSignalI(uart_drv->_wait.thread_ctx, EVT_DATA); chSysUnlockFromISR(); } if (uart_drv->_rts_is_active) { @@ -549,7 +549,7 @@ int16_t UARTDriver::read() if (!_rts_is_active) { update_rts_line(); } - + return byte; } @@ -577,7 +577,7 @@ size_t UARTDriver::write(uint8_t c) if (lock_write_key != 0 || !_write_mutex.take_nonblocking()) { return 0; } - + if (!_initialised) { _write_mutex.give(); return 0; @@ -651,7 +651,7 @@ bool UARTDriver::lock_port(uint32_t write_key, uint32_t read_key) return true; } -/* +/* write to a locked port. If port is locked and key is not correct then 0 is returned and write is discarded. All writes are non-blocking */ @@ -811,7 +811,7 @@ void UARTDriver::write_pending_bytes_NODMA(uint32_t n) nwritten += ret; } _writebuf.advance(ret); - + /* We wrote less than we asked for, stop */ if ((unsigned)ret != vec[i].len) { break; @@ -843,7 +843,7 @@ void UARTDriver::write_pending_bytes(void) if (n <= 0) { return; } - + #ifndef HAL_UART_NODMA if (sdef.dma_tx) { write_pending_bytes_DMA(n); @@ -852,7 +852,7 @@ void UARTDriver::write_pending_bytes(void) { write_pending_bytes_NODMA(n); } - + // handle AUTO flow control mode if (_flow_control == FLOW_CONTROL_AUTO) { if (_first_write_started_us == 0) { @@ -991,7 +991,7 @@ void UARTDriver::_timer_tick(void) _readbuf.commit((unsigned)ret); receive_timestamp_update(); - + /* stop reading as we read less than we asked for */ if ((unsigned)ret < vec[i].len) { break; @@ -999,7 +999,7 @@ void UARTDriver::_timer_tick(void) } } if (_wait.thread_ctx && _readbuf.available() >= _wait.n) { - chEvtSignal(_wait.thread_ctx, EVT_DATA); + chEvtSignal(_wait.thread_ctx, EVT_DATA); } if (unbuffered_writes) { // now send pending bytes. If we are doing "unbuffered" writes @@ -1025,7 +1025,7 @@ 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; @@ -1034,7 +1034,7 @@ void UARTDriver::set_flow_control(enum flow_control flowcontrol) return; } switch (_flow_control) { - + case FLOW_CONTROL_DISABLE: // force RTS active when flow disabled palSetLineMode(sdef.rts_line, 1); @@ -1096,7 +1096,7 @@ void UARTDriver::update_rts_line(void) } } -/* +/* setup unbuffered writes for lower latency */ bool UARTDriver::set_unbuffered_writes(bool on) @@ -1166,7 +1166,7 @@ void UARTDriver::configure_parity(uint8_t v) SD_PARITY_ERROR); } #endif - + #ifndef HAL_UART_NODMA if(sdef.dma_rx) { //Configure serial driver to skip handling RX packets @@ -1189,7 +1189,7 @@ void UARTDriver::set_stop_bits(int n) #if HAL_USE_SERIAL // stop and start to take effect sdStop((SerialDriver*)sdef.serial); - + switch (n) { case 1: sercfg.cr2 = _cr2_options | USART_CR2_STOP1_BITS; @@ -1211,7 +1211,7 @@ void UARTDriver::set_stop_bits(int n) } -// record timestamp of new incoming data +// record timestamp of new incoming data void UARTDriver::receive_timestamp_update(void) { _receive_timestamp[_receive_timestamp_idx^1] = AP_HAL::micros64(); @@ -1224,11 +1224,11 @@ void UARTDriver::receive_timestamp_update(void) time constraint, not an exact time. It is guaranteed that the packet did not start being received after this time, but it could have been in a system buffer before the returned time. - + This takes account of the baudrate of the link. For transports that have no baudrate (such as USB) the time estimate may be less accurate. - + A return value of zero means the HAL does not support this API */ uint64_t UARTDriver::receive_time_constraint_us(uint16_t nbytes) diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.h b/libraries/AP_HAL_ChibiOS/UARTDriver.h index 0e921b066f..5c5731d63a 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.h +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.h @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #pragma once @@ -56,11 +56,11 @@ public: // control optional features bool set_options(uint8_t options) override; uint8_t get_options(void) const override; - + // write to a locked port. If port is locked and key is not correct then 0 is returned // and write is discarded size_t write_locked(const uint8_t *buffer, size_t size, uint32_t key) override; - + struct SerialDef { BaseSequentialStream* serial; bool is_usb; @@ -159,7 +159,7 @@ private: const stm32_dma_stream_t* rxdma; const stm32_dma_stream_t* txdma; #endif - virtual_timer_t tx_timeout; + virtual_timer_t tx_timeout; bool _in_timer; bool _blocking_writes; bool _initialised; @@ -201,7 +201,7 @@ private: event_listener_t ev_listener; bool parity_enabled; #endif - + #ifndef HAL_UART_NODMA static void rx_irq_cb(void* sd); #endif @@ -223,7 +223,7 @@ private: void write_pending_bytes(void); void receive_timestamp_update(void); - + void thread_init(); static void uart_thread(void *); }; diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp index 8c7d2e8369..a5b3bee893 100644 --- a/libraries/AP_HAL_ChibiOS/Util.cpp +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #include @@ -133,7 +133,7 @@ void Util::set_imu_temp(float current) // average over temperatures to remove noise heater.count++; heater.sum += current; - + // update once a second uint32_t now = AP_HAL::millis(); if (now - heater.last_update_ms < 1000) { @@ -158,14 +158,14 @@ void Util::set_imu_temp(float current) // limit to 65 degrees to prevent damage target = constrain_float(target, 0, 65); - + float err = target - current; heater.integrator += kI * err; heater.integrator = constrain_float(heater.integrator, 0, 70); heater.output = constrain_float(kP * err + heater.integrator, 0, 100); - + //hal.console->printf("integrator %.1f out=%.1f temp=%.2f err=%.2f\n", heater.integrator, heater.output, current, err); #if HAL_WITH_IO_MCU @@ -281,16 +281,16 @@ bool Util::get_system_id(char buf[40]) { uint8_t serialid[12]; char board_name[14]; - + memcpy(serialid, (const void *)UDID_START, 12); strncpy(board_name, CHIBIOS_SHORT_BOARD_NAME, 13); board_name[13] = 0; - + // this format is chosen to match the format used by HAL_PX4 snprintf(buf, 40, "%s %02X%02X%02X%02X %02X%02X%02X%02X %02X%02X%02X%02X", board_name, - (unsigned)serialid[3], (unsigned)serialid[2], (unsigned)serialid[1], (unsigned)serialid[0], - (unsigned)serialid[7], (unsigned)serialid[6], (unsigned)serialid[5], (unsigned)serialid[4], + (unsigned)serialid[3], (unsigned)serialid[2], (unsigned)serialid[1], (unsigned)serialid[0], + (unsigned)serialid[7], (unsigned)serialid[6], (unsigned)serialid[5], (unsigned)serialid[4], (unsigned)serialid[11], (unsigned)serialid[10], (unsigned)serialid[9],(unsigned)serialid[8]); buf[39] = 0; return true; diff --git a/libraries/AP_HAL_ChibiOS/Util.h b/libraries/AP_HAL_ChibiOS/Util.h index 8fa14ea4c5..5e44b851c7 100644 --- a/libraries/AP_HAL_ChibiOS/Util.h +++ b/libraries/AP_HAL_ChibiOS/Util.h @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #pragma once diff --git a/libraries/AP_HAL_ChibiOS/bxcan.hpp b/libraries/AP_HAL_ChibiOS/bxcan.hpp index ba255b60a0..ceede4be3c 100644 --- a/libraries/AP_HAL_ChibiOS/bxcan.hpp +++ b/libraries/AP_HAL_ChibiOS/bxcan.hpp @@ -1,18 +1,18 @@ /* * The MIT License (MIT) - * + * * Copyright (c) 2014 Pavel Kirienko - * + * * Permission is hereby granted, free of charge, to any person obtaining a copy of * this software and associated documentation files (the "Software"), to deal in * the Software without restriction, including without limitation the rights to * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of * the Software, and to permit persons to whom the Software is furnished to do so, * subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in all * copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR @@ -34,7 +34,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Modified for Ardupilot by Siddharth Bharat Purohit */ @@ -327,4 +327,4 @@ constexpr unsigned long FMR_FINIT = (1U << 0); /* Bit 0: Filter Init # undef constexpr #endif #endif //!defined(STM32H7XX) -#endif //HAL_WITH_UAVCAN \ No newline at end of file +#endif //HAL_WITH_UAVCAN diff --git a/libraries/AP_HAL_ChibiOS/fdcan.hpp b/libraries/AP_HAL_ChibiOS/fdcan.hpp index 67165833cd..0382579623 100644 --- a/libraries/AP_HAL_ChibiOS/fdcan.hpp +++ b/libraries/AP_HAL_ChibiOS/fdcan.hpp @@ -87,4 +87,4 @@ CanType* const Can[UAVCAN_STM32_NUM_IFACES] = { # undef constexpr #endif #endif //defined(STM32H7XX) -#endif //HAL_WITH_UAVCAN \ No newline at end of file +#endif //HAL_WITH_UAVCAN diff --git a/libraries/AP_HAL_ChibiOS/sdcard.cpp b/libraries/AP_HAL_ChibiOS/sdcard.cpp index 9608051618..9906dec0a5 100644 --- a/libraries/AP_HAL_ChibiOS/sdcard.cpp +++ b/libraries/AP_HAL_ChibiOS/sdcard.cpp @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * */ #include "SPIDevice.h" @@ -97,7 +97,7 @@ bool sdcard_init() return false; } device->set_slowdown(sd_slowdown); - + mmcObjectInit(&MMCD1); mmcconfig.spip = @@ -224,4 +224,3 @@ void spiReceiveHook(SPIDriver *spip, size_t n, void *rxbuf) } #endif - diff --git a/libraries/AP_HAL_ChibiOS/sdcard.h b/libraries/AP_HAL_ChibiOS/sdcard.h index b4f7438885..b0bd4ab1e2 100644 --- a/libraries/AP_HAL_ChibiOS/sdcard.h +++ b/libraries/AP_HAL_ChibiOS/sdcard.h @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * */ #pragma once diff --git a/libraries/AP_HAL_ChibiOS/shared_dma.cpp b/libraries/AP_HAL_ChibiOS/shared_dma.cpp index a32e6161b6..25eabcda71 100644 --- a/libraries/AP_HAL_ChibiOS/shared_dma.cpp +++ b/libraries/AP_HAL_ChibiOS/shared_dma.cpp @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #include "shared_dma.h" diff --git a/libraries/AP_HAL_ChibiOS/shared_dma.h b/libraries/AP_HAL_ChibiOS/shared_dma.h index a0af229752..7473fb1385 100644 --- a/libraries/AP_HAL_ChibiOS/shared_dma.h +++ b/libraries/AP_HAL_ChibiOS/shared_dma.h @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #pragma once @@ -37,13 +37,13 @@ public: // initialise the stream locks static void init(void); - + // blocking lock call void lock(void); // non-blocking lock call bool lock_nonblock(void); - + // unlock call. The DMA channel will not be immediately // deallocated. Instead it will be deallocated if another driver // needs it @@ -54,17 +54,17 @@ public: // unlock call from a chSysLock zone void unlock_from_lockzone(void); - + //should be called inside the destructor of Shared DMA participants void unregister(void); // return true if this DMA channel is being actively contended for // by multiple drivers bool has_contention(void) const { return contention; } - + // lock all shared DMA channels. Used on reboot static void lock_all(void); - + private: dma_allocate_fn_t allocate; dma_allocate_fn_t deallocate; @@ -90,7 +90,7 @@ private: // lock one stream, non-blocking bool lock_stream_nonblocking(uint8_t stream_id); - + static struct dma_lock { // semaphore to ensure only one peripheral uses a DMA channel at a time #if CH_CFG_USE_SEMAPHORES == TRUE @@ -104,6 +104,3 @@ private: Shared_DMA *obj; } locks[SHARED_DMA_MAX_STREAM_ID+1]; }; - - - diff --git a/libraries/AP_HAL_ChibiOS/stdio.cpp b/libraries/AP_HAL_ChibiOS/stdio.cpp index 7e5ac8e383..991cd3f126 100644 --- a/libraries/AP_HAL_ChibiOS/stdio.cpp +++ b/libraries/AP_HAL_ChibiOS/stdio.cpp @@ -36,11 +36,11 @@ int __wrap_snprintf(char *str, size_t size, const char *fmt, ...) { va_list arg; int done; - + va_start (arg, fmt); done = hal.util->vsnprintf(str, size, fmt, arg); va_end (arg); - + return done; } @@ -91,11 +91,11 @@ int __wrap_printf(const char *fmt, ...) #ifndef HAL_NO_PRINTF va_list arg; int done; - + va_start (arg, fmt); done = vprintf_console_hook(fmt, arg); va_end (arg); - + return done; #else (void)fmt; @@ -112,11 +112,11 @@ int __wrap_fprintf(void *f, const char *fmt, ...) #ifndef HAL_NO_PRINTF va_list arg; int done; - + va_start (arg, fmt); done = vprintf_console_hook(fmt, arg); va_end (arg); - + return done; #else (void)fmt; @@ -135,4 +135,3 @@ extern "C" { // alias fiprintf() to fprintf(). This saves flash space int __wrap_fiprintf(const char *fmt, ...) __attribute__((alias("__wrap_fprintf"))); } - diff --git a/libraries/AP_HAL_ChibiOS/system.cpp b/libraries/AP_HAL_ChibiOS/system.cpp index 63089d3f6e..9c5a004082 100644 --- a/libraries/AP_HAL_ChibiOS/system.cpp +++ b/libraries/AP_HAL_ChibiOS/system.cpp @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #include @@ -40,7 +40,7 @@ typedef enum { void *__dso_handle; -void __cxa_pure_virtual(void); +void __cxa_pure_virtual(void); void __cxa_pure_virtual() { while (1); } //TODO: Handle properly, maybe generate a traceback void NMI_Handler(void);