From ee073787c88c9580c4b31786efa349e7c0344207 Mon Sep 17 00:00:00 2001 From: Caio Marcelo de Oliveira Filho Date: Fri, 20 Nov 2015 12:04:16 +0900 Subject: [PATCH] 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 (1b29a1af46e693f147098697eded1cf2b86b1ea4) 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 | 2 +- APMrover2/GCS_Mavlink.cpp | 6 +++--- APMrover2/Log.cpp | 16 ++++++++-------- APMrover2/Parameters.cpp | 2 +- APMrover2/Rover.h | 5 +++-- APMrover2/compat.cpp | 11 ----------- APMrover2/control_modes.cpp | 2 +- APMrover2/failsafe.cpp | 2 +- APMrover2/radio.cpp | 4 ++-- APMrover2/sensors.cpp | 8 ++++---- 10 files changed, 24 insertions(+), 34 deletions(-) diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index d500224a55..16ea7db74a 100644 --- a/APMrover2/APMrover2.cpp +++ b/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; diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index ee2d5f4c1d..6a9d990473 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/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; } diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp index 022df2fd0e..0784da4c44 100644 --- a/APMrover2/Log.cpp +++ b/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() }; diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index bc9f2bce30..b1e1d2301e 100644 --- a/APMrover2/Parameters.cpp +++ b/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() || diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index ff47f9d076..55d2211884 100644 --- a/APMrover2/Rover.h +++ b/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_ diff --git a/APMrover2/compat.cpp b/APMrover2/compat.cpp index 5146951711..b122a95f1c 100644 --- a/APMrover2/compat.cpp +++ b/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(); -} - diff --git a/APMrover2/control_modes.cpp b/APMrover2/control_modes.cpp index dc2efb8af2..8c85ccbd64 100644 --- a/APMrover2/control_modes.cpp +++ b/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; } diff --git a/APMrover2/failsafe.cpp b/APMrover2/failsafe.cpp index 84525ac072..eadf3ab150 100644 --- a/APMrover2/failsafe.cpp +++ b/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 diff --git a/APMrover2/radio.cpp b/APMrover2/radio.cpp index 955e1bef61..7e2abb975f 100644 --- a/APMrover2/radio.cpp +++ b/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); diff --git a/APMrover2/sensors.cpp b/APMrover2/sensors.cpp index 25336ca16e..0c0ef88b61 100644 --- a/APMrover2/sensors.cpp +++ b/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;