From 85d12efbef3ee8fc75912a295d6b93254c7c26a7 Mon Sep 17 00:00:00 2001 From: Caio Marcelo de Oliveira Filho Date: Fri, 20 Nov 2015 12:11:17 +0900 Subject: [PATCH] AP_HAL_SITL: use millis/micros/panic functions --- libraries/AP_HAL_SITL/SITL_State.cpp | 14 +++++++------- libraries/AP_HAL_SITL/SITL_cmdline.cpp | 2 +- libraries/AP_HAL_SITL/UARTDriver.cpp | 6 +++--- libraries/AP_HAL_SITL/sitl_barometer.cpp | 2 +- libraries/AP_HAL_SITL/sitl_compass.cpp | 2 +- libraries/AP_HAL_SITL/sitl_gps.cpp | 14 +++++++------- libraries/AP_HAL_SITL/sitl_ins.cpp | 2 +- libraries/AP_HAL_SITL/sitl_optical_flow.cpp | 2 +- 8 files changed, 22 insertions(+), 22 deletions(-) diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index bcbddb55e4..9b97cdba0e 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -177,8 +177,8 @@ void SITL_State::_fdm_input_step(void) } // simulate RC input at 50Hz - if (hal.scheduler->millis() - last_pwm_input >= 20 && _sitl->rc_fail == 0) { - last_pwm_input = hal.scheduler->millis(); + if (AP_HAL::millis() - last_pwm_input >= 20 && _sitl->rc_fail == 0) { + last_pwm_input = AP_HAL::millis(); new_rc_input = true; } @@ -214,7 +214,7 @@ void SITL_State::_fdm_input_step(void) void SITL_State::wait_clock(uint64_t wait_time_usec) { - while (hal.scheduler->micros64() < wait_time_usec) { + while (AP_HAL::micros64() < wait_time_usec) { _fdm_input_step(); } } @@ -270,10 +270,10 @@ void SITL_State::_fdm_input(void) _update_count++; count++; - if (hal.scheduler->millis() - last_report > 1000) { + if (AP_HAL::millis() - last_report > 1000) { //fprintf(stdout, "SIM %u FPS\n", count); count = 0; - last_report = hal.scheduler->millis(); + last_report = AP_HAL::millis(); } break; @@ -385,7 +385,7 @@ void SITL_State::_simulator_servos(Aircraft::sitl_input &input) } // output at chosen framerate - uint32_t now = hal.scheduler->micros(); + uint32_t now = AP_HAL::micros(); float deltat = (now - last_update_usec) * 1.0e-6f; last_update_usec = now; @@ -494,7 +494,7 @@ void SITL_State::_simulator_output(bool synthetic_clock_mode) control.turbulance = _sitl->wind_turbulance * 100; // zero the wind for the first 15s to allow pitot calibration - if (hal.scheduler->millis() < 15000) { + if (AP_HAL::millis() < 15000) { control.speed = 0; } diff --git a/libraries/AP_HAL_SITL/SITL_cmdline.cpp b/libraries/AP_HAL_SITL/SITL_cmdline.cpp index 0825ecc19c..acc72dac79 100644 --- a/libraries/AP_HAL_SITL/SITL_cmdline.cpp +++ b/libraries/AP_HAL_SITL/SITL_cmdline.cpp @@ -86,7 +86,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) float speedup = 1.0f; if (asprintf(&autotest_dir, SKETCHBOOK "/Tools/autotest") <= 0) { - hal.scheduler->panic("out of memory"); + AP_HAL::panic("out of memory"); } signal(SIGFPE, _sig_fpe); diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp index 6f2bb8ec53..39e9160487 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.cpp +++ b/libraries/AP_HAL_SITL/UARTDriver.cpp @@ -86,7 +86,7 @@ void SITLUARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace) _tcp_start_connection(port, wait); } else if (strcmp(devtype, "tcpclient") == 0) { if (args2 == NULL) { - hal.scheduler->panic("Invalid tcp client path: %s", path); + AP_HAL::panic("Invalid tcp client path: %s", path); } uint16_t port = atoi(args2); _tcp_start_client(args1, port); @@ -95,7 +95,7 @@ void SITLUARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace) ::printf("UART connection %s:%u\n", args1, baudrate); _uart_start_connection(args1, baudrate); } else { - hal.scheduler->panic("Invalid device path: %s", path); + AP_HAL::panic("Invalid device path: %s", path); } free(s); } @@ -363,7 +363,7 @@ void SITLUARTDriver::_uart_start_connection(const char *path, uint32_t baudrate) } if (_fd == -1) { - hal.scheduler->panic("Unable to open UART %s", path); + AP_HAL::panic("Unable to open UART %s", path); } // set non-blocking diff --git a/libraries/AP_HAL_SITL/sitl_barometer.cpp b/libraries/AP_HAL_SITL/sitl_barometer.cpp index da5af2b4ed..c932bb48f5 100644 --- a/libraries/AP_HAL_SITL/sitl_barometer.cpp +++ b/libraries/AP_HAL_SITL/sitl_barometer.cpp @@ -44,7 +44,7 @@ void SITL_State::_update_barometer(float altitude) } // 80Hz - uint32_t now = hal.scheduler->millis(); + uint32_t now = AP_HAL::millis(); if ((now - last_update) < 12) { return; } diff --git a/libraries/AP_HAL_SITL/sitl_compass.cpp b/libraries/AP_HAL_SITL/sitl_compass.cpp index 55e670486a..38442775c7 100644 --- a/libraries/AP_HAL_SITL/sitl_compass.cpp +++ b/libraries/AP_HAL_SITL/sitl_compass.cpp @@ -48,7 +48,7 @@ void SITL_State::_update_compass(float rollDeg, float pitchDeg, float yawDeg) Vector3f new_mag_data = _compass->getHIL(0) + noise + motor; // 100Hz - uint32_t now = hal.scheduler->millis(); + uint32_t now = AP_HAL::millis(); if ((now - last_update) < 10) { return; } diff --git a/libraries/AP_HAL_SITL/sitl_gps.cpp b/libraries/AP_HAL_SITL/sitl_gps.cpp index 3e7d91c847..9ab3209356 100644 --- a/libraries/AP_HAL_SITL/sitl_gps.cpp +++ b/libraries/AP_HAL_SITL/sitl_gps.cpp @@ -75,7 +75,7 @@ int SITL_State::gps_pipe(void) pipe(fd); gps_state.gps_fd = fd[1]; gps_state.client_fd = fd[0]; - gps_state.last_update = _scheduler->millis(); + gps_state.last_update = AP_HAL::millis(); HALSITL::SITLUARTDriver::_set_nonblocking(gps_state.gps_fd); HALSITL::SITLUARTDriver::_set_nonblocking(fd[0]); return gps_state.client_fd; @@ -93,7 +93,7 @@ int SITL_State::gps2_pipe(void) pipe(fd); gps2_state.gps_fd = fd[1]; gps2_state.client_fd = fd[0]; - gps2_state.last_update = _scheduler->millis(); + gps2_state.last_update = AP_HAL::millis(); HALSITL::SITLUARTDriver::_set_nonblocking(gps2_state.gps_fd); HALSITL::SITLUARTDriver::_set_nonblocking(fd[0]); return gps2_state.client_fd; @@ -248,7 +248,7 @@ void SITL_State::_update_gps_ubx(const struct gps_data *d) status.differential_status = 0; status.res = 0; status.time_to_first_fix = 0; - status.uptime = hal.scheduler->millis(); + status.uptime = AP_HAL::millis(); velned.time = time_week_ms; velned.ned_north = 100.0f * d->speedN; @@ -752,7 +752,7 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude, } // run at configured GPS rate (default 5Hz) - if ((hal.scheduler->millis() - gps_state.last_update) < (uint32_t)(1000/_sitl->gps_hertz)) { + if ((AP_HAL::millis() - gps_state.last_update) < (uint32_t)(1000/_sitl->gps_hertz)) { return; } @@ -764,8 +764,8 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude, read(gps2_state.gps_fd, &c, 1); } - gps_state.last_update = hal.scheduler->millis(); - gps2_state.last_update = hal.scheduler->millis(); + gps_state.last_update = AP_HAL::millis(); + gps2_state.last_update = AP_HAL::millis(); d.latitude = latitude + glitch_offsets.x; d.longitude = longitude + glitch_offsets.y; @@ -773,7 +773,7 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude, if (_sitl->gps_drift_alt > 0) { // slow altitude drift - d.altitude += _sitl->gps_drift_alt*sinf(hal.scheduler->millis()*0.001f*0.02f); + d.altitude += _sitl->gps_drift_alt*sinf(AP_HAL::millis()*0.001f*0.02f); } d.speedN = speedN; diff --git a/libraries/AP_HAL_SITL/sitl_ins.cpp b/libraries/AP_HAL_SITL/sitl_ins.cpp index 65f5fdd62a..0aa1afe962 100644 --- a/libraries/AP_HAL_SITL/sitl_ins.cpp +++ b/libraries/AP_HAL_SITL/sitl_ins.cpp @@ -43,7 +43,7 @@ uint16_t SITL_State::_airspeed_sensor(float airspeed) return 0xFFFF; } // add delay - uint32_t now = hal.scheduler->millis(); + uint32_t now = AP_HAL::millis(); uint32_t best_time_delta_wind = 200; // initialise large time representing buffer entry closest to current time - delay. uint8_t best_index_wind = 0; // initialise number representing the index of the entry in buffer closest to delay. diff --git a/libraries/AP_HAL_SITL/sitl_optical_flow.cpp b/libraries/AP_HAL_SITL/sitl_optical_flow.cpp index 77eb890876..01f305cb54 100644 --- a/libraries/AP_HAL_SITL/sitl_optical_flow.cpp +++ b/libraries/AP_HAL_SITL/sitl_optical_flow.cpp @@ -42,7 +42,7 @@ void SITL_State::_update_flow(void) } // update at the requested rate - uint32_t now = hal.scheduler->millis(); + uint32_t now = AP_HAL::millis(); if (now - last_flow_ms < 1000*(1.0f/_sitl->flow_rate)) { return; }