Plane: added local millis() and micros() to reduce code size a bit

ArduPlane/ArduPlane.cpp

@@ -104,7 +104,7 @@ void Plane::loop()
     // wait for an INS sample
     ins.wait_for_sample();
 
-    uint32_t timer = hal.scheduler->micros();
+    uint32_t timer = micros();
 
     delta_us_fast_loop  = timer - fast_loopTimer_us;
     G_Dt                = delta_us_fast_loop * 1.0e-6f;
@@ -128,7 +128,7 @@ void Plane::loop()
     // in multiples of the main loop tick. So if they don't run on
     // the first call to the scheduler they won't run on a later
     // call until scheduler.tick() is called again
-    uint32_t remaining = (timer + 20000) - hal.scheduler->micros();
+    uint32_t remaining = (timer + 20000) - micros();
     if (remaining > 19500) {
         remaining = 19500;
     }
@@ -869,7 +869,7 @@ void Plane::determine_is_flying(void)
       been flying for. This helps for crash detection and auto-disarm
      */
     if (is_flying()) {
-        auto_state.last_flying_ms = hal.scheduler->millis();
+        auto_state.last_flying_ms = millis();
     }
 }
 

ArduPlane/GCS_Mavlink.cpp

@@ -93,7 +93,7 @@ void Plane::send_attitude(mavlink_channel_t chan)
     Vector3f omega = ahrs.get_gyro();
     mavlink_msg_attitude_send(
         chan,
-        hal.scheduler->millis(),
+        millis(),
         ahrs.roll,
         ahrs.pitch - radians(g.pitch_trim_cd*0.01f),
         ahrs.yaw,
@@ -307,7 +307,7 @@ void Plane::send_location(mavlink_channel_t chan)
     if (gps.status() >= AP_GPS::GPS_OK_FIX_2D) {
         fix_time_ms = gps.last_fix_time_ms();
     } else {
-        fix_time_ms = hal.scheduler->millis();
+        fix_time_ms = millis();
     }
     const Vector3f &vel = gps.velocity();
     mavlink_msg_global_position_int_send(
@@ -345,7 +345,7 @@ void Plane::send_servo_out(mavlink_channel_t chan)
     // HIL maintainers
     mavlink_msg_rc_channels_scaled_send(
         chan,
-        hal.scheduler->millis(),
+        millis(),
         0, // port 0
         10000 * channel_roll->norm_output() * (channel_roll->get_reverse()?-1:1),
         10000 * channel_pitch->norm_output() * (channel_pitch->get_reverse()?-1:1),
@@ -363,7 +363,7 @@ void Plane::send_radio_out(mavlink_channel_t chan)
     if (g.hil_mode==1 && !g.hil_servos) {
         mavlink_msg_servo_output_raw_send(
             chan,
-            hal.scheduler->micros(),
+            micros(),
             0,     // port
             RC_Channel::rc_channel(0)->radio_out,
             RC_Channel::rc_channel(1)->radio_out,
@@ -377,7 +377,7 @@ void Plane::send_radio_out(mavlink_channel_t chan)
     }
     mavlink_msg_servo_output_raw_send(
         chan,
-        hal.scheduler->micros(),
+        micros(),
         0,     // port
         hal.rcout->read(0),
         hal.rcout->read(1),
@@ -483,7 +483,7 @@ void Plane::send_statustext(mavlink_channel_t chan)
 // are we still delaying telemetry to try to avoid Xbee bricking?
 bool Plane::telemetry_delayed(mavlink_channel_t chan)
 {
-    uint32_t tnow = hal.scheduler->millis() >> 10;
+    uint32_t tnow = millis() >> 10;
     if (tnow > (uint32_t)g.telem_delay) {
         return false;
     }
@@ -520,7 +520,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 = hal.scheduler->millis();
+        plane.gcs[chan-MAVLINK_COMM_0].last_heartbeat_time = plane.millis();
         plane.send_heartbeat(chan);
         return true;
 
@@ -1470,12 +1470,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 = hal.scheduler->millis();
+            plane.failsafe.last_valid_rc_ms = plane.millis();
         }
 
         // a RC override message is consiered to be a 'heartbeat' from
         // the ground station for failsafe purposes
-        plane.failsafe.last_heartbeat_ms = hal.scheduler->millis();
+        plane.failsafe.last_heartbeat_ms = plane.millis();
         break;
     }
 
@@ -1484,7 +1484,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 = hal.scheduler->millis();
+        plane.failsafe.last_heartbeat_ms = plane.millis();
         break;
     }
 
@@ -1632,7 +1632,7 @@ void Plane::mavlink_delay_cb()
 
     in_mavlink_delay = true;
 
-    uint32_t tnow = hal.scheduler->millis();
+    uint32_t tnow = millis();
     if (tnow - last_1hz > 1000) {
         last_1hz = tnow;
         gcs_send_message(MSG_HEARTBEAT);

ArduPlane/Log.cpp

@@ -201,7 +201,7 @@ void Plane::Log_Write_Performance()
 {
     struct log_Performance pkt = {
         LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG),
-        loop_time       : hal.scheduler->millis() - perf_mon_timer,
+        loop_time       : millis() - perf_mon_timer,
         main_loop_count : mainLoop_count,
         g_dt_max        : G_Dt_max,
         gyro_drift_x    : (int16_t)(ahrs.get_gyro_drift().x * 1000),
@@ -270,7 +270,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_ms         : hal.scheduler->millis(),
+        time_ms         : millis(),
         nav_roll_cd     : (int16_t)nav_roll_cd,
         roll            : (int16_t)ahrs.roll_sensor,
         nav_pitch_cd    : (int16_t)nav_pitch_cd,
@@ -306,7 +306,7 @@ void Plane::Log_Write_Nav_Tuning()
 {
     struct log_Nav_Tuning pkt = {
         LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG),
-        time_ms             : hal.scheduler->millis(),
+        time_ms             : millis(),
         yaw                 : (uint16_t)ahrs.yaw_sensor,
         wp_distance         : auto_state.wp_distance,
         target_bearing_cd   : (int16_t)nav_controller->target_bearing_cd(),
@@ -332,7 +332,7 @@ void Plane::Log_Write_Status()
 {
     struct log_Status pkt = {
         LOG_PACKET_HEADER_INIT(LOG_STATUS_MSG)
-        ,timestamp   : hal.scheduler->millis()
+        ,timestamp   : millis()
         ,is_flying   : is_flying()
         ,is_flying_probability : isFlyingProbability
         ,armed       : hal.util->get_soft_armed()
@@ -359,7 +359,7 @@ void Plane::Log_Write_Sonar()
 {
     struct log_Sonar pkt = {
         LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG),
-        timestamp   : hal.scheduler->millis(),
+        timestamp   : millis(),
         distance    : (float)rangefinder.distance_cm(),
         voltage     : rangefinder.voltage_mv()*0.001f,
         baro_alt    : barometer.get_altitude(),
@@ -393,7 +393,7 @@ void Plane::Log_Write_Optflow()
     const Vector2f &bodyRate = optflow.bodyRate();
     struct log_Optflow pkt = {
         LOG_PACKET_HEADER_INIT(LOG_OPTFLOW_MSG),
-        time_ms         : hal.scheduler->millis(),
+        time_ms         : millis(),
         surface_quality : optflow.quality(),
         flow_x           : flowRate.x,
         flow_y           : flowRate.y,
@@ -422,7 +422,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_ms                 : hal.scheduler->millis(),
+        time_ms                 : millis(),
         arm_state               : arming.is_armed(),
         arm_checks              : arming.get_enabled_checks()      
     };

ArduPlane/Parameters.cpp

@@ -1243,10 +1243,10 @@ void Plane::load_parameters(void)
         g.format_version.set_and_save(Parameters::k_format_version);
         cliSerial->println_P(PSTR("done."));
     } else {
-        uint32_t before = hal.scheduler->micros();
+        uint32_t before = micros();
         // Load all auto-loaded EEPROM variables
         AP_Param::load_all();
         AP_Param::convert_old_parameters(&conversion_table[0], sizeof(conversion_table)/sizeof(conversion_table[0]));
-        cliSerial->printf_P(PSTR("load_all took %luus\n"), hal.scheduler->micros() - before);
+        cliSerial->printf_P(PSTR("load_all took %luus\n"), micros() - before);
     }
 }

ArduPlane/Plane.h

@@ -886,6 +886,8 @@ private:
     void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode);
     void run_cli(AP_HAL::UARTDriver *port);
     void log_init();
+    uint32_t millis() const;
+    uint32_t micros() const;
 
 public:
     void mavlink_delay_cb();

ArduPlane/altitude.cpp

@@ -525,7 +525,7 @@ float Plane::lookahead_adjustment(void)
  */
 float Plane::rangefinder_correction(void)
 {
-    if (hal.scheduler->millis() - rangefinder_state.last_correction_time_ms > 5000) {
+    if (millis() - rangefinder_state.last_correction_time_ms > 5000) {
         // we haven't had any rangefinder data for 5s - don't use it
         return 0;
     }
@@ -583,11 +583,11 @@ void Plane::rangefinder_height_update(void)
 
         // remember the last correction. Use a low pass filter unless
         // the old data is more than 5 seconds old
-        if (hal.scheduler->millis() - rangefinder_state.last_correction_time_ms > 5000) {
+        if (millis() - rangefinder_state.last_correction_time_ms > 5000) {
             rangefinder_state.correction = correction;
         } else {
             rangefinder_state.correction = 0.8f*rangefinder_state.correction + 0.2f*correction;
         }
-        rangefinder_state.last_correction_time_ms = hal.scheduler->millis();    
+        rangefinder_state.last_correction_time_ms = millis();    
     }
 }

ArduPlane/commands_logic.cpp

@@ -394,10 +394,10 @@ bool Plane::verify_takeoff()
         if (auto_state.takeoff_speed_time_ms == 0 && 
             gps.status() >= AP_GPS::GPS_OK_FIX_3D && 
             gps.ground_speed() > min_gps_speed) {
-            auto_state.takeoff_speed_time_ms = hal.scheduler->millis();
+            auto_state.takeoff_speed_time_ms = millis();
         }
         if (auto_state.takeoff_speed_time_ms != 0 &&
-            hal.scheduler->millis() - auto_state.takeoff_speed_time_ms >= 2000) {
+            millis() - auto_state.takeoff_speed_time_ms >= 2000) {
             // once we reach sufficient speed for good GPS course
             // estimation we save our current GPS ground course
             // corrected for summed yaw to set the take off
@@ -509,9 +509,9 @@ bool Plane::verify_loiter_time()
     if (loiter.start_time_ms == 0) {
         if (nav_controller->reached_loiter_target()) {
             // we've reached the target, start the timer
-            loiter.start_time_ms = hal.scheduler->millis();
+            loiter.start_time_ms = millis();
         }
-    } else if ((hal.scheduler->millis() - loiter.start_time_ms) > loiter.time_max_ms) {
+    } else if ((millis() - loiter.start_time_ms) > loiter.time_max_ms) {
         gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER time complete"));
         return true;
     }
@@ -631,7 +631,7 @@ bool Plane::verify_continue_and_change_alt()
 
 void Plane::do_wait_delay(const AP_Mission::Mission_Command& cmd)
 {
-    condition_start = hal.scheduler->millis();
+    condition_start = millis();
     condition_value  = cmd.content.delay.seconds * 1000;    // convert seconds to milliseconds
 }
 
@@ -663,7 +663,7 @@ void Plane::do_within_distance(const AP_Mission::Mission_Command& cmd)
 
 bool Plane::verify_wait_delay()
 {
-    if ((unsigned)(hal.scheduler->millis() - condition_start) > (unsigned)condition_value) {
+    if ((unsigned)(millis() - condition_start) > (unsigned)condition_value) {
         condition_value         = 0;
         return true;
     }

ArduPlane/control_modes.cpp

@@ -17,7 +17,7 @@ void Plane::read_control_switch()
         return;
     }
 
-    if (hal.scheduler->millis() - failsafe.last_valid_rc_ms > 100) {
+    if (millis() - failsafe.last_valid_rc_ms > 100) {
         // only use signals that are less than 0.1s old.
         return;
     }

ArduPlane/events.cpp

@@ -6,7 +6,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype)
 {
     // This is how to handle a short loss of control signal failsafe.
     failsafe.state = fstype;
-    failsafe.ch3_timer_ms = hal.scheduler->millis();
+    failsafe.ch3_timer_ms = millis();
     gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event on, "));
     switch(control_mode)
     {

ArduPlane/failsafe.cpp

@@ -21,7 +21,7 @@ void Plane::failsafe_check(void)
     static uint16_t last_mainLoop_count;
     static uint32_t last_timestamp;
     static bool in_failsafe;
-    uint32_t tnow = hal.scheduler->micros();
+    uint32_t tnow = micros();
 
     if (mainLoop_count != last_mainLoop_count) {
         // the main loop is running, all is OK

ArduPlane/geofence.cpp

@@ -356,7 +356,7 @@ void Plane::geofence_check(bool altitude_check_only)
     // we are outside, and have not previously triggered.
     geofence_state->fence_triggered = true;
     geofence_state->breach_count++;
-    geofence_state->breach_time = hal.scheduler->millis();
+    geofence_state->breach_time = millis();
     geofence_state->breach_type = breach_type;
 
  #if FENCE_TRIGGERED_PIN > 0

ArduPlane/landing.cpp

@@ -58,7 +58,7 @@ bool Plane::verify_land()
         (fabsf(auto_state.land_sink_rate) < 0.2f && !is_flying())) {
 
         if (!auto_state.land_complete) {
-            if (!is_flying() && (hal.scheduler->millis()-auto_state.last_flying_ms) > 3000) {
+            if (!is_flying() && (millis()-auto_state.last_flying_ms) > 3000) {
                 gcs_send_text_fmt(PSTR("Flare crash detected: speed=%.1f"), (double)gps.ground_speed());
             } else {
                 gcs_send_text_fmt(PSTR("Flare %.1fm sink=%.2f speed=%.1f"), 
@@ -112,7 +112,7 @@ void Plane::disarm_if_autoland_complete()
         arming.arming_required() != AP_Arming::NO &&
         arming.is_armed()) {
         /* we have auto disarm enabled. See if enough time has passed */
-        if (hal.scheduler->millis() - auto_state.last_flying_ms >= g.land_disarm_delay*1000UL) {
+        if (millis() - auto_state.last_flying_ms >= g.land_disarm_delay*1000UL) {
             if (disarm_motors()) {
                 gcs_send_text_P(SEVERITY_LOW,PSTR("Auto-Disarmed"));
             }

ArduPlane/navigation.cpp

@@ -144,10 +144,10 @@ void Plane::update_cruise()
         gps.ground_speed() >= 3 &&
         cruise_state.lock_timer_ms == 0) {
         // user wants to lock the heading - start the timer
-        cruise_state.lock_timer_ms = hal.scheduler->millis();
+        cruise_state.lock_timer_ms = millis();
     }
     if (cruise_state.lock_timer_ms != 0 &&
-        (hal.scheduler->millis() - cruise_state.lock_timer_ms) > 500) {
+        (millis() - cruise_state.lock_timer_ms) > 500) {
         // lock the heading after 0.5 seconds of zero heading input
         // from user
         cruise_state.locked_heading = true;

ArduPlane/radio.cpp

@@ -110,7 +110,7 @@ void Plane::rudder_arm_check()
 
     // full right rudder starts arming counter
     if (channel_rudder->control_in > 4000) {
-        uint32_t now = hal.scheduler->millis();
+        uint32_t now = millis();
 
         if (rudder_arm_timer == 0 || 
             now - rudder_arm_timer < 3000) {
@@ -133,7 +133,7 @@ void Plane::read_radio()
         return;
     }
 
-    failsafe.last_valid_rc_ms = hal.scheduler->millis();
+    failsafe.last_valid_rc_ms = millis();
 
     elevon.ch1_temp = channel_roll->read();
     elevon.ch2_temp = channel_pitch->read();
@@ -190,7 +190,7 @@ void Plane::read_radio()
 
 void Plane::control_failsafe(uint16_t pwm)
 {
-    if (hal.scheduler->millis() - failsafe.last_valid_rc_ms > 1000 || rc_failsafe_active()) {
+    if (millis() - failsafe.last_valid_rc_ms > 1000 || rc_failsafe_active()) {
         // we do not have valid RC input. Set all primary channel
         // control inputs to the trim value and throttle to min
         channel_roll->radio_in     = channel_roll->radio_trim;
@@ -312,7 +312,7 @@ bool Plane::rc_failsafe_active(void)
     if (!g.throttle_fs_enabled) {
         return false;
     }
-    if (hal.scheduler->millis() - failsafe.last_valid_rc_ms > 1000) {
+    if (millis() - failsafe.last_valid_rc_ms > 1000) {
         // we haven't had a valid RC frame for 1 seconds
         return true;
     }

ArduPlane/system.cpp

@@ -289,7 +289,7 @@ void Plane::startup_ground(void)
 
     // reset last heartbeat time, so we don't trigger failsafe on slow
     // startup
-    failsafe.last_heartbeat_ms = hal.scheduler->millis();
+    failsafe.last_heartbeat_ms = millis();
 
     // we don't want writes to the serial port to cause us to pause
     // mid-flight, so set the serial ports non-blocking once we are
@@ -472,7 +472,7 @@ void Plane::exit_mode(enum FlightMode mode)
 
 void Plane::check_long_failsafe()
 {
-    uint32_t tnow = hal.scheduler->millis();
+    uint32_t tnow = millis();
     // only act on changes
     // -------------------
     if(failsafe.state != FAILSAFE_LONG && failsafe.state != FAILSAFE_GCS) {
@@ -575,7 +575,7 @@ void Plane::resetPerfData(void)
     mainLoop_count                  = 0;
     G_Dt_max                        = 0;
     G_Dt_min                        = 0;
-    perf_mon_timer                  = hal.scheduler->millis();
+    perf_mon_timer                  = millis();
 }
 
 
@@ -769,3 +769,16 @@ bool Plane::disarm_motors(void)
 
     return true;
 }
+
+/*
+  having local millis() and micros() reduces code size a bit on AVR
+ */
+uint32_t Plane::millis(void) const
+{
+    return hal.scheduler->millis();
+}
+
+uint32_t Plane::micros(void) const
+{
+    return hal.scheduler->micros();
+}

ArduPlane/takeoff.cpp

@@ -14,7 +14,7 @@
 bool Plane::auto_takeoff_check(void)
 {
     // this is a more advanced check that relies on TECS
-    uint32_t now = hal.scheduler->millis();
+    uint32_t now = millis();
     static bool launchTimerStarted;
     static uint32_t last_tkoff_arm_time;
     static uint32_t last_check_ms;

ArduPlane/test.cpp

@@ -384,8 +384,8 @@ int8_t Plane::test_ins(uint8_t argc, const Menu::arg *argv)
 
     while(1) {
         hal.scheduler->delay(20);
-        if (hal.scheduler->micros() - fast_loopTimer_us > 19000UL) {
+        if (micros() - fast_loopTimer_us > 19000UL) {
-            fast_loopTimer_us       = hal.scheduler->micros();
+            fast_loopTimer_us       = micros();
 
             // INS
             // ---
@@ -446,8 +446,8 @@ int8_t Plane::test_mag(uint8_t argc, const Menu::arg *argv)
 
     while(1) {
         hal.scheduler->delay(20);
-        if (hal.scheduler->micros() - fast_loopTimer_us > 19000UL) {
+        if (micros() - fast_loopTimer_us > 19000UL) {
-            fast_loopTimer_us       = hal.scheduler->micros();
+            fast_loopTimer_us       = micros();
 
             // INS
             // ---