diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index 93c6c410c0..5fc40db454 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -181,7 +181,7 @@ void Copter::setup() // setup initial performance counters perf_info_reset(); - fast_loopTimer = hal.scheduler->micros(); + fast_loopTimer = AP_HAL::micros(); } /* diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 132ff08be0..32eea03320 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -680,8 +680,6 @@ private: void log_picture(); uint8_t mavlink_compassmot(mavlink_channel_t chan); void delay(uint32_t ms); - uint32_t millis(); - uint32_t micros(); bool acro_init(bool ignore_checks); void acro_run(); void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out); @@ -1024,4 +1022,7 @@ public: extern const AP_HAL::HAL& hal; extern Copter copter; +using AP_HAL::millis; +using AP_HAL::micros; + #endif // _COPTER_H_ diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index ec6ea0bfc4..b4ad632167 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -531,7 +531,7 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id) switch(id) { case MSG_HEARTBEAT: CHECK_PAYLOAD_SIZE(HEARTBEAT); - copter.gcs[chan-MAVLINK_COMM_0].last_heartbeat_time = hal.scheduler->millis(); + copter.gcs[chan-MAVLINK_COMM_0].last_heartbeat_time = AP_HAL::millis(); copter.send_heartbeat(chan); break; @@ -1002,7 +1002,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 != copter.g.sysid_my_gcs) break; - copter.failsafe.last_heartbeat_ms = hal.scheduler->millis(); + copter.failsafe.last_heartbeat_ms = AP_HAL::millis(); copter.pmTest1++; break; } @@ -1124,7 +1124,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) copter.failsafe.rc_override_active = hal.rcin->set_overrides(v, 8); // a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes - copter.failsafe.last_heartbeat_ms = hal.scheduler->millis(); + copter.failsafe.last_heartbeat_ms = AP_HAL::millis(); break; } diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index 353bb7da91..f9e7976980 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -172,7 +172,7 @@ void Copter::Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_targ { struct log_AutoTune pkt = { LOG_PACKET_HEADER_INIT(LOG_AUTOTUNE_MSG), - time_us : hal.scheduler->micros64(), + time_us : AP_HAL::micros64(), axis : axis, tune_step : tune_step, meas_target : meas_target, @@ -198,7 +198,7 @@ void Copter::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds) { struct log_AutoTuneDetails pkt = { LOG_PACKET_HEADER_INIT(LOG_AUTOTUNEDETAILS_MSG), - time_us : hal.scheduler->micros64(), + time_us : AP_HAL::micros64(), angle_cd : angle_cd, rate_cds : rate_cds }; @@ -237,7 +237,7 @@ void Copter::Log_Write_Optflow() const Vector2f &bodyRate = optflow.bodyRate(); struct log_Optflow pkt = { LOG_PACKET_HEADER_INIT(LOG_OPTFLOW_MSG), - time_us : hal.scheduler->micros64(), + time_us : AP_HAL::micros64(), surface_quality : optflow.quality(), flow_x : flowRate.x, flow_y : flowRate.y, @@ -274,7 +274,7 @@ void Copter::Log_Write_Nav_Tuning() struct log_Nav_Tuning pkt = { LOG_PACKET_HEADER_INIT(LOG_NAV_TUNING_MSG), - time_us : hal.scheduler->micros64(), + time_us : AP_HAL::micros64(), desired_pos_x : pos_target.x, desired_pos_y : pos_target.y, pos_x : position.x, @@ -309,7 +309,7 @@ void Copter::Log_Write_Control_Tuning() { struct log_Control_Tuning pkt = { LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG), - time_us : hal.scheduler->micros64(), + time_us : AP_HAL::micros64(), throttle_in : channel_throttle->control_in, angle_boost : attitude_control.angle_boost(), throttle_out : motors.get_throttle(), @@ -340,7 +340,7 @@ void Copter::Log_Write_Performance() { struct log_Performance pkt = { LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG), - time_us : hal.scheduler->micros64(), + time_us : AP_HAL::micros64(), num_long_running : perf_info_get_num_long_running(), num_loops : perf_info_get_num_loops(), max_time : perf_info_get_max_time(), @@ -394,7 +394,7 @@ void Copter::Log_Write_Rate() const Vector3f &accel_target = pos_control.get_accel_target(); struct log_Rate pkt_rate = { LOG_PACKET_HEADER_INIT(LOG_RATE_MSG), - time_us : hal.scheduler->micros64(), + time_us : AP_HAL::micros64(), control_roll : (float)rate_targets.x, roll : (float)(ahrs.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100), roll_out : motors.get_roll(), @@ -426,7 +426,7 @@ void Copter::Log_Write_MotBatt() #if FRAME_CONFIG != HELI_FRAME struct log_MotBatt pkt_mot = { LOG_PACKET_HEADER_INIT(LOG_MOTBATT_MSG), - time_us : hal.scheduler->micros64(), + time_us : AP_HAL::micros64(), lift_max : (float)(motors.get_lift_max()), bat_volt : (float)(motors.get_batt_voltage_filt()), bat_res : (float)(motors.get_batt_resistance()), @@ -446,7 +446,7 @@ void Copter::Log_Write_Startup() { struct log_Startup pkt = { LOG_PACKET_HEADER_INIT(LOG_STARTUP_MSG), - time_us : hal.scheduler->micros64() + time_us : AP_HAL::micros64() }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); } @@ -463,7 +463,7 @@ void Copter::Log_Write_Event(uint8_t id) if (should_log(MASK_LOG_ANY)) { struct log_Event pkt = { LOG_PACKET_HEADER_INIT(LOG_EVENT_MSG), - time_us : hal.scheduler->micros64(), + time_us : AP_HAL::micros64(), id : id }; DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt)); @@ -484,7 +484,7 @@ void Copter::Log_Write_Data(uint8_t id, int16_t value) if (should_log(MASK_LOG_ANY)) { struct log_Data_Int16t pkt = { LOG_PACKET_HEADER_INIT(LOG_DATA_INT16_MSG), - time_us : hal.scheduler->micros64(), + time_us : AP_HAL::micros64(), id : id, data_value : value }; @@ -506,7 +506,7 @@ void Copter::Log_Write_Data(uint8_t id, uint16_t value) if (should_log(MASK_LOG_ANY)) { struct log_Data_UInt16t pkt = { LOG_PACKET_HEADER_INIT(LOG_DATA_UINT16_MSG), - time_us : hal.scheduler->micros64(), + time_us : AP_HAL::micros64(), id : id, data_value : value }; @@ -527,7 +527,7 @@ void Copter::Log_Write_Data(uint8_t id, int32_t value) if (should_log(MASK_LOG_ANY)) { struct log_Data_Int32t pkt = { LOG_PACKET_HEADER_INIT(LOG_DATA_INT32_MSG), - time_us : hal.scheduler->micros64(), + time_us : AP_HAL::micros64(), id : id, data_value : value }; @@ -548,7 +548,7 @@ void Copter::Log_Write_Data(uint8_t id, uint32_t value) if (should_log(MASK_LOG_ANY)) { struct log_Data_UInt32t pkt = { LOG_PACKET_HEADER_INIT(LOG_DATA_UINT32_MSG), - time_us : hal.scheduler->micros64(), + time_us : AP_HAL::micros64(), id : id, data_value : value }; @@ -570,7 +570,7 @@ void Copter::Log_Write_Data(uint8_t id, float value) if (should_log(MASK_LOG_ANY)) { struct log_Data_Float pkt = { LOG_PACKET_HEADER_INIT(LOG_DATA_FLOAT_MSG), - time_us : hal.scheduler->micros64(), + time_us : AP_HAL::micros64(), id : id, data_value : value }; @@ -590,7 +590,7 @@ void Copter::Log_Write_Error(uint8_t sub_system, uint8_t error_code) { struct log_Error pkt = { LOG_PACKET_HEADER_INIT(LOG_ERROR_MSG), - time_us : hal.scheduler->micros64(), + time_us : AP_HAL::micros64(), sub_system : sub_system, error_code : error_code, }; @@ -616,7 +616,7 @@ void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t { struct log_ParameterTuning pkt_tune = { LOG_PACKET_HEADER_INIT(LOG_PARAMTUNE_MSG), - time_us : hal.scheduler->micros64(), + time_us : AP_HAL::micros64(), parameter : param, tuning_value : tuning_val, control_in : control_in, @@ -671,7 +671,7 @@ void Copter::Log_Write_Heli() { struct log_Heli pkt_heli = { LOG_PACKET_HEADER_INIT(LOG_HELI_MSG), - time_us : hal.scheduler->micros64(), + time_us : AP_HAL::micros64(), desired_rotor_speed : motors.get_desired_rotor_speed(), main_rotor_speed : motors.get_main_rotor_speed(), }; @@ -706,7 +706,7 @@ void Copter::Log_Write_Precland() const Vector3f &target_pos_ofs = precland.last_target_pos_offset(); struct log_Precland pkt = { LOG_PACKET_HEADER_INIT(LOG_PRECLAND_MSG), - time_us : hal.scheduler->micros64(), + time_us : AP_HAL::micros64(), healthy : precland.healthy(), bf_angle_x : degrees(bf_angle.x), bf_angle_y : degrees(bf_angle.y), diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 06c464f5e3..486753c9aa 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1142,7 +1142,7 @@ void Copter::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"); } // disable centrifugal force correction, it will be enabled as part of the arming process diff --git a/ArduCopter/compassmot.cpp b/ArduCopter/compassmot.cpp index aa9298b489..b4544e4aba 100644 --- a/ArduCopter/compassmot.cpp +++ b/ArduCopter/compassmot.cpp @@ -221,8 +221,8 @@ uint8_t Copter::mavlink_compassmot(mavlink_channel_t chan) current_amps_max = max(current_amps_max, battery.current_amps()); } - if (hal.scheduler->millis() - last_send_time > 500) { - last_send_time = hal.scheduler->millis(); + if (AP_HAL::millis() - last_send_time > 500) { + last_send_time = AP_HAL::millis(); mavlink_msg_compassmot_status_send(chan, channel_throttle->control_in, battery.current_amps(), diff --git a/ArduCopter/compat.cpp b/ArduCopter/compat.cpp index 1ac9ae3c12..51546c4384 100644 --- a/ArduCopter/compat.cpp +++ b/ArduCopter/compat.cpp @@ -4,13 +4,3 @@ void Copter::delay(uint32_t ms) { hal.scheduler->delay(ms); } - -uint32_t Copter::millis() -{ - return hal.scheduler->millis(); -} - -uint32_t Copter::micros() -{ - return hal.scheduler->micros(); -} diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp index d3d02f16e5..86cc749860 100644 --- a/ArduCopter/control_guided.cpp +++ b/ArduCopter/control_guided.cpp @@ -513,7 +513,7 @@ void Copter::guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_m void Copter::guided_limit_init_time_and_pos() { // initialise start time - guided_limit.start_time = hal.scheduler->millis(); + guided_limit.start_time = AP_HAL::millis(); // initialise start position from current position guided_limit.start_pos = inertial_nav.get_position(); diff --git a/ArduCopter/ekf_check.cpp b/ArduCopter/ekf_check.cpp index a0df0d0ffb..e3ae039b5f 100644 --- a/ArduCopter/ekf_check.cpp +++ b/ArduCopter/ekf_check.cpp @@ -59,9 +59,9 @@ void Copter::ekf_check() // log an error in the dataflash Log_Write_Error(ERROR_SUBSYSTEM_EKFCHECK, ERROR_CODE_EKFCHECK_BAD_VARIANCE); // send message to gcs - if ((hal.scheduler->millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) { + if ((AP_HAL::millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) { gcs_send_text(MAV_SEVERITY_CRITICAL,"EKF variance"); - ekf_check_state.last_warn_time = hal.scheduler->millis(); + ekf_check_state.last_warn_time = AP_HAL::millis(); } failsafe_ekf_event(); } diff --git a/ArduCopter/failsafe.cpp b/ArduCopter/failsafe.cpp index 7b16d5b954..a63aad1be0 100644 --- a/ArduCopter/failsafe.cpp +++ b/ArduCopter/failsafe.cpp @@ -36,7 +36,7 @@ void Copter::failsafe_disable() // void Copter::failsafe_check() { - uint32_t tnow = hal.scheduler->micros(); + uint32_t tnow = AP_HAL::micros(); if (mainLoop_count != failsafe_last_mainLoop_count) { // the main loop is running, all is OK diff --git a/ArduCopter/motor_test.cpp b/ArduCopter/motor_test.cpp index 35dcb80942..a704ffe912 100644 --- a/ArduCopter/motor_test.cpp +++ b/ArduCopter/motor_test.cpp @@ -27,7 +27,7 @@ void Copter::motor_test_output() } // check for test timeout - if ((hal.scheduler->millis() - motor_test_start_ms) >= motor_test_timeout_ms) { + if ((AP_HAL::millis() - motor_test_start_ms) >= motor_test_timeout_ms) { // stop motor test motor_test_stop(); } else { @@ -128,7 +128,7 @@ uint8_t Copter::mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_s } // set timeout - motor_test_start_ms = hal.scheduler->millis(); + motor_test_start_ms = AP_HAL::millis(); motor_test_timeout_ms = min(timeout_sec * 1000, MOTOR_TEST_TIMEOUT_MS_MAX); // store required output