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

commander: use macro with wait in all calibrations

Browse Source
This commit is contained in:
Julian Oes
2016-04-05 15:36:03 -07:00
committed by Lorenz Meier
parent f583f51027
commit 8707cfe9f2
5 changed files with 101 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/esc_calibration.cpp
+15 -14
View File
@@ -41,6 +41,7 @@
#include "esc_calibration.h"
#include "calibration_messages.h"
#include "calibration_routines.h"
#include <stdio.h>
#include <stdlib.h>
@@ -93,18 +94,18 @@ int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* a
hrt_abstime pwm_high_timeout = 10000000;
hrt_abstime timeout_start;
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc");
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc");
batt_sub = orb_subscribe(ORB_ID(battery_status));
if (batt_sub < 0) {
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Subscribe to battery");
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Subscribe to battery");
goto Error;
}
// Make sure battery is disconnected
orb_copy(ORB_ID(battery_status), batt_sub, &battery);
if (battery.voltage_filtered_v > 3.0f) {
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Disconnect battery and try again");
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Disconnect battery and try again");
goto Error;
}
@@ -113,29 +114,29 @@ int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* a
fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0);
if (fd < 0) {
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Can't open PWM device");
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Can't open PWM device");
goto Error;
}
/* tell IO/FMU that its ok to disable its safety with the switch */
if (px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0) != OK) {
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to disable safety switch");
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to disable safety switch");
goto Error;
}
/* tell IO/FMU that the system is armed (it will output values if safety is off) */
if (px4_ioctl(fd, PWM_SERVO_ARM, 0) != OK) {
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to arm system");
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to arm system");
goto Error;
}
/* tell IO to switch off safety without using the safety switch */
if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0) != OK) {
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to force safety off");
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to force safety off");
goto Error;
}
mavlink_and_console_log_info(mavlink_log_pub, "[cal] Connect battery now");
calibration_log_info(mavlink_log_pub, "[cal] Connect battery now");
timeout_start = hrt_absolute_time();
@@ -146,7 +147,7 @@ int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* a
if (hrt_absolute_time() - timeout_start > timeout_wait) {
if (!batt_connected) {
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Timeout waiting for battery");
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Timeout waiting for battery");
goto Error;
}
@@ -162,7 +163,7 @@ int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* a
// Battery is connected, signal to user and start waiting again
batt_connected = true;
timeout_start = hrt_absolute_time();
mavlink_and_console_log_info(mavlink_log_pub, "[cal] Battery connected");
calibration_log_info(mavlink_log_pub, "[cal] Battery connected");
}
}
}
@@ -175,20 +176,20 @@ Out:
}
if (fd != -1) {
if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0) != OK) {
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_WARNING_MSG, "Safety switch still off");
calibration_log_info(mavlink_log_pub, CAL_QGC_WARNING_MSG, "Safety switch still off");
}
if (px4_ioctl(fd, PWM_SERVO_DISARM, 0) != OK) {
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_WARNING_MSG, "Servos still armed");
calibration_log_info(mavlink_log_pub, CAL_QGC_WARNING_MSG, "Servos still armed");
}
if (px4_ioctl(fd, PWM_SERVO_CLEAR_ARM_OK, 0) != OK) {
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_WARNING_MSG, "Safety switch still deactivated");
calibration_log_info(mavlink_log_pub, CAL_QGC_WARNING_MSG, "Safety switch still deactivated");
}
px4_close(fd);
}
armed->in_esc_calibration_mode = false;
if (return_code == OK) {
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc");
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc");
}
return return_code;