From dd3fb0a689422adeb03ab4ef6a90bae3f76de601 Mon Sep 17 00:00:00 2001 From: Caio Marcelo de Oliveira Filho Date: Fri, 20 Nov 2015 12:04:37 +0900 Subject: [PATCH] Tracker: use millis/micros/panic functions --- AntennaTracker/GCS_Mavlink.cpp | 8 ++++---- AntennaTracker/Parameters.cpp | 4 ++-- AntennaTracker/servos.cpp | 4 ++-- AntennaTracker/system.cpp | 2 +- AntennaTracker/tracking.cpp | 10 +++++----- 5 files changed, 14 insertions(+), 14 deletions(-) diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index acda41b1a3..e213d8a80f 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -69,7 +69,7 @@ void Tracker::send_attitude(mavlink_channel_t chan) Vector3f omega = ahrs.get_gyro(); mavlink_msg_attitude_send( chan, - hal.scheduler->millis(), + AP_HAL::millis(), ahrs.roll, ahrs.pitch, ahrs.yaw, @@ -85,7 +85,7 @@ void Tracker::send_location(mavlink_channel_t chan) if (gps.status() >= AP_GPS::GPS_OK_FIX_2D) { fix_time = gps.last_fix_time_ms(); } else { - fix_time = hal.scheduler->millis(); + fix_time = AP_HAL::millis(); } const Vector3f &vel = gps.velocity(); mavlink_msg_global_position_int_send( @@ -105,7 +105,7 @@ void Tracker::send_radio_out(mavlink_channel_t chan) { mavlink_msg_servo_output_raw_send( chan, - hal.scheduler->micros(), + AP_HAL::micros(), 0, // port hal.rcout->read(0), hal.rcout->read(1), @@ -902,7 +902,7 @@ void Tracker::mavlink_delay_cb() in_mavlink_delay = true; - uint32_t tnow = hal.scheduler->millis(); + uint32_t tnow = AP_HAL::millis(); if (tnow - last_1hz > 1000) { last_1hz = tnow; gcs_send_message(MSG_HEARTBEAT); diff --git a/AntennaTracker/Parameters.cpp b/AntennaTracker/Parameters.cpp index bde15a13c1..c54b75c51d 100644 --- a/AntennaTracker/Parameters.cpp +++ b/AntennaTracker/Parameters.cpp @@ -291,9 +291,9 @@ void Tracker::load_parameters(void) g.format_version.set_and_save(Parameters::k_format_version); hal.console->println("done."); } else { - uint32_t before = hal.scheduler->micros(); + uint32_t before = AP_HAL::micros(); // Load all auto-loaded EEPROM variables AP_Param::load_all(); - hal.console->printf("load_all took %luus\n", (unsigned long)(hal.scheduler->micros() - before)); + hal.console->printf("load_all took %luus\n", (unsigned long)(AP_HAL::micros() - before)); } } diff --git a/AntennaTracker/servos.cpp b/AntennaTracker/servos.cpp index bc093c5873..1d3c6c63c2 100644 --- a/AntennaTracker/servos.cpp +++ b/AntennaTracker/servos.cpp @@ -232,11 +232,11 @@ void Tracker::update_yaw_position_servo(float yaw) if (abs(channel_yaw.servo_out) == 18000 && labs(angle_err) > margin && making_progress && - hal.scheduler->millis() - slew_start_ms > g.min_reverse_time*1000) { + AP_HAL::millis() - slew_start_ms > g.min_reverse_time*1000) { // we are at the limit of the servo and are not moving in the // right direction, so slew the other way new_slew_dir = -channel_yaw.servo_out / 18000; - slew_start_ms = hal.scheduler->millis(); + slew_start_ms = AP_HAL::millis(); } /* diff --git a/AntennaTracker/system.cpp b/AntennaTracker/system.cpp index d6fea5a8ea..7c7f6cc17a 100644 --- a/AntennaTracker/system.cpp +++ b/AntennaTracker/system.cpp @@ -181,7 +181,7 @@ void Tracker::disarm_servos() */ void Tracker::prepare_servos() { - start_time_ms = hal.scheduler->millis(); + start_time_ms = AP_HAL::millis(); channel_yaw.radio_out = channel_yaw.radio_trim; channel_pitch.radio_out = channel_pitch.radio_trim; channel_yaw.output(); diff --git a/AntennaTracker/tracking.cpp b/AntennaTracker/tracking.cpp index 882f3a4b65..b629667ff8 100644 --- a/AntennaTracker/tracking.cpp +++ b/AntennaTracker/tracking.cpp @@ -9,7 +9,7 @@ void Tracker::update_vehicle_pos_estimate() { // calculate time since last actual position update - float dt = (hal.scheduler->micros() - vehicle.last_update_us) * 1.0e-6f; + float dt = (AP_HAL::micros() - vehicle.last_update_us) * 1.0e-6f; // if less than 5 seconds since last position update estimate the position if (dt < TRACKING_TIMEOUT_SEC) { @@ -80,7 +80,7 @@ void Tracker::update_tracking(void) // do not perform any servo updates until startup delay has passed if (g.startup_delay > 0 && - hal.scheduler->millis() - start_time_ms < g.startup_delay*1000) { + AP_HAL::millis() - start_time_ms < g.startup_delay*1000) { return; } @@ -119,8 +119,8 @@ void Tracker::tracking_update_position(const mavlink_global_position_int_t &msg) vehicle.location.alt = msg.alt/10; vehicle.heading = msg.hdg * 0.01f; vehicle.ground_speed = pythagorous2(msg.vx, msg.vy) * 0.01f; - vehicle.last_update_us = hal.scheduler->micros(); - vehicle.last_update_ms = hal.scheduler->millis(); + vehicle.last_update_us = AP_HAL::micros(); + vehicle.last_update_ms = AP_HAL::millis(); } @@ -165,7 +165,7 @@ void Tracker::tracking_manual_control(const mavlink_manual_control_t &msg) */ void Tracker::update_armed_disarmed() { - if (vehicle.last_update_ms != 0 && (hal.scheduler->millis() - vehicle.last_update_ms) < TRACKING_TIMEOUT_MS) { + if (vehicle.last_update_ms != 0 && (AP_HAL::millis() - vehicle.last_update_ms) < TRACKING_TIMEOUT_MS) { AP_Notify::flags.armed = true; } else { AP_Notify::flags.armed = false;