diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index c33c9d98d7ecb..1c0a1880592c1 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -2077,7 +2077,7 @@ bool ModeAuto::verify_loiter_time(const AP_Mission::Mission_Command& cmd) } // start our loiter timer - if ( loiter_time == 0 ) { + if ( loiter_time == 0_ms ) { loiter_time = millis(); } @@ -2152,7 +2152,7 @@ bool ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd) } // start timer if necessary - if (loiter_time == 0) { + if (loiter_time == 0_ms) { loiter_time = millis(); if (loiter_time_max > 0) { // play a tone @@ -2227,7 +2227,7 @@ bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd) bool ModeAuto::verify_nav_delay(const AP_Mission::Mission_Command& cmd) { if (millis() - nav_delay_time_start_ms > nav_delay_time_max_ms) { - nav_delay_time_max_ms = 0; + nav_delay_time_max_ms = 0_ms; return true; } return false; diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index 91814ab45f346..e0b73009f1a9e 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -318,7 +318,7 @@ void ModeFlowHold::run() get_pilot_desired_lean_angles(bf_angles.x, bf_angles.y, angle_max, attitude_control->get_althold_lean_angle_max_cd()); if (quality_filtered >= flow_min_quality && - AP_HAL::millis() - copter.arm_time_ms > 3000) { + AP_HAL::millis() - copter.arm_time_ms > 3_s) { // don't use for first 3s when we are just taking off Vector2f flow_angles; @@ -353,7 +353,7 @@ void ModeFlowHold::update_height_estimate(void) // assume on ground when disarmed, or if we have only just started spooling the motors up if (!hal.util->get_soft_armed() || copter.motors->get_desired_spool_state() != AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED || - AP_HAL::millis() - copter.arm_time_ms < 1500) { + AP_HAL::millis() - copter.arm_time_ms < 1500_ms) { height_offset = -ins_height; last_ins_height = ins_height; return; diff --git a/ArduCopter/mode_follow.cpp b/ArduCopter/mode_follow.cpp index a0649c5956c24..938cbea8329e8 100644 --- a/ArduCopter/mode_follow.cpp +++ b/ArduCopter/mode_follow.cpp @@ -152,7 +152,7 @@ void ModeFollow::run() // log output at 10hz uint32_t now = AP_HAL::millis(); bool log_request = false; - if ((now - last_log_ms >= 100) || (last_log_ms == 0)) { + if ((now - last_log_ms >= 100_ms) || (last_log_ms == 0_ms)) { log_request = true; last_log_ms = now; } diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 9c2914de8b7bf..9f2c7d159fc99 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -1034,7 +1034,7 @@ void ModeGuided::limit_init_time_and_pos() bool ModeGuided::limit_check() { // check if we have passed the timeout - if ((guided_limit.timeout_ms > 0) && (millis() - guided_limit.start_time >= guided_limit.timeout_ms)) { + if ((guided_limit.timeout_ms > 0_ms) && (millis() - guided_limit.start_time >= guided_limit.timeout_ms)) { return true; } diff --git a/ArduCopter/mode_smart_rtl.cpp b/ArduCopter/mode_smart_rtl.cpp index dacd7cc5f29ab..eed6d1f2c1843 100644 --- a/ArduCopter/mode_smart_rtl.cpp +++ b/ArduCopter/mode_smart_rtl.cpp @@ -118,7 +118,7 @@ void ModeSmartRTL::path_follow_run() } else if (path_follow_last_pop_fail_ms == 0) { // first time we've failed to pop off (ever, or after a success) path_follow_last_pop_fail_ms = AP_HAL::millis(); - } else if (AP_HAL::millis() - path_follow_last_pop_fail_ms > 10000) { + } else if (AP_HAL::millis() - path_follow_last_pop_fail_ms > 10_s) { // we failed to pop a point off for 10 seconds. This is // almost certainly a bug. INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); diff --git a/ArduCopter/mode_throw.cpp b/ArduCopter/mode_throw.cpp index c9ff702e3fd88..c97daff6a8e82 100644 --- a/ArduCopter/mode_throw.cpp +++ b/ArduCopter/mode_throw.cpp @@ -202,7 +202,7 @@ void ModeThrow::run() // log at 10hz or if stage changes uint32_t now = AP_HAL::millis(); - if ((stage != prev_stage) || (now - last_log_ms) > 100) { + if ((stage != prev_stage) || (now - last_log_ms) > 100_ms) { prev_stage = stage; last_log_ms = now; const float velocity = inertial_nav.get_velocity_neu_cms().length(); @@ -276,13 +276,13 @@ bool ModeThrow::throw_detected() bool possible_throw_detected = (free_falling || high_speed) && changing_height && no_throw_action; // Record time and vertical velocity when we detect the possible throw - if (possible_throw_detected && ((AP_HAL::millis() - free_fall_start_ms) > 500)) { + if (possible_throw_detected && ((AP_HAL::millis() - free_fall_start_ms) > 500_ms)) { free_fall_start_ms = AP_HAL::millis(); free_fall_start_velz = inertial_nav.get_velocity_z_up_cms(); } // Once a possible throw condition has been detected, we check for 2.5 m/s of downwards velocity change in less than 0.5 seconds to confirm - bool throw_condition_confirmed = ((AP_HAL::millis() - free_fall_start_ms < 500) && ((inertial_nav.get_velocity_z_up_cms() - free_fall_start_velz) < -250.0f)); + bool throw_condition_confirmed = ((AP_HAL::millis() - free_fall_start_ms < 500_ms) && ((inertial_nav.get_velocity_z_up_cms() - free_fall_start_velz) < -250.0f)); // start motors and enter the control mode if we are in continuous freefall return throw_condition_confirmed; diff --git a/ArduCopter/mode_turtle.cpp b/ArduCopter/mode_turtle.cpp index 74dc49421ecff..27dc647979eb4 100644 --- a/ArduCopter/mode_turtle.cpp +++ b/ArduCopter/mode_turtle.cpp @@ -172,7 +172,7 @@ void ModeTurtle::output_to_motors() // throttle needs to be raised if (is_zero(channel_throttle->norm_input_dz())) { const uint32_t now = AP_HAL::millis(); - if (now - last_throttle_warning_output_ms > 5000) { + if (now - last_throttle_warning_output_ms > 5_s) { gcs().send_text(MAV_SEVERITY_WARNING, "Turtle: raise throttle to arm"); last_throttle_warning_output_ms = now; } diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index 2952158ec615a..71284388cda1c 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -404,7 +404,7 @@ bool ModeZigZag::reached_destination() // wait at time which is set in zigzag_wp_delay uint32_t now = AP_HAL::millis(); - if (reach_wp_time_ms == 0) { + if (reach_wp_time_ms == 0_ms) { reach_wp_time_ms = now; } return ((now - reach_wp_time_ms) >= (uint16_t)constrain_int16(_wp_delay, 0, 127) * 1000);