fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

WIP: PWM ESC calibration

Browse Source
This commit is contained in:
Daniel Agar 2022-01-04 11:48:49 -05:00
parent 846f807eff
commit c36f58e84a
No known key found for this signature in database
GPG Key ID: FD3CBA98017A69DE
6 changed files with 154 additions and 209 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

106
src/drivers/pwm_out/PWMOut.cpp
View File

@ -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()

3
src/drivers/pwm_out/PWMOut.hpp
View File

@ -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;

51
src/drivers/px4io/px4io.cpp
View File

@ -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 */

15
src/lib/mixer_module/mixer_module.cpp
View File

@ -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) {

23
src/modules/commander/Commander.cpp
View File

@ -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_s> 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) {

165
src/modules/commander/esc_calibration.cpp
View File

@ -78,42 +78,14 @@ bool check_battery_disconnected(orb_advert_t *mavlink_log_pub)
return false;
}
static void set_motor_actuators(uORB::Publication<actuator_test_s> &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_s> 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_s> 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<battery_status_s> 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<battery_status_s> 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);
}
}