From a6af1fc0faf529b214f9158bf172f34488517be7 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Fri, 22 May 2015 09:28:52 -0700 Subject: [PATCH 01/12] 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 02/12] 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 03/12] 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"); } } From d530cbcda270962c5d5e8ca4d8163222c9afe7fa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 May 2015 21:46:58 +0200 Subject: [PATCH 04/12] Ignore battery voltage only when below 2.5V, as we could be still running at that voltage --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index e4829dd6e4..203564ec97 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -129,7 +129,7 @@ #endif #define BATT_V_LOWPASS 0.001f -#define BATT_V_IGNORE_THRESHOLD 4.8f +#define BATT_V_IGNORE_THRESHOLD 2.5f /** * HACK - true temperature is much less than indicated temperature in baro, From d385a7f483e3279adf6f2165671770e1b0290d9b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 May 2015 21:46:58 +0200 Subject: [PATCH 05/12] Ignore battery voltage only when below 2.5V, as we could be still running at that voltage --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 34bcc88e50..cf1f51c9dd 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -133,7 +133,7 @@ #endif #define BATT_V_LOWPASS 0.001f -#define BATT_V_IGNORE_THRESHOLD 4.8f +#define BATT_V_IGNORE_THRESHOLD 2.5f /** * HACK - true temperature is much less than indicated temperature in baro, From 7f5a5e085cedc9f76f549f4fc5ecebe62b9ad0aa Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Fri, 22 May 2015 09:28:52 -0700 Subject: [PATCH 06/12] 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 3e9b1de6f7..f752a37b98 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -319,6 +319,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]); } @@ -2735,14 +2737,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 071d22a49a11157c79cd4a23a97583d1addda31a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 May 2015 09:54:55 +0200 Subject: [PATCH 07/12] commander: Update ESC calibration to match QGC --- src/modules/commander/calibration_messages.h | 1 + src/modules/commander/esc_calibration.cpp | 175 +++++++++++-------- src/modules/commander/esc_calibration.h | 6 +- 3 files changed, 109 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 541aca0531..22f1d4f99f 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 @@ -62,91 +64,122 @@ #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 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"); + + 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 = open(PWM_OUTPUT0_DEVICE_PATH, 0); -int do_esc_calibration(int mavlink_fd) { - - int fd = 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 = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); - if (ret != OK) - err(1, "PWM_SERVO_SET_ARM_OK"); + if (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 = ioctl(fd, PWM_SERVO_ARM, 0); - if (ret != OK) - err(1, "PWM_SERVO_ARM"); + if (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 = ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0); - if(ret!=0) { - err(1,"PWM_SERVO_SET_FORCE_SAFETY_OFF"); + if (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"); + + timeout_start = 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) { + // 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; } + + // PWM was high long enough + 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 is connected, signal to user and start waiting again + batt_connected = true; + timeout_start = hrt_absolute_time(); + mavlink_and_console_log_info(mavlink_fd, "[cal] Battery connected"); + } + } } usleep(50000); } - /* disarm */ - ret = ioctl(fd, PWM_SERVO_DISARM, 0); - if (ret != OK) - err(1, "PWM_SERVO_DISARM"); - - mavlink_log_info(mavlink_fd,"ESC calibration finished"); - return OK; - } \ No newline at end of file +Out: + if (batt_sub != -1) { + orb_unsubscribe(batt_sub); + } + if (fd != -1) { + if (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 (ioctl(fd, PWM_SERVO_DISARM, 0) != OK) { + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_WARNING_MSG, "Servos still armed"); + } + if (ioctl(fd, PWM_SERVO_CLEAR_ARM_OK, 0) != OK) { + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_WARNING_MSG, "Safety switch still deactivated"); + } + 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 4dd343e2de0e026858e2c74bbb159ef993d59596 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Fri, 15 May 2015 11:17:58 +0200 Subject: [PATCH 08/12] added field for autopilot capabilities --- msg/vehicle_status.msg | 2 ++ 1 file changed, 2 insertions(+) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index c5d5ee9a16..4913a02f57 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -97,6 +97,8 @@ int32 system_type # system type, inspired by MAVLink's VEHICLE_TYPE enum int32 system_id # system id, inspired by MAVLink's system ID field int32 component_id # subsystem / component id, inspired by MAVLink's component ID field +uint64 autopilot_capabilites # bitmask which gives info about autopilot capabilites + bool is_rotary_wing # True if system is in rotary wing configuration, so for a VTOL this is only true while flying as a multicopter bool is_vtol # True if the system is VTOL capable bool vtol_fw_permanent_stab # True if vtol should stabilize attitude for fw in manual mode From 0eeaa83b3d5f0e5a52a1042f1fbe8d3b9e2c88b0 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Fri, 15 May 2015 11:18:35 +0200 Subject: [PATCH 09/12] send autopilot version message on startup and on request --- src/modules/mavlink/mavlink_main.cpp | 14 ++++++++++++++ src/modules/mavlink/mavlink_main.h | 1 + src/modules/mavlink/mavlink_receiver.cpp | 4 +++- 3 files changed, 18 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index b6050fa56b..b30ab6fc64 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -912,6 +912,18 @@ Mavlink::send_statustext(unsigned char severity, const char *string) mavlink_logbuffer_write(&_logbuffer, &logmsg); } +void Mavlink::send_autopilot_capabilites() { + struct vehicle_status_s status; + + MavlinkOrbSubscription *status_sub = this->add_orb_subscription(ORB_ID(vehicle_status)); + + if (status_sub->update(&status)) { + mavlink_autopilot_version_t msg; + msg.capabilities = status.autopilot_capabilites; + this->send_message(MAVLINK_MSG_ID_AUTOPILOT_VERSION, &msg); + } +} + MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, int instance) { /* check if already subscribed to this topic */ @@ -1434,6 +1446,8 @@ Mavlink::task_main(int argc, char *argv[]) /* now the instance is fully initialized and we can bump the instance count */ LL_APPEND(_mavlink_instances, this); + send_autopilot_capabilites(); + while (!_task_should_exit) { /* main loop */ usleep(_main_loop_delay); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 90b84061f6..b732355b44 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -229,6 +229,7 @@ public: * @param severity the log level */ void send_statustext(unsigned char severity, const char *string); + void send_autopilot_capabilites(); MavlinkStream * get_streams() const { return _streams; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 40adc6b054..29b4fbcabb 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -278,7 +278,9 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) /* terminate other threads and this thread */ _mavlink->_task_should_exit = true; - + } else if (cmd_mavlink.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) { + /* send autopilot version message */ + _mavlink->send_autopilot_capabilites(); } else { if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { From 6d7e063148e2172edc41ebe5432794d4d39206f8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 May 2015 22:22:12 +0200 Subject: [PATCH 10/12] Added GIT version which recompiles on each build --- Makefile | 4 ++- msg/vehicle_status.msg | 2 -- src/modules/mavlink/mavlink_main.cpp | 37 ++++++++++++++++++++-- src/modules/sdlog2/module.mk | 2 -- src/modules/sdlog2/sdlog2.c | 3 +- src/modules/systemlib/.gitignore | 1 + src/modules/systemlib/git_version.h | 46 ++++++++++++++++++++++++++++ src/modules/systemlib/module.mk | 3 +- src/modules/uavcan/module.mk | 2 +- src/modules/uavcan/uavcan_main.cpp | 3 +- src/systemcmds/ver/module.mk | 2 -- src/systemcmds/ver/ver.c | 3 +- 12 files changed, 94 insertions(+), 14 deletions(-) create mode 100644 src/modules/systemlib/.gitignore create mode 100644 src/modules/systemlib/git_version.h diff --git a/Makefile b/Makefile index 201187e021..3715beb6ba 100644 --- a/Makefile +++ b/Makefile @@ -48,7 +48,9 @@ GIT_DESC := $(shell git log -1 --pretty=format:%H) ifneq ($(words $(GIT_DESC)),1) GIT_DESC := "unknown_git_version" endif -export GIT_DESC + +$(shell echo "#include \"git_version.h\"" > src/modules/systemlib/git_version.c) +$(shell echo "const char* px4_git_version = \"$(GIT_DESC)\";" >> src/modules/systemlib/git_version.c) # # Canned firmware configurations that we (know how to) build. diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 4913a02f57..c5d5ee9a16 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -97,8 +97,6 @@ int32 system_type # system type, inspired by MAVLink's VEHICLE_TYPE enum int32 system_id # system id, inspired by MAVLink's system ID field int32 component_id # subsystem / component id, inspired by MAVLink's component ID field -uint64 autopilot_capabilites # bitmask which gives info about autopilot capabilites - bool is_rotary_wing # True if system is in rotary wing configuration, so for a VTOL this is only true while flying as a multicopter bool is_vtol # True if the system is VTOL capable bool vtol_fw_permanent_stab # True if vtol should stabilize attitude for fw in manual mode diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index b30ab6fc64..3a0d1c1f68 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -66,6 +66,8 @@ #include #include #include +#include +#include #include #include #include @@ -918,8 +920,39 @@ void Mavlink::send_autopilot_capabilites() { MavlinkOrbSubscription *status_sub = this->add_orb_subscription(ORB_ID(vehicle_status)); if (status_sub->update(&status)) { - mavlink_autopilot_version_t msg; - msg.capabilities = status.autopilot_capabilites; + mavlink_autopilot_version_t msg = {}; + + const char* git_version = px4_git_version; + + msg.capabilities = MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT; + msg.capabilities |= MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT; + msg.capabilities |= MAV_PROTOCOL_CAPABILITY_COMMAND_INT; + msg.capabilities |= MAV_PROTOCOL_CAPABILITY_FTP; + msg.capabilities |= MAV_PROTOCOL_CAPABILITY_FTP; + msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET; + msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED; + msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_ACTUATOR_TARGET; + msg.flight_sw_version = 0; + msg.middleware_sw_version = 0; + msg.os_sw_version = 0; + msg.board_version = 0; + memcpy(&msg.flight_custom_version, git_version, sizeof(msg.flight_custom_version)); + memcpy(&msg.middleware_custom_version, git_version, sizeof(msg.middleware_custom_version)); + memset(&msg.os_custom_version, 0, sizeof(msg.os_custom_version)); + #ifdef CONFIG_CDCACM_VENDORID + msg.vendor_id = CONFIG_CDCACM_VENDORID; + #else + msg.vendor_id = 0; + #endif + #ifdef CONFIG_CDCACM_PRODUCTID + msg.product_id = CONFIG_CDCACM_PRODUCTID; + #else + msg.product_id = 0; + #endif + uint32_t uid[3]; + mcu_unique_id(uid); + msg.uid = (((uint64_t)uid[1]) << 32) | uid[2]; + this->send_message(MAVLINK_MSG_ID_AUTOPILOT_VERSION, &msg); } } diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index 6964acf339..08dddf219b 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -47,5 +47,3 @@ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os EXTRACFLAGS = -Wframe-larger-than=1400 - -EXTRADEFINES = -DGIT_VERSION='"$(GIT_DESC)"' diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 37234ca5d3..b88400c276 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -104,6 +104,7 @@ #include #include #include +#include #include #include @@ -795,7 +796,7 @@ int write_version(int fd) }; /* fill version message and write it */ - strncpy(log_msg_VER.body.fw_git, GIT_VERSION, sizeof(log_msg_VER.body.fw_git)); + strncpy(log_msg_VER.body.fw_git, px4_git_version, sizeof(log_msg_VER.body.fw_git)); strncpy(log_msg_VER.body.arch, HW_ARCH, sizeof(log_msg_VER.body.arch)); return write(fd, &log_msg_VER, sizeof(log_msg_VER)); } diff --git a/src/modules/systemlib/.gitignore b/src/modules/systemlib/.gitignore new file mode 100644 index 0000000000..58031bb131 --- /dev/null +++ b/src/modules/systemlib/.gitignore @@ -0,0 +1 @@ +git_version.c diff --git a/src/modules/systemlib/git_version.h b/src/modules/systemlib/git_version.h new file mode 100644 index 0000000000..cd8dc564ed --- /dev/null +++ b/src/modules/systemlib/git_version.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file git_version.h + * + * GIT repository version + */ + +#pragma once + +__BEGIN_DECLS + +__EXPORT extern const char* px4_git_version; + +__END_DECLS diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index f2499bbb13..40b7723107 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -55,7 +55,8 @@ SRCS = err.c \ pwm_limit/pwm_limit.c \ circuit_breaker.cpp \ circuit_breaker_params.c \ - mcu_version.c + mcu_version.c \ + git_version.c MAXOPTIMIZATION = -Os diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 437feb3014..e3fd31a9a1 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -63,7 +63,7 @@ SRCS += $(subst $(PX4_MODULE_SRC),../../,$(LIBUAVCAN_SRC)) INCLUDE_DIRS += $(LIBUAVCAN_INC) # Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile # because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode. -override EXTRADEFINES := $(EXTRADEFINES) -DGIT_VERSION='"$(GIT_DESC)"' -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS +override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS # diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index f04ab9f17c..474739bd66 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -212,7 +213,7 @@ void UavcanNode::fill_node_info() // Extracting the first 8 hex digits of GIT_VERSION and converting them to int char fw_git_short[9] = {}; - std::memmove(fw_git_short, GIT_VERSION, 8); + std::memmove(fw_git_short, px4_git_version, 8); assert(fw_git_short[8] == '\0'); char *end = nullptr; swver.vcs_commit = std::strtol(fw_git_short, &end, 16); diff --git a/src/systemcmds/ver/module.mk b/src/systemcmds/ver/module.mk index 4597b5f110..2eeb80b616 100644 --- a/src/systemcmds/ver/module.mk +++ b/src/systemcmds/ver/module.mk @@ -42,5 +42,3 @@ SRCS = ver.c MODULE_STACKSIZE = 1024 MAXOPTIMIZATION = -Os - -EXTRADEFINES = -DGIT_VERSION='"$(GIT_DESC)"' diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index b794e8b2fa..a248dfb79c 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -45,6 +45,7 @@ #include #include #include +#include /* string constants for version commands */ static const char sz_ver_hw_str[] = "hw"; @@ -101,7 +102,7 @@ int ver_main(int argc, char *argv[]) } if (show_all || !strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) { - printf("FW git-hash: %s\n", GIT_VERSION); + printf("FW git-hash: %s\n", px4_git_version); ret = 0; } From 0d134aac53f4e601ec75840aaf6c7e5af0b0c17f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 May 2015 10:18:20 +0200 Subject: [PATCH 11/12] systemlib: Use build directory for git version --- Makefile | 4 ++-- src/modules/systemlib/.gitignore | 1 - src/modules/systemlib/module.mk | 2 +- 3 files changed, 3 insertions(+), 4 deletions(-) delete mode 100644 src/modules/systemlib/.gitignore diff --git a/Makefile b/Makefile index 3715beb6ba..fde3f858f0 100644 --- a/Makefile +++ b/Makefile @@ -49,8 +49,8 @@ ifneq ($(words $(GIT_DESC)),1) GIT_DESC := "unknown_git_version" endif -$(shell echo "#include \"git_version.h\"" > src/modules/systemlib/git_version.c) -$(shell echo "const char* px4_git_version = \"$(GIT_DESC)\";" >> src/modules/systemlib/git_version.c) +$(shell echo "#include " > $(BUILD_DIR)git_version.c) +$(shell echo "const char* px4_git_version = \"$(GIT_DESC)\";" >> $(BUILD_DIR)git_version.c) # # Canned firmware configurations that we (know how to) build. diff --git a/src/modules/systemlib/.gitignore b/src/modules/systemlib/.gitignore deleted file mode 100644 index 58031bb131..0000000000 --- a/src/modules/systemlib/.gitignore +++ /dev/null @@ -1 +0,0 @@ -git_version.c diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 40b7723107..ff409fa473 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -56,7 +56,7 @@ SRCS = err.c \ circuit_breaker.cpp \ circuit_breaker_params.c \ mcu_version.c \ - git_version.c + $(BUILD_DIR)git_version.c MAXOPTIMIZATION = -Os From ccac324f5bd72a3adf4b0bf18eae796bfbfa0933 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 May 2015 11:04:19 +0200 Subject: [PATCH 12/12] mavlink: Fix GIT string transmission --- Makefile | 3 +++ src/modules/mavlink/mavlink_main.cpp | 6 ++---- src/modules/systemlib/git_version.h | 3 +++ 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/Makefile b/Makefile index fde3f858f0..47fe326b92 100644 --- a/Makefile +++ b/Makefile @@ -49,8 +49,11 @@ ifneq ($(words $(GIT_DESC)),1) GIT_DESC := "unknown_git_version" endif +GIT_DESC_SHORT := $(shell echo $(GIT_DESC) | cut -c1-16) + $(shell echo "#include " > $(BUILD_DIR)git_version.c) $(shell echo "const char* px4_git_version = \"$(GIT_DESC)\";" >> $(BUILD_DIR)git_version.c) +$(shell echo "const uint64_t px4_git_version_binary = 0x$(GIT_DESC_SHORT);" >> $(BUILD_DIR)git_version.c) # # Canned firmware configurations that we (know how to) build. diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 3a0d1c1f68..be63ae861c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -922,8 +922,6 @@ void Mavlink::send_autopilot_capabilites() { if (status_sub->update(&status)) { mavlink_autopilot_version_t msg = {}; - const char* git_version = px4_git_version; - msg.capabilities = MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT; msg.capabilities |= MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT; msg.capabilities |= MAV_PROTOCOL_CAPABILITY_COMMAND_INT; @@ -936,8 +934,8 @@ void Mavlink::send_autopilot_capabilites() { msg.middleware_sw_version = 0; msg.os_sw_version = 0; msg.board_version = 0; - memcpy(&msg.flight_custom_version, git_version, sizeof(msg.flight_custom_version)); - memcpy(&msg.middleware_custom_version, git_version, sizeof(msg.middleware_custom_version)); + memcpy(&msg.flight_custom_version, &px4_git_version_binary, sizeof(msg.flight_custom_version)); + memcpy(&msg.middleware_custom_version, &px4_git_version_binary, sizeof(msg.middleware_custom_version)); memset(&msg.os_custom_version, 0, sizeof(msg.os_custom_version)); #ifdef CONFIG_CDCACM_VENDORID msg.vendor_id = CONFIG_CDCACM_VENDORID; diff --git a/src/modules/systemlib/git_version.h b/src/modules/systemlib/git_version.h index cd8dc564ed..9b8f9e860e 100644 --- a/src/modules/systemlib/git_version.h +++ b/src/modules/systemlib/git_version.h @@ -39,8 +39,11 @@ #pragma once +#include + __BEGIN_DECLS __EXPORT extern const char* px4_git_version; +__EXPORT extern const uint64_t px4_git_version_binary; __END_DECLS