From c36f58e84a9787b11b69652e01400cafce1e72bc Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 4 Jan 2022 11:48:49 -0500 Subject: [PATCH] WIP: PWM ESC calibration --- src/drivers/pwm_out/PWMOut.cpp | 106 ++++++++++---- src/drivers/pwm_out/PWMOut.hpp | 3 + src/drivers/px4io/px4io.cpp | 51 ++++++- src/lib/mixer_module/mixer_module.cpp | 15 +- src/modules/commander/Commander.cpp | 23 ++- src/modules/commander/esc_calibration.cpp | 165 +--------------------- 6 files changed, 154 insertions(+), 209 deletions(-) diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp index 1491d9831b..d26f24a441 100644 --- a/src/drivers/pwm_out/PWMOut.cpp +++ b/src/drivers/pwm_out/PWMOut.cpp @@ -449,47 +449,97 @@ void PWMOut::Run() return; } - perf_begin(_cycle_perf); + // ESC calibration + if (_pwm_initialized && !_mixing_output.armed().armed) { + if (_mixing_output.armed().in_esc_calibration_mode) { + _esc_calibration_mode = true; + _esc_calibration_last = _mixing_output.armed().timestamp; + + // set outputs to maximum + for (size_t i = 0; i < _num_outputs; i++) { + // TODO: ESCs only + // - PWM only (no oneshot) + if (_pwm_mask & (1 << (i + _output_base))) { + up_pwm_servo_set(_output_base + i, PWM_DEFAULT_MAX); + } + } + + ScheduleDelayed(10_ms); + return; + + } else if (_esc_calibration_mode) { + if (hrt_elapsed_time(&_esc_calibration_last) < 3_s) { + // set outputs to minimum + for (size_t i = 0; i < _num_outputs; i++) { + // TODO: ESCs only + if (_pwm_mask & (1 << (i + _output_base))) { + up_pwm_servo_set(_output_base + i, PWM_DEFAULT_MIN); + } + } + + ScheduleDelayed(10_ms); + return; + + } else { + // calibration finished + _esc_calibration_mode = false; + _esc_calibration_last = 0; + } + } + } + + + perf_count(_interval_perf); - if (!_mixing_output.useDynamicMixing()) { - // push backup schedule - ScheduleDelayed(_backup_schedule_interval_us); - } + if (_mixing_output.armed().in_esc_calibration_mode) { + // do calibration - _mixing_output.update(); - /* update PWM status if armed or if disarmed PWM values are set */ - bool pwm_on = _mixing_output.armed().armed || (_num_disarmed_set > 0) || _mixing_output.useDynamicMixing() - || _mixing_output.armed().in_esc_calibration_mode; - if (_pwm_on != pwm_on || _require_arming[_instance].load()) { + } else { + perf_begin(_cycle_perf); - if (update_pwm_out_state(pwm_on)) { - _pwm_on = pwm_on; + if (!_mixing_output.useDynamicMixing()) { + // push backup schedule + ScheduleDelayed(_backup_schedule_interval_us); } - } - // check for parameter updates - if (_parameter_update_sub.updated()) { - // clear update - parameter_update_s pupdate; - _parameter_update_sub.copy(&pupdate); + _mixing_output.update(); - // update parameters from storage - if (_mixing_output.useDynamicMixing()) { // do not update PWM params for now (was interfering with VTOL PWM settings) - update_params(); + /* update PWM status if armed or if disarmed PWM values are set */ + bool pwm_on = _mixing_output.armed().armed || (_num_disarmed_set > 0) || _mixing_output.useDynamicMixing() + || _mixing_output.armed().in_esc_calibration_mode; + + if (_pwm_on != pwm_on || _require_arming[_instance].load()) { + + if (update_pwm_out_state(pwm_on)) { + _pwm_on = pwm_on; + } } + + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + if (_mixing_output.useDynamicMixing()) { // do not update PWM params for now (was interfering with VTOL PWM settings) + update_params(); + } + } + + if (_pwm_initialized && _current_update_rate == 0 && !_mixing_output.useDynamicMixing()) { + update_current_rate(); + } + + // check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread) + _mixing_output.updateSubscriptions(true, true); + perf_end(_cycle_perf); } - if (_pwm_initialized && _current_update_rate == 0 && !_mixing_output.useDynamicMixing()) { - update_current_rate(); - } - // check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread) - _mixing_output.updateSubscriptions(true, true); - - perf_end(_cycle_perf); } void PWMOut::update_params() diff --git a/src/drivers/pwm_out/PWMOut.hpp b/src/drivers/pwm_out/PWMOut.hpp index fd6491f54d..7f72fea8ce 100644 --- a/src/drivers/pwm_out/PWMOut.hpp +++ b/src/drivers/pwm_out/PWMOut.hpp @@ -149,6 +149,9 @@ private: bool _pwm_initialized{false}; bool _test_mode{false}; + bool _esc_calibration_mode{false}; + hrt_abstime _esc_calibration_last{0}; + unsigned _num_disarmed_set{0}; perf_counter_t _cycle_perf; diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 2b7673beba..c5d077b304 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -218,6 +218,9 @@ private: hrt_abstime _last_status_publish{0}; + bool _esc_calibration_mode{false}; + hrt_abstime _esc_calibration_last{0}; + bool _param_update_force{true}; ///< force a parameter update bool _timer_rates_configured{false}; @@ -579,6 +582,52 @@ void PX4IO::Run() return; } + SmartLock lock_guard(_lock); + + // ESC calibration + if (!_mixing_output.armed().armed && !_test_fmu_fail && !_in_test_mode) { + if (_mixing_output.armed().in_esc_calibration_mode) { + _esc_calibration_mode = true; + _esc_calibration_last = _mixing_output.armed().timestamp; + + // set outputs to maximum + uint16_t outputs[MAX_ACTUATORS]; + + for (unsigned i = 0; i < _max_actuators; i++) { + outputs[i] = PWM_DEFAULT_MAX; + } + + // TODO: ESCs only + // - PWM only (no oneshot) + io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, outputs, _max_actuators); + + ScheduleDelayed(10_ms); + return; + + } else if (_esc_calibration_mode) { + if (hrt_elapsed_time(&_esc_calibration_last) < 3_s) { + // set outputs to minimum + uint16_t outputs[MAX_ACTUATORS]; + + for (unsigned i = 0; i < _max_actuators; i++) { + outputs[i] = PWM_DEFAULT_MIN; + } + + // TODO: ESCs only + io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, outputs, _max_actuators); + + ScheduleDelayed(10_ms); + return; + + } else { + // calibration finished + _esc_calibration_mode = false; + _esc_calibration_last = 0; + } + } + } + + perf_begin(_cycle_perf); perf_count(_interval_perf); @@ -592,7 +641,7 @@ void PX4IO::Run() _mixing_output.update(); } - SmartLock lock_guard(_lock); + if (hrt_elapsed_time(&_poll_last) >= 20_ms) { /* run at 50 */ diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index ecd0f9ae8d..b1347d0987 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -683,19 +683,6 @@ bool MixingOutput::updateStaticMixer() if (_control_subs[i].copy(&_controls[i])) { n_updates++; } - - /* During ESC calibration, we overwrite the throttle value. */ - if (i == 0 && _armed.in_esc_calibration_mode) { - - /* Set all controls to 0 */ - memset(&_controls[i], 0, sizeof(_controls[i])); - - /* except thrust to maximum. */ - _controls[i].control[actuator_controls_s::INDEX_THROTTLE] = 1.0f; - - /* Switch off the output limit ramp for the calibration. */ - _output_limit.state = OUTPUT_LIMIT_STATE_ON; - } } } @@ -1043,7 +1030,7 @@ int MixingOutput::controlCallback(uintptr_t handle, uint8_t control_group, uint8 } /* throttle not arming - mark throttle input as invalid */ - if (output->armNoThrottle() && !output->_armed.in_esc_calibration_mode) { + if (output->armNoThrottle()) { if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE || control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) && control_index == actuator_controls_s::INDEX_THROTTLE) { diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 79a1f3defd..5d55e6b124 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1300,14 +1300,25 @@ Commander::handle_command(const vehicle_command_s &cmd) } else if ((int)(cmd.param7) == 1) { /* do esc calibration */ - if (check_battery_disconnected(&_mavlink_log_pub)) { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); - _status_flags.condition_calibration_enabled = true; - _armed.in_esc_calibration_mode = true; - _worker_thread.startTask(WorkerThread::Request::ESCCalibration); + + // check safety + uORB::SubscriptionData safety_sub{ORB_ID(safety)}; + safety_sub.update(); + + if (safety_sub.get().safety_switch_available && !safety_sub.get().safety_off) { + //mavlink_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Disable safety first"); // TODO + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); } else { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); + if (check_battery_disconnected(&_mavlink_log_pub)) { + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + _status_flags.condition_calibration_enabled = true; + _armed.in_esc_calibration_mode = true; + _worker_thread.startTask(WorkerThread::Request::ESCCalibration); + + } else { + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); + } } } else if ((int)(cmd.param4) == 0) { diff --git a/src/modules/commander/esc_calibration.cpp b/src/modules/commander/esc_calibration.cpp index 17c2bb6a89..045cef2b55 100644 --- a/src/modules/commander/esc_calibration.cpp +++ b/src/modules/commander/esc_calibration.cpp @@ -78,42 +78,14 @@ bool check_battery_disconnected(orb_advert_t *mavlink_log_pub) return false; } -static void set_motor_actuators(uORB::Publication &publisher, float value, bool release_control) +int do_esc_calibration(orb_advert_t *mavlink_log_pub) { - actuator_test_s actuator_test{}; - actuator_test.timestamp = hrt_absolute_time(); - actuator_test.value = value; - actuator_test.action = release_control ? actuator_test_s::ACTION_RELEASE_CONTROL : actuator_test_s::ACTION_DO_CONTROL; - actuator_test.timeout_ms = 0; + calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc"); + px4_usleep(100000); - for (int i = 0; i < actuator_test_s::MAX_NUM_MOTORS; ++i) { - actuator_test.function = actuator_test_s::FUNCTION_MOTOR1 + i; - publisher.publish(actuator_test); - } -} - -int do_esc_calibration_ctrl_alloc(orb_advert_t *mavlink_log_pub) -{ - // check safety - uORB::SubscriptionData safety_sub{ORB_ID(safety)}; - safety_sub.update(); - - if (safety_sub.get().safety_switch_available && !safety_sub.get().safety_off) { - calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Disable safety first"); - return PX4_ERROR; - } - - 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 - set_motor_actuators(actuator_test_pub, 1.f, false); + int return_code = PX4_OK; 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(); @@ -121,8 +93,7 @@ int do_esc_calibration_ctrl_alloc(orb_advert_t *mavlink_log_pub) hrt_abstime timeout_start = hrt_absolute_time(); while (true) { - // We are either waiting for the user to connect the battery. Or we are waiting to let the PWM - // sit high. + // 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; @@ -152,134 +123,8 @@ int do_esc_calibration_ctrl_alloc(orb_advert_t *mavlink_log_pub) } if (return_code == PX4_OK) { - // set motors to low - 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; } - -static int do_esc_calibration_ioctl(orb_advert_t *mavlink_log_pub) -{ - int return_code = PX4_OK; - hrt_abstime timeout_start = 0; - - uORB::SubscriptionData batt_sub{ORB_ID(battery_status)}; - const battery_status_s &battery = batt_sub.get(); - batt_sub.update(); - bool batt_connected = battery.connected; - - int fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0); - - if (fd < 0) { - calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Can't open PWM device"); - return_code = PX4_ERROR; - goto Out; - } - - /* tell IO/FMU that its ok to disable its safety with the switch */ - if (px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0) != PX4_OK) { - calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to disable safety switch"); - return_code = PX4_ERROR; - goto Out; - } - - /* tell IO/FMU that the system is armed (it will output values if safety is off) */ - if (px4_ioctl(fd, PWM_SERVO_ARM, 0) != PX4_OK) { - calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to arm system"); - return_code = PX4_ERROR; - goto Out; - } - - /* tell IO to switch off safety without using the safety switch */ - if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0) != PX4_OK) { - calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to force safety off"); - return_code = PX4_ERROR; - goto Out; - } - - calibration_log_info(mavlink_log_pub, "[cal] Connect battery now"); - - timeout_start = hrt_absolute_time(); - - 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; - - 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; - goto Out; - } - - // PWM was high long enough - 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"); - } - } - } - - px4_usleep(50000); - } - -Out: - - if (fd != -1) { - if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0) != PX4_OK) { - calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Safety switch still off"); - } - - if (px4_ioctl(fd, PWM_SERVO_DISARM, 0) != PX4_OK) { - calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Servos still armed"); - } - - if (px4_ioctl(fd, PWM_SERVO_CLEAR_ARM_OK, 0) != PX4_OK) { - calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Safety switch still deactivated"); - } - - px4_close(fd); - } - - if (return_code == PX4_OK) { - calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc"); - } - - return return_code; -} - -int do_esc_calibration(orb_advert_t *mavlink_log_pub) -{ - calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc"); - - param_t p_ctrl_alloc = param_find("SYS_CTRL_ALLOC"); - int32_t ctrl_alloc = 0; - - if (p_ctrl_alloc != PARAM_INVALID) { - param_get(p_ctrl_alloc, &ctrl_alloc); - } - - if (ctrl_alloc == 1) { - return do_esc_calibration_ctrl_alloc(mavlink_log_pub); - - } else { - return do_esc_calibration_ioctl(mavlink_log_pub); - } -}