From b92c2409e4a2532477c221503e596c143feb5be4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 14 May 2015 08:05:32 +1000 Subject: [PATCH] Plane: added local millis() and micros() to reduce code size a bit --- ArduPlane/ArduPlane.cpp | 6 +++--- ArduPlane/GCS_Mavlink.cpp | 22 +++++++++++----------- ArduPlane/Log.cpp | 14 +++++++------- ArduPlane/Parameters.cpp | 4 ++-- ArduPlane/Plane.h | 2 ++ ArduPlane/altitude.cpp | 6 +++--- ArduPlane/commands_logic.cpp | 12 ++++++------ ArduPlane/control_modes.cpp | 2 +- ArduPlane/events.cpp | 2 +- ArduPlane/failsafe.cpp | 2 +- ArduPlane/geofence.cpp | 2 +- ArduPlane/landing.cpp | 4 ++-- ArduPlane/navigation.cpp | 4 ++-- ArduPlane/radio.cpp | 8 ++++---- ArduPlane/system.cpp | 19 ++++++++++++++++--- ArduPlane/takeoff.cpp | 2 +- ArduPlane/test.cpp | 8 ++++---- 17 files changed, 67 insertions(+), 52 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 0485ab7a09..f1c12a5545 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/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(); } } diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index d1b39b4b51..f1002ccbbd 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/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); diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 4f83629752..212b3d8b45 100644 --- a/ArduPlane/Log.cpp +++ b/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() }; diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index f222584183..af05440961 100644 --- a/ArduPlane/Parameters.cpp +++ b/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); } } diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 0796b721e7..2f3c808a5a 100644 --- a/ArduPlane/Plane.h +++ b/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(); diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index b5d739d55e..d08ed5a5a7 100644 --- a/ArduPlane/altitude.cpp +++ b/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(); } } diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 27aaefd836..438e57700d 100644 --- a/ArduPlane/commands_logic.cpp +++ b/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; } diff --git a/ArduPlane/control_modes.cpp b/ArduPlane/control_modes.cpp index 43def168a9..e23302adb7 100644 --- a/ArduPlane/control_modes.cpp +++ b/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; } diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index 08a61472bd..ef15f1826a 100644 --- a/ArduPlane/events.cpp +++ b/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) { diff --git a/ArduPlane/failsafe.cpp b/ArduPlane/failsafe.cpp index 2cd3477bd6..764d9e5578 100644 --- a/ArduPlane/failsafe.cpp +++ b/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 diff --git a/ArduPlane/geofence.cpp b/ArduPlane/geofence.cpp index 44fe6b7ca7..87afcc144c 100644 --- a/ArduPlane/geofence.cpp +++ b/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 diff --git a/ArduPlane/landing.cpp b/ArduPlane/landing.cpp index 65ffaaba3a..aa7c4b7c48 100644 --- a/ArduPlane/landing.cpp +++ b/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")); } diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index f9a8bb4f23..58d5f85a11 100644 --- a/ArduPlane/navigation.cpp +++ b/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; diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index 10cde40d45..3ddba07a2a 100644 --- a/ArduPlane/radio.cpp +++ b/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; } diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 5d1034d0d5..39eb302101 100644 --- a/ArduPlane/system.cpp +++ b/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(); +} diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index 32ccad7fa1..c50bb01ea1 100644 --- a/ArduPlane/takeoff.cpp +++ b/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; diff --git a/ArduPlane/test.cpp b/ArduPlane/test.cpp index 3ea17c05f4..b0059a2702 100644 --- a/ArduPlane/test.cpp +++ b/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) { - fast_loopTimer_us = hal.scheduler->micros(); + if (micros() - fast_loopTimer_us > 19000UL) { + 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) { - fast_loopTimer_us = hal.scheduler->micros(); + if (micros() - fast_loopTimer_us > 19000UL) { + fast_loopTimer_us = micros(); // INS // ---