From a6af1fc0faf529b214f9158bf172f34488517be7 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Fri, 22 May 2015 09:28:52 -0700 Subject: [PATCH 1/3] Add "calibrate esc" command --- src/modules/commander/commander.cpp | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ab42cef7a9..f81cbfb2b6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -326,6 +326,8 @@ int commander_main(int argc, char *argv[]) calib_ret = do_gyro_calibration(mavlink_fd); } else if (!strcmp(argv[2], "level")) { calib_ret = do_level_calibration(mavlink_fd); + } else if (!strcmp(argv[2], "esc")) { + calib_ret = do_esc_calibration(mavlink_fd, &armed); } else { warnx("argument %s unsupported.", argv[2]); } @@ -2744,14 +2746,9 @@ void *commander_low_prio_loop(void *arg) } else if ((int)(cmd.param7) == 1) { /* do esc calibration */ - calib_ret = check_if_batt_disconnected(mavlink_fd); - if(calib_ret == OK) { - answer_command(cmd,VEHICLE_CMD_RESULT_ACCEPTED); - armed.in_esc_calibration_mode = true; - calib_ret = do_esc_calibration(mavlink_fd); - armed.in_esc_calibration_mode = false; - } - + answer_command(cmd,VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_esc_calibration(mavlink_fd, &armed); + } else if ((int)(cmd.param4) == 0) { /* RC calibration ended - have we been in one worth confirming? */ if (status.rc_input_blocked) { From 25e7a1a49e6b2218278040977412c7fae860f196 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Fri, 22 May 2015 09:29:59 -0700 Subject: [PATCH 2/3] ESC calibration re-work - Uses standard QGC cal mavlink messaging - Timeout if no battery connect - Better error handling and cleanup --- src/modules/commander/calibration_messages.h | 1 + src/modules/commander/esc_calibration.cpp | 166 +++++++++++-------- src/modules/commander/esc_calibration.h | 6 +- 3 files changed, 100 insertions(+), 73 deletions(-) diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h index 7b4baae623..53775ffe4f 100644 --- a/src/modules/commander/calibration_messages.h +++ b/src/modules/commander/calibration_messages.h @@ -52,6 +52,7 @@ #define CAL_QGC_STARTED_MSG "[cal] calibration started: 1 %s" #define CAL_QGC_DONE_MSG "[cal] calibration done: %s" #define CAL_QGC_FAILED_MSG "[cal] calibration failed: %s" +#define CAL_QGC_WARNING_MSG "[cal] calibration warning: %s" #define CAL_QGC_CANCELLED_MSG "[cal] calibration cancelled" #define CAL_QGC_PROGRESS_MSG "[cal] progress <%u>" #define CAL_QGC_ORIENTATION_DETECTED_MSG "[cal] %s orientation detected" diff --git a/src/modules/commander/esc_calibration.cpp b/src/modules/commander/esc_calibration.cpp index 2e8f1e6f34..01afae9fd8 100644 --- a/src/modules/commander/esc_calibration.cpp +++ b/src/modules/commander/esc_calibration.cpp @@ -40,6 +40,8 @@ */ #include "esc_calibration.h" +#include "calibration_messages.h" + #include #include #include @@ -63,91 +65,113 @@ #endif static const int ERROR = -1; -int check_if_batt_disconnected(int mavlink_fd) { - struct battery_status_s battery; - memset(&battery,0,sizeof(battery)); - int batt_sub = orb_subscribe(ORB_ID(battery_status)); - orb_copy(ORB_ID(battery_status), batt_sub, &battery); - - if (battery.voltage_filtered_v > 3.0f && !(hrt_absolute_time() - battery.timestamp > 500000)) { - mavlink_log_info(mavlink_fd, "Please disconnect battery and try again!"); - return ERROR; +int do_esc_calibration(int mavlink_fd, struct actuator_armed_s* armed) +{ + int return_code = OK; + int fd = -1; + struct battery_status_s battery; + int batt_sub = -1; + bool batt_updated = false; + bool batt_connected = false; + hrt_abstime start_time; + + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, "esc"); + + batt_sub = orb_subscribe(ORB_ID(battery_status)); + if (batt_sub < 0) { + mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Subscribe to battery"); + goto Error; } - return OK; -} + // 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_fd, CAL_QGC_FAILED_MSG, "Disconnect battery and try again"); + goto Error; + } + + armed->in_esc_calibration_mode = true; + + fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0); -int do_esc_calibration(int mavlink_fd) { - - int fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0); - int ret; - - if(fd < 0) { - err(1,"Can't open %s", PWM_OUTPUT0_DEVICE_PATH); + if (fd < 0) { + mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Can't open PWM device"); + goto Error; } /* tell IO/FMU that its ok to disable its safety with the switch */ - ret = px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); - if (ret != OK) - err(1, "PWM_SERVO_SET_ARM_OK"); + if (px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0) != OK) { + mavlink_and_console_log_critical(mavlink_fd, 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) */ - ret = px4_ioctl(fd, PWM_SERVO_ARM, 0); - if (ret != OK) - err(1, "PWM_SERVO_ARM"); + if (px4_ioctl(fd, PWM_SERVO_ARM, 0) != OK) { + mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Unable to arm system"); + goto Error; + } + /* tell IO to switch off safety without using the safety switch */ - ret = px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0); - if(ret!=0) { - err(1,"PWM_SERVO_SET_FORCE_SAFETY_OFF"); + if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0) != OK) { + mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Unable to force safety off"); + goto Error; } - mavlink_log_info(mavlink_fd,"Please connect battery now"); + mavlink_and_console_log_info(mavlink_fd, "[cal] Connect battery now"); + + start_time = hrt_absolute_time(); - struct battery_status_s battery; - memset(&battery,0,sizeof(battery)); - int batt_sub = orb_subscribe(ORB_ID(battery_status)); - orb_copy(ORB_ID(vehicle_command),batt_sub, &battery); - bool updated = false; - - struct vehicle_command_s cmd; - memset(&cmd, 0, sizeof(cmd)); - int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); - orb_copy(ORB_ID(vehicle_command),cmd_sub, &cmd); - - /* wait for one of the following events: - 1) user has pressed the button in QGroundControl - 2) timeout of 5 seconds is reached - */ - hrt_abstime start_time = hrt_absolute_time(); - - while(true) { - orb_check(batt_sub,&updated); - if(updated) { - orb_copy(ORB_ID(battery_status), batt_sub, &battery); - } - // user has connected battery - if(battery.voltage_filtered_v > 3.0f) { - orb_check(cmd_sub,&updated); - if(updated) { - orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); - } - if((int)(cmd.param7) == 2 && cmd.command == VEHICLE_CMD_PREFLIGHT_CALIBRATION) { - break; - } else if (hrt_absolute_time() - start_time > 5000000) { - // waited for 5 seconds, switch to low pwm - break; + while (true) { + if (hrt_absolute_time() - start_time > 5000000) { + if (!batt_connected) { + mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Timeout waiting for battery"); + goto Error; } + + // 5 seconds at high pwm + break; } - else { - start_time = hrt_absolute_time(); + + if (!batt_connected) { + orb_check(batt_sub, &batt_updated); + if (batt_updated) { + orb_copy(ORB_ID(battery_status), batt_sub, &battery); + if (battery.voltage_filtered_v > 3.0f) { + // Battery connected, wait for 5 seconds at high pwm + batt_connected = true; + start_time = hrt_absolute_time(); + mavlink_and_console_log_info(mavlink_fd, "[cal] Battery connected"); + } + } } usleep(50000); } - /* disarm */ - ret = px4_ioctl(fd, PWM_SERVO_DISARM, 0); - if (ret != OK) - err(1, "PWM_SERVO_DISARM"); - - mavlink_log_info(mavlink_fd,"ESC calibration finished"); - return OK; - } +Out: + if (batt_sub != -1) { + orb_unsubscribe(batt_sub); + } + if (fd != -1) { + if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0) != OK) { + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_WARNING_MSG, "Safety switch still off"); + } + if (px4_ioctl(fd, PWM_SERVO_DISARM, 0) != OK) { + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_WARNING_MSG, "Servos still armed"); + } + if (px4_ioctl(fd, PWM_SERVO_CLEAR_ARM_OK, 0) != OK) { + mavlink_and_console_log_info(mavlink_fd, 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_fd, CAL_QGC_DONE_MSG, "esc"); + } + + return return_code; + +Error: + return_code = ERROR; + goto Out; +} diff --git a/src/modules/commander/esc_calibration.h b/src/modules/commander/esc_calibration.h index 5539955ebc..ac37b278c6 100644 --- a/src/modules/commander/esc_calibration.h +++ b/src/modules/commander/esc_calibration.h @@ -41,7 +41,9 @@ #ifndef ESC_CALIBRATION_H_ #define ESC_CALIBRATION_H_ -int check_if_batt_disconnected(int mavlink_fd); -int do_esc_calibration(int mavlink_fd); + +#include + +int do_esc_calibration(int mavlink_fd, struct actuator_armed_s* armed); #endif \ No newline at end of file From fe357a9a667e5076fccbdb28ee9bcae55c18f5b1 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Fri, 22 May 2015 11:24:07 -0700 Subject: [PATCH 3/3] Separate timeouts for battery and high pwm --- src/modules/commander/esc_calibration.cpp | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/esc_calibration.cpp b/src/modules/commander/esc_calibration.cpp index 01afae9fd8..6c8d196421 100644 --- a/src/modules/commander/esc_calibration.cpp +++ b/src/modules/commander/esc_calibration.cpp @@ -68,12 +68,17 @@ static const int ERROR = -1; int do_esc_calibration(int mavlink_fd, struct actuator_armed_s* armed) { int return_code = OK; + int fd = -1; + struct battery_status_s battery; int batt_sub = -1; bool batt_updated = false; bool batt_connected = false; - hrt_abstime start_time; + + hrt_abstime battery_connect_wait_timeout = 20000000; + hrt_abstime pwm_high_timeout = 5000000; + hrt_abstime timeout_start; mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, "esc"); @@ -119,16 +124,20 @@ int do_esc_calibration(int mavlink_fd, struct actuator_armed_s* armed) mavlink_and_console_log_info(mavlink_fd, "[cal] Connect battery now"); - start_time = hrt_absolute_time(); + timeout_start = hrt_absolute_time(); while (true) { - if (hrt_absolute_time() - start_time > 5000000) { + // We are either waiting for the user to connect the battery. Or we are waiting to let the PWM + // sit high. + hrt_abstime timeout_wait = batt_connected ? pwm_high_timeout : battery_connect_wait_timeout; + + if (hrt_absolute_time() - timeout_start > timeout_wait) { if (!batt_connected) { mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Timeout waiting for battery"); goto Error; } - // 5 seconds at high pwm + // PWM was high long enough break; } @@ -137,9 +146,9 @@ int do_esc_calibration(int mavlink_fd, struct actuator_armed_s* armed) if (batt_updated) { orb_copy(ORB_ID(battery_status), batt_sub, &battery); if (battery.voltage_filtered_v > 3.0f) { - // Battery connected, wait for 5 seconds at high pwm + // Battery is connected, signal to user and start waiting again batt_connected = true; - start_time = hrt_absolute_time(); + timeout_start = hrt_absolute_time(); mavlink_and_console_log_info(mavlink_fd, "[cal] Battery connected"); } }