Rover: use millis/micros/panic functions

Instead of going through 'hal' then 'scheduler', use directly the AP_HAL
functions. Besides removing indirection that is not necessary for such
functions, this patch ends up reducing the code size in the call sites.

For example, building ArduCopter for PX4 with this change (compared to
before introduction of the functions) yields almost 3k bytes of code
size.

    # ArduCopter build before the functions (1b29a1af46)
       text	   data	    bss	    dec	    hex	filename
     895264	   2812	  62732	 960808	  ea928	/.../px4fmu-v2_APM.build/firmware.elf

    # ArduCopter build after this patch
       text	   data	    bss	    dec	    hex	filename
     892264	   2812	  62732	 957808	  e9d70	/.../px4fmu-v2_APM.build/firmware.elf

A later patch will remove the unused functions in the Schedulers.

APMrover2/APMrover2.cpp

@@ -114,7 +114,7 @@ void Rover::loop()
     // wait for an INS sample
     ins.wait_for_sample();
 
-    uint32_t timer = hal.scheduler->micros();
+    uint32_t timer = AP_HAL::micros();
 
     delta_us_fast_loop  = timer - fast_loopTimer_us;
     G_Dt                = delta_us_fast_loop * 1.0e-6f;

APMrover2/GCS_Mavlink.cpp

@@ -427,7 +427,7 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
     switch (id) {
     case MSG_HEARTBEAT:
         CHECK_PAYLOAD_SIZE(HEARTBEAT);
-        rover.gcs[chan-MAVLINK_COMM_0].last_heartbeat_time = hal.scheduler->millis();        
+        rover.gcs[chan-MAVLINK_COMM_0].last_heartbeat_time = AP_HAL::millis();        
         rover.send_heartbeat(chan);
         return true;
 
@@ -1232,7 +1232,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
         hal.rcin->set_overrides(v, 8);
 
-        rover.failsafe.rc_override_timer = hal.scheduler->millis();
+        rover.failsafe.rc_override_timer = AP_HAL::millis();
         rover.failsafe_trigger(FAILSAFE_EVENT_RC, false);
         break;
     }
@@ -1241,7 +1241,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 != rover.g.sysid_my_gcs) break;
-            rover.last_heartbeat_ms = rover.failsafe.rc_override_timer = hal.scheduler->millis();
+            rover.last_heartbeat_ms = rover.failsafe.rc_override_timer = AP_HAL::millis();
             rover.failsafe_trigger(FAILSAFE_EVENT_GCS, false);
             break;
         }

APMrover2/Log.cpp

@@ -172,7 +172,7 @@ void Rover::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,
@@ -197,7 +197,7 @@ void Rover::Log_Write_Steering()
 {
     struct log_Steering pkt = {
         LOG_PACKET_HEADER_INIT(LOG_STEERING_MSG),
-        time_us        : hal.scheduler->micros64(),
+        time_us        : AP_HAL::micros64(),
         demanded_accel : lateral_acceleration,
         achieved_accel : gps.ground_speed() * ins.get_gyro().z,
     };
@@ -216,7 +216,7 @@ bool Rover::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()
     };
@@ -239,7 +239,7 @@ void Rover::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(),
         steer_out       : (int16_t)channel_steer->servo_out,
         roll            : (int16_t)ahrs.roll_sensor,
         pitch           : (int16_t)ahrs.pitch_sensor,
@@ -264,7 +264,7 @@ void Rover::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         : wp_distance,
         target_bearing_cd   : (uint16_t)nav_controller->target_bearing_cd(),
