From c8888329e10e9a0ee7b670e946d12f161faaa08b Mon Sep 17 00:00:00 2001 From: Caio Marcelo de Oliveira Filho Date: Fri, 20 Nov 2015 12:04:52 +0900 Subject: [PATCH] Plane: use millis/micros/panic functions --- ArduPlane/GCS_Mavlink.cpp | 8 ++++---- ArduPlane/Log.cpp | 16 ++++++++-------- ArduPlane/Parameters.cpp | 2 +- ArduPlane/Plane.h | 5 +++-- ArduPlane/commands_logic.cpp | 4 ++-- ArduPlane/is_flying.cpp | 4 ++-- ArduPlane/system.cpp | 10 ---------- 7 files changed, 20 insertions(+), 29 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 0f9e6c0386..44780dfdd2 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -612,7 +612,7 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id) switch (id) { case MSG_HEARTBEAT: CHECK_PAYLOAD_SIZE(HEARTBEAT); - plane.gcs[chan-MAVLINK_COMM_0].last_heartbeat_time = plane.millis(); + plane.gcs[chan-MAVLINK_COMM_0].last_heartbeat_time = AP_HAL::millis(); plane.send_heartbeat(chan); return true; @@ -1724,12 +1724,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) v[7] = packet.chan8_raw; if (hal.rcin->set_overrides(v, 8)) { - plane.failsafe.last_valid_rc_ms = plane.millis(); + plane.failsafe.last_valid_rc_ms = AP_HAL::millis(); } // a RC override message is consiered to be a 'heartbeat' from // the ground station for failsafe purposes - plane.failsafe.last_heartbeat_ms = plane.millis(); + plane.failsafe.last_heartbeat_ms = AP_HAL::millis(); break; } @@ -1738,7 +1738,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // We keep track of the last time we received a heartbeat from // our GCS for failsafe purposes if (msg->sysid != plane.g.sysid_my_gcs) break; - plane.failsafe.last_heartbeat_ms = plane.millis(); + plane.failsafe.last_heartbeat_ms = AP_HAL::millis(); break; } diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 956927df76..765ecb3ac8 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -202,7 +202,7 @@ void Plane::Log_Write_Performance() { struct log_Performance pkt = { LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG), - time_us : hal.scheduler->micros64(), + time_us : AP_HAL::micros64(), loop_time : millis() - perf_mon_timer, main_loop_count : mainLoop_count, g_dt_max : G_Dt_max, @@ -227,7 +227,7 @@ bool Plane::Log_Write_Startup(uint8_t type) { struct log_Startup pkt = { LOG_PACKET_HEADER_INIT(LOG_STARTUP_MSG), - time_us : hal.scheduler->micros64(), + time_us : AP_HAL::micros64(), startup_type : type, command_total : mission.num_commands() }; @@ -252,7 +252,7 @@ void Plane::Log_Write_Control_Tuning() Vector3f accel = ins.get_accel(); struct log_Control_Tuning pkt = { LOG_PACKET_HEADER_INIT(LOG_CTUN_MSG), - time_us : hal.scheduler->micros64(), + time_us : AP_HAL::micros64(), nav_roll_cd : (int16_t)nav_roll_cd, roll : (int16_t)ahrs.roll_sensor, nav_pitch_cd : (int16_t)nav_pitch_cd, @@ -288,7 +288,7 @@ void Plane::Log_Write_Nav_Tuning() { struct log_Nav_Tuning pkt = { LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG), - time_us : hal.scheduler->micros64(), + time_us : AP_HAL::micros64(), yaw : (uint16_t)ahrs.yaw_sensor, wp_distance : auto_state.wp_distance, target_bearing_cd : (int16_t)nav_controller->target_bearing_cd(), @@ -317,7 +317,7 @@ void Plane::Log_Write_Status() { struct log_Status pkt = { LOG_PACKET_HEADER_INIT(LOG_STATUS_MSG) - ,time_us : hal.scheduler->micros64() + ,time_us : AP_HAL::micros64() ,is_flying : is_flying() ,is_flying_probability : isFlyingProbability ,armed : hal.util->get_soft_armed() @@ -353,7 +353,7 @@ void Plane::Log_Write_Sonar() struct log_Sonar pkt = { LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG), - time_us : hal.scheduler->micros64(), + time_us : AP_HAL::micros64(), distance : distance, voltage : rangefinder.voltage_mv()*0.001f, baro_alt : barometer.get_altitude(), @@ -390,7 +390,7 @@ void Plane::Log_Write_Optflow() const Vector2f &bodyRate = optflow.bodyRate(); struct log_Optflow pkt = { LOG_PACKET_HEADER_INIT(LOG_OPTFLOW_MSG), - time_us : hal.scheduler->micros64(), + time_us : AP_HAL::micros64(), surface_quality : optflow.quality(), flow_x : flowRate.x, flow_y : flowRate.y, @@ -419,7 +419,7 @@ void Plane::Log_Write_Current() void Plane::Log_Arm_Disarm() { struct log_Arm_Disarm pkt = { LOG_PACKET_HEADER_INIT(LOG_ARM_DISARM_MSG), - time_us : hal.scheduler->micros64(), + time_us : AP_HAL::micros64(), arm_state : arming.is_armed(), arm_checks : arming.get_enabled_checks() }; diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 9487661b8b..d8b5700157 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1252,7 +1252,7 @@ void Plane::load_parameters(void) { if (!AP_Param::check_var_info()) { cliSerial->printf("Bad parameter table\n"); - hal.scheduler->panic("Bad parameter table"); + AP_HAL::panic("Bad parameter table"); } if (!g.format_version.load() || g.format_version != Parameters::k_format_version) { diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index f248cc13d1..22a9e04a44 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -969,8 +969,6 @@ private: void run_cli(AP_HAL::UARTDriver *port); bool restart_landing_sequence(); void log_init(); - uint32_t millis() const; - uint32_t micros() const; void init_capabilities(void); void dataflash_periodic(void); uint16_t throttle_min(void) const; @@ -1017,4 +1015,7 @@ public: extern const AP_HAL::HAL& hal; extern Plane plane; +using AP_HAL::millis; +using AP_HAL::micros; + #endif // _PLANE_H_ diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index efcbf6a89e..5093ab7e7c 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -756,9 +756,9 @@ bool Plane::verify_altitude_wait(const AP_Mission::Mission_Command &cmd) if (cmd.content.altitude_wait.wiggle_time != 0) { static uint32_t last_wiggle_ms; if (auto_state.idle_wiggle_stage == 0 && - hal.scheduler->millis() - last_wiggle_ms > cmd.content.altitude_wait.wiggle_time*1000) { + AP_HAL::millis() - last_wiggle_ms > cmd.content.altitude_wait.wiggle_time*1000) { auto_state.idle_wiggle_stage = 1; - last_wiggle_ms = hal.scheduler->millis(); + last_wiggle_ms = AP_HAL::millis(); } // idle_wiggle_stage is updated in set_servos_idle() } diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index 15b507194f..7d965fc9ef 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -14,7 +14,7 @@ void Plane::update_is_flying_5Hz(void) { float aspeed; bool is_flying_bool; - uint32_t now_ms = hal.scheduler->millis(); + uint32_t now_ms = AP_HAL::millis(); uint32_t ground_speed_thresh_cm = (g.min_gndspeed_cm > 0) ? ((uint32_t)(g.min_gndspeed_cm*0.9f)) : 500; bool gps_confirmed_movement = (gps.status() >= AP_GPS::GPS_OK_FIX_3D) && @@ -146,7 +146,7 @@ void Plane::crash_detection_update(void) return; } - uint32_t now_ms = hal.scheduler->millis(); + uint32_t now_ms = AP_HAL::millis(); bool auto_launch_detected; bool crashed_near_land_waypoint = false; bool crashed = false; diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 99974d1efe..c3d64a17b5 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -775,13 +775,3 @@ bool Plane::disarm_motors(void) return true; } - -uint32_t Plane::millis(void) const -{ - return hal.scheduler->millis(); -} - -uint32_t Plane::micros(void) const -{ - return hal.scheduler->micros(); -}