From 53bcb8789f695d4f457dde48eff9dd90dd997329 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 9 Jun 2023 17:11:12 +0200 Subject: [PATCH] esc_calibration: Improve readability and robustness - Change timings for a more reliable calibration. - Make sure there's an error message when battery measurement is not available also when it gets disabled after system boot in the power just above the calibration button. - Safety check if measured electrical current goes up after issuing the high calibration value for the case the user did not unplug power and the detection either fails or is not enforced. --- src/modules/commander/esc_calibration.cpp | 110 ++++++++++++---------- 1 file changed, 62 insertions(+), 48 deletions(-) diff --git a/src/modules/commander/esc_calibration.cpp b/src/modules/commander/esc_calibration.cpp index e2e8e99b4c..c9db7124d0 100644 --- a/src/modules/commander/esc_calibration.cpp +++ b/src/modules/commander/esc_calibration.cpp @@ -58,18 +58,19 @@ using namespace time_literals; bool check_battery_disconnected(orb_advert_t *mavlink_log_pub) { - uORB::SubscriptionData batt_sub{ORB_ID(battery_status)}; - const battery_status_s &battery = batt_sub.get(); - batt_sub.update(); + uORB::SubscriptionData battery_status_sub{ORB_ID(battery_status)}; + battery_status_sub.update(); - if (battery.timestamp == 0) { - calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "battery unavailable"); + const bool recent_battery_measurement = (hrt_absolute_time() - battery_status_sub.get().timestamp) < 1_s; + + if (!recent_battery_measurement) { + // We have to send this message for now because "battery unavailable" gets ignored by QGC + calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Disconnect battery and try again"); return false; } - // Make sure battery is disconnected - // battery is not connected if the connected flag is not set and we have a recent battery measurement - if (!battery.connected && (hrt_elapsed_time(&battery.timestamp) < 500_ms)) { + // Make sure battery is reported to be disconnected + if (recent_battery_measurement && !battery_status_sub.get().connected) { return true; } @@ -93,66 +94,79 @@ static void set_motor_actuators(uORB::Publication &publisher, f int do_esc_calibration(orb_advert_t *mavlink_log_pub) { - calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc"); + // 1 Initialization + bool calibration_failed = false; - int return_code = PX4_OK; uORB::Publication actuator_test_pub{ORB_ID(actuator_test)}; // since we publish multiple at once, make sure the output driver subscribes before we publish actuator_test_pub.advertise(); - px4_usleep(10000); - // set motors to high + uORB::SubscriptionData battery_status_sub{ORB_ID(battery_status)}; + battery_status_sub.update(); + const bool battery_connected_before_calibration = battery_status_sub.get().connected; + const float current_before_calibration = battery_status_sub.get().current_a; + + calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc"); + + px4_usleep(10_ms); + + // 2 Set motors to high set_motor_actuators(actuator_test_pub, 1.f, false); calibration_log_info(mavlink_log_pub, "[cal] Connect battery now"); - - uORB::SubscriptionData batt_sub{ORB_ID(battery_status)}; - const battery_status_s &battery = batt_sub.get(); - batt_sub.update(); - bool batt_connected = battery.connected; hrt_abstime timeout_start = hrt_absolute_time(); + // 3 Wait for user to connect power while (true) { - // We are either waiting for the user to connect the battery. Or we are waiting to let the PWM - // sit high. - static constexpr hrt_abstime battery_connect_wait_timeout{20_s}; - static constexpr hrt_abstime pwm_high_timeout{3_s}; - hrt_abstime timeout_wait = batt_connected ? pwm_high_timeout : battery_connect_wait_timeout; + hrt_abstime now = hrt_absolute_time(); + battery_status_sub.update(); - if (hrt_elapsed_time(&timeout_start) > timeout_wait) { - if (!batt_connected) { - calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Timeout waiting for battery"); - return_code = PX4_ERROR; - } - - // PWM was high long enough + if ((now - timeout_start) < 1_s && (battery_status_sub.get().current_a > current_before_calibration + 1.f)) { + // Safety termination, current rises immediately, user didn't unplug power before + calibration_failed = true; break; } - if (!batt_connected) { - if (batt_sub.update()) { - if (battery.connected) { - // Battery is connected, signal to user and start waiting again - batt_connected = true; - timeout_start = hrt_absolute_time(); - calibration_log_info(mavlink_log_pub, "[cal] Battery connected"); - } - } + if (!battery_connected_before_calibration && battery_status_sub.get().connected) { + // Battery connection detected we can go to the next step immediately + break; } - px4_usleep(50000); + if ((now - timeout_start) > 10_s) { + // Timeout, we abort here + calibration_failed = true; + break; + } + + px4_usleep(50_ms); } - if (return_code == PX4_OK) { - // set motors to low + // 4 Wait for ESCs to measure high signal + if (!calibration_failed) { + calibration_log_info(mavlink_log_pub, "[cal] Battery connected"); + px4_usleep(8_s); + } + + // 5 Set motors to low + if (!calibration_failed) { set_motor_actuators(actuator_test_pub, 0.f, false); - px4_usleep(4000000); - - // release control - set_motor_actuators(actuator_test_pub, 0.f, true); - - calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc"); } - return return_code; + // 6 Wait for ESCs to measure low signal + if (!calibration_failed) { + px4_usleep(8_s); + } + + // 7 release control + set_motor_actuators(actuator_test_pub, 0.f, true); + + // 8 Report + if (calibration_failed) { + calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Timeout waiting for battery"); + return PX4_ERROR; + + } else { + calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc"); + return PX4_OK; + } }