Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-24 17:48:35 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_PX4: use millis/micros/panic functions

Browse Source
This commit is contained in:
Caio Marcelo de Oliveira Filho 2015-11-20 12:11:07 +09:00 committed by Randy Mackay
parent 7675913d5b
commit a76c9e0051
7 changed files with 30 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_HAL_PX4/AnalogIn.cpp
View File

@ -202,7 +202,7 @@ void PX4AnalogIn::init(void* machtnichts)
{ {
_adc_fd = open(ADC0_DEVICE_PATH, O_RDONLY | O_NONBLOCK); _adc_fd = open(ADC0_DEVICE_PATH, O_RDONLY | O_NONBLOCK);
if (_adc_fd == -1) { if (_adc_fd == -1) {
hal.scheduler->panic("Unable to open " ADC0_DEVICE_PATH); AP_HAL::panic("Unable to open " ADC0_DEVICE_PATH);
} }
_battery_handle = orb_subscribe(ORB_ID(battery_status)); _battery_handle = orb_subscribe(ORB_ID(battery_status));
_servorail_handle = orb_subscribe(ORB_ID(servorail_status)); _servorail_handle = orb_subscribe(ORB_ID(servorail_status));
@ -223,7 +223,7 @@ void PX4AnalogIn::next_stop_pin(void)
PX4::PX4AnalogSource *c = _channels[idx]; PX4::PX4AnalogSource *c = _channels[idx];
if (c && c->_stop_pin != -1) { if (c && c->_stop_pin != -1) {
// found another stop pin // found another stop pin
_stop_pin_change_time = hal.scheduler->millis(); _stop_pin_change_time = AP_HAL::millis();
_current_stop_pin_i = idx; _current_stop_pin_i = idx;
// set that pin high // set that pin high
@ -249,7 +249,7 @@ void PX4AnalogIn::next_stop_pin(void)
void PX4AnalogIn::_timer_tick(void) void PX4AnalogIn::_timer_tick(void)
{ {
// read adc at 100Hz // read adc at 100Hz
uint32_t now = hal.scheduler->micros(); uint32_t now = AP_HAL::micros();
uint32_t delta_t = now - _last_run; uint32_t delta_t = now - _last_run;
if (delta_t < 10000) { if (delta_t < 10000) {
return; return;
@ -288,7 +288,7 @@ void PX4AnalogIn::_timer_tick(void)
// the stop pin has been settling for enough time // the stop pin has been settling for enough time
if (c->_stop_pin == -1 || if (c->_stop_pin == -1 ||
(_current_stop_pin_i == j && (_current_stop_pin_i == j &&
hal.scheduler->millis() - _stop_pin_change_time > c->_settle_time_ms)) { AP_HAL::millis() - _stop_pin_change_time > c->_settle_time_ms)) {
c->_add_value(buf_adc[i].am_data, _board_voltage); c->_add_value(buf_adc[i].am_data, _board_voltage);
if (c->_stop_pin != -1 && _current_stop_pin_i == j) { if (c->_stop_pin != -1 && _current_stop_pin_i == j) {
next_stop_pin(); next_stop_pin();

6
libraries/AP_HAL_PX4/GPIO.cpp
View File

@ -34,7 +34,7 @@ void PX4GPIO::init()
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
_led_fd = open(LED0_DEVICE_PATH, O_RDWR); _led_fd = open(LED0_DEVICE_PATH, O_RDWR);
if (_led_fd == -1) { if (_led_fd == -1) {
hal.scheduler->panic("Unable to open " LED0_DEVICE_PATH); AP_HAL::panic("Unable to open " LED0_DEVICE_PATH);
} }
if (ioctl(_led_fd, LED_OFF, LED_BLUE) != 0) { if (ioctl(_led_fd, LED_OFF, LED_BLUE) != 0) {
hal.console->printf("GPIO: Unable to setup GPIO LED BLUE\n"); hal.console->printf("GPIO: Unable to setup GPIO LED BLUE\n");
@ -45,12 +45,12 @@ void PX4GPIO::init()
#endif #endif
_tone_alarm_fd = open(TONEALARM0_DEVICE_PATH, O_WRONLY); _tone_alarm_fd = open(TONEALARM0_DEVICE_PATH, O_WRONLY);
if (_tone_alarm_fd == -1) { if (_tone_alarm_fd == -1) {
hal.scheduler->panic("Unable to open " TONEALARM0_DEVICE_PATH); AP_HAL::panic("Unable to open " TONEALARM0_DEVICE_PATH);
} }
_gpio_fmu_fd = open(PX4FMU_DEVICE_PATH, 0); _gpio_fmu_fd = open(PX4FMU_DEVICE_PATH, 0);
if (_gpio_fmu_fd == -1) { if (_gpio_fmu_fd == -1) {
hal.scheduler->panic("Unable to open GPIO"); AP_HAL::panic("Unable to open GPIO");
} }
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
if (ioctl(_gpio_fmu_fd, GPIO_CLEAR, GPIO_EXT_1) != 0) { if (ioctl(_gpio_fmu_fd, GPIO_CLEAR, GPIO_EXT_1) != 0) {

2
libraries/AP_HAL_PX4/I2CDriver.cpp
View File

@ -45,7 +45,7 @@ PX4I2CDriver::PX4I2CDriver(void)
{ {
px4_i2c = new PX4_I2C(PX4_I2C_BUS_EXPANSION); px4_i2c = new PX4_I2C(PX4_I2C_BUS_EXPANSION);
if (px4_i2c == nullptr) { if (px4_i2c == nullptr) {
hal.scheduler->panic("Unable to allocate PX4 I2C driver"); AP_HAL::panic("Unable to allocate PX4 I2C driver");
} }
} }

2
libraries/AP_HAL_PX4/RCInput.cpp
View File

@ -17,7 +17,7 @@ void PX4RCInput::init(void* unused)
_perf_rcin = perf_alloc(PC_ELAPSED, "APM_rcin"); _perf_rcin = perf_alloc(PC_ELAPSED, "APM_rcin");
_rc_sub = orb_subscribe(ORB_ID(input_rc)); _rc_sub = orb_subscribe(ORB_ID(input_rc));
if (_rc_sub == -1) { if (_rc_sub == -1) {
hal.scheduler->panic("Unable to subscribe to input_rc"); AP_HAL::panic("Unable to subscribe to input_rc");
} }
clear_overrides(); clear_overrides();
pthread_mutex_init(&rcin_mutex, NULL); pthread_mutex_init(&rcin_mutex, NULL);

4
libraries/AP_HAL_PX4/RCOutput.cpp
View File

@ -23,7 +23,7 @@ void PX4RCOutput::init(void* unused)
_perf_rcout = perf_alloc(PC_ELAPSED, "APM_rcout"); _perf_rcout = perf_alloc(PC_ELAPSED, "APM_rcout");
_pwm_fd = open(PWM_OUTPUT0_DEVICE_PATH, O_RDWR); _pwm_fd = open(PWM_OUTPUT0_DEVICE_PATH, O_RDWR);
if (_pwm_fd == -1) { if (_pwm_fd == -1) {
hal.scheduler->panic("Unable to open " PWM_OUTPUT0_DEVICE_PATH); AP_HAL::panic("Unable to open " PWM_OUTPUT0_DEVICE_PATH);
} }
if (ioctl(_pwm_fd, PWM_SERVO_ARM, 0) != 0) { if (ioctl(_pwm_fd, PWM_SERVO_ARM, 0) != 0) {
hal.console->printf("RCOutput: Unable to setup IO arming\n"); hal.console->printf("RCOutput: Unable to setup IO arming\n");
@ -330,7 +330,7 @@ void PX4RCOutput::_publish_actuators(void)
void PX4RCOutput::_timer_tick(void) void PX4RCOutput::_timer_tick(void)
{ {
uint32_t now = hal.scheduler->micros(); uint32_t now = AP_HAL::micros();
if ((_enabled_channels & ((1U<<_servo_count)-1)) == 0) { if ((_enabled_channels & ((1U<<_servo_count)-1)) == 0) {
// no channels enabled // no channels enabled

22
libraries/AP_HAL_PX4/Storage.cpp
View File

@ -45,14 +45,14 @@ uint32_t PX4Storage::_mtd_signature(void)
{ {
int mtd_fd = open(MTD_PARAMS_FILE, O_RDONLY); int mtd_fd = open(MTD_PARAMS_FILE, O_RDONLY);
if (mtd_fd == -1) { if (mtd_fd == -1) {
hal.scheduler->panic("Failed to open " MTD_PARAMS_FILE); AP_HAL::panic("Failed to open " MTD_PARAMS_FILE);
} }
uint32_t v; uint32_t v;
if (lseek(mtd_fd, MTD_SIGNATURE_OFFSET, SEEK_SET) != MTD_SIGNATURE_OFFSET) { if (lseek(mtd_fd, MTD_SIGNATURE_OFFSET, SEEK_SET) != MTD_SIGNATURE_OFFSET) {
hal.scheduler->panic("Failed to seek in " MTD_PARAMS_FILE); AP_HAL::panic("Failed to seek in " MTD_PARAMS_FILE);
} }
if (read(mtd_fd, &v, sizeof(v)) != sizeof(v)) { if (read(mtd_fd, &v, sizeof(v)) != sizeof(v)) {
hal.scheduler->panic("Failed to read signature from " MTD_PARAMS_FILE); AP_HAL::panic("Failed to read signature from " MTD_PARAMS_FILE);
} }
close(mtd_fd); close(mtd_fd);
return v; return v;
@ -65,14 +65,14 @@ void PX4Storage::_mtd_write_signature(void)
{ {
int mtd_fd = open(MTD_PARAMS_FILE, O_WRONLY); int mtd_fd = open(MTD_PARAMS_FILE, O_WRONLY);
if (mtd_fd == -1) { if (mtd_fd == -1) {
hal.scheduler->panic("Failed to open " MTD_PARAMS_FILE); AP_HAL::panic("Failed to open " MTD_PARAMS_FILE);
} }
uint32_t v = MTD_SIGNATURE; uint32_t v = MTD_SIGNATURE;
if (lseek(mtd_fd, MTD_SIGNATURE_OFFSET, SEEK_SET) != MTD_SIGNATURE_OFFSET) { if (lseek(mtd_fd, MTD_SIGNATURE_OFFSET, SEEK_SET) != MTD_SIGNATURE_OFFSET) {
hal.scheduler->panic("Failed to seek in " MTD_PARAMS_FILE); AP_HAL::panic("Failed to seek in " MTD_PARAMS_FILE);
} }
if (write(mtd_fd, &v, sizeof(v)) != sizeof(v)) { if (write(mtd_fd, &v, sizeof(v)) != sizeof(v)) {
hal.scheduler->panic("Failed to write signature in " MTD_PARAMS_FILE); AP_HAL::panic("Failed to write signature in " MTD_PARAMS_FILE);
} }
close(mtd_fd); close(mtd_fd);
} }
@ -92,7 +92,7 @@ void PX4Storage::_upgrade_to_mtd(void)
int mtd_fd = open(MTD_PARAMS_FILE, O_WRONLY); int mtd_fd = open(MTD_PARAMS_FILE, O_WRONLY);
if (mtd_fd == -1) { if (mtd_fd == -1) {
hal.scheduler->panic("Unable to open MTD for upgrade"); AP_HAL::panic("Unable to open MTD for upgrade");
} }
if (::read(old_fd, _buffer, sizeof(_buffer)) != sizeof(_buffer)) { if (::read(old_fd, _buffer, sizeof(_buffer)) != sizeof(_buffer)) {
@ -105,7 +105,7 @@ void PX4Storage::_upgrade_to_mtd(void)
ssize_t ret; ssize_t ret;
if ((ret=::write(mtd_fd, _buffer, sizeof(_buffer))) != sizeof(_buffer)) { if ((ret=::write(mtd_fd, _buffer, sizeof(_buffer))) != sizeof(_buffer)) {
::printf("mtd write of %u bytes returned %d errno=%d\n", sizeof(_buffer), ret, errno); ::printf("mtd write of %u bytes returned %d errno=%d\n", sizeof(_buffer), ret, errno);
hal.scheduler->panic("Unable to write MTD for upgrade"); AP_HAL::panic("Unable to write MTD for upgrade");
} }
close(mtd_fd); close(mtd_fd);
#if STORAGE_RENAME_OLD_FILE #if STORAGE_RENAME_OLD_FILE
@ -126,7 +126,7 @@ void PX4Storage::_storage_open(void)
// PX4 should always have /fs/mtd_params // PX4 should always have /fs/mtd_params
if (!_have_mtd) { if (!_have_mtd) {
hal.scheduler->panic("Failed to find " MTD_PARAMS_FILE); AP_HAL::panic("Failed to find " MTD_PARAMS_FILE);
} }
/* /*
@ -152,7 +152,7 @@ void PX4Storage::_storage_open(void)
_dirty_mask = 0; _dirty_mask = 0;
int fd = open(MTD_PARAMS_FILE, O_RDONLY); int fd = open(MTD_PARAMS_FILE, O_RDONLY);
if (fd == -1) { if (fd == -1) {
hal.scheduler->panic("Failed to open " MTD_PARAMS_FILE); AP_HAL::panic("Failed to open " MTD_PARAMS_FILE);
} }
const uint16_t chunk_size = 128; const uint16_t chunk_size = 128;
for (uint16_t ofs=0; ofs<sizeof(_buffer); ofs += chunk_size) { for (uint16_t ofs=0; ofs<sizeof(_buffer); ofs += chunk_size) {
@ -160,7 +160,7 @@ void PX4Storage::_storage_open(void)
if (ret != chunk_size) { if (ret != chunk_size) {
::printf("storage read of %u bytes at %u to %p failed - got %d errno=%d\n", ::printf("storage read of %u bytes at %u to %p failed - got %d errno=%d\n",
(unsigned)sizeof(_buffer), (unsigned)ofs, &_buffer[ofs], (int)ret, (int)errno); (unsigned)sizeof(_buffer), (unsigned)ofs, &_buffer[ofs], (int)ret, (int)errno);
hal.scheduler->panic("Failed to read " MTD_PARAMS_FILE); AP_HAL::panic("Failed to read " MTD_PARAMS_FILE);
} }
} }
close(fd); close(fd);

16
libraries/AP_HAL_PX4/UARTDriver.cpp
View File

@ -185,10 +185,10 @@ void PX4UARTDriver::try_initialise(void)
if (_initialised) { if (_initialised) {
return; return;
} }
if ((hal.scheduler->millis() - _last_initialise_attempt_ms) < 2000) { if ((AP_HAL::millis() - _last_initialise_attempt_ms) < 2000) {
return; return;
} }
_last_initialise_attempt_ms = hal.scheduler->millis(); _last_initialise_attempt_ms = AP_HAL::millis();
if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_ARMED || !hal.util->get_soft_armed()) { if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_ARMED || !hal.util->get_soft_armed()) {
begin(0); begin(0);
} }
@ -394,7 +394,7 @@ int PX4UARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
} }
} else { } else {
if (_os_start_auto_space - nwrite + 1 >= _total_written && if (_os_start_auto_space - nwrite + 1 >= _total_written &&
(hal.scheduler->micros64() - _first_write_time) > 500*1000UL) { (AP_HAL::micros64() - _first_write_time) > 500*1000UL) {
// it doesn't look like hw flow control is working // it doesn't look like hw flow control is working
::printf("disabling flow control on %s _total_written=%u\n", ::printf("disabling flow control on %s _total_written=%u\n",
_devpath, (unsigned)_total_written); _devpath, (unsigned)_total_written);
@ -412,7 +412,7 @@ int PX4UARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
if (ret > 0) { if (ret > 0) {
BUF_ADVANCEHEAD(_writebuf, ret); BUF_ADVANCEHEAD(_writebuf, ret);
_last_write_time = hal.scheduler->micros64(); _last_write_time = AP_HAL::micros64();
_total_written += ret; _total_written += ret;
if (! _first_write_time && _total_written > 5) { if (! _first_write_time && _total_written > 5) {
_first_write_time = _last_write_time; _first_write_time = _last_write_time;
@ -420,12 +420,12 @@ int PX4UARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
return ret; return ret;
} }
if (hal.scheduler->micros64() - _last_write_time > 2000 && if (AP_HAL::micros64() - _last_write_time > 2000 &&
_flow_control == FLOW_CONTROL_DISABLE) { _flow_control == FLOW_CONTROL_DISABLE) {
#if 0 #if 0
// this trick is disabled for now, as it sometimes blocks on // this trick is disabled for now, as it sometimes blocks on
// re-opening the ttyACM0 port, which would cause a crash // re-opening the ttyACM0 port, which would cause a crash
if (hal.scheduler->micros64() - _last_write_time > 2000000) { if (AP_HAL::micros64() - _last_write_time > 2000000) {
// we haven't done a successful write for 2 seconds - try // we haven't done a successful write for 2 seconds - try
// reopening the port // reopening the port
_initialised = false; _initialised = false;
@ -438,11 +438,11 @@ int PX4UARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
return n; return n;
} }
_last_write_time = hal.scheduler->micros64(); _last_write_time = AP_HAL::micros64();
_initialised = true; _initialised = true;
} }
#else #else
_last_write_time = hal.scheduler->micros64(); _last_write_time = AP_HAL::micros64();
#endif #endif
// we haven't done a successful write for 2ms, which means the // we haven't done a successful write for 2ms, which means the
// port is running at less than 500 bytes/sec. Start // port is running at less than 500 bytes/sec. Start