From 7675913d5ba796f439a839e0856e343981d596b2 Mon Sep 17 00:00:00 2001 From: Caio Marcelo de Oliveira Filho Date: Fri, 20 Nov 2015 12:10:58 +0900 Subject: [PATCH] AP_HAL_Linux: use millis/micros/panic functions --- libraries/AP_HAL_Linux/AnalogIn_ADS1115.cpp | 4 ++-- libraries/AP_HAL_Linux/GPIO_BBB.cpp | 4 ++-- libraries/AP_HAL_Linux/GPIO_RPI.cpp | 4 ++-- libraries/AP_HAL_Linux/Heat_Pwm.cpp | 16 ++++++++-------- libraries/AP_HAL_Linux/I2CDriver.cpp | 6 +++--- libraries/AP_HAL_Linux/PWM_Sysfs.cpp | 6 +++--- libraries/AP_HAL_Linux/RCInput.cpp | 2 +- libraries/AP_HAL_Linux/RCInput_AioPRU.cpp | 2 +- libraries/AP_HAL_Linux/RCInput_PRU.cpp | 2 +- libraries/AP_HAL_Linux/RCInput_RPI.cpp | 2 +- libraries/AP_HAL_Linux/RCInput_Raspilot.cpp | 6 +++--- libraries/AP_HAL_Linux/RCInput_UART.cpp | 6 ++---- libraries/AP_HAL_Linux/RCInput_ZYNQ.cpp | 2 +- libraries/AP_HAL_Linux/RCOutput_AioPRU.cpp | 4 +--- libraries/AP_HAL_Linux/RCOutput_Bebop.cpp | 2 +- libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp | 2 +- libraries/AP_HAL_Linux/RCOutput_PRU.cpp | 3 +-- libraries/AP_HAL_Linux/RCOutput_Raspilot.cpp | 6 +++--- libraries/AP_HAL_Linux/RCOutput_Sysfs.cpp | 4 +--- libraries/AP_HAL_Linux/RCOutput_ZYNQ.cpp | 3 +-- libraries/AP_HAL_Linux/RPIOUARTDriver.cpp | 8 ++++---- libraries/AP_HAL_Linux/RaspilotAnalogIn.cpp | 6 +++--- libraries/AP_HAL_Linux/SPIDriver.cpp | 10 +++++----- libraries/AP_HAL_Linux/SPIUARTDriver.cpp | 8 ++++---- libraries/AP_HAL_Linux/Semaphores.cpp | 4 ++-- libraries/AP_HAL_Linux/Storage.cpp | 12 ++++++------ libraries/AP_HAL_Linux/Storage_FRAM.cpp | 12 ++++++------ libraries/AP_HAL_Linux/TCPServerDevice.cpp | 8 ++++---- libraries/AP_HAL_Linux/ToneAlarmDriver.cpp | 2 +- 29 files changed, 74 insertions(+), 82 deletions(-) diff --git a/libraries/AP_HAL_Linux/AnalogIn_ADS1115.cpp b/libraries/AP_HAL_Linux/AnalogIn_ADS1115.cpp index 1d7e994d54..6bf2895fca 100644 --- a/libraries/AP_HAL_Linux/AnalogIn_ADS1115.cpp +++ b/libraries/AP_HAL_Linux/AnalogIn_ADS1115.cpp @@ -84,7 +84,7 @@ void ADS1115AnalogIn::init(void* implspecific) void ADS1115AnalogIn::_update() { - if (hal.scheduler->micros() - _last_update_timestamp < 100000) { + if (AP_HAL::micros() - _last_update_timestamp < 100000) { return; } @@ -108,7 +108,7 @@ void ADS1115AnalogIn::_update() } } - _last_update_timestamp = hal.scheduler->micros(); + _last_update_timestamp = AP_HAL::micros(); } #endif diff --git a/libraries/AP_HAL_Linux/GPIO_BBB.cpp b/libraries/AP_HAL_Linux/GPIO_BBB.cpp index 3aee3af4e9..344200e2b8 100644 --- a/libraries/AP_HAL_Linux/GPIO_BBB.cpp +++ b/libraries/AP_HAL_Linux/GPIO_BBB.cpp @@ -32,7 +32,7 @@ void GPIO_BBB::init() uint8_t bank_enable[3] = { 5, 65, 105 }; int export_fd = open("/sys/class/gpio/export", O_WRONLY); if (export_fd == -1) { - hal.scheduler->panic("unable to open /sys/class/gpio/export"); + AP_HAL::panic("unable to open /sys/class/gpio/export"); } for (uint8_t i=0; i<3; i++) { dprintf(export_fd, "%u\n", (unsigned)bank_enable[i]); @@ -51,7 +51,7 @@ void GPIO_BBB::init() for (uint8_t i=0; ipanic("unable to map GPIO bank"); + AP_HAL::panic("unable to map GPIO bank"); } gpio_bank[i].oe = gpio_bank[i].base + GPIO_OE; gpio_bank[i].in = gpio_bank[i].base + GPIO_IN; diff --git a/libraries/AP_HAL_Linux/GPIO_RPI.cpp b/libraries/AP_HAL_Linux/GPIO_RPI.cpp index 394492d851..8aecbc6c94 100644 --- a/libraries/AP_HAL_Linux/GPIO_RPI.cpp +++ b/libraries/AP_HAL_Linux/GPIO_RPI.cpp @@ -30,7 +30,7 @@ void GPIO_RPI::init() uint32_t clk_address = rpi_version == 1 ? CLOCK_BASE(BCM2708_PERI_BASE) : CLOCK_BASE(BCM2709_PERI_BASE); // open /dev/mem if ((mem_fd = open("/dev/mem", O_RDWR|O_SYNC) ) < 0) { - hal.scheduler->panic("Can't open /dev/mem"); + AP_HAL::panic("Can't open /dev/mem"); } // mmap GPIO @@ -64,7 +64,7 @@ void GPIO_RPI::init() close(mem_fd); // No need to keep mem_fd open after mmap if (gpio_map == MAP_FAILED || pwm_map == MAP_FAILED || clk_map == MAP_FAILED) { - hal.scheduler->panic("Can't open /dev/mem"); + AP_HAL::panic("Can't open /dev/mem"); } gpio = (volatile uint32_t *)gpio_map; // Always use volatile pointer! diff --git a/libraries/AP_HAL_Linux/Heat_Pwm.cpp b/libraries/AP_HAL_Linux/Heat_Pwm.cpp index 0dff2e7d61..fa707dda76 100644 --- a/libraries/AP_HAL_Linux/Heat_Pwm.cpp +++ b/libraries/AP_HAL_Linux/Heat_Pwm.cpp @@ -52,32 +52,32 @@ HeatPwm::HeatPwm(const char* pwm_sysfs_path, float Kp, float Ki, uint32_t period char *run_path; if (asprintf(&duty_path, "%s/%s", pwm_sysfs_path, HEAT_PWM_DUTY) == -1) { - hal.scheduler->panic("HeatPwm not enough memory\n"); + AP_HAL::panic("HeatPwm not enough memory\n"); } _duty_fd = open(duty_path, O_RDWR); if (_duty_fd == -1) { perror("opening duty"); - hal.scheduler->panic("Error Initializing Pwm heat\n"); + AP_HAL::panic("Error Initializing Pwm heat\n"); } free(duty_path); if (asprintf(&period_path, "%s/%s", pwm_sysfs_path, HEAT_PWM_PERIOD) == -1) { - hal.scheduler->panic("HeatPwm not enough memory\n"); + AP_HAL::panic("HeatPwm not enough memory\n"); } _period_fd = open(period_path, O_RDWR); if (_period_fd == -1) { perror("opening period"); - hal.scheduler->panic("Error Initializing Pwm heat\n"); + AP_HAL::panic("Error Initializing Pwm heat\n"); } free(period_path); if (asprintf(&run_path, "%s/%s", pwm_sysfs_path, HEAT_PWM_RUN) == -1) { - hal.scheduler->panic("HeatPwm not enough memory\n"); + AP_HAL::panic("HeatPwm not enough memory\n"); } _run_fd = open(run_path, O_RDWR); if (_run_fd == -1) { perror("opening run"); - hal.scheduler->panic("Error Initializing Pwm heat\n"); + AP_HAL::panic("Error Initializing Pwm heat\n"); } free(run_path); @@ -90,7 +90,7 @@ void HeatPwm::set_imu_temp(float current) { float error, output; - if (hal.scheduler->millis() - _last_temp_update < 5) { + if (AP_HAL::millis() - _last_temp_update < 5) { return; } @@ -112,7 +112,7 @@ void HeatPwm::set_imu_temp(float current) } _set_duty(output); - _last_temp_update = hal.scheduler->millis(); + _last_temp_update = AP_HAL::millis(); } void HeatPwm::_set_duty(uint32_t duty) diff --git a/libraries/AP_HAL_Linux/I2CDriver.cpp b/libraries/AP_HAL_Linux/I2CDriver.cpp index c38bbd5c1a..ae1881611e 100644 --- a/libraries/AP_HAL_Linux/I2CDriver.cpp +++ b/libraries/AP_HAL_Linux/I2CDriver.cpp @@ -33,7 +33,7 @@ I2CDriver::I2CDriver(AP_HAL::Semaphore* semaphore, const char *device) : #if CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_NONE if (!((Util*)hal.util)->is_chardev_node(_device)) - hal.scheduler->panic("I2C device is not a chardev node"); + AP_HAL::panic("I2C device is not a chardev node"); #endif } @@ -51,7 +51,7 @@ I2CDriver::I2CDriver(AP_HAL::Semaphore* semaphore, d = opendir(dirname); if (!d) - hal.scheduler->panic("Could not get list of I2C buses"); + AP_HAL::panic("Could not get list of I2C buses"); for (de = readdir(d); de; de = readdir(d)) { const char *p, * const *t; @@ -85,7 +85,7 @@ I2CDriver::I2CDriver(AP_HAL::Semaphore* semaphore, closedir(d); if (!((Util*)hal.util)->is_chardev_node(_device)) - hal.scheduler->panic("I2C device is not a chardev node"); + AP_HAL::panic("I2C device is not a chardev node"); } I2CDriver::~I2CDriver() diff --git a/libraries/AP_HAL_Linux/PWM_Sysfs.cpp b/libraries/AP_HAL_Linux/PWM_Sysfs.cpp index 898a7cb512..8420573de1 100644 --- a/libraries/AP_HAL_Linux/PWM_Sysfs.cpp +++ b/libraries/AP_HAL_Linux/PWM_Sysfs.cpp @@ -57,7 +57,7 @@ PWM_Sysfs::PWM_Sysfs(uint8_t chip, uint8_t channel) r = snprintf(path, sizeof(path), PWM_BASE_PATH "%u/export", _chip); if (r < 0 || r >= (int)sizeof(path)) { - hal.scheduler->panic("LinuxPWM_Sysfs: chip=%u channel=%u " + AP_HAL::panic("LinuxPWM_Sysfs: chip=%u channel=%u " "Error formatting pwm export: %s", _chip, _channel, strerror(errno)); } @@ -72,14 +72,14 @@ PWM_Sysfs::PWM_Sysfs(uint8_t chip, uint8_t channel) r = snprintf(path, sizeof(path), PWM_BASE_PATH "%u/pwm%u/duty_cycle", _chip, _channel); if (r < 0 || r >= (int)sizeof(path)) { - hal.scheduler->panic("LinuxPWM_Sysfs: chip=%u channel=%u " + AP_HAL::panic("LinuxPWM_Sysfs: chip=%u channel=%u " "Error formatting channel duty cycle: %s", _chip, _channel, strerror(errno)); } _duty_cycle_fd = ::open(path, O_RDWR | O_CLOEXEC); if (_duty_cycle_fd < 0) { - hal.scheduler->panic("LinuxPWM_Sysfs: chip=%u channel=%u " + AP_HAL::panic("LinuxPWM_Sysfs: chip=%u channel=%u " "Unable to open file %s: %s", _chip, _channel, path, strerror(errno)); } diff --git a/libraries/AP_HAL_Linux/RCInput.cpp b/libraries/AP_HAL_Linux/RCInput.cpp index 8168329857..ab1a6cbc65 100644 --- a/libraries/AP_HAL_Linux/RCInput.cpp +++ b/libraries/AP_HAL_Linux/RCInput.cpp @@ -285,7 +285,7 @@ void RCInput::_process_dsm_pulse(uint16_t width_s0, uint16_t width_s1) } uint16_t values[8]; uint16_t num_values=0; - if (dsm_decode(hal.scheduler->micros64(), bytes, values, &num_values, 8) && + if (dsm_decode(AP_HAL::micros64(), bytes, values, &num_values, 8) && num_values >= 5) { for (i=0; ipanic("Unable to open /dev/mem"); + AP_HAL::panic("Unable to open /dev/mem"); } ring_buffer = (volatile struct ring_buffer*) mmap(0, 0x1000, PROT_READ|PROT_WRITE, MAP_SHARED, mem_fd, RCIN_PRUSS_RAM_BASE); close(mem_fd); diff --git a/libraries/AP_HAL_Linux/RCInput_PRU.cpp b/libraries/AP_HAL_Linux/RCInput_PRU.cpp index 7bfed62c8d..dbc48087e0 100644 --- a/libraries/AP_HAL_Linux/RCInput_PRU.cpp +++ b/libraries/AP_HAL_Linux/RCInput_PRU.cpp @@ -28,7 +28,7 @@ void RCInput_PRU::init(void*) { int mem_fd = open("/dev/mem", O_RDWR|O_SYNC); if (mem_fd == -1) { - hal.scheduler->panic("Unable to open /dev/mem"); + AP_HAL::panic("Unable to open /dev/mem"); } ring_buffer = (volatile struct ring_buffer*) mmap(0, 0x1000, PROT_READ|PROT_WRITE, MAP_SHARED, mem_fd, RCIN_PRUSS_SHAREDRAM_BASE); diff --git a/libraries/AP_HAL_Linux/RCInput_RPI.cpp b/libraries/AP_HAL_Linux/RCInput_RPI.cpp index 7139af956a..27f1d9e907 100644 --- a/libraries/AP_HAL_Linux/RCInput_RPI.cpp +++ b/libraries/AP_HAL_Linux/RCInput_RPI.cpp @@ -245,7 +245,7 @@ void RCInput_RPI::stop_dma() void RCInput_RPI::termination_handler(int signum) { stop_dma(); - hal.scheduler->panic("Interrupted"); + AP_HAL::panic("Interrupted"); } diff --git a/libraries/AP_HAL_Linux/RCInput_Raspilot.cpp b/libraries/AP_HAL_Linux/RCInput_Raspilot.cpp index fc81a34eef..e5a1d00868 100644 --- a/libraries/AP_HAL_Linux/RCInput_Raspilot.cpp +++ b/libraries/AP_HAL_Linux/RCInput_Raspilot.cpp @@ -25,7 +25,7 @@ void RCInput_Raspilot::init(void*) _spi_sem = _spi->get_semaphore(); if (_spi_sem == NULL) { - hal.scheduler->panic("PANIC: RCIutput_Raspilot did not get " + AP_HAL::panic("PANIC: RCIutput_Raspilot did not get " "valid SPI semaphore!"); return; // never reached } @@ -37,11 +37,11 @@ void RCInput_Raspilot::init(void*) void RCInput_Raspilot::_poll_data(void) { // Throttle read rate to 100hz maximum. - if (hal.scheduler->micros() - _last_timer < 10000) { + if (AP_HAL::micros() - _last_timer < 10000) { return; } - _last_timer = hal.scheduler->micros(); + _last_timer = AP_HAL::micros(); if (!_spi_sem->take_nonblocking()) { return; diff --git a/libraries/AP_HAL_Linux/RCInput_UART.cpp b/libraries/AP_HAL_Linux/RCInput_UART.cpp index aa7cb2b021..992a3d2ea0 100644 --- a/libraries/AP_HAL_Linux/RCInput_UART.cpp +++ b/libraries/AP_HAL_Linux/RCInput_UART.cpp @@ -15,15 +15,13 @@ #define MAGIC 0x55AA -static const AP_HAL::HAL& hal = AP_HAL::get_HAL(); - using namespace Linux; RCInput_UART::RCInput_UART(const char *path) { _fd = open(path, O_RDONLY|O_NOCTTY|O_NONBLOCK|O_NDELAY); if (_fd < 0) { - hal.scheduler->panic("RCInput_UART: Error opening '%s': %s", + AP_HAL::panic("RCInput_UART: Error opening '%s': %s", path, strerror(errno)); } } @@ -50,7 +48,7 @@ void RCInput_UART::init(void*) options.c_oflag &= ~OPOST; if (tcsetattr(_fd, TCSANOW, &options) != 0) { - hal.scheduler->panic("RCInput_UART: error configuring device: %s", + AP_HAL::panic("RCInput_UART: error configuring device: %s", strerror(errno)); } diff --git a/libraries/AP_HAL_Linux/RCInput_ZYNQ.cpp b/libraries/AP_HAL_Linux/RCInput_ZYNQ.cpp index 2cb6d25747..86ed37a5dd 100644 --- a/libraries/AP_HAL_Linux/RCInput_ZYNQ.cpp +++ b/libraries/AP_HAL_Linux/RCInput_ZYNQ.cpp @@ -25,7 +25,7 @@ void RCInput_ZYNQ::init(void*) { int mem_fd = open("/dev/mem", O_RDWR|O_SYNC); if (mem_fd == -1) { - hal.scheduler->panic("Unable to open /dev/mem"); + AP_HAL::panic("Unable to open /dev/mem"); } pulse_input = (volatile uint32_t*) mmap(0, 0x1000, PROT_READ|PROT_WRITE, MAP_SHARED, mem_fd, RCIN_ZYNQ_PULSE_INPUT_BASE); diff --git a/libraries/AP_HAL_Linux/RCOutput_AioPRU.cpp b/libraries/AP_HAL_Linux/RCOutput_AioPRU.cpp index 6410174b0b..54f1f2fb20 100644 --- a/libraries/AP_HAL_Linux/RCOutput_AioPRU.cpp +++ b/libraries/AP_HAL_Linux/RCOutput_AioPRU.cpp @@ -27,11 +27,9 @@ using namespace Linux; -static const AP_HAL::HAL& hal = AP_HAL::get_HAL(); - static void catch_sigbus(int sig) { - hal.scheduler->panic("RCOutputAioPRU.cpp:SIGBUS error gernerated\n"); + AP_HAL::panic("RCOutputAioPRU.cpp:SIGBUS error gernerated\n"); } void RCOutput_AioPRU::init(void* machtnicht) { diff --git a/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp b/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp index 90679f49cd..30f1a51ce2 100644 --- a/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp +++ b/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp @@ -247,7 +247,7 @@ void RCOutput_Bebop::init(void* dummy) _i2c_sem = hal.i2c1->get_semaphore(); if (_i2c_sem == NULL) { - hal.scheduler->panic("RCOutput_Bebop: can't get i2c sem"); + AP_HAL::panic("RCOutput_Bebop: can't get i2c sem"); return; /* never reached */ } diff --git a/libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp b/libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp index 6f206a3510..d9a789bb33 100644 --- a/libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp +++ b/libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp @@ -84,7 +84,7 @@ void RCOutput_PCA9685::init(void* machtnicht) { _i2c_sem = hal.i2c->get_semaphore(); if (_i2c_sem == NULL) { - hal.scheduler->panic("PANIC: RCOutput_PCA9685 did not get " + AP_HAL::panic("PANIC: RCOutput_PCA9685 did not get " "valid I2C semaphore!"); return; /* never reached */ } diff --git a/libraries/AP_HAL_Linux/RCOutput_PRU.cpp b/libraries/AP_HAL_Linux/RCOutput_PRU.cpp index b11c5eed17..9b803576c2 100644 --- a/libraries/AP_HAL_Linux/RCOutput_PRU.cpp +++ b/libraries/AP_HAL_Linux/RCOutput_PRU.cpp @@ -24,10 +24,9 @@ using namespace Linux; static const uint8_t chan_pru_map[]= {10,8,11,9,7,6,5,4,3,2,1,0}; //chan_pru_map[CHANNEL_NUM] = PRU_REG_R30/31_NUM; static const uint8_t pru_chan_map[]= {11,10,9,8,7,6,5,4,1,3,0,2}; //pru_chan_map[PRU_REG_R30/31_NUM] = CHANNEL_NUM; -static const AP_HAL::HAL& hal = AP_HAL::get_HAL(); static void catch_sigbus(int sig) { - hal.scheduler->panic("RCOutput.cpp:SIGBUS error gernerated\n"); + AP_HAL::panic("RCOutput.cpp:SIGBUS error gernerated\n"); } void RCOutput_PRU::init(void* machtnicht) { diff --git a/libraries/AP_HAL_Linux/RCOutput_Raspilot.cpp b/libraries/AP_HAL_Linux/RCOutput_Raspilot.cpp index 316a91825a..46ddd7c3dd 100644 --- a/libraries/AP_HAL_Linux/RCOutput_Raspilot.cpp +++ b/libraries/AP_HAL_Linux/RCOutput_Raspilot.cpp @@ -28,7 +28,7 @@ void RCOutput_Raspilot::init(void* machtnicht) _spi_sem = _spi->get_semaphore(); if (_spi_sem == NULL) { - hal.scheduler->panic("PANIC: RCOutput_Raspilot did not get " + AP_HAL::panic("PANIC: RCOutput_Raspilot did not get " "valid SPI semaphore!"); return; // never reached } @@ -100,11 +100,11 @@ void RCOutput_Raspilot::_update(void) { int i; - if (hal.scheduler->micros() - _last_update_timestamp < 10000) { + if (AP_HAL::micros() - _last_update_timestamp < 10000) { return; } - _last_update_timestamp = hal.scheduler->micros(); + _last_update_timestamp = AP_HAL::micros(); if (!_spi_sem->take_nonblocking()) { return; diff --git a/libraries/AP_HAL_Linux/RCOutput_Sysfs.cpp b/libraries/AP_HAL_Linux/RCOutput_Sysfs.cpp index d85a9a24c0..8e953f7074 100644 --- a/libraries/AP_HAL_Linux/RCOutput_Sysfs.cpp +++ b/libraries/AP_HAL_Linux/RCOutput_Sysfs.cpp @@ -27,8 +27,6 @@ namespace Linux { -static const AP_HAL::HAL &hal = AP_HAL::get_HAL(); - RCOutput_Sysfs::RCOutput_Sysfs(uint8_t chip, uint8_t channel_count) : _chip(chip) , _channel_count(channel_count) @@ -50,7 +48,7 @@ void RCOutput_Sysfs::init(void *machtnichts) for (uint8_t i = 0; i < _channel_count; i++) { _pwm_channels[i] = new PWM_Sysfs(_chip, i); if (!_pwm_channels[i]) { - hal.scheduler->panic("RCOutput_Sysfs_PWM: Unable to setup PWM pin."); + AP_HAL::panic("RCOutput_Sysfs_PWM: Unable to setup PWM pin."); } _pwm_channels[i]->enable(false); diff --git a/libraries/AP_HAL_Linux/RCOutput_ZYNQ.cpp b/libraries/AP_HAL_Linux/RCOutput_ZYNQ.cpp index 46b0541074..1e4aae64e0 100644 --- a/libraries/AP_HAL_Linux/RCOutput_ZYNQ.cpp +++ b/libraries/AP_HAL_Linux/RCOutput_ZYNQ.cpp @@ -20,10 +20,9 @@ using namespace Linux; #define PWM_CHAN_COUNT 8 // FIXME -static const AP_HAL::HAL& hal = AP_HAL::get_HAL(); static void catch_sigbus(int sig) { - hal.scheduler->panic("RCOutput.cpp:SIGBUS error gernerated\n"); + AP_HAL::panic("RCOutput.cpp:SIGBUS error gernerated\n"); } void RCOutput_ZYNQ::init(void* machtnicht) { diff --git a/libraries/AP_HAL_Linux/RPIOUARTDriver.cpp b/libraries/AP_HAL_Linux/RPIOUARTDriver.cpp index 0ad12e31da..8839a41ffc 100644 --- a/libraries/AP_HAL_Linux/RPIOUARTDriver.cpp +++ b/libraries/AP_HAL_Linux/RPIOUARTDriver.cpp @@ -108,13 +108,13 @@ void RPIOUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) _spi = hal.spi->device(AP_HAL::SPIDevice_RASPIO); if (_spi == NULL) { - hal.scheduler->panic("Cannot get SPIDevice_RASPIO"); + AP_HAL::panic("Cannot get SPIDevice_RASPIO"); } _spi_sem = _spi->get_semaphore(); if (_spi_sem == NULL) { - hal.scheduler->panic("PANIC: RASPIOUARTDriver did not get " + AP_HAL::panic("PANIC: RASPIOUARTDriver did not get " "valid SPI semaphore!"); return; // never reached } @@ -191,7 +191,7 @@ void RPIOUARTDriver::_timer_tick(void) if (!_initialised) return; /* lower the update rate */ - if (hal.scheduler->micros() - _last_update_timestamp < RPIOUART_POLL_TIME_INTERVAL) { + if (AP_HAL::micros() - _last_update_timestamp < RPIOUART_POLL_TIME_INTERVAL) { return; } @@ -287,7 +287,7 @@ void RPIOUARTDriver::_timer_tick(void) _in_timer = false; - _last_update_timestamp = hal.scheduler->micros(); + _last_update_timestamp = AP_HAL::micros(); } #endif diff --git a/libraries/AP_HAL_Linux/RaspilotAnalogIn.cpp b/libraries/AP_HAL_Linux/RaspilotAnalogIn.cpp index 147e7e08c9..5ada5ef52e 100644 --- a/libraries/AP_HAL_Linux/RaspilotAnalogIn.cpp +++ b/libraries/AP_HAL_Linux/RaspilotAnalogIn.cpp @@ -91,7 +91,7 @@ void RaspilotAnalogIn::init(void* implspecific) _spi_sem = _spi->get_semaphore(); if (_spi_sem == NULL) { - hal.scheduler->panic("PANIC: RCIutput_Raspilot did not get " + AP_HAL::panic("PANIC: RCIutput_Raspilot did not get " "valid SPI semaphore!"); return; // never reached } @@ -103,7 +103,7 @@ void RaspilotAnalogIn::init(void* implspecific) void RaspilotAnalogIn::_update() { - if (hal.scheduler->micros() - _last_update_timestamp < 100000) { + if (AP_HAL::micros() - _last_update_timestamp < 100000) { return; } @@ -141,7 +141,7 @@ void RaspilotAnalogIn::_update() } - _last_update_timestamp = hal.scheduler->micros(); + _last_update_timestamp = AP_HAL::micros(); } #endif diff --git a/libraries/AP_HAL_Linux/SPIDriver.cpp b/libraries/AP_HAL_Linux/SPIDriver.cpp index 47db5ef9bf..508ad202fb 100644 --- a/libraries/AP_HAL_Linux/SPIDriver.cpp +++ b/libraries/AP_HAL_Linux/SPIDriver.cpp @@ -96,7 +96,7 @@ void SPIDeviceDriver::init() if(_cs_pin != SPI_CS_KERNEL) { _cs = hal.gpio->channel(_cs_pin); if (_cs == NULL) { - hal.scheduler->panic("Unable to instantiate cs pin"); + AP_HAL::panic("Unable to instantiate cs pin"); } _cs->mode(HAL_GPIO_OUTPUT); _cs->write(1); // do not hold the SPI bus initially @@ -150,7 +150,7 @@ void SPIDeviceManager::init(void *) { for (uint8_t i=0; i= LINUX_SPI_MAX_BUSES) { - hal.scheduler->panic("SPIDriver: invalid bus number"); + AP_HAL::panic("SPIDriver: invalid bus number"); } char path[255]; snprintf(path, sizeof(path), "/dev/spidev%u.%u", @@ -158,7 +158,7 @@ void SPIDeviceManager::init(void *) _device[i]._fd = open(path, O_RDWR); if (_device[i]._fd == -1) { printf("Unable to open %s - %s\n", path, strerror(errno)); - hal.scheduler->panic("SPIDriver: unable to open SPI bus"); + AP_HAL::panic("SPIDriver: unable to open SPI bus"); } #if SPI_DEBUGGING printf("Opened %s\n", path); @@ -178,7 +178,7 @@ void SPIDeviceManager::cs_assert(enum AP_HAL::SPIDevice type) } } if (i == LINUX_SPI_DEVICE_NUM_DEVICES) { - hal.scheduler->panic("Bad device type"); + AP_HAL::panic("Bad device type"); } // Kernel-mode CS handling @@ -214,7 +214,7 @@ void SPIDeviceManager::cs_release(enum AP_HAL::SPIDevice type) } } if (i == LINUX_SPI_DEVICE_NUM_DEVICES) { - hal.scheduler->panic("Bad device type"); + AP_HAL::panic("Bad device type"); } // Kernel-mode CS handling diff --git a/libraries/AP_HAL_Linux/SPIUARTDriver.cpp b/libraries/AP_HAL_Linux/SPIUARTDriver.cpp index b72b5bfda3..638fa193f3 100644 --- a/libraries/AP_HAL_Linux/SPIUARTDriver.cpp +++ b/libraries/AP_HAL_Linux/SPIUARTDriver.cpp @@ -93,14 +93,14 @@ void SPIUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) _buffer = new uint8_t[rxS]; if (_buffer == NULL) { hal.console->printf("Not enough memory\n"); - hal.scheduler->panic("Not enough memory\n"); + AP_HAL::panic("Not enough memory\n"); } } _spi = hal.spi->device(AP_HAL::SPIDevice_Ublox); if (_spi == NULL) { - hal.scheduler->panic("Cannot get SPIDevice_Ublox"); + AP_HAL::panic("Cannot get SPIDevice_Ublox"); } _spi_sem = _spi->get_semaphore(); @@ -230,13 +230,13 @@ void SPIUARTDriver::_timer_tick(void) return; } /* lower the update rate */ - if (hal.scheduler->micros() - _last_update_timestamp < 10000) { + if (AP_HAL::micros() - _last_update_timestamp < 10000) { return; } UARTDriver::_timer_tick(); - _last_update_timestamp = hal.scheduler->micros(); + _last_update_timestamp = AP_HAL::micros(); } #endif diff --git a/libraries/AP_HAL_Linux/Semaphores.cpp b/libraries/AP_HAL_Linux/Semaphores.cpp index 6e0aa2a810..8dec86f9c4 100644 --- a/libraries/AP_HAL_Linux/Semaphores.cpp +++ b/libraries/AP_HAL_Linux/Semaphores.cpp @@ -21,13 +21,13 @@ bool Semaphore::take(uint32_t timeout_ms) if (take_nonblocking()) { return true; } - uint64_t start = hal.scheduler->micros64(); + uint64_t start = AP_HAL::micros64(); do { hal.scheduler->delay_microseconds(200); if (take_nonblocking()) { return true; } - } while ((hal.scheduler->micros64() - start) < timeout_ms*1000); + } while ((AP_HAL::micros64() - start) < timeout_ms*1000); return false; } diff --git a/libraries/AP_HAL_Linux/Storage.cpp b/libraries/AP_HAL_Linux/Storage.cpp index 878ede41c4..5de71951e4 100644 --- a/libraries/AP_HAL_Linux/Storage.cpp +++ b/libraries/AP_HAL_Linux/Storage.cpp @@ -35,12 +35,12 @@ void Storage::_storage_create(void) unlink(STORAGE_FILE); int fd = open(STORAGE_FILE, O_RDWR|O_CREAT, 0666); if (fd == -1) { - hal.scheduler->panic("Failed to create " STORAGE_FILE); + AP_HAL::panic("Failed to create " STORAGE_FILE); } for (uint16_t loc=0; locpanic("Error filling " STORAGE_FILE); + AP_HAL::panic("Error filling " STORAGE_FILE); } } // ensure the directory is updated with the new size @@ -60,7 +60,7 @@ void Storage::_storage_open(void) _storage_create(); fd = open(STORAGE_FILE, O_RDWR); if (fd == -1) { - hal.scheduler->panic("Failed to open " STORAGE_FILE); + AP_HAL::panic("Failed to open " STORAGE_FILE); } } memset(_buffer, 0, sizeof(_buffer)); @@ -71,7 +71,7 @@ void Storage::_storage_open(void) ssize_t ret = read(fd, _buffer, sizeof(_buffer)); if (ret == 4096 && ret != sizeof(_buffer)) { if (ftruncate(fd, sizeof(_buffer)) != 0) { - hal.scheduler->panic("Failed to expand " STORAGE_FILE); + AP_HAL::panic("Failed to expand " STORAGE_FILE); } ret = sizeof(_buffer); } @@ -80,10 +80,10 @@ void Storage::_storage_open(void) _storage_create(); fd = open(STORAGE_FILE, O_RDONLY); if (fd == -1) { - hal.scheduler->panic("Failed to open " STORAGE_FILE); + AP_HAL::panic("Failed to open " STORAGE_FILE); } if (read(fd, _buffer, sizeof(_buffer)) != sizeof(_buffer)) { - hal.scheduler->panic("Failed to read " STORAGE_FILE); + AP_HAL::panic("Failed to read " STORAGE_FILE); } } close(fd); diff --git a/libraries/AP_HAL_Linux/Storage_FRAM.cpp b/libraries/AP_HAL_Linux/Storage_FRAM.cpp index 0cadd07c47..5ee4e97b6a 100644 --- a/libraries/AP_HAL_Linux/Storage_FRAM.cpp +++ b/libraries/AP_HAL_Linux/Storage_FRAM.cpp @@ -34,11 +34,11 @@ void Storage_FRAM::_storage_create(void) hal.console->println("Storage: FRAM is getting reset to default values"); if (fd == -1) { - hal.scheduler->panic("Failed to load FRAM"); + AP_HAL::panic("Failed to load FRAM"); } for (uint16_t loc=0; locpanic("Error filling FRAM"); + AP_HAL::panic("Error filling FRAM"); } } @@ -60,7 +60,7 @@ void Storage_FRAM::_storage_open(void) _storage_create(); fd = open(); if (fd == -1) { - hal.scheduler->panic("Failed to access FRAM"); + AP_HAL::panic("Failed to access FRAM"); } } @@ -68,10 +68,10 @@ void Storage_FRAM::_storage_open(void) _storage_create(); fd = open(); if (fd == -1) { - hal.scheduler->panic("Failed to access FRAM"); + AP_HAL::panic("Failed to access FRAM"); } if (read(fd, _buffer, sizeof(_buffer)) != sizeof(_buffer)) { - hal.scheduler->panic("Failed to read FRAM"); + AP_HAL::panic("Failed to read FRAM"); } } _initialised = true; @@ -150,7 +150,7 @@ int8_t Storage_FRAM::open() hal.scheduler->delay(1000); } if(i==4){ - hal.scheduler->panic("FRAM: Failed to receive Manufacturer ID 5 times"); + AP_HAL::panic("FRAM: Failed to receive Manufacturer ID 5 times"); } } diff --git a/libraries/AP_HAL_Linux/TCPServerDevice.cpp b/libraries/AP_HAL_Linux/TCPServerDevice.cpp index c1d1c307c6..36b7b2e676 100644 --- a/libraries/AP_HAL_Linux/TCPServerDevice.cpp +++ b/libraries/AP_HAL_Linux/TCPServerDevice.cpp @@ -64,23 +64,23 @@ bool TCPServerDevice::open() listener.reuseaddress(); if (!listener.bind(_ip, _port)) { - if (hal.scheduler->millis() - _last_bind_warning > 5000) { + if (AP_HAL::millis() - _last_bind_warning > 5000) { ::printf("bind failed on %s port %u - %s\n", _ip, _port, strerror(errno)); - _last_bind_warning = hal.scheduler->millis(); + _last_bind_warning = AP_HAL::millis(); } return false; } if (!listener.listen(1)) { - if (hal.scheduler->millis() - _last_bind_warning > 5000) { + if (AP_HAL::millis() - _last_bind_warning > 5000) { ::printf("listen failed on %s port %u - %s\n", _ip, _port, strerror(errno)); - _last_bind_warning = hal.scheduler->millis(); + _last_bind_warning = AP_HAL::millis(); } return false; } diff --git a/libraries/AP_HAL_Linux/ToneAlarmDriver.cpp b/libraries/AP_HAL_Linux/ToneAlarmDriver.cpp index a8e291d173..14447d2697 100644 --- a/libraries/AP_HAL_Linux/ToneAlarmDriver.cpp +++ b/libraries/AP_HAL_Linux/ToneAlarmDriver.cpp @@ -89,7 +89,7 @@ void ToneAlarm::stop() bool ToneAlarm::play() { - uint16_t cur_time = hal.scheduler->millis(); + uint16_t cur_time = AP_HAL::millis(); if(tune_num != prev_tune_num){ tune_changed = true; return true;