diff options
44 files changed, 1035 insertions, 319 deletions
diff --git a/NuttX b/NuttX -Subproject 41fffa0df192e4a26d2325c7ac5d9b5d7ba0211 +Subproject 5ee4b2b2c26bbc35d1669840f0676e8aa383b98 diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index c4be16943..b1aa8c00b 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -27,3 +27,4 @@ fi set MIXER FMU_quad_w set PWM_OUTPUTS 1234 +set PWM_MIN 1200 diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index b46db00b5..d4e461226 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -178,9 +178,9 @@ class uploader(object): MAVLINK_REBOOT_ID1 = bytearray(b'\xfe\x21\x72\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x01\x00\x00\x48\xf0') MAVLINK_REBOOT_ID0 = bytearray(b'\xfe\x21\x45\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x00\x00\x00\xd7\xac') - def __init__(self, portname, baudrate, interCharTimeout=0.001, timeout=0.5): + def __init__(self, portname, baudrate): # open the port, keep the default timeout short so we can poll quickly - self.port = serial.Serial(portname, baudrate) + self.port = serial.Serial(portname, baudrate, timeout=0.5) self.otp = b'' self.sn = b'' @@ -195,7 +195,7 @@ class uploader(object): def __recv(self, count=1): c = self.port.read(count) if len(c) < 1: - raise RuntimeError("timeout waiting for data (%u bytes)", count) + raise RuntimeError("timeout waiting for data (%u bytes)" % count) # print("recv " + binascii.hexlify(c)) return c @@ -459,6 +459,7 @@ if os.path.exists("/usr/sbin/ModemManager"): # Load the firmware file fw = firmware(args.firmware) print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'))) +print("If the board does not respond within 1-2 seconds, unplug and re-plug the USB connector.") # Spin waiting for a device to show up while True: @@ -508,9 +509,12 @@ while True: except Exception: # most probably a timeout talking to the port, no bootloader, try to reboot the board print("attempting reboot on %s..." % port) + print("if the board does not respond, unplug and re-plug the USB connector.") up.send_reboot() # wait for the reboot, without we might run into Serial I/O Error 5 time.sleep(0.5) + # always close the port + up.close() continue try: diff --git a/Tools/sdlog2/sdlog2_dump.py b/Tools/sdlog2/sdlog2_dump.py index ca2efb021..8b0ccb2d7 100644..100755 --- a/Tools/sdlog2/sdlog2_dump.py +++ b/Tools/sdlog2/sdlog2_dump.py @@ -154,8 +154,8 @@ class SDLog2Parser: first_data_msg = False self.__parseMsg(msg_descr) bytes_read += self.__ptr - if not self.__debug_out and self.__time_msg != None and self.__csv_updated: - self.__printCSVRow() + if not self.__debug_out and self.__time_msg != None and self.__csv_updated: + self.__printCSVRow() f.close() def __bytesLeft(self): diff --git a/Tools/usb_serialload.py b/Tools/usb_serialload.py new file mode 100644 index 000000000..5c864532f --- /dev/null +++ b/Tools/usb_serialload.py @@ -0,0 +1,55 @@ +import serial, time + + +port = serial.Serial('/dev/ttyACM0', baudrate=57600, timeout=2) + +data = '01234567890123456789012345678901234567890123456789' +#data = 'hellohello' +outLine = 'echo %s\n' % data + +port.write('\n\n\n') +port.write('free\n') +line = port.readline(80) +while line != '': + print(line) + line = port.readline(80) + + +i = 0 +bytesOut = 0 +bytesIn = 0 + +startTime = time.time() +lastPrint = startTime +while True: + bytesOut += port.write(outLine) + line = port.readline(80) + bytesIn += len(line) + # check command line echo + if (data not in line): + print('command error %d: %s' % (i,line)) + #break + # read echo output + line = port.readline(80) + if (data not in line): + print('echo output error %d: %s' % (i,line)) + #break + bytesIn += len(line) + #print('%d: %s' % (i,line)) + #print('%d: bytesOut: %d, bytesIn: %d' % (i, bytesOut, bytesIn)) + + elapsedT = time.time() - lastPrint + if (time.time() - lastPrint >= 5): + outRate = bytesOut / elapsedT + inRate = bytesIn / elapsedT + usbRate = (bytesOut + bytesIn) / elapsedT + lastPrint = time.time() + print('elapsed time: %f' % (time.time() - startTime)) + print('data rates (bytes/sec): out: %f, in: %f, total: %f' % (outRate, inRate, usbRate)) + + bytesOut = 0 + bytesIn = 0 + + i += 1 + #if (i > 2): break + diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 -Subproject 8587a007cf89cb13f146ac46be94b61276343ce +Subproject 90383fac84d031aef17989a1497c2473dfa6434 diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index ef9bb5cad..10a93be0b 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -77,6 +77,7 @@ #define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) #define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) #define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) +#define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11) /* Safety switch button *******************************************************/ diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c index 9f8c0eeb2..5c3343ccc 100644 --- a/src/drivers/boards/px4io-v2/px4iov2_init.c +++ b/src/drivers/boards/px4io-v2/px4iov2_init.c @@ -108,6 +108,7 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_LED1); stm32_configgpio(GPIO_LED2); stm32_configgpio(GPIO_LED3); + stm32_configgpio(GPIO_LED4); stm32_configgpio(GPIO_BTN_SAFETY); diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index f66ec7c95..6873f24b6 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -213,6 +213,9 @@ ORB_DECLARE(output_pwm); /** make failsafe non-recoverable (termination) if it occurs */ #define PWM_SERVO_SET_TERMINATION_FAILSAFE _IOC(_PWM_SERVO_BASE, 25) +/** force safety switch on (to enable use of safety switch) */ +#define PWM_SERVO_SET_FORCE_SAFETY_ON _IOC(_PWM_SERVO_BASE, 26) + /* * * diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index cfae8761c..82fa5cc6e 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -176,6 +176,7 @@ static const int ERROR = -1; #define L3G4200D_DEFAULT_RATE 800 #define L3GD20_DEFAULT_RANGE_DPS 2000 #define L3GD20_DEFAULT_FILTER_FREQ 30 +#define L3GD20_TEMP_OFFSET_CELSIUS 40 #ifndef SENSOR_BOARD_ROTATION_DEFAULT #define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG @@ -856,7 +857,7 @@ L3GD20::measure() #pragma pack(push, 1) struct { uint8_t cmd; - uint8_t temp; + int8_t temp; uint8_t status; int16_t x; int16_t y; @@ -930,6 +931,8 @@ L3GD20::measure() report.z_raw = raw_report.z; + report.temperature_raw = raw_report.temp; + report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; @@ -938,6 +941,8 @@ L3GD20::measure() report.y = _gyro_filter_y.apply(report.y); report.z = _gyro_filter_z.apply(report.z); + report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp; + // apply user specified rotation rotate_3f(_rotation, report.x, report.y, report.z); @@ -1091,9 +1096,11 @@ test() warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x); warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y); warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z); + warnx("temp: \t%d\tC", (int)g_report.temperature); warnx("gyro x: \t%d\traw", (int)g_report.x_raw); warnx("gyro y: \t%d\traw", (int)g_report.y_raw); warnx("gyro z: \t%d\traw", (int)g_report.z_raw); + warnx("temp: \t%d\traw", (int)g_report.temperature_raw); warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index a69e6ee55..6793acd81 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -73,14 +73,19 @@ /* Configuration Constants */ #define LL40LS_BUS PX4_I2C_BUS_EXPANSION -#define LL40LS_BASEADDR 0x42 /* 7-bit address */ -#define LL40LS_DEVICE_PATH "/dev/ll40ls" +#define LL40LS_BASEADDR 0x62 /* 7-bit address */ +#define LL40LS_BASEADDR_OLD 0x42 /* previous 7-bit address */ +#define LL40LS_DEVICE_PATH_INT "/dev/ll40ls_int" +#define LL40LS_DEVICE_PATH_EXT "/dev/ll40ls_ext" /* LL40LS Registers addresses */ #define LL40LS_MEASURE_REG 0x00 /* Measure range register */ -#define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */ +#define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */ #define LL40LS_DISTHIGH_REG 0x8F /* High byte of distance register, auto increment */ +#define LL40LS_WHO_AM_I_REG 0x11 +#define LL40LS_WHO_AM_I_REG_VAL 0xCA +#define LL40LS_SIGNAL_STRENGTH_REG 0x5b /* Device limits */ #define LL40LS_MIN_DISTANCE (0.00f) @@ -101,7 +106,7 @@ static const int ERROR = -1; class LL40LS : public device::I2C { public: - LL40LS(int bus = LL40LS_BUS, int address = LL40LS_BASEADDR); + LL40LS(int bus, const char *path, int address = LL40LS_BASEADDR); virtual ~LL40LS(); virtual int init(); @@ -116,6 +121,7 @@ public: protected: virtual int probe(); + virtual int read_reg(uint8_t reg, uint8_t &val); private: float _min_distance; @@ -132,6 +138,10 @@ private: perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; + uint16_t _last_distance; + + /**< the bus the device is connected to */ + int _bus; /** * Test whether the device supported by the driver is present at a @@ -188,8 +198,8 @@ private: */ extern "C" __EXPORT int ll40ls_main(int argc, char *argv[]); -LL40LS::LL40LS(int bus, int address) : - I2C("LL40LS", LL40LS_DEVICE_PATH, bus, address, 100000), +LL40LS::LL40LS(int bus, const char *path, int address) : + I2C("LL40LS", path, bus, address, 100000), _min_distance(LL40LS_MIN_DISTANCE), _max_distance(LL40LS_MAX_DISTANCE), _reports(nullptr), @@ -200,10 +210,12 @@ LL40LS::LL40LS(int bus, int address) : _range_finder_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")), _comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")) + _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")), + _last_distance(0), + _bus(bus) { // up the retries since the device misses the first measure attempts - I2C::_retries = 3; + _retries = 3; // enable debug() calls _debug_enabled = false; @@ -271,8 +283,50 @@ out: } int +LL40LS::read_reg(uint8_t reg, uint8_t &val) +{ + return transfer(®, 1, &val, 1); +} + +int LL40LS::probe() { + // cope with both old and new I2C bus address + const uint8_t addresses[2] = {LL40LS_BASEADDR, LL40LS_BASEADDR_OLD}; + + // more retries for detection + _retries = 10; + + for (uint8_t i=0; i<sizeof(addresses); i++) { + uint8_t val=0, who_am_i=0; + + // set the I2C bus address + set_address(addresses[i]); + + if (read_reg(LL40LS_WHO_AM_I_REG, who_am_i) == OK && who_am_i == LL40LS_WHO_AM_I_REG_VAL) { + // it is responding correctly to a WHO_AM_I + goto ok; + } + + if (read_reg(LL40LS_SIGNAL_STRENGTH_REG, val) == OK && val != 0) { + // very likely to be a ll40ls. px4flow does not + // respond to this + goto ok; + } + + debug("WHO_AM_I byte mismatch 0x%02x should be 0x%02x val=0x%02x\n", + (unsigned)who_am_i, + LL40LS_WHO_AM_I_REG_VAL, + (unsigned)val); + } + + // not found on any address + return -EIO; + +ok: + _retries = 3; + + // start a measurement return measure(); } @@ -521,6 +575,8 @@ LL40LS::collect() float si_units = distance * 0.01f; /* cm to m */ struct range_finder_report report; + _last_distance = distance; + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); @@ -648,6 +704,8 @@ LL40LS::print_info() perf_print_counter(_buffer_overflows); printf("poll interval: %u ticks\n", _measure_ticks); _reports->print_info("report queue"); + printf("distance: %ucm (0x%04x)\n", + (unsigned)_last_distance, (unsigned)_last_distance); } /** @@ -662,55 +720,89 @@ namespace ll40ls #endif const int ERROR = -1; -LL40LS *g_dev; +LL40LS *g_dev_int; +LL40LS *g_dev_ext; -void start(); -void stop(); -void test(); -void reset(); -void info(); +void start(int bus); +void stop(int bus); +void test(int bus); +void reset(int bus); +void info(int bus); +void usage(); /** * Start the driver. */ void -start() +start(int bus) { - int fd; - - if (g_dev != nullptr) { - errx(1, "already started"); + /* create the driver, attempt expansion bus first */ + if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) { + if (g_dev_ext != nullptr) + errx(0, "already started external"); + g_dev_ext = new LL40LS(PX4_I2C_BUS_EXPANSION, LL40LS_DEVICE_PATH_EXT); + if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { + delete g_dev_ext; + g_dev_ext = nullptr; + } } - /* create the driver */ - g_dev = new LL40LS(LL40LS_BUS); - - if (g_dev == nullptr) { - goto fail; - } +#ifdef PX4_I2C_BUS_ONBOARD + /* if this failed, attempt onboard sensor */ + if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) { + if (g_dev_int != nullptr) + errx(0, "already started internal"); + g_dev_int = new LL40LS(PX4_I2C_BUS_ONBOARD, LL40LS_DEVICE_PATH_INT); + if (g_dev_int != nullptr && OK != g_dev_int->init()) { + /* tear down the failing onboard instance */ + delete g_dev_int; + g_dev_int = nullptr; - if (OK != g_dev->init()) { - goto fail; + if (bus == PX4_I2C_BUS_ONBOARD) { + goto fail; + } + } + if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) { + goto fail; + } } +#endif /* set the poll rate to default, starts automatic data collection */ - fd = open(LL40LS_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - goto fail; - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - goto fail; - } + if (g_dev_int != nullptr) { + int fd = open(LL40LS_DEVICE_PATH_INT, O_RDONLY); + if (fd == -1) { + goto fail; + } + int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); + close(fd); + if (ret < 0) { + goto fail; + } + } + + if (g_dev_ext != nullptr) { + int fd = open(LL40LS_DEVICE_PATH_EXT, O_RDONLY); + if (fd == -1) { + goto fail; + } + int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); + close(fd); + if (ret < 0) { + goto fail; + } + } exit(0); fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; + if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) { + delete g_dev_int; + g_dev_int = nullptr; + } + if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) { + delete g_dev_ext; + g_dev_ext = nullptr; } errx(1, "driver start failed"); @@ -719,11 +811,12 @@ fail: /** * Stop the driver */ -void stop() +void stop(int bus) { - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; + LL40LS **g_dev = (bus == PX4_I2C_BUS_ONBOARD?&g_dev_int:&g_dev_ext); + if (*g_dev != nullptr) { + delete *g_dev; + *g_dev = nullptr; } else { errx(1, "driver not running"); @@ -738,16 +831,17 @@ void stop() * and automatic modes. */ void -test() +test(int bus) { struct range_finder_report report; ssize_t sz; int ret; + const char *path = (bus==PX4_I2C_BUS_ONBOARD?LL40LS_DEVICE_PATH_INT:LL40LS_DEVICE_PATH_EXT); - int fd = open(LL40LS_DEVICE_PATH, O_RDONLY); + int fd = open(path, O_RDONLY); if (fd < 0) { - err(1, "%s open failed (try 'll40ls start' if the driver is not running", LL40LS_DEVICE_PATH); + err(1, "%s open failed (try 'll40ls start' if the driver is not running", path); } /* do a simple demand read */ @@ -803,9 +897,10 @@ test() * Reset the driver. */ void -reset() +reset(int bus) { - int fd = open(LL40LS_DEVICE_PATH, O_RDONLY); + const char *path = (bus==PX4_I2C_BUS_ONBOARD?LL40LS_DEVICE_PATH_INT:LL40LS_DEVICE_PATH_EXT); + int fd = open(path, O_RDONLY); if (fd < 0) { err(1, "failed "); @@ -826,8 +921,9 @@ reset() * Print a little info about the driver. */ void -info() +info(int bus) { + LL40LS *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext); if (g_dev == nullptr) { errx(1, "driver not running"); } @@ -838,44 +934,76 @@ info() exit(0); } +void +usage() +{ + warnx("missing command: try 'start', 'stop', 'info', 'test', 'reset', 'info'"); + warnx("options:"); + warnx(" -X only external bus"); +#ifdef PX4_I2C_BUS_ONBOARD + warnx(" -I only internal bus"); +#endif +} + } // namespace int ll40ls_main(int argc, char *argv[]) { + int ch; + int bus = -1; + + while ((ch = getopt(argc, argv, "XI")) != EOF) { + switch (ch) { +#ifdef PX4_I2C_BUS_ONBOARD + case 'I': + bus = PX4_I2C_BUS_ONBOARD; + break; +#endif + case 'X': + bus = PX4_I2C_BUS_EXPANSION; + break; + default: + ll40ls::usage(); + exit(0); + } + } + + const char *verb = argv[optind]; + /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) { - ll40ls::start(); + if (!strcmp(verb, "start")) { + ll40ls::start(bus); } /* * Stop the driver */ - if (!strcmp(argv[1], "stop")) { - ll40ls::stop(); + if (!strcmp(verb, "stop")) { + ll40ls::stop(bus); } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) { - ll40ls::test(); + if (!strcmp(verb, "test")) { + ll40ls::test(bus); } /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) { - ll40ls::reset(); + if (!strcmp(verb, "reset")) { + ll40ls::reset(bus); } /* * Print driver information. */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { - ll40ls::info(); + if (!strcmp(verb, "info") || !strcmp(verb, "status")) { + ll40ls::info(bus); } errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index b22bb2e07..a95e041a1 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -229,6 +229,7 @@ private: perf_counter_t _gyro_reads; perf_counter_t _sample_perf; perf_counter_t _bad_transfers; + perf_counter_t _good_transfers; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -404,6 +405,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")), _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")), _bad_transfers(perf_alloc(PC_COUNT, "mpu6000_bad_transfers")), + _good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -456,6 +458,7 @@ MPU6000::~MPU6000() perf_free(_accel_reads); perf_free(_gyro_reads); perf_free(_bad_transfers); + perf_free(_good_transfers); } int @@ -1279,8 +1282,14 @@ MPU6000::measure() // all zero data - probably a SPI bus error perf_count(_bad_transfers); perf_end(_sample_perf); + // note that we don't call reset() here as a reset() + // costs 20ms with interrupts disabled. That means if + // the mpu6k does go bad it would cause a FMU failure, + // regardless of whether another sensor is available, return; } + + perf_count(_good_transfers); /* @@ -1399,6 +1408,8 @@ MPU6000::print_info() perf_print_counter(_sample_perf); perf_print_counter(_accel_reads); perf_print_counter(_gyro_reads); + perf_print_counter(_bad_transfers); + perf_print_counter(_good_transfers); _accel_reports->print_info("accel queue"); _gyro_reports->print_info("gyro queue"); } diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 60ad3c1af..6c68636c6 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -76,7 +76,7 @@ #define PX4FLOW_BUS PX4_I2C_BUS_EXPANSION #define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84 //range 0x42 - 0x49 - + /* PX4FLOW Registers addresses */ #define PX4FLOW_REG 0x00 /* Measure Register */ @@ -212,8 +212,9 @@ PX4FLOW::~PX4FLOW() stop(); /* free any existing reports */ - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } } int @@ -222,22 +223,25 @@ PX4FLOW::init() int ret = ERROR; /* do I2C init (and probe) first */ - if (I2C::init() != OK) + if (I2C::init() != OK) { goto out; + } /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(struct optical_flow_s)); - if (_reports == nullptr) + if (_reports == nullptr) { goto out; + } /* get a publish handle on the px4flow topic */ struct optical_flow_s zero_report; memset(&zero_report, 0, sizeof(zero_report)); _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report); - if (_px4flow_topic < 0) + if (_px4flow_topic < 0) { debug("failed to create px4flow object. Did you start uOrb?"); + } ret = OK; /* sensor is ok, but we don't really know if it is within range */ @@ -249,6 +253,17 @@ out: int PX4FLOW::probe() { + uint8_t val[22]; + + // to be sure this is not a ll40ls Lidar (which can also be on + // 0x42) we check if a 22 byte transfer works from address + // 0. The ll40ls gives an error for that, whereas the flow + // happily returns some data + if (transfer(nullptr, 0, &val[0], 22) != OK) { + return -EIO; + } + + // that worked, so start a measurement cycle return measure(); } @@ -260,20 +275,20 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _measure_ticks = 0; return OK; - /* external signalling (DRDY) not supported */ + /* external signalling (DRDY) not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ @@ -283,13 +298,14 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) _measure_ticks = USEC2TICK(PX4FLOW_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_measure_ticks == 0); @@ -298,15 +314,17 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) + if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) { return -EINVAL; + } /* update interval for next measurement */ _measure_ticks = ticks; /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } @@ -314,25 +332,29 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) + if (_measure_ticks == 0) { return SENSOR_POLLRATE_MANUAL; + } return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqstate_t flags = irqsave(); - if (!_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; - } - irqrestore(flags); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _reports->size(); @@ -355,8 +377,9 @@ PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_measure_ticks > 0) { @@ -417,13 +440,13 @@ PX4FLOW::measure() uint8_t cmd = PX4FLOW_REG; ret = transfer(&cmd, 1, nullptr, 0); - if (OK != ret) - { + if (OK != ret) { perf_count(_comms_errors); log("i2c::transfer returned %d", ret); - printf("i2c::transfer flow returned %d"); + printf("i2c::transfer flow returned %d"); return ret; } + ret = OK; return ret; @@ -435,14 +458,13 @@ PX4FLOW::collect() int ret = -EIO; /* read from the sensor */ - uint8_t val[22] = {0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0}; + uint8_t val[22] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; perf_begin(_sample_perf); ret = transfer(nullptr, 0, &val[0], 22); - if (ret < 0) - { + if (ret < 0) { log("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); @@ -467,12 +489,12 @@ PX4FLOW::collect() int16_t gdist = val[21] << 8 | val[20]; struct optical_flow_s report; - report.flow_comp_x_m = float(flowcx)/1000.0f; - report.flow_comp_y_m = float(flowcy)/1000.0f; - report.flow_raw_x= val[3] << 8 | val[2]; - report.flow_raw_y= val[5] << 8 | val[4]; - report.ground_distance_m =float(gdist)/1000.0f; - report.quality= val[10]; + report.flow_comp_x_m = float(flowcx) / 1000.0f; + report.flow_comp_y_m = float(flowcy) / 1000.0f; + report.flow_raw_x = val[3] << 8 | val[2]; + report.flow_raw_y = val[5] << 8 | val[4]; + report.ground_distance_m = float(gdist) / 1000.0f; + report.quality = val[10]; report.sensor_id = 0; report.timestamp = hrt_absolute_time(); @@ -509,11 +531,13 @@ PX4FLOW::start() true, true, true, - SUBSYSTEM_TYPE_OPTICALFLOW}; + SUBSYSTEM_TYPE_OPTICALFLOW + }; static orb_advert_t pub = -1; if (pub > 0) { orb_publish(ORB_ID(subsystem_info), pub, &info); + } else { pub = orb_advertise(ORB_ID(subsystem_info), &info); } @@ -567,8 +591,9 @@ PX4FLOW::cycle() } /* measurement phase */ - if (OK != measure()) + if (OK != measure()) { log("measure error"); + } /* next phase is collection */ _collect_phase = true; @@ -619,33 +644,37 @@ start() { int fd; - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(1, "already started"); + } /* create the driver */ g_dev = new PX4FLOW(PX4FLOW_BUS); - if (g_dev == nullptr) + if (g_dev == nullptr) { goto fail; + } - if (OK != g_dev->init()) + if (OK != g_dev->init()) { goto fail; + } /* set the poll rate to default, starts automatic data collection */ fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) { goto fail; + } exit(0); fail: - if (g_dev != nullptr) - { + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } @@ -658,15 +687,14 @@ fail: */ void stop() { - if (g_dev != nullptr) - { + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; - } - else - { + + } else { errx(1, "driver not running"); } + exit(0); } @@ -684,14 +712,17 @@ test() int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "%s open failed (try 'px4flow start' if the driver is not running", PX4FLOW_DEVICE_PATH); + } /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); if (sz != sizeof(report)) - // err(1, "immediate read failed"); + { + warnx("immediate read failed"); + } warnx("single read"); warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m); @@ -700,8 +731,9 @@ test() /* start the sensor polling at 2Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { errx(1, "failed to set 2Hz poll rate"); + } /* read the sensor 5x and report each value */ for (unsigned i = 0; i < 5; i++) { @@ -712,14 +744,16 @@ test() fds.events = POLLIN; ret = poll(&fds, 1, 2000); - if (ret != 1) + if (ret != 1) { errx(1, "timed out waiting for sensor data"); + } /* now go get it */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) + if (sz != sizeof(report)) { err(1, "periodic read failed"); + } warnx("periodic read %u", i); warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m); @@ -740,14 +774,17 @@ reset() { int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "driver reset failed"); + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "driver poll restart failed"); + } exit(0); } @@ -758,8 +795,9 @@ reset() void info() { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver not running"); + } printf("state @ %p\n", g_dev); g_dev->print_info(); @@ -775,32 +813,37 @@ px4flow_main(int argc, char *argv[]) /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) + if (!strcmp(argv[1], "start")) { px4flow::start(); + } - /* - * Stop the driver - */ - if (!strcmp(argv[1], "stop")) - px4flow::stop(); + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) { + px4flow::stop(); + } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { px4flow::test(); + } /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(argv[1], "reset")) { px4flow::reset(); + } /* * Print driver information. */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { px4flow::info(); + } errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); } diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 122a3cd17..3d3e1b0eb 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -829,6 +829,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_SET_ARM_OK: case PWM_SERVO_CLEAR_ARM_OK: case PWM_SERVO_SET_FORCE_SAFETY_OFF: + case PWM_SERVO_SET_FORCE_SAFETY_ON: // these are no-ops, as no safety switch break; diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index d212be766..6d68d9f60 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1247,28 +1247,40 @@ PX4IO::io_set_rc_config() */ param_get(param_find("RC_MAP_ROLL"), &ichan); + /* subtract one from 1-based index - this might be + * a negative number now + */ + ichan -= 1; + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan - 1] = 0; + input_map[ichan] = 0; param_get(param_find("RC_MAP_PITCH"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan - 1] = 1; + input_map[ichan] = 1; param_get(param_find("RC_MAP_YAW"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan - 1] = 2; + input_map[ichan] = 2; param_get(param_find("RC_MAP_THROTTLE"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan - 1] = 3; + input_map[ichan] = 3; - param_get(param_find("RC_MAP_MODE_SW"), &ichan); + param_get(param_find("RC_MAP_FLAPS"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan - 1] = 4; + input_map[ichan] = 4; + + param_get(param_find("RC_MAP_MODE_SW"), &ichan); + + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) { + /* use out of normal bounds index to indicate special channel */ + input_map[ichan] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH; + } /* * Iterate all possible RC inputs. @@ -2271,6 +2283,11 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC); break; + case PWM_SERVO_SET_FORCE_SAFETY_ON: + /* force safety switch on */ + ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC); + break; + case PWM_SERVO_SET_FORCE_FAILSAFE: /* force failsafe mode instantly */ if (arg == 0) { diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 5187b448f..917c7f830 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -74,6 +74,7 @@ enum Rotation { ROTATION_ROLL_270_YAW_135 = 23, ROTATION_PITCH_90 = 24, ROTATION_PITCH_270 = 25, + ROTATION_ROLL_270_YAW_270 = 26, ROTATION_MAX }; @@ -109,7 +110,8 @@ const rot_lookup_t rot_lookup[] = { {270, 0, 90 }, {270, 0, 135 }, { 0, 90, 0 }, - { 0, 270, 0 } + { 0, 270, 0 }, + {270, 0, 270 } }; /** diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 6a2a61b04..0ed210cf4 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -299,7 +299,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM // Calculate throttle demand // If underspeed condition is set, then demand full throttle if (_underspeed) { - _throttle_dem_unc = 1.0f; + _throttle_dem = 1.0f; } else { // Calculate gain scaler from specific energy error to throttle @@ -363,10 +363,10 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM } else { _throttle_dem = ff_throttle; } - } - // Constrain throttle demand - _throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf); + // Constrain throttle demand + _throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf); + } } void TECS::_detect_bad_descent(void) diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 8ac31de6f..eb45237b7 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -345,9 +345,6 @@ private: // climbout mode bool _climbOutDem; - // throttle demand before limiting - float _throttle_dem_unc; - // pitch demand before limiting float _pitch_dem_unc; diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 7a4e7a766..0cb41489f 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -83,7 +83,7 @@ * | accel_T[1][i] | * [ accel_T[2][i] ] * - * b = [ accel_corr_ref[0][i] ] // One measurement per axis is enough + * b = [ accel_corr_ref[0][i] ] // One measurement per side is enough * | accel_corr_ref[2][i] | * [ accel_corr_ref[4][i] ] * @@ -162,6 +162,11 @@ int do_accel_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_log_info(mavlink_fd, "You need to put the system on all six sides"); + sleep(3); + mavlink_log_info(mavlink_fd, "Follow the instructions on the screen"); + sleep(5); + struct accel_scale accel_scale = { 0.0f, 1.0f, @@ -258,7 +263,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float const int samples_num = 2500; float accel_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; - const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" }; + const char *orientation_strs[6] = { "front", "back", "left", "right", "top", "bottom" }; int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -287,29 +292,37 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float break; } - mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", - (!data_collected[0]) ? "x+ " : "", - (!data_collected[1]) ? "x- " : "", - (!data_collected[2]) ? "y+ " : "", - (!data_collected[3]) ? "y- " : "", - (!data_collected[4]) ? "z+ " : "", - (!data_collected[5]) ? "z- " : ""); + /* inform user which axes are still needed */ + mavlink_log_info(mavlink_fd, "pending: %s%s%s%s%s%s", + (!data_collected[0]) ? "front " : "", + (!data_collected[1]) ? "back " : "", + (!data_collected[2]) ? "left " : "", + (!data_collected[3]) ? "right " : "", + (!data_collected[4]) ? "up " : "", + (!data_collected[5]) ? "down " : ""); + + /* allow user enough time to read the message */ + sleep(3); int orient = detect_orientation(mavlink_fd, sensor_combined_sub); if (orient < 0) { - res = ERROR; - break; + mavlink_log_info(mavlink_fd, "invalid motion, hold still..."); + sleep(3); + continue; } + /* inform user about already handled side */ if (data_collected[orient]) { - mavlink_log_info(mavlink_fd, "%s done, rotate to a different axis", orientation_strs[orient]); + mavlink_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]); + sleep(4); continue; } - mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]); + mavlink_log_info(mavlink_fd, "Hold still, starting to measure %s side", orientation_strs[orient]); + sleep(1); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); - mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient], + mavlink_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", orientation_strs[orient], (double)accel_ref[orient][0], (double)accel_ref[orient][1], (double)accel_ref[orient][2]); @@ -400,7 +413,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) /* is still now */ if (t_still == 0) { /* first time */ - mavlink_log_info(mavlink_fd, "detected rest position, waiting..."); + mavlink_log_info(mavlink_fd, "detected rest position, hold still..."); t_still = t; t_timeout = t + timeout; @@ -418,6 +431,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) /* not still, reset still start time */ if (t_still != 0) { mavlink_log_info(mavlink_fd, "detected motion, hold still..."); + sleep(3); t_still = 0; } } diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 339b11bbe..cae1d7684 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -61,6 +61,15 @@ static const int ERROR = -1; static const char *sensor_name = "dpress"; +#define HUMAN_ASPD_CAL_FAILED_MSG "Calibration failed, see http://px4.io/help/aspd" + +static void feedback_calibration_failed(int mavlink_fd) +{ + sleep(5); + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + mavlink_log_critical(mavlink_fd, HUMAN_ASPD_CAL_FAILED_MSG); +} + int do_airspeed_calibration(int mavlink_fd) { /* give directions */ @@ -99,7 +108,7 @@ int do_airspeed_calibration(int mavlink_fd) float analog_scaling = 0.0f; param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)); if (fabsf(analog_scaling) < 0.1f) { - mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]"); + mavlink_log_critical(mavlink_fd, "No airspeed sensor, see http://px4.io/help/aspd"); close(diff_pres_sub); return ERROR; } @@ -138,7 +147,7 @@ int do_airspeed_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ - mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); close(diff_pres_sub); return ERROR; } @@ -175,7 +184,7 @@ int do_airspeed_calibration(int mavlink_fd) } } else { - mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); close(diff_pres_sub); return ERROR; } @@ -207,7 +216,7 @@ int do_airspeed_calibration(int mavlink_fd) if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { if (calibration_counter % 500 == 0) { - mavlink_log_info(mavlink_fd, "Create airflow! (%d, wanted: 50 Pa)", + mavlink_log_info(mavlink_fd, "Create air pressure! (got %d, wanted: 50 Pa)", (int)diff_pres.differential_pressure_raw_pa); } continue; @@ -215,9 +224,9 @@ int do_airspeed_calibration(int mavlink_fd) /* do not allow negative values */ if (diff_pres.differential_pressure_raw_pa < 0.0f) { - mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!"); mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)", (int)diff_pres.differential_pressure_raw_pa); + mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!"); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -235,7 +244,7 @@ int do_airspeed_calibration(int mavlink_fd) close(diff_pres_sub); - mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); return ERROR; } else { mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)", @@ -245,14 +254,14 @@ int do_airspeed_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ - mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); close(diff_pres_sub); return ERROR; } } if (calibration_counter == maxcount) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); close(diff_pres_sub); return ERROR; } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 616b855df..46caddd46 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -207,7 +207,9 @@ void usage(const char *reason); /** * React to commands that are sent e.g. from the mavlink module. */ -bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub); +bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, + struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, + orb_advert_t *home_pub); /** * Mainloop of commander. @@ -230,7 +232,8 @@ void print_reject_arm(const char *msg); void print_status(); -transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); +transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, + struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy); @@ -260,7 +263,7 @@ int commander_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 2950, + 3200, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); @@ -393,7 +396,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char // Transition the armed state. By passing mavlink_fd to arming_state_transition it will // output appropriate error messages if the state cannot transition. - arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd_local); + arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, + true /* fRunPreArmChecks */, mavlink_fd_local); if (arming_res == TRANSITION_CHANGED && mavlink_fd) { mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); @@ -406,11 +410,12 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char } bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local, - struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local, - struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) + struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local, + struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) { /* only handle commands that are meant to be handled by this system and component */ - if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components + if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) + && (cmd->target_component != 0))) { // component_id 0: valid for all components return false; } @@ -537,13 +542,13 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } else { mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f", - (double)cmd->param1, - (double)cmd->param2, - (double)cmd->param3, - (double)cmd->param4, - (double)cmd->param5, - (double)cmd->param6, - (double)cmd->param7); + (double)cmd->param1, + (double)cmd->param2, + (double)cmd->param3, + (double)cmd->param4, + (double)cmd->param5, + (double)cmd->param6, + (double)cmd->param7); } } break; @@ -554,35 +559,43 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s //XXX update state machine? armed_local->force_failsafe = true; warnx("forcing failsafe (termination)"); + } else { armed_local->force_failsafe = false; warnx("disabling failsafe (termination)"); } + /* param2 is currently used for other failsafe modes */ status_local->engine_failure_cmd = false; status_local->data_link_lost_cmd = false; status_local->gps_failure_cmd = false; status_local->rc_signal_lost_cmd = false; + if ((int)cmd->param2 <= 0) { /* reset all commanded failure modes */ warnx("reset all non-flighttermination failsafe commands"); + } else if ((int)cmd->param2 == 1) { /* trigger engine failure mode */ status_local->engine_failure_cmd = true; warnx("engine failure mode commanded"); + } else if ((int)cmd->param2 == 2) { /* trigger data link loss mode */ status_local->data_link_lost_cmd = true; warnx("data link loss mode commanded"); + } else if ((int)cmd->param2 == 3) { /* trigger gps loss mode */ status_local->gps_failure_cmd = true; warnx("gps loss mode commanded"); + } else if ((int)cmd->param2 == 4) { /* trigger rc loss mode */ status_local->rc_signal_lost_cmd = true; warnx("rc loss mode commanded"); } + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } break; @@ -633,21 +646,27 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } } break; + case VEHICLE_CMD_NAV_GUIDED_ENABLE: { transition_result_t res = TRANSITION_DENIED; static main_state_t main_state_pre_offboard = MAIN_STATE_MANUAL; + if (status_local->main_state != MAIN_STATE_OFFBOARD) { main_state_pre_offboard = status_local->main_state; } + if (cmd->param1 > 0.5f) { res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); + if (res == TRANSITION_DENIED) { print_reject_mode(status_local, "OFFBOARD"); status_local->offboard_control_set_by_command = false; + } else { /* Set flag that offboard was set via command, main state is not overridden by rc */ status_local->offboard_control_set_by_command = true; } + } else { /* If the mavlink command is used to enable or disable offboard control: * switch back to previous mode when disabling */ @@ -656,6 +675,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } } break; + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: case VEHICLE_CMD_PREFLIGHT_CALIBRATION: case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: @@ -802,6 +822,7 @@ int commander_thread_main(int argc, char *argv[]) /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); + if (status_pub < 0) { warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); warnx("exiting."); @@ -827,11 +848,14 @@ int commander_thread_main(int argc, char *argv[]) /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ orb_advert_t mission_pub = -1; mission_s mission; + if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { if (mission.dataman_id >= 0 && mission.dataman_id <= 1) { - warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, mission.current_seq); + warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, + mission.current_seq); mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d", - mission.dataman_id, mission.count, mission.current_seq); + mission.dataman_id, mission.count, mission.current_seq); + } else { const char *missionfail = "reading mission state failed"; warnx("%s", missionfail); @@ -1097,6 +1121,7 @@ int commander_thread_main(int argc, char *argv[]) status.offboard_control_signal_lost = false; status_changed = true; } + } else { if (!status.offboard_control_signal_lost) { status.offboard_control_signal_lost = true; @@ -1115,9 +1140,9 @@ int commander_thread_main(int argc, char *argv[]) /* perform system checks when new telemetry link connected */ if (mavlink_fd && - telemetry_last_heartbeat[i] == 0 && - telemetry.heartbeat_time > 0 && - hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) { + telemetry_last_heartbeat[i] == 0 && + telemetry.heartbeat_time > 0 && + hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) { (void)rc_calibration_check(mavlink_fd); } @@ -1130,6 +1155,7 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + /* Check if the barometer is healthy and issue a warning in the GCS if not so. * Because the barometer is used for calculating AMSL altitude which is used to ensure * vertical separation from other airtraffic the operator has to know when the @@ -1142,6 +1168,7 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; mavlink_log_critical(mavlink_fd, "baro healthy"); } + } else { if (!status.barometer_failure) { status.barometer_failure = true; @@ -1164,10 +1191,11 @@ int commander_thread_main(int argc, char *argv[]) if (hrt_elapsed_time(&system_power.timestamp) < 200000) { if (system_power.servo_valid && - !system_power.brick_valid && - !system_power.usb_connected) { + !system_power.brick_valid && + !system_power.usb_connected) { /* flying only on servo rail, this is unsafe */ status.condition_power_input_valid = false; + } else { status.condition_power_input_valid = true; } @@ -1187,9 +1215,11 @@ int commander_thread_main(int argc, char *argv[]) /* disarm if safety is now on and still armed */ if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { - arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : + ARMING_STATE_STANDBY_ERROR); - if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd)) { + if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, + true /* fRunPreArmChecks */, mavlink_fd)) { mavlink_log_info(mavlink_fd, "DISARMED by safety switch"); arming_state_changed = true; } @@ -1233,7 +1263,8 @@ int commander_thread_main(int argc, char *argv[]) } } - check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed); + check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), + &status_changed); /* update home position */ if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && @@ -1283,8 +1314,11 @@ int commander_thread_main(int argc, char *argv[]) local_eph_good = false; } } - check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && local_eph_good, &(status.condition_local_position_valid), &status_changed); - check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); + + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid + && local_eph_good, &(status.condition_local_position_valid), &status_changed); + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, + &(status.condition_local_altitude_valid), &status_changed); if (status.condition_local_altitude_valid) { if (status.condition_landed != local_position.landed) { @@ -1315,7 +1349,8 @@ int commander_thread_main(int argc, char *argv[]) /* get throttle (if armed), as we only care about energy negative throttle also counts */ float throttle = (armed.armed) ? fabsf(actuator_controls.control[3]) : 0.0f; - status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah, throttle); + status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah, + throttle); } } @@ -1372,8 +1407,8 @@ int commander_thread_main(int argc, char *argv[]) last_idle_time = system_load.tasks[0].total_runtime; /* check if board is connected via USB */ - //struct stat statbuf; - //on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0); + struct stat statbuf; + on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0); } /* if battery voltage is getting lower, warn using buzzer, etc. */ @@ -1383,26 +1418,30 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; - } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.09f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { + } else if (!on_usb_power && status.condition_battery_voltage_valid && status.battery_remaining < 0.09f + && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, + mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } } else { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, + mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } } + status_changed = true; } @@ -1411,7 +1450,8 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { /* TODO: check for sensors */ - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, + mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -1439,23 +1479,25 @@ int commander_thread_main(int argc, char *argv[]) /* Initialize map projection if gps is valid */ if (!map_projection_global_initialized() - && (gps_position.eph < eph_threshold) - && (gps_position.epv < epv_threshold) - && hrt_elapsed_time((hrt_abstime*)&gps_position.timestamp_position) < 1e6) { + && (gps_position.eph < eph_threshold) + && (gps_position.epv < epv_threshold) + && hrt_elapsed_time((hrt_abstime *)&gps_position.timestamp_position) < 1e6) { /* set reference for global coordinates <--> local coordiantes conversion and map_projection */ - globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); + globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, + (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); } /* check if GPS fix is ok */ if (status.circuit_breaker_engaged_gpsfailure_check || - (gps_position.fix_type >= 3 && - hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) { + (gps_position.fix_type >= 3 && + hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) { /* handle the case where gps was regained */ if (status.gps_failure) { status.gps_failure = false; status_changed = true; mavlink_log_critical(mavlink_fd, "gps regained"); } + } else { if (!status.gps_failure) { status.gps_failure = true; @@ -1477,12 +1519,14 @@ int commander_thread_main(int argc, char *argv[]) armed.force_failsafe = true; status_changed = true; static bool flight_termination_printed = false; + if (!flight_termination_printed) { warnx("Flight termination because of navigator request or geofence"); mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); flight_termination_printed = true; } - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); } } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination @@ -1490,7 +1534,7 @@ int commander_thread_main(int argc, char *argv[]) /* RC input check */ if (!status.rc_input_blocked && sp_man.timestamp != 0 && - hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { + hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; @@ -1515,11 +1559,15 @@ int commander_thread_main(int argc, char *argv[]) if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ - arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd); + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : + ARMING_STATE_STANDBY_ERROR); + arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, + mavlink_fd); + if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } + stick_off_counter = 0; } else { @@ -1541,8 +1589,11 @@ int commander_thread_main(int argc, char *argv[]) */ if (status.main_state != MAIN_STATE_MANUAL) { print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); + } else { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, + mavlink_fd); + if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } @@ -1565,6 +1616,7 @@ int commander_thread_main(int argc, char *argv[]) } else { mavlink_log_info(mavlink_fd, "DISARMED by RC"); } + arming_state_changed = true; } else if (arming_ret == TRANSITION_DENIED) { @@ -1600,18 +1652,20 @@ int commander_thread_main(int argc, char *argv[]) /* data links check */ bool have_link = false; + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { if (telemetry_last_heartbeat[i] != 0 && - hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) { + hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) { /* handle the case where data link was regained, * accept datalink as healthy only after datalink_regain_timeout seconds * */ if (telemetry_lost[i] && - hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) { + hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) { mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; have_link = true; + } else if (!telemetry_lost[i]) { /* telemetry was healthy also in last iteration * we don't have to check a timeout */ @@ -1620,6 +1674,7 @@ int commander_thread_main(int argc, char *argv[]) } else { telemetry_last_dl_loss[i] = hrt_absolute_time(); + if (!telemetry_lost[i]) { mavlink_log_critical(mavlink_fd, "data link %i lost", i); telemetry_lost[i] = true; @@ -1647,24 +1702,26 @@ int commander_thread_main(int argc, char *argv[]) * only for fixed wing for now */ if (!status.circuit_breaker_engaged_enginefailure_check && - status.is_rotary_wing == false && - armed.armed && - ((actuator_controls.control[3] > ef_throttle_thres && - battery.current_a/actuator_controls.control[3] < - ef_current2throttle_thres) || - (status.engine_failure))) { + status.is_rotary_wing == false && + armed.armed && + ((actuator_controls.control[3] > ef_throttle_thres && + battery.current_a / actuator_controls.control[3] < + ef_current2throttle_thres) || + (status.engine_failure))) { /* potential failure, measure time */ if (timestamp_engine_healthy > 0 && - hrt_elapsed_time(×tamp_engine_healthy) > - ef_time_thres * 1e6 && - !status.engine_failure) { - status.engine_failure = true; - status_changed = true; - mavlink_log_critical(mavlink_fd, "Engine Failure"); + hrt_elapsed_time(×tamp_engine_healthy) > + ef_time_thres * 1e6 && + !status.engine_failure) { + status.engine_failure = true; + status_changed = true; + mavlink_log_critical(mavlink_fd, "Engine Failure"); } + } else { /* no failure reset flag */ timestamp_engine_healthy = hrt_absolute_time(); + if (status.engine_failure) { status.engine_failure = false; status_changed = true; @@ -1691,20 +1748,22 @@ int commander_thread_main(int argc, char *argv[]) * If we are not in a manual (RC stick controlled mode) * and both failed we want to terminate the flight */ if (status.main_state != MAIN_STATE_MANUAL && - status.main_state != MAIN_STATE_ACRO && - status.main_state != MAIN_STATE_ALTCTL && - status.main_state != MAIN_STATE_POSCTL && - ((status.data_link_lost && status.gps_failure) || - (status.data_link_lost_cmd && status.gps_failure_cmd))) { + status.main_state != MAIN_STATE_ACRO && + status.main_state != MAIN_STATE_ALTCTL && + status.main_state != MAIN_STATE_POSCTL && + ((status.data_link_lost && status.gps_failure) || + (status.data_link_lost_cmd && status.gps_failure_cmd))) { armed.force_failsafe = true; status_changed = true; static bool flight_termination_printed = false; + if (!flight_termination_printed) { warnx("Flight termination because of data link loss && gps failure"); mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); flight_termination_printed = true; } - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); } } @@ -1713,19 +1772,21 @@ int commander_thread_main(int argc, char *argv[]) * If we are in manual (controlled with RC): * if both failed we want to terminate the flight */ if ((status.main_state == MAIN_STATE_ACRO || - status.main_state == MAIN_STATE_MANUAL || - status.main_state == MAIN_STATE_ALTCTL || - status.main_state == MAIN_STATE_POSCTL) && - ((status.rc_signal_lost && status.gps_failure) || - (status.rc_signal_lost_cmd && status.gps_failure_cmd))) { + status.main_state == MAIN_STATE_MANUAL || + status.main_state == MAIN_STATE_ALTCTL || + status.main_state == MAIN_STATE_POSCTL) && + ((status.rc_signal_lost && status.gps_failure) || + (status.rc_signal_lost_cmd && status.gps_failure_cmd))) { armed.force_failsafe = true; status_changed = true; static bool flight_termination_printed = false; + if (!flight_termination_printed) { warnx("Flight termination because of RC signal loss && gps failure"); flight_termination_printed = true; } - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); } } @@ -1766,6 +1827,7 @@ int commander_thread_main(int argc, char *argv[]) /* mark home position as set */ status.condition_home_position_valid = true; } + arming_state_changed = false; } @@ -1810,7 +1872,8 @@ int commander_thread_main(int argc, char *argv[]) } /* play arming and battery warning tunes */ - if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) { + if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available + && safety.safety_off))) { /* play tune when armed */ set_tune(TONE_ARMING_WARNING_TUNE); arm_tune_played = true; @@ -1988,6 +2051,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s /* offboard switch overrides main switch */ if (sp_man->offboard_switch == SWITCH_POS_ON) { res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); + if (res == TRANSITION_DENIED) { print_reject_mode(status_local, "OFFBOARD"); @@ -2009,6 +2073,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s } else { res = main_state_transition(status_local, MAIN_STATE_MANUAL); } + // TRANSITION_DENIED is not possible here break; @@ -2023,7 +2088,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s print_reject_mode(status_local, "POSCTL"); } - // fallback to ALTCTL + // fallback to ALTCTL res = main_state_transition(status_local, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { @@ -2047,14 +2112,14 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s break; // changed successfully or already in this state } - print_reject_mode(status_local, "AUTO_RTL"); + print_reject_mode(status_local, "AUTO_RTL"); - // fallback to LOITER if home position not set - res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); + // fallback to LOITER if home position not set + res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } } else if (sp_man->loiter_switch == SWITCH_POS_ON) { res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); @@ -2063,7 +2128,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s break; // changed successfully or already in this state } - print_reject_mode(status_local, "AUTO_LOITER"); + print_reject_mode(status_local, "AUTO_LOITER"); } else { res = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); @@ -2072,22 +2137,22 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s break; // changed successfully or already in this state } - print_reject_mode(status_local, "AUTO_MISSION"); + print_reject_mode(status_local, "AUTO_MISSION"); - // fallback to LOITER if home position not set - res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); + // fallback to LOITER if home position not set + res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } } - // fallback to POSCTL - res = main_state_transition(status_local, MAIN_STATE_POSCTL); + // fallback to POSCTL + res = main_state_transition(status_local, MAIN_STATE_POSCTL); - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } // fallback to ALTCTL res = main_state_transition(status_local, MAIN_STATE_ALTCTL); @@ -2169,6 +2234,7 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; break; + case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; @@ -2177,6 +2243,7 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; break; + case OFFBOARD_CONTROL_MODE_DIRECT_FORCE: control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = false; @@ -2186,6 +2253,7 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; break; + case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED: case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED: case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED: @@ -2198,6 +2266,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = true; //XXX: the flags could depend on sp_offboard.ignore break; + default: control_mode.flag_control_rates_enabled = false; control_mode.flag_control_attitude_enabled = false; @@ -2206,6 +2275,7 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; } + break; case NAVIGATION_STATE_POSCTL: @@ -2415,7 +2485,8 @@ void *commander_low_prio_loop(void *arg) int calib_ret = ERROR; /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, true /* fRunPreArmChecks */, mavlink_fd)) { + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, + true /* fRunPreArmChecks */, mavlink_fd)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index d89c67c2b..8ab14dd52 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -63,7 +63,10 @@ static const char *sensor_name = "gyro"; int do_gyro_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - mavlink_log_info(mavlink_fd, "don't move system"); + mavlink_log_info(mavlink_fd, "HOLD STILL"); + + /* wait for the user to respond */ + sleep(2); struct gyro_scale gyro_scale = { 0.0f, diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 23900f386..7be8de9c6 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -155,7 +155,7 @@ int do_mag_calibration(int mavlink_fd) uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; unsigned poll_errcount = 0; - mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis"); + mavlink_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down"); calibration_counter = 0; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index a61346c2e..685f5e12f 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -597,12 +597,12 @@ FixedwingEstimator::check_filter_state() const char* const feedback[] = { 0, "NaN in states, resetting", - "stale IMU data, resetting", + "stale sensor data, resetting", "got initial position lock", "excessive gyro offsets", - "GPS velocity divergence", + "velocity diverted, check accel config", "excessive covariances", - "unknown condition"}; + "unknown condition, resetting"}; // Print out error condition if (check) { @@ -614,7 +614,7 @@ FixedwingEstimator::check_filter_state() } warnx("reset: %s", feedback[warn_index]); - mavlink_log_critical(_mavlink_fd, "[ekf] re-init: %s", feedback[warn_index]); + mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]); } struct estimator_status_report rep; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0d932d20a..6b4edff78 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -763,7 +763,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) _last_write_try_time = hrt_absolute_time(); /* check if there is space in the buffer, let it overflow else */ - if (buf_free < TX_BUFFER_GAP) { + if ((buf_free < TX_BUFFER_GAP) || (buf_free < packet_len)) { /* no enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); @@ -1634,7 +1634,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2700, + 2900, (main_t)&Mavlink::start_helper, (const char **)argv); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a730b6c98..fb110600c 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -341,11 +341,18 @@ private: /* do not allow top copying this class */ MavlinkStreamStatustext(MavlinkStreamStatustext &); MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &); + FILE *fp = nullptr; protected: explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink) {} + ~MavlinkStreamStatustext() { + if (fp) { + fclose(fp); + } + } + void send(const hrt_abstime t) { if (!mavlink_logbuffer_is_empty(_mavlink->get_logbuffer())) { @@ -359,6 +366,31 @@ protected: strncpy(msg.text, logmsg.text, sizeof(msg.text)); _mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg); + + /* write log messages in first instance to disk */ + if (_mavlink->get_instance_id() == 0) { + if (fp) { + fputs(msg.text, fp); + fputs("\n", fp); + fsync(fileno(fp)); + } else { + /* string to hold the path to the log */ + char log_file_name[32] = ""; + char log_file_path[64] = ""; + + timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ + time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); + struct tm t; + gmtime_r(&gps_time_sec, &t); + + // XXX we do not want to interfere here with the SD log app + strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &t); + snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name); + fp = fopen(log_file_path, "ab"); + } + } } } } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e8d783847..bc092c7e9 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -391,11 +391,9 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) f.timestamp = hrt_absolute_time(); f.flow_timestamp = flow.time_usec; - f.flow_raw_x = flow.flow_x; - f.flow_raw_y = flow.flow_y; - f.flow_comp_x_m = flow.flow_comp_m_x; - f.flow_comp_y_m = flow.flow_comp_m_y; - f.ground_distance_m = flow.ground_distance; + f.flow_raw_x = flow.integrated_x; + f.flow_raw_y = flow.integrated_y; + f.ground_distance_m = flow.distance; f.quality = flow.quality; f.sensor_id = flow.sensor_id; @@ -413,7 +411,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) r.timestamp = hrt_absolute_time(); r.error_count = 0; r.type = RANGE_FINDER_TYPE_LASER; - r.distance = flow.ground_distance; + r.distance = flow.distance; r.minimum_distance = 0.0f; r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance); @@ -1398,7 +1396,7 @@ MavlinkReceiver::receive_start(Mavlink *parent) struct sched_param param; (void)pthread_attr_getschedparam(&receiveloop_attr, ¶m); - param.sched_priority = SCHED_PRIORITY_MAX - 40; + param.sched_priority = SCHED_PRIORITY_MAX - 80; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); pthread_attr_setstacksize(&receiveloop_attr, 2900); diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 7b127759a..ad60ee03e 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -60,6 +60,8 @@ static perf_counter_t c_gather_ppm; static int _dsm_fd; +static uint16_t rc_value_override = 0; + bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated) { perf_begin(c_gather_dsm); @@ -316,6 +318,9 @@ controls_tick() { r_rc_values[mapped] = SIGNED_TO_REG(scaled); assigned_channels |= (1 << mapped); + } else if (mapped == PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH) { + /* pick out override channel, indicated by special mapping */ + rc_value_override = SIGNED_TO_REG(scaled); } } } @@ -409,7 +414,7 @@ controls_tick() { * requested override. * */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) < RC_CHANNEL_LOW_THRESH)) + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) override = true; if (override) { diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 3e19333d8..c0b8ac358 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -60,13 +60,6 @@ extern "C" { */ #define FMU_INPUT_DROP_LIMIT_US 500000 -/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */ -#define ROLL 0 -#define PITCH 1 -#define YAW 2 -#define THROTTLE 3 -#define OVERRIDE 4 - /* current servo arm/disarm state */ static bool mixer_servos_armed = false; static bool should_arm = false; diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 89a470b44..9b2e047cb 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -221,6 +221,7 @@ enum { /* DSM bind states */ hence index 12 can safely be used. */ #define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */ +#define PX4IO_P_SETUP_FORCE_SAFETY_ON 14 /* force safety switch into 'disarmed' (PWM disabled state) */ #define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */ /* autopilot control values, -10000..10000 */ @@ -246,6 +247,7 @@ enum { /* DSM bind states */ #define PX4IO_P_RC_CONFIG_MAX 2 /**< highest input value */ #define PX4IO_P_RC_CONFIG_DEADZONE 3 /**< band around center that is ignored */ #define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /**< mapped input value */ +#define PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH 100 /**< magic value for mode switch */ #define PX4IO_P_RC_CONFIG_OPTIONS 5 /**< channel options bitmask */ #define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED (1 << 0) #define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index cd134ccb4..30f32b38e 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -124,6 +124,9 @@ heartbeat_blink(void) { static bool heartbeat = false; LED_BLUE(heartbeat = !heartbeat); +#ifdef GPIO_LED4 + LED_RING(heartbeat); +#endif } static uint64_t reboot_time; @@ -191,6 +194,9 @@ user_start(int argc, char *argv[]) LED_AMBER(false); LED_BLUE(false); LED_SAFETY(false); +#ifdef GPIO_LED4 + LED_RING(false); +#endif /* turn on servo power (if supported) */ #ifdef POWER_SERVO diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index e32fcc72b..8186e4c78 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -140,6 +140,7 @@ extern pwm_limit_t pwm_limit; #define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) #define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) #define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) +#define LED_RING(_s) stm32_gpiowrite(GPIO_LED4, !(_s)) #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 7a5a5e484..49c2a9f56 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -603,6 +603,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) dsm_bind(value & 0x0f, (value >> 4) & 0xF); break; + case PX4IO_P_SETUP_FORCE_SAFETY_ON: + if (value == PX4IO_FORCE_SAFETY_MAGIC) { + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF; + } + break; + case PX4IO_P_SETUP_FORCE_SAFETY_OFF: if (value == PX4IO_FORCE_SAFETY_MAGIC) { r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; @@ -686,7 +692,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) { disabled = true; - } else if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { + } else if ((conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) && + (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] != PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH)) { count++; } diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 0f0414148..6ead38d61 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -61,7 +61,7 @@ /* Measured values with Futaba FX-30/R6108SB: -+100% on TX: PCM 1.100/1.520/1.950ms -> SBus raw values: 350/1024/1700 (100% ATV) - -+140% on TX: PCM 0.930/1.520/2.112ms -> SBus raw values: 78/1024/1964 (140% ATV) + -+140% on TX: PCM 0.930/1.520/2.112ms -> SBus raw values: 78/1024/1964 (140% ATV) -+152% on TX: PCM 0.884/1.520/2.160ms -> SBus raw values: 1/1024/2047 (140% ATV plus dirty tricks) */ @@ -87,13 +87,15 @@ static unsigned partial_frame_count; unsigned sbus_frame_drops; -static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); +static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, + bool *sbus_frame_drop, uint16_t max_channels); int sbus_init(const char *device) { - if (sbus_fd < 0) + if (sbus_fd < 0) { sbus_fd = open(device, O_RDWR | O_NONBLOCK); + } if (sbus_fd >= 0) { struct termios t; @@ -113,6 +115,7 @@ sbus_init(const char *device) } else { debug("S.Bus: open failed"); } + return sbus_fd; } @@ -167,8 +170,9 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb ret = read(sbus_fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count); /* if the read failed for any reason, just give up here */ - if (ret < 1) + if (ret < 1) { return false; + } last_rx_time = now; @@ -180,8 +184,9 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb /* * If we don't have a full frame, return */ - if (partial_frame_count < SBUS_FRAME_SIZE) + if (partial_frame_count < SBUS_FRAME_SIZE) { return false; + } /* * Great, it looks like we might have a frame. Go ahead and @@ -228,7 +233,8 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { }; static bool -sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values) +sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, + uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x0f)) { @@ -237,23 +243,27 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool } switch (frame[24]) { - case 0x00: + case 0x00: /* this is S.BUS 1 */ break; - case 0x03: + + case 0x03: /* S.BUS 2 SLOT0: RX battery and external voltage */ break; - case 0x83: + + case 0x83: /* S.BUS 2 SLOT1 */ break; - case 0x43: - case 0xC3: - case 0x23: - case 0xA3: - case 0x63: - case 0xE3: + + case 0x43: + case 0xC3: + case 0x23: + case 0xA3: + case 0x63: + case 0xE3: break; - default: + + default: /* we expect one of the bits above, but there are some we don't know yet */ break; } @@ -283,7 +293,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool /* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */ - values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET; + values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; } /* decode switch channels if data fields are wide enough */ @@ -304,16 +314,17 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool /* report that we failed to read anything valid off the receiver */ *sbus_failsafe = true; *sbus_frame_drop = true; - } - else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ + + } else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ /* set a special warning flag - * - * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this - * condition as fail-safe greatly reduces the reliability and range of the radio link, + * + * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this + * condition as fail-safe greatly reduces the reliability and range of the radio link, * e.g. by prematurely issueing return-to-launch!!! */ *sbus_failsafe = false; *sbus_frame_drop = true; + } else { *sbus_failsafe = false; *sbus_frame_drop = false; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e13593077..af580f1f7 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -90,6 +90,7 @@ #include <uORB/topics/system_power.h> #include <uORB/topics/servorail_status.h> #include <uORB/topics/wind_estimate.h> +#include <uORB/topics/encoders.h> #include <systemlib/systemlib.h> #include <systemlib/param/param.h> @@ -628,6 +629,9 @@ void sdlog2_start_log() perf_print_all(perf_fd); close(perf_fd); + /* reset performance counters to get in-flight min and max values in post flight log */ + perf_reset_all(); + logging_enabled = true; } @@ -951,6 +955,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct servorail_status_s servorail_status; struct satellite_info_s sat_info; struct wind_estimate_s wind_estimate; + struct encoders_s encoders; } buf; memset(&buf, 0, sizeof(buf)); @@ -993,6 +998,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GS1B_s log_GS1B; struct log_TECS_s log_TECS; struct log_WIND_s log_WIND; + struct log_ENCD_s log_ENCD; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1030,12 +1036,12 @@ int sdlog2_thread_main(int argc, char *argv[]) int system_power_sub; int servorail_status_sub; int wind_sub; + int encoders_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); subs.status_sub = orb_subscribe(ORB_ID(vehicle_status)); subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info)); subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); @@ -1054,9 +1060,6 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.esc_sub = orb_subscribe(ORB_ID(esc_status)); subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint)); subs.battery_sub = orb_subscribe(ORB_ID(battery_status)); - for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { - subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); - } subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status)); subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status)); @@ -1065,6 +1068,25 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); /* we need to rate-limit wind, as we do not need the full update rate */ orb_set_interval(subs.wind_sub, 90); + subs.encoders_sub = orb_subscribe(ORB_ID(encoders)); + + /* add new topics HERE */ + + + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); + } + + if (_extended_logging) { + subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info)); + } + + /* close non-needed fd's */ + + /* close stdin */ + close(0); + /* close stdout */ + close(1); thread_running = true; @@ -1649,6 +1671,16 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(WIND); } + /* --- ENCODERS --- */ + if (copy_if_updated(ORB_ID(encoders), subs.encoders_sub, &buf.encoders)) { + log_msg.msg_type = LOG_ENCD_MSG; + log_msg.body.log_ENCD.cnt0 = buf.encoders.counts[0]; + log_msg.body.log_ENCD.vel0 = buf.encoders.velocity[0]; + log_msg.body.log_ENCD.cnt1 = buf.encoders.counts[1]; + log_msg.body.log_ENCD.vel1 = buf.encoders.velocity[1]; + LOGBUFFER_WRITE_AND_COUNT(ENCD); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { /* only request write if several packets can be written at once */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index d49fc0c79..fa9bdacb8 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -407,6 +407,16 @@ struct log_VISN_s { float qw; }; +/* --- ENCODERS - ENCODER DATA --- */ +#define LOG_ENCD_MSG 39 +struct log_ENCD_s { + int64_t cnt0; + float vel0; + int64_t cnt1; + float vel1; +}; + + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -471,6 +481,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), + LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index b921865eb..b91a00c1e 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -201,6 +201,9 @@ ORB_DEFINE(telemetry_status_1, struct telemetry_status_s); ORB_DEFINE(telemetry_status_2, struct telemetry_status_s); ORB_DEFINE(telemetry_status_3, struct telemetry_status_s); +#include "topics/test_motor.h" +ORB_DEFINE(test_motor, struct test_motor_s); + #include "topics/debug_key_value.h" ORB_DEFINE(debug_key_value, struct debug_key_value_s); diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 8978de471..16916cc4d 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -34,6 +34,8 @@ /** * @file rc_channels.h * Definition of the rc_channels uORB topic. + * + * @deprecated DO NOT USE FOR NEW CODE */ #ifndef RC_CHANNELS_H_ @@ -63,10 +65,13 @@ enum RC_CHANNELS_FUNCTION { AUX_2, AUX_3, AUX_4, - AUX_5, - RC_CHANNELS_FUNCTION_MAX /**< Indicates the number of functions. There can be more functions than RC channels. */ + AUX_5 }; +// MAXIMUM FUNCTIONS IS != MAXIMUM RC INPUT CHANNELS + +#define RC_CHANNELS_FUNCTION_MAX 18 + /** * @addtogroup topics * @{ @@ -76,7 +81,6 @@ struct rc_channels_s { uint64_t timestamp_last_valid; /**< Timestamp of last valid RC signal */ float channels[RC_CHANNELS_FUNCTION_MAX]; /**< Scaled to -1..1 (throttle: 0..1) */ uint8_t channel_count; /**< Number of valid channels */ - char function_name[RC_CHANNELS_FUNCTION_MAX][20]; /**< String array to store the names of the functions */ int8_t function[RC_CHANNELS_FUNCTION_MAX]; /**< Functions mapping */ uint8_t rssi; /**< Receive signal strength index */ bool signal_lost; /**< Control signal lost, should be checked together with topic timeout */ diff --git a/src/modules/uORB/topics/test_motor.h b/src/modules/uORB/topics/test_motor.h new file mode 100644 index 000000000..491096934 --- /dev/null +++ b/src/modules/uORB/topics/test_motor.h @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (C) 2014 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 test_motor.h + * + * Test a single drive motor while the vehicle is disarmed + * + * Publishing values to this topic causes a single motor to spin, e.g. for + * ground test purposes. This topic will be ignored while the vehicle is armed. + * + */ + +#ifndef TOPIC_TEST_MOTOR_H +#define TOPIC_TEST_MOTOR_H + +#include <stdint.h> +#include "../uORB.h" + +#define NUM_MOTOR_OUTPUTS 8 + +/** + * @addtogroup topics + * @{ + */ + +struct test_motor_s { + uint64_t timestamp; /**< output timestamp in us since system boot */ + unsigned motor_number; /**< number of motor to spin */ + float value; /**< output data, in natural output units */ +}; + +/** + * @} + */ + +/* actuator output sets; this list can be expanded as more drivers emerge */ +ORB_DECLARE(test_motor); + +#endif diff --git a/src/systemcmds/motor_test/module.mk b/src/systemcmds/motor_test/module.mk new file mode 100644 index 000000000..eb36d2ded --- /dev/null +++ b/src/systemcmds/motor_test/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2014 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. +# +############################################################################ + +# +# Build the motor_test tool. +# + +MODULE_COMMAND = motor_test +SRCS = motor_test.c + +MODULE_STACKSIZE = 4096 diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.c new file mode 100644 index 000000000..079f99674 --- /dev/null +++ b/src/systemcmds/motor_test/motor_test.c @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Author: Holger Steinhaus <hsteinhaus@gmx.de> + * + * 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 motor_test.c + * + * Tool for drive testing + */ + +#include <nuttx/config.h> + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> + +#include <arch/board/board.h> +#include <drivers/drv_hrt.h> +#include <uORB/topics/test_motor.h> + + +#include "systemlib/systemlib.h" +#include "systemlib/err.h" + + +__EXPORT int motor_test_main(int argc, char *argv[]); +static void motor_test(unsigned channel, float value); +static void usage(const char *reason); + +void motor_test(unsigned channel, float value) +{ + orb_advert_t _test_motor_pub; + struct test_motor_s _test_motor; + + _test_motor.motor_number = channel; + _test_motor.timestamp = hrt_absolute_time(); + _test_motor.value = value; + + if (_test_motor_pub > 0) { + /* publish armed state */ + orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor); + } else { + /* advertise and publish */ + _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor); + } +} + +static void usage(const char *reason) +{ + if (reason != NULL) + warnx("%s", reason); + + errx(1, + "usage:\n" + "motor_test\n" + " -m <channel> Motor to test (0..7)\n" + " -p <power> Power (0..100)\n"); +} + +int motor_test_main(int argc, char *argv[]) +{ + unsigned long channel, lval; + float value; + int ch; + + if (argc != 5) { + usage("please specify motor and power"); + } + + while ((ch = getopt(argc, argv, "m:p:")) != EOF) { + switch (ch) { + + case 'm': + /* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */ + channel = strtoul(optarg, NULL, 0); + break; + + case 'p': + /* Read in custom low value */ + lval = strtoul(optarg, NULL, 0); + + if (lval > 100) + usage("value invalid"); + + value = (float)lval/100.f; + break; + default: + usage(NULL); + } + } + + motor_test(channel, value); + + printf("motor %d set to %.2f\n", channel, value); + + exit(0); +} diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk index 7d2c59f91..a12bc369e 100644 --- a/src/systemcmds/nshterm/module.mk +++ b/src/systemcmds/nshterm/module.mk @@ -38,6 +38,8 @@ MODULE_COMMAND = nshterm SRCS = nshterm.c -MODULE_STACKSIZE = 1400 +MODULE_STACKSIZE = 1600 MAXOPTIMIZATION = -Os + +MODULE_PRIORITY = "SCHED_PRIORITY_DEFAULT-30" diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index fca1798e6..f06c49552 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -87,7 +87,7 @@ nshterm_main(int argc, char *argv[]) /* Back up the original uart configuration to restore it after exit */ if ((termios_state = tcgetattr(fd, &uart_config)) < 0) { - warnx("ERROR get termios config %s: %d\n", argv[1], termios_state); + warnx("ERR get config %s: %d\n", argv[1], termios_state); close(fd); return -1; } @@ -96,7 +96,7 @@ nshterm_main(int argc, char *argv[]) uart_config.c_oflag |= (ONLCR | OPOST/* | OCRNL*/); if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { - warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", argv[1]); + warnx("ERR set config %s\n", argv[1]); close(fd); return -1; } diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index e3f26924f..0f56704e6 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -110,7 +110,9 @@ const struct { {"conv", test_conv, OPT_NOJIGTEST | OPT_NOALLTEST}, {"mount", test_mount, OPT_NOJIGTEST | OPT_NOALLTEST}, {"mtd", test_mtd, 0}, +#ifndef TESTS_MATHLIB_DISABLE {"mathlib", test_mathlib, 0}, +#endif {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; |