@@ -312,11 +312,11 @@ void Rover::Log_Write_Sonar()
 {
     uint16_t turn_time = 0;
     if (!is_zero(obstacle.turn_angle)) {
-        turn_time = hal.scheduler->millis() - obstacle.detected_time_ms;
+        turn_time = AP_HAL::millis() - obstacle.detected_time_ms;
     }
     struct log_Sonar pkt = {
         LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG),
-        time_us         : hal.scheduler->micros64(),
+        time_us         : AP_HAL::micros64(),
         lateral_accel   : lateral_acceleration,
         sonar1_distance : (uint16_t)sonar.distance_cm(0),
         sonar2_distance : (uint16_t)sonar.distance_cm(1),
@@ -347,7 +347,7 @@ struct PACKED log_Arm_Disarm {
 void Rover::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()
     };

APMrover2/Parameters.cpp

@@ -563,7 +563,7 @@ void Rover::load_parameters(void)
 {
     if (!AP_Param::check_var_info()) {
         cliSerial->printf("Bad var table\n");
-        hal.scheduler->panic("Bad var table");
+        AP_HAL::panic("Bad var table");
     }
 
 	if (!g.format_version.load() ||

APMrover2/Rover.h

@@ -457,8 +457,6 @@ private:
     void update_commands(void);
     void delay(uint32_t ms);
     void mavlink_delay(uint32_t ms);
-    uint32_t millis();
-    uint32_t micros();
     void read_control_switch();
     uint8_t readSwitch(void);
     void reset_control_switch();
@@ -566,4 +564,7 @@ public:
 extern const AP_HAL::HAL& hal;
 extern Rover rover;
 
+using AP_HAL::millis;
+using AP_HAL::micros;
+
 #endif // _ROVER_H_

APMrover2/compat.cpp

@@ -9,14 +9,3 @@ void Rover::mavlink_delay(uint32_t ms)
 {
     hal.scheduler->delay(ms);
 }
-
-uint32_t Rover::millis()
-{
-    return hal.scheduler->millis();
-}
-
-uint32_t Rover::micros()
-{
-    return hal.scheduler->micros();
-}
-

APMrover2/control_modes.cpp

@@ -11,7 +11,7 @@ void Rover::read_control_switch()
 	// If we get this value we do not want to change modes.
 	if(switchPosition == 255) return;
 
-    if (hal.scheduler->millis() - failsafe.last_valid_rc_ms > 100) {
+    if (AP_HAL::millis() - failsafe.last_valid_rc_ms > 100) {
         // only use signals that are less than 0.1s old.
         return;
     }

APMrover2/failsafe.cpp

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

APMrover2/radio.cpp

@@ -125,7 +125,7 @@ void Rover::read_radio()
         return;
     }
 
-    failsafe.last_valid_rc_ms = hal.scheduler->millis();
+    failsafe.last_valid_rc_ms = AP_HAL::millis();
 
     RC_Channel::set_pwm_all();
 
@@ -190,7 +190,7 @@ void Rover::control_failsafe(uint16_t pwm)
         failsafe_trigger(FAILSAFE_EVENT_RC, (millis() - failsafe.rc_override_timer) > 1500);
 	} else if (g.fs_throttle_enabled) {
         bool failed = pwm < (uint16_t)g.fs_throttle_value;
-        if (hal.scheduler->millis() - failsafe.last_valid_rc_ms > 2000) {
+        if (AP_HAL::millis() - failsafe.last_valid_rc_ms > 2000) {
             failed = true;
         }
         failsafe_trigger(FAILSAFE_EVENT_THROTTLE, failed);

APMrover2/sensors.cpp

@@ -59,7 +59,7 @@ void Rover::read_sonars(void)
                 gcs_send_text_fmt(MAV_SEVERITY_INFO, "Sonar1 obstacle %u cm",
                                   (unsigned)obstacle.sonar1_distance_cm);
             }
-            obstacle.detected_time_ms = hal.scheduler->millis();
+            obstacle.detected_time_ms = AP_HAL::millis();
             obstacle.turn_angle = g.sonar_turn_angle;
         } else if (obstacle.sonar2_distance_cm <= (uint16_t)g.sonar_trigger_cm) {
             // we have an object on the right
@@ -70,7 +70,7 @@ void Rover::read_sonars(void)
                 gcs_send_text_fmt(MAV_SEVERITY_INFO, "Sonar2 obstacle %u cm",
                                   (unsigned)obstacle.sonar2_distance_cm);
             }
-            obstacle.detected_time_ms = hal.scheduler->millis();
+            obstacle.detected_time_ms = AP_HAL::millis();
             obstacle.turn_angle = -g.sonar_turn_angle;
         }
     } else {
@@ -86,7 +86,7 @@ void Rover::read_sonars(void)
                 gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Sonar obstacle %u cm",
                                   (unsigned)obstacle.sonar1_distance_cm);
             }
-            obstacle.detected_time_ms = hal.scheduler->millis();
+            obstacle.detected_time_ms = AP_HAL::millis();
             obstacle.turn_angle = g.sonar_turn_angle;
         }
     }
@@ -95,7 +95,7 @@ void Rover::read_sonars(void)
 
     // no object detected - reset after the turn time
     if (obstacle.detected_count >= g.sonar_debounce &&
-        hal.scheduler->millis() > obstacle.detected_time_ms + g.sonar_turn_time*1000) { 
+        AP_HAL::millis() > obstacle.detected_time_ms + g.sonar_turn_time*1000) { 
         gcs_send_text_fmt(MAV_SEVERITY_INFO, "Obstacle passed");
         obstacle.detected_count = 0;
         obstacle.turn_angle = 0;