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

delete unused PWM IOCTLs

Browse Source
This commit is contained in:
Daniel Agar
2022-09-09 09:14:09 -04:00
parent bcdd2203d3
commit ce337a3d80
35 changed files with 198 additions and 1331 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/esc_calibration.cpp
+3 -110
View File
@@ -91,8 +91,10 @@ static void set_motor_actuators(uORB::Publication<actuator_test_s> &publisher, f
}
}
int do_esc_calibration_ctrl_alloc(orb_advert_t *mavlink_log_pub)
int do_esc_calibration(orb_advert_t *mavlink_log_pub)
{
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc");
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
@@ -154,112 +156,3 @@ int do_esc_calibration_ctrl_alloc(orb_advert_t *mavlink_log_pub)
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;
}
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_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);
}
}