diff --git a/Makefile b/Makefile index a14fd13a65..d2ca5255fd 100644 --- a/Makefile +++ b/Makefile @@ -55,7 +55,12 @@ GIT_DESC := $(shell git log -1 --pretty=format:%H) ifneq ($(words $(GIT_DESC)),1) GIT_DESC := "unknown_git_version" endif -export GIT_DESC + +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/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/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) { diff --git a/src/modules/commander/esc_calibration.cpp b/src/modules/commander/esc_calibration.cpp index 2e8f1e6f34..6c8d196421 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,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 = 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"); + + 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 = 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 diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 76f36e694f..b12003447a 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -69,6 +69,8 @@ #include #include #include +#include +#include #include #include //#include @@ -949,6 +951,47 @@ 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 = 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, &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; + #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); + } +} + MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, int instance) { /* check if already subscribed to this topic */ @@ -1482,6 +1525,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 4d2a833a0d..5fd897d4c7 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -241,6 +241,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 b84d381c6f..ac7ab99c05 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -285,7 +285,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) { diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index 30fc846a27..d22d0a9d93 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -49,5 +49,3 @@ MAXOPTIMIZATION = -Os ifeq ($(PX4_TARGET_OS),nuttx) EXTRACFLAGS = -Wframe-larger-than=1400 endif - -EXTRADEFINES = -DGIT_VERSION='"$(GIT_DESC)"' diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 00c1d7be40..3f1047742e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -105,6 +105,7 @@ #include #include #include +#include #include #include @@ -798,7 +799,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/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, diff --git a/src/modules/systemlib/git_version.h b/src/modules/systemlib/git_version.h new file mode 100644 index 0000000000..9b8f9e860e --- /dev/null +++ b/src/modules/systemlib/git_version.h @@ -0,0 +1,49 @@ +/**************************************************************************** + * + * 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 + +#include + +__BEGIN_DECLS + +__EXPORT extern const char* px4_git_version; +__EXPORT extern const uint64_t px4_git_version_binary; + +__END_DECLS diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index cb599e2052..54f76cda39 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. +# Copyright (c) 2012-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 @@ -52,7 +52,8 @@ SRCS = \ mcu_version.c \ bson/tinybson.c \ circuit_breaker.cpp \ - circuit_breaker_params.c + circuit_breaker_params.c \ + $(BUILD_DIR)git_version.c ifeq ($(PX4_TARGET_OS),nuttx) SRCS += err.c \ 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 75e8eeff95..0e055f05f8 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; }