From bbd5f33a22f6371a0dd6c0c6ae4a585101e81398 Mon Sep 17 00:00:00 2001 From: Sebastian Verling Date: Mon, 23 Feb 2015 18:38:55 +0100 Subject: [PATCH 001/112] removed duplicate line --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) 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 1c79cb61d5..7278d4f66c 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 @@ -457,9 +457,7 @@ int AttitudePositionEstimatorEKF::check_filter_state() rep.states[i] = ekf_report.states[i]; } - for (size_t i = 0; i < rep.n_states; i++) { - rep.states[i] = ekf_report.states[i]; - } + if (_estimator_status_pub > 0) { orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep); From aeef634f9b111d1c50809905802d63b7087c9ac4 Mon Sep 17 00:00:00 2001 From: Sebastian Verling Date: Mon, 23 Feb 2015 18:40:48 +0100 Subject: [PATCH 002/112] adapted comment about accelerometer offset --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 7278d4f66c..7d3cb53960 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 @@ -1054,7 +1054,7 @@ void AttitudePositionEstimatorEKF::print_status() // 4-6: Velocity - m/sec (North, East, Down) // 7-9: Position - m (North, East, Down) // 10-12: Delta Angle bias - rad (X,Y,Z) - // 13: Accelerometer offset + // 13: Delta Velocity Bias - m/s (Z) // 14-15: Wind Vector - m/sec (North,East) // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) From a77420ede84b293e41855ddc7b23b647cc64da96 Mon Sep 17 00:00:00 2001 From: Sebastian Verling Date: Mon, 23 Feb 2015 18:44:38 +0100 Subject: [PATCH 003/112] corrected rate offset calculation such that units match --- .../ekf_att_pos_estimator_main.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) 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 7d3cb53960..903158129b 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 @@ -772,14 +772,14 @@ void AttitudePositionEstimatorEKF::publishAttitude() _att.pitch = euler(1); _att.yaw = euler(2); - _att.rollspeed = _ekf->angRate.x - _ekf->states[10]; - _att.pitchspeed = _ekf->angRate.y - _ekf->states[11]; - _att.yawspeed = _ekf->angRate.z - _ekf->states[12]; + _att.rollspeed = _ekf->angRate.x - _ekf->states[10] / _ekf->dtIMU; + _att.pitchspeed = _ekf->angRate.y - _ekf->states[11] / _ekf->dtIMU; + _att.yawspeed = _ekf->angRate.z - _ekf->states[12] / _ekf->dtIMU; // gyro offsets - _att.rate_offsets[0] = _ekf->states[10]; - _att.rate_offsets[1] = _ekf->states[11]; - _att.rate_offsets[2] = _ekf->states[12]; + _att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMU; + _att.rate_offsets[1] = _ekf->states[11] / _ekf->dtIMU; + _att.rate_offsets[2] = _ekf->states[12] / _ekf->dtIMU; /* lazily publish the attitude only once available */ if (_att_pub > 0) { From 81648f84cd97ec865ba7f20cebeb6285f7ef6d18 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 27 Feb 2015 15:49:40 +0100 Subject: [PATCH 004/112] increase RTL descend altitude --- src/modules/navigator/rtl_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index 1568233b05..10394fed18 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -83,7 +83,7 @@ PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 60); * @max 100 * @group RTL */ -PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20); +PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 30); /** * RTL delay From e4830eb53fc712669e1f6d9ac312e311ec88fd40 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Feb 2015 04:01:10 +0100 Subject: [PATCH 005/112] mag detection (sensors / commander): Default all sensors to internal, set the ones which have been found explicitely to zero if they were -1. --- src/modules/commander/state_machine_helper.cpp | 4 ++-- src/modules/sensors/sensor_params.c | 6 +++--- src/modules/sensors/sensors.cpp | 11 +++++++++-- 3 files changed, 14 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 40aa4a2f0c..0154f235f6 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,7 +35,7 @@ * @file state_machine_helper.cpp * State machine helper functions implementations * - * @author Thomas Gubler + * @author Thomas Gubler * @author Julian Oes */ diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 3e21ec2a91..5ec6780331 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -130,7 +130,7 @@ PARAM_DEFINE_INT32(CAL_MAG0_ID, 0); * @max 30 * @group Sensor Calibration */ -PARAM_DEFINE_INT32(CAL_MAG0_ROT, 0); +PARAM_DEFINE_INT32(CAL_MAG0_ROT, -1); /** * Magnetometer X-axis offset @@ -308,7 +308,7 @@ PARAM_DEFINE_INT32(CAL_MAG1_ID, 0); * @max 30 * @group Sensor Calibration */ -PARAM_DEFINE_INT32(CAL_MAG1_ROT, 0); +PARAM_DEFINE_INT32(CAL_MAG1_ROT, -1); /** * Magnetometer X-axis offset @@ -486,7 +486,7 @@ PARAM_DEFINE_INT32(CAL_MAG2_ID, 0); * @max 30 * @group Sensor Calibration */ -PARAM_DEFINE_INT32(CAL_MAG2_ROT, 0); +PARAM_DEFINE_INT32(CAL_MAG2_ROT, -1); /** * Magnetometer X-axis offset diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 9a90c84e04..1d392b0daa 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1511,14 +1511,21 @@ Sensors::parameter_update_poll(bool forced) if (ioctl(fd, MAGIOCGEXTERNAL, 0) <= 0) { /* mag is internal */ _mag_rotation[s] = _board_rotation; - /* reset param to -1 to indicate external mag */ + /* reset param to -1 to indicate internal mag */ int32_t minus_one = MAG_ROT_VAL_INTERNAL; param_set_no_notification(param_find(str), &minus_one); } else { - int32_t mag_rot = 0; + int32_t mag_rot; param_get(param_find(str), &mag_rot); + /* check if this mag is still set as internal */ + if (mag_rot < 0) { + /* it was marked as internal, change to external with no rotation */ + mag_rot = 0; + param_set_no_notification(param_find(str), &mag_rot); + } + /* handling of old setups, will be removed later (noted Feb 2015) */ int32_t deprecated_mag_rot = 0; param_get(param_find("SENS_EXT_MAG_ROT"), &deprecated_mag_rot); From d6962de4452a365701a0302a15d448fad68e6092 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 27 Feb 2015 15:46:22 +0100 Subject: [PATCH 006/112] change FW airspeed defaults --- src/modules/fw_att_control/fw_att_control_params.c | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index a61108c4cd..2a26159bcb 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -302,10 +302,9 @@ PARAM_DEFINE_INT32(FW_YCO_METHOD, 0); * * @unit m/s * @min 0.0 - * @max 30.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 13.0f); +PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f); /** * Trim Airspeed @@ -314,10 +313,9 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 13.0f); * * @unit m/s * @min 0.0 - * @max 30.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f); +PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f); /** * Maximum Airspeed @@ -327,10 +325,9 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f); * * @unit m/s * @min 0.0 - * @max 30.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 50.0f); +PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f); /** * Roll Setpoint Offset From 5f326650fd2b83adc86cce6ca5ced4371bd16a60 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sat, 28 Feb 2015 10:55:31 +0100 Subject: [PATCH 007/112] added max values for airspeed --- src/modules/fw_att_control/fw_att_control_params.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 2a26159bcb..6ab3ddbfc8 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -302,6 +302,7 @@ PARAM_DEFINE_INT32(FW_YCO_METHOD, 0); * * @unit m/s * @min 0.0 + * @max 40 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f); @@ -313,6 +314,7 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f); * * @unit m/s * @min 0.0 + * @max 40 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f); @@ -325,6 +327,7 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f); * * @unit m/s * @min 0.0 + * @max 40 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f); From 6646e3bdf10013a347f1600f0df8df937086c6e9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Feb 2015 11:15:13 +0100 Subject: [PATCH 008/112] Do not abort if IO cannot be started or put into safe mode --- ROMFS/px4fmu_common/init.d/rcS | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 580043a1d4..ea3f721f34 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -198,8 +198,12 @@ then tone_alarm MLL32CP8MB - px4io start - px4io safety_on + if px4io start + then + if px4io safety_on + then + fi + fi if px4io forceupdate 14662 ${IO_FILE} then From 07970689d4d326c9140bc128ddf495d056bf5fd6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Feb 2015 11:20:49 +0100 Subject: [PATCH 009/112] Rover config: Add comments and set correct output group --- ROMFS/px4fmu_common/init.d/rc.rover_defaults | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_defaults b/ROMFS/px4fmu_common/init.d/rc.rover_defaults index bda3a9a66e..23bf47df16 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.rover_defaults @@ -2,22 +2,27 @@ set VEHICLE_TYPE rover -# i dont think AUTOCNF is needed here, can it be removed? - +# This section can be enabled once tuning parameters for this particular +# rover model are known. It allows to configure default gains via the GUI #if [ $AUTOCNF == yes ] #then # # param set MC_ROLL_P 7.0 #fi -#PWM Hz +# PWM Hz - 50 Hz is the normal rate in RC cars, higher rates +# may damage analog servos. set PWM_RATE 50 -#PWW default value for "disarmed" mode +# PWM default value for "disarmed" mode +# this centers the steering and throttle, which means no motion +# for a rover set PWM_DISARMED 1500 -#PWM range +# PWM range set PWM_MIN 1200 set PWM_MAX 1800 -#enable servo output on pins 3 and 4 (steering and thrust) -set PWM_OUT 34 +# Enable servo output on pins 3 and 4 (steering and thrust) +# but also include 1+2 as they form together one output group +# and need to be set together. +set PWM_OUT 1234 From dfdf741b130d02681092cbbf3b49c1f0d051f14f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Feb 2015 17:57:58 +0100 Subject: [PATCH 010/112] FMU: Make peripheral rail power controllable --- src/drivers/drv_gpio.h | 2 ++ src/drivers/px4fmu/fmu.cpp | 59 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 61 insertions(+) diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index bacafe1dce..be9604b6ee 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -159,4 +159,6 @@ #define GPIO_SENSOR_RAIL_RESET GPIOC(13) +#define GPIO_PERIPHERAL_RAIL_RESET GPIOC(14) + #endif /* _DRV_GPIO_H */ diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 8e7e93679b..b5907c1cce 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -182,6 +182,7 @@ private: void gpio_reset(void); void sensor_reset(int ms); + void peripheral_reset(int ms); void gpio_set_function(uint32_t gpios, int function); void gpio_write(uint32_t gpios, int function); uint32_t gpio_read(void); @@ -1376,6 +1377,29 @@ PX4FMU::sensor_reset(int ms) #endif } +void +PX4FMU::peripheral_reset(int ms) +{ +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + + if (ms < 1) { + ms = 1; + } + + /* set the peripheral rails off */ + stm32_configgpio(GPIO_VDD_5V_PERIPH_EN); + stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 0); + + /* wait for the peripheral rail to reach GND */ + usleep(ms * 1000); + warnx("reset done, %d ms", ms); + + /* re-enable power */ + + /* switch the peripheral rail back on */ + stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 1); +#endif +} void PX4FMU::gpio_reset(void) @@ -1488,6 +1512,10 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg) sensor_reset(arg); break; + case GPIO_PERIPHERAL_RAIL_RESET: + peripheral_reset(arg); + break; + case GPIO_SET_OUTPUT: case GPIO_SET_INPUT: case GPIO_SET_ALT_1: @@ -1674,6 +1702,24 @@ sensor_reset(int ms) close(fd); } +void +peripheral_reset(int ms) +{ + int fd; + + fd = open(PX4FMU_DEVICE_PATH, O_RDWR); + + if (fd < 0) { + errx(1, "open fail"); + } + + if (ioctl(fd, GPIO_PERIPHERAL_RAIL_RESET, ms) < 0) { + warnx("peripheral rail reset failed"); + } + + close(fd); +} + void test(void) { @@ -1895,6 +1941,19 @@ fmu_main(int argc, char *argv[]) exit(0); } + if (!strcmp(verb, "peripheral_reset")) { + if (argc > 2) { + int reset_time = strtol(argv[2], 0, 0); + peripheral_reset(reset_time); + + } else { + peripheral_reset(0); + warnx("resettet default time"); + } + + exit(0); + } + if (!strcmp(verb, "i2c")) { if (argc > 3) { int bus = strtol(argv[2], 0, 0); From 2cf0aec3328e5fbd0c90f632241dc2cd7994e632 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Feb 2015 14:31:39 +0100 Subject: [PATCH 011/112] MPU6K: Allow stop --- src/drivers/mpu6000/mpu6000.cpp | 45 ++++++++++++++++++++++++++------- 1 file changed, 36 insertions(+), 9 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 4aa05a9808..b48ea85776 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1881,6 +1881,7 @@ MPU6000 *g_dev_int; // on internal bus MPU6000 *g_dev_ext; // on external bus void start(bool, enum Rotation); +void stop(bool); void test(bool); void reset(bool); void info(bool); @@ -1946,6 +1947,20 @@ fail: errx(1, "driver start failed"); } +void +stop(bool external_bus) +{ + MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; + if (*g_dev_ptr != nullptr) { + delete *g_dev_ptr; + *g_dev_ptr = nullptr; + } else { + /* warn, but not an error */ + warnx("already stopped."); + } + exit(0); +} + /** * Perform some basic functional tests on the driver; * make sure we can collect data from the sensor in polled @@ -2111,7 +2126,7 @@ factorytest(bool external_bus) void usage() { - warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump', 'factorytest', 'testerror'"); + warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'factorytest', 'testerror'"); warnx("options:"); warnx(" -X (external bus)"); warnx(" -R rotation"); @@ -2147,38 +2162,50 @@ mpu6000_main(int argc, char *argv[]) * Start/load the driver. */ - if (!strcmp(verb, "start")) + if (!strcmp(verb, "start")) { mpu6000::start(external_bus, rotation); + } + + if (!strcmp(verb, "stop")) { + mpu6000::stop(external_bus); + } /* * Test the driver/device. */ - if (!strcmp(verb, "test")) + if (!strcmp(verb, "test")) { mpu6000::test(external_bus); + } /* * Reset the driver. */ - if (!strcmp(verb, "reset")) + if (!strcmp(verb, "reset")) { mpu6000::reset(external_bus); + } /* * Print driver information. */ - if (!strcmp(verb, "info")) + if (!strcmp(verb, "info")) { mpu6000::info(external_bus); + } /* * Print register information. */ - if (!strcmp(verb, "regdump")) + if (!strcmp(verb, "regdump")) { mpu6000::regdump(external_bus); + } - if (!strcmp(verb, "factorytest")) + if (!strcmp(verb, "factorytest")) { mpu6000::factorytest(external_bus); + } - if (!strcmp(verb, "testerror")) + if (!strcmp(verb, "testerror")) { mpu6000::testerror(external_bus); + } - errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'regdump', 'factorytest' or 'testerror'"); + mpu6000::usage(); + exit(1); } From 4e1905c1d84f48885380594d94c2cd4d990112fe Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Feb 2015 14:32:11 +0100 Subject: [PATCH 012/112] FMU: Fix peripheral reset --- src/drivers/px4fmu/fmu.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index b5907c1cce..7b09a4676a 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1383,12 +1383,12 @@ PX4FMU::peripheral_reset(int ms) #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) if (ms < 1) { - ms = 1; + ms = 10; } /* set the peripheral rails off */ stm32_configgpio(GPIO_VDD_5V_PERIPH_EN); - stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 0); + stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 1); /* wait for the peripheral rail to reach GND */ usleep(ms * 1000); @@ -1397,7 +1397,7 @@ PX4FMU::peripheral_reset(int ms) /* re-enable power */ /* switch the peripheral rail back on */ - stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 1); + stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 0); #endif } From 677bfddcb669235be236ce15ed1b480a167efb56 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Feb 2015 17:59:47 +0100 Subject: [PATCH 013/112] uORB: More timing tests. --- src/modules/uORB/uORB.cpp | 54 ++++++++++++++++++++++++++++++++++++--- 1 file changed, 50 insertions(+), 4 deletions(-) diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 764e33179c..93490f579b 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -684,6 +684,8 @@ namespace ORBDevMaster *g_dev; bool pubsubtest_passed = false; +bool pubsubtest_print = false; +int pubsubtest_res = OK; struct orb_test { int val; @@ -736,7 +738,9 @@ int pubsublatency_main(void) struct orb_test t; - const unsigned maxruns = 10; + const unsigned maxruns = 1000; + + unsigned *timings = new unsigned[maxruns]; for (unsigned i = 0; i < maxruns; i++) { /* wait for up to 500ms for data */ @@ -750,17 +754,34 @@ int pubsublatency_main(void) hrt_abstime elt = hrt_elapsed_time(&t.time); latency_integral += elt; + timings[i] = elt; } orb_unsubscribe(test_multi_sub); + if (pubsubtest_print) { + for (unsigned i = 0; i < maxruns; i++) { + printf("%u\n", timings[i]); + } + } + + delete[] timings; + warnx("mean: %8.4f", static_cast(latency_integral / maxruns)); pubsubtest_passed = true; - return OK; + if (static_cast(latency_integral / maxruns) > 30.0f) { + pubsubtest_res = ERROR; + } else { + pubsubtest_res = OK; + } + + return pubsubtest_res; } +int latency_test(bool print); + int test() { @@ -874,6 +895,25 @@ test() if (prio != ORB_PRIO_MIN) return test_fail("prio: %d", prio); + if (OK != latency_test(false)) + return test_fail("latency test failed"); + + return test_note("PASS"); +} + +int +latency_test(bool print) +{ + struct orb_test t; + t.val = 308; + t.time = hrt_absolute_time(); + + int pfd0 = orb_advertise(ORB_ID(orb_multitest), &t); + + pubsubtest_print = print; + + pubsubtest_passed = false; + /* test pub / sub latency */ int pubsub_task = task_spawn_cmd("uorb_latency", @@ -885,7 +925,7 @@ test() /* give the test task some data */ while (!pubsubtest_passed) { - t.val = 303; + t.val = 308; t.time = hrt_absolute_time(); if (OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t)) return test_fail("mult. pub0 timing fail"); @@ -898,7 +938,7 @@ test() return test_fail("failed launching task"); } - return test_note("PASS"); + return pubsubtest_res; } int @@ -955,6 +995,12 @@ uorb_main(int argc, char *argv[]) if (!strcmp(argv[1], "test")) return test(); + /* + * Test the latency. + */ + if (!strcmp(argv[1], "latency_test")) + return latency_test(true); + /* * Print driver information. */ From 19811bc73fb87662bd4e2a2145d3c4f5bc5aa201 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Feb 2015 19:30:08 +0100 Subject: [PATCH 014/112] Better timing tests --- src/modules/uORB/uORB.cpp | 91 ++++++++++++++++++++++++++++++++------- 1 file changed, 75 insertions(+), 16 deletions(-) diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 93490f579b..b4f81d4293 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -695,6 +695,22 @@ struct orb_test { ORB_DEFINE(orb_test, struct orb_test); ORB_DEFINE(orb_multitest, struct orb_test); +struct orb_test_medium { + int val; + hrt_abstime time; + char junk[64]; +}; + +ORB_DEFINE(orb_test_medium, struct orb_test_medium); + +struct orb_test_large { + int val; + hrt_abstime time; + char junk[512]; +}; + +ORB_DEFINE(orb_test_large, struct orb_test_large); + int test_fail(const char *fmt, ...) { @@ -729,23 +745,44 @@ int pubsublatency_main(void) float latency_integral = 0.0f; /* wakeup source(s) */ - struct pollfd fds[1]; + struct pollfd fds[3]; - int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_multitest), 0); + int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_test), 0); + int test_multi_sub_medium = orb_subscribe_multi(ORB_ID(orb_test_medium), 0); + int test_multi_sub_large = orb_subscribe_multi(ORB_ID(orb_test_large), 0); + + struct orb_test_large t; + + /* clear all ready flags */ + orb_copy(ORB_ID(orb_test), test_multi_sub, &t); + orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t); + orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t); fds[0].fd = test_multi_sub; fds[0].events = POLLIN; - - struct orb_test t; + fds[1].fd = test_multi_sub_medium; + fds[1].events = POLLIN; + fds[2].fd = test_multi_sub_large; + fds[2].events = POLLIN; const unsigned maxruns = 1000; + unsigned timingsgroup = 0; unsigned *timings = new unsigned[maxruns]; for (unsigned i = 0; i < maxruns; i++) { /* wait for up to 500ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); - orb_copy(ORB_ID(orb_multitest), test_multi_sub, &t); + if (fds[0].revents & POLLIN) { + orb_copy(ORB_ID(orb_test), test_multi_sub, &t); + timingsgroup = 0; + } else if (fds[1].revents & POLLIN) { + orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t); + timingsgroup = 1; + } else if (fds[2].revents & POLLIN) { + orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t); + timingsgroup = 2; + } if (pret < 0) { warn("poll error %d, %d", pret, errno); @@ -758,11 +795,23 @@ int pubsublatency_main(void) } orb_unsubscribe(test_multi_sub); + orb_unsubscribe(test_multi_sub_medium); + orb_unsubscribe(test_multi_sub_large); if (pubsubtest_print) { + char fname[32]; + sprintf(fname, "/fs/microsd/timings%u.txt", timingsgroup); + FILE *f = fopen(fname, "w"); + if (f == NULL) { + warnx("Error opening file!\n"); + return ERROR; + } + for (unsigned i = 0; i < maxruns; i++) { - printf("%u\n", timings[i]); + fprintf(f, "%u\n", timings[i]); } + + fclose(f); } delete[] timings; @@ -780,7 +829,7 @@ int pubsublatency_main(void) return pubsubtest_res; } -int latency_test(bool print); +template int latency_test(orb_id_t T, bool print); int test() @@ -895,20 +944,20 @@ test() if (prio != ORB_PRIO_MIN) return test_fail("prio: %d", prio); - if (OK != latency_test(false)) + if (OK != latency_test(ORB_ID(orb_test), false)) return test_fail("latency test failed"); return test_note("PASS"); } -int -latency_test(bool print) +template int +latency_test(orb_id_t T, bool print) { - struct orb_test t; + S t; t.val = 308; t.time = hrt_absolute_time(); - int pfd0 = orb_advertise(ORB_ID(orb_multitest), &t); + int pfd0 = orb_advertise(T, &t); pubsubtest_print = print; @@ -927,13 +976,15 @@ latency_test(bool print) while (!pubsubtest_passed) { t.val = 308; t.time = hrt_absolute_time(); - if (OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t)) + if (OK != orb_publish(T, pfd0, &t)) return test_fail("mult. pub0 timing fail"); /* simulate >800 Hz system operation */ usleep(1000); } + close(pfd0); + if (pubsub_task < 0) { return test_fail("failed launching task"); } @@ -998,8 +1049,16 @@ uorb_main(int argc, char *argv[]) /* * Test the latency. */ - if (!strcmp(argv[1], "latency_test")) - return latency_test(true); + if (!strcmp(argv[1], "latency_test")) { + + if (argc > 2 && !strcmp(argv[2], "medium")) { + return latency_test(ORB_ID(orb_test_medium), true); + } else if (argc > 2 && !strcmp(argv[2], "large")) { + return latency_test(ORB_ID(orb_test_large), true); + } else { + return latency_test(ORB_ID(orb_test), true); + } + } /* * Print driver information. @@ -1007,7 +1066,7 @@ uorb_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) return info(); - errx(-EINVAL, "unrecognized command, try 'start', 'test' or 'status'"); + errx(-EINVAL, "unrecognized command, try 'start', 'test', 'latency_test' or 'status'"); } /* From 2b2f7e9407a551682fc76401462e08df9871cc98 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 13 Feb 2015 23:19:43 +0100 Subject: [PATCH 015/112] introduce offboard control mode topic Replace offboard_control_setpoint with offboard_control_mode Remove all setpoint data from the topic as it's not used anymore (setpoint data is directly routed into position/attitude setpoint topics for some time now) Remove mode enum and replace with ignore booleans which map better to the mavlink message Mavlink: Rework parsing of offboard setpoints Commander: in offboard mode set control flags based on ignore flags instead of enum --- src/modules/commander/commander.cpp | 85 ++---- src/modules/mavlink/mavlink_messages.cpp | 1 - src/modules/mavlink/mavlink_receiver.cpp | 171 ++++------- src/modules/mavlink/mavlink_receiver.h | 6 +- src/modules/uORB/objects_common.cpp | 4 +- .../uORB/topics/offboard_control_mode.h | 73 +++++ .../uORB/topics/offboard_control_setpoint.h | 276 ------------------ 7 files changed, 162 insertions(+), 454 deletions(-) create mode 100644 src/modules/uORB/topics/offboard_control_mode.h delete mode 100644 src/modules/uORB/topics/offboard_control_setpoint.h diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 242d8a4861..5a362666ca 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -65,7 +65,7 @@ #include #include #include -#include +#include #include #include #include @@ -180,7 +180,7 @@ static struct vehicle_status_s status; static struct actuator_armed_s armed; static struct safety_s safety; static struct vehicle_control_mode_s control_mode; -static struct offboard_control_setpoint_s sp_offboard; +static struct offboard_control_mode_s offboard_control_mode; /* tasks waiting for low prio thread */ typedef enum { @@ -1016,8 +1016,8 @@ int commander_thread_main(int argc, char *argv[]) memset(&sp_man, 0, sizeof(sp_man)); /* Subscribe to offboard control data */ - int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - memset(&sp_offboard, 0, sizeof(sp_offboard)); + int offboard_control_mode_sub = orb_subscribe(ORB_ID(offboard_control_mode)); + memset(&offboard_control_mode, 0, sizeof(offboard_control_mode)); /* Subscribe to telemetry status topics */ int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM]; @@ -1227,14 +1227,14 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); } - orb_check(sp_offboard_sub, &updated); + orb_check(offboard_control_mode_sub, &updated); if (updated) { - orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); + orb_copy(ORB_ID(offboard_control_mode), offboard_control_mode_sub, &offboard_control_mode); } - if (sp_offboard.timestamp != 0 && - sp_offboard.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) { + if (offboard_control_mode.timestamp != 0 && + offboard_control_mode.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) { if (status.offboard_control_signal_lost) { status.offboard_control_signal_lost = false; status_changed = true; @@ -2051,7 +2051,7 @@ int commander_thread_main(int argc, char *argv[]) led_deinit(); buzzer_deinit(); close(sp_man_sub); - close(sp_offboard_sub); + close(offboard_control_mode_sub); close(local_position_sub); close(global_position_sub); close(gps_sub); @@ -2426,56 +2426,31 @@ set_control_mode() control_mode.flag_control_auto_enabled = false; control_mode.flag_control_offboard_enabled = true; - switch (sp_offboard.mode) { - case OFFBOARD_CONTROL_MODE_DIRECT_RATES: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; + /* + * The control flags depend on what is ignored according to the offboard control mode topic + * Inner loop flags (e.g. attitude) also depend on outer loop ignore flags (e.g. position) + */ + control_mode.flag_control_rates_enabled = !offboard_control_mode.ignore_bodyrate || + !offboard_control_mode.ignore_attitude || + !offboard_control_mode.ignore_position || + !offboard_control_mode.ignore_velocity || + !offboard_control_mode.ignore_acceleration_force; - case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; + control_mode.flag_control_attitude_enabled = !offboard_control_mode.ignore_attitude || + !offboard_control_mode.ignore_position || + !offboard_control_mode.ignore_velocity || + !offboard_control_mode.ignore_acceleration_force; - case OFFBOARD_CONTROL_MODE_DIRECT_FORCE: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_force_enabled = true; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - 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: - case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = true; - control_mode.flag_control_velocity_enabled = true; - //XXX: the flags could depend on sp_offboard.ignore - break; + control_mode.flag_control_velocity_enabled = !offboard_control_mode.ignore_velocity || + !offboard_control_mode.ignore_position; - default: - control_mode.flag_control_rates_enabled = false; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - } + control_mode.flag_control_climb_rate_enabled = !offboard_control_mode.ignore_velocity || + !offboard_control_mode.ignore_position; + + control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position; + + control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_position; break; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 9711d8fc33..7d6b60e227 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -51,7 +51,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 90c3cb47f9..3a5ae47abb 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -108,7 +108,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _cmd_pub(-1), _flow_pub(-1), _range_pub(-1), - _offboard_control_sp_pub(-1), + _offboard_control_mode_pub(-1), _global_vel_sp_pub(-1), _att_sp_pub(-1), _rates_sp_pub(-1), @@ -517,8 +517,8 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t mavlink_set_position_target_local_ned_t set_position_target_local_ned; mavlink_msg_set_position_target_local_ned_decode(msg, &set_position_target_local_ned); - struct offboard_control_setpoint_s offboard_control_sp; - memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));//XXX breaks compatibility with multiple setpoints + struct offboard_control_mode_s offboard_control_mode; + memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));//XXX breaks compatibility with multiple setpoints /* Only accept messages which are intended for this system */ if ((mavlink_system.sysid == set_position_target_local_ned.target_system || @@ -527,64 +527,24 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t set_position_target_local_ned.target_component == 0)) { /* convert mavlink type (local, NED) to uORB offboard control struct */ - switch (set_position_target_local_ned.coordinate_frame) { - case MAV_FRAME_LOCAL_NED: - offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED; - break; - case MAV_FRAME_LOCAL_OFFSET_NED: - offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED; - break; - case MAV_FRAME_BODY_NED: - offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED; - break; - case MAV_FRAME_BODY_OFFSET_NED: - offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED; - break; - default: - /* invalid setpoint, avoid publishing */ - return; - } - offboard_control_sp.position[0] = set_position_target_local_ned.x; - offboard_control_sp.position[1] = set_position_target_local_ned.y; - offboard_control_sp.position[2] = set_position_target_local_ned.z; - offboard_control_sp.velocity[0] = set_position_target_local_ned.vx; - offboard_control_sp.velocity[1] = set_position_target_local_ned.vy; - offboard_control_sp.velocity[2] = set_position_target_local_ned.vz; - offboard_control_sp.acceleration[0] = set_position_target_local_ned.afx; - offboard_control_sp.acceleration[1] = set_position_target_local_ned.afy; - offboard_control_sp.acceleration[2] = set_position_target_local_ned.afz; - offboard_control_sp.yaw = set_position_target_local_ned.yaw; - offboard_control_sp.yaw_rate = set_position_target_local_ned.yaw_rate; - offboard_control_sp.isForceSetpoint = (bool)(set_position_target_local_ned.type_mask & (1 << 9)); + offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7); + offboard_control_mode.ignore_velocity = (bool)(set_position_target_local_ned.type_mask & 0x38); + offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_local_ned.type_mask & 0x1C0); + bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9)); + /* yaw ignore flag mapps to ignore_attitude */ + offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400); + /* yawrate ignore flag mapps to ignore_bodyrate */ + offboard_control_mode.ignore_bodyrate = (bool)(set_position_target_local_ned.type_mask & 0x800); - /* If we are in force control mode, for now set offboard mode to force control */ - if (offboard_control_sp.isForceSetpoint) { - offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_FORCE; - } - /* set ignore flags */ - for (int i = 0; i < 9; i++) { - offboard_control_sp.ignore &= ~(1 << i); - offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << i)); - } - offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAW); - if (set_position_target_local_ned.type_mask & (1 << 10)) { - offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAW); - } + offboard_control_mode.timestamp = hrt_absolute_time(); - offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAWRATE); - if (set_position_target_local_ned.type_mask & (1 << 11)) { - offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAWRATE); - } - - offboard_control_sp.timestamp = hrt_absolute_time(); - - if (_offboard_control_sp_pub < 0) { - _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + if (_offboard_control_mode_pub < 0) { + _offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode); } else { - orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); + orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode); } /* If we are in offboard control mode and offboard control loop through is enabled @@ -596,15 +556,14 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } if (_control_mode.flag_control_offboard_enabled) { - if (offboard_control_sp.isForceSetpoint && - offboard_control_sp_ignore_position_all(offboard_control_sp) && - offboard_control_sp_ignore_velocity_all(offboard_control_sp)) { + if (is_force_sp && offboard_control_mode.ignore_position && + offboard_control_mode.ignore_velocity) { /* The offboard setpoint is a force setpoint only, directly writing to the force * setpoint topic and not publishing the setpoint triplet topic */ struct vehicle_force_setpoint_s force_sp; - force_sp.x = offboard_control_sp.acceleration[0]; - force_sp.y = offboard_control_sp.acceleration[1]; - force_sp.z = offboard_control_sp.acceleration[2]; + force_sp.x = set_position_target_local_ned.afx; + force_sp.y = set_position_target_local_ned.afy; + force_sp.z = set_position_target_local_ned.afz; //XXX: yaw if (_force_sp_pub < 0) { _force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp); @@ -619,62 +578,53 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t pos_sp_triplet.current.valid = true; pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; //XXX support others - /* set the local pos values if the setpoint type is 'local pos' and none - * of the local pos fields is set to 'ignore' */ - if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_position_some(offboard_control_sp)) { + /* set the local pos values */ + if (!offboard_control_mode.ignore_position) { pos_sp_triplet.current.position_valid = true; - pos_sp_triplet.current.x = offboard_control_sp.position[0]; - pos_sp_triplet.current.y = offboard_control_sp.position[1]; - pos_sp_triplet.current.z = offboard_control_sp.position[2]; + pos_sp_triplet.current.x = set_position_target_local_ned.x; + pos_sp_triplet.current.y = set_position_target_local_ned.y; + pos_sp_triplet.current.z = set_position_target_local_ned.z; } else { pos_sp_triplet.current.position_valid = false; } - /* set the local vel values if the setpoint type is 'local pos' and none - * of the local vel fields is set to 'ignore' */ - if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_velocity_some(offboard_control_sp)) { + /* set the local vel values */ + if (!offboard_control_mode.ignore_velocity) { pos_sp_triplet.current.velocity_valid = true; - pos_sp_triplet.current.vx = offboard_control_sp.velocity[0]; - pos_sp_triplet.current.vy = offboard_control_sp.velocity[1]; - pos_sp_triplet.current.vz = offboard_control_sp.velocity[2]; + pos_sp_triplet.current.vx = set_position_target_local_ned.vx; + pos_sp_triplet.current.vy = set_position_target_local_ned.vy; + pos_sp_triplet.current.vz = set_position_target_local_ned.vz; } else { pos_sp_triplet.current.velocity_valid = false; } /* set the local acceleration values if the setpoint type is 'local pos' and none * of the accelerations fields is set to 'ignore' */ - if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) { + if (!offboard_control_mode.ignore_acceleration_force) { pos_sp_triplet.current.acceleration_valid = true; - pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0]; - pos_sp_triplet.current.a_y = offboard_control_sp.acceleration[1]; - pos_sp_triplet.current.a_z = offboard_control_sp.acceleration[2]; + pos_sp_triplet.current.a_x = set_position_target_local_ned.afx; + pos_sp_triplet.current.a_y = set_position_target_local_ned.afy; + pos_sp_triplet.current.a_z = set_position_target_local_ned.afz; pos_sp_triplet.current.acceleration_is_force = - offboard_control_sp.isForceSetpoint; + is_force_sp; } else { pos_sp_triplet.current.acceleration_valid = false; } - /* set the yaw sp value if the setpoint type is 'local pos' and the yaw - * field is not set to 'ignore' */ - if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_yaw(offboard_control_sp)) { + /* set the yaw sp value */ + if (!offboard_control_mode.ignore_attitude) { pos_sp_triplet.current.yaw_valid = true; - pos_sp_triplet.current.yaw = offboard_control_sp.yaw; + pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw; } else { pos_sp_triplet.current.yaw_valid = false; } - /* set the yawrate sp value if the setpoint type is 'local pos' and the yawrate - * field is not set to 'ignore' */ - if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_yawrate(offboard_control_sp)) { + /* set the yawrate sp value */ + if (!offboard_control_mode.ignore_bodyrate) { pos_sp_triplet.current.yawspeed_valid = true; - pos_sp_triplet.current.yawspeed = offboard_control_sp.yaw_rate; + pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate; } else { pos_sp_triplet.current.yawspeed_valid = false; @@ -743,42 +693,29 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) mavlink_set_attitude_target_t set_attitude_target; mavlink_msg_set_attitude_target_decode(msg, &set_attitude_target); - struct offboard_control_setpoint_s offboard_control_sp; - memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); //XXX breaks compatibility with multiple setpoints + struct offboard_control_mode_s offboard_control_mode; + memset(&offboard_control_mode, 0, sizeof(offboard_control_mode)); /* Only accept messages which are intended for this system */ if ((mavlink_system.sysid == set_attitude_target.target_system || set_attitude_target.target_system == 0) && (mavlink_system.compid == set_attitude_target.target_component || set_attitude_target.target_component == 0)) { - for (int i = 0; i < 4; i++) { - offboard_control_sp.attitude[i] = set_attitude_target.q[i]; - } - offboard_control_sp.attitude_rate[0] = set_attitude_target.body_roll_rate; - offboard_control_sp.attitude_rate[1] = set_attitude_target.body_pitch_rate; - offboard_control_sp.attitude_rate[2] = set_attitude_target.body_yaw_rate; /* set correct ignore flags for body rate fields: copy from mavlink message */ - for (int i = 0; i < 3; i++) { - offboard_control_sp.ignore &= ~(1 << (i + OFB_IGN_BIT_BODYRATE_X)); - offboard_control_sp.ignore |= (set_attitude_target.type_mask & (1 << i)) << OFB_IGN_BIT_BODYRATE_X; - } + offboard_control_mode.ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7); /* set correct ignore flags for thrust field: copy from mavlink message */ - offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_THRUST); - offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 6)) << OFB_IGN_BIT_THRUST); + offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); /* set correct ignore flags for attitude field: copy from mavlink message */ - offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_ATT); - offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 7)) << OFB_IGN_BIT_ATT); + offboard_control_mode.ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7)); + offboard_control_mode.timestamp = hrt_absolute_time(); - offboard_control_sp.timestamp = hrt_absolute_time(); - offboard_control_sp.mode =OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; //XXX handle rate control mode - - if (_offboard_control_sp_pub < 0) { - _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + if (_offboard_control_mode_pub < 0) { + _offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode); } else { - orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); + orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode); } /* If we are in offboard control mode and offboard control loop through is enabled @@ -793,8 +730,8 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) if (_control_mode.flag_control_offboard_enabled) { /* Publish attitude setpoint if attitude and thrust ignore bits are not set */ - if (!(offboard_control_sp_ignore_attitude(offboard_control_sp) || - offboard_control_sp_ignore_thrust(offboard_control_sp))) { + if (!(offboard_control_mode.ignore_attitude || + offboard_control_mode.ignore_thrust)) { struct vehicle_attitude_setpoint_s att_sp; att_sp.timestamp = hrt_absolute_time(); mavlink_quaternion_to_euler(set_attitude_target.q, @@ -814,8 +751,8 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) /* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */ ///XXX add support for ignoring individual axes - if (!(offboard_control_sp_ignore_bodyrates_some(offboard_control_sp) || - offboard_control_sp_ignore_thrust(offboard_control_sp))) { + if (!(offboard_control_mode.ignore_bodyrate || + offboard_control_mode.ignore_thrust)) { struct vehicle_rates_setpoint_s rates_sp; rates_sp.timestamp = hrt_absolute_time(); rates_sp.roll = set_attitude_target.body_roll_rate; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 699996860a..15943753d0 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -53,7 +53,7 @@ #include #include #include -#include +#include #include #include #include @@ -142,7 +142,7 @@ private: /** * Exponential moving average filter to smooth time offset */ - void smooth_time_offset(uint64_t offset_ns); + void smooth_time_offset(uint64_t offset_ns); mavlink_status_t status; struct vehicle_local_position_s hil_local_pos; @@ -162,7 +162,7 @@ private: orb_advert_t _cmd_pub; orb_advert_t _flow_pub; orb_advert_t _range_pub; - orb_advert_t _offboard_control_sp_pub; + orb_advert_t _offboard_control_mode_pub; orb_advert_t _global_vel_sp_pub; orb_advert_t _att_sp_pub; orb_advert_t _rates_sp_pub; diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index f60aa8d86a..dbed297744 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -163,8 +163,8 @@ ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s); #include "topics/vehicle_control_debug.h" ORB_DEFINE(vehicle_control_debug, struct vehicle_control_debug_s); -#include "topics/offboard_control_setpoint.h" -ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s); +#include "topics/offboard_control_mode.h" +ORB_DEFINE(offboard_control_mode, struct offboard_control_mode_s); #include "topics/optical_flow.h" ORB_DEFINE(optical_flow, struct optical_flow_s); diff --git a/src/modules/uORB/topics/offboard_control_mode.h b/src/modules/uORB/topics/offboard_control_mode.h new file mode 100644 index 0000000000..559659a1d4 --- /dev/null +++ b/src/modules/uORB/topics/offboard_control_mode.h @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2015 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * + * 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 offboard_control_mode.h + * Definition of the manual_control_setpoint uORB topic. + */ + +#ifndef TOPIC_OFFBOARD_CONTROL_MODE_H_ +#define TOPIC_OFFBOARD_CONTROL_MODE_H_ + +#include +#include "../uORB.h" + +/** + * Off-board control mode + */ + +/** + * @addtogroup topics + * @{ + */ + +struct offboard_control_mode_s { + uint64_t timestamp; + + bool ignore_thrust; + bool ignore_attitude; + bool ignore_bodyrate; + bool ignore_position; + bool ignore_velocity; + bool ignore_acceleration_force; + +}; /**< offboard control inputs */ +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(offboard_control_mode); + +#endif diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h deleted file mode 100644 index 72a28e5017..0000000000 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ /dev/null @@ -1,276 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * - * 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 offboard_control_setpoint.h - * Definition of the manual_control_setpoint uORB topic. - */ - -#ifndef TOPIC_OFFBOARD_CONTROL_SETPOINT_H_ -#define TOPIC_OFFBOARD_CONTROL_SETPOINT_H_ - -#include -#include "../uORB.h" - -/** - * Off-board control inputs. - * - * Typically sent by a ground control station / joystick or by - * some off-board controller via C or SIMULINK. - */ -enum OFFBOARD_CONTROL_MODE { - OFFBOARD_CONTROL_MODE_DIRECT = 0, - OFFBOARD_CONTROL_MODE_DIRECT_RATES = 1, - OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE = 2, - OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED = 3, - OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED = 4, - OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED = 5, - OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED = 6, - OFFBOARD_CONTROL_MODE_DIRECT_GLOBAL = 7, - OFFBOARD_CONTROL_MODE_DIRECT_FORCE = 8, - OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 9, - OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 10, - OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 11 /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ -}; - -enum OFFBOARD_CONTROL_FRAME { - OFFBOARD_CONTROL_FRAME_LOCAL_NED = 0, - OFFBOARD_CONTROL_FRAME_LOCAL_OFFSET_NED = 1, - OFFBOARD_CONTROL_FRAME_BODY_NED = 2, - OFFBOARD_CONTROL_FRAME_BODY_OFFSET_NED = 3, - OFFBOARD_CONTROL_FRAME_GLOBAL = 4 -}; - -/* mappings for the ignore bitmask */ -enum {OFB_IGN_BIT_POS_X, - OFB_IGN_BIT_POS_Y, - OFB_IGN_BIT_POS_Z, - OFB_IGN_BIT_VEL_X, - OFB_IGN_BIT_VEL_Y, - OFB_IGN_BIT_VEL_Z, - OFB_IGN_BIT_ACC_X, - OFB_IGN_BIT_ACC_Y, - OFB_IGN_BIT_ACC_Z, - OFB_IGN_BIT_BODYRATE_X, - OFB_IGN_BIT_BODYRATE_Y, - OFB_IGN_BIT_BODYRATE_Z, - OFB_IGN_BIT_ATT, - OFB_IGN_BIT_THRUST, - OFB_IGN_BIT_YAW, - OFB_IGN_BIT_YAWRATE, -}; - -/** - * @addtogroup topics - * @{ - */ - -struct offboard_control_setpoint_s { - uint64_t timestamp; - - enum OFFBOARD_CONTROL_MODE mode; /**< The current control inputs mode */ - - double position[3]; /**< lat, lon, alt / x, y, z */ - float velocity[3]; /**< x vel, y vel, z vel */ - float acceleration[3]; /**< x acc, y acc, z acc */ - float attitude[4]; /**< attitude of vehicle (quaternion) */ - float attitude_rate[3]; /**< body angular rates (x, y, z) */ - float thrust; /**< thrust */ - float yaw; /**< yaw: this is the yaw from the position_target message - (not from the full attitude_target message) */ - float yaw_rate; /**< yaw rate: this is the yaw from the position_target message - (not from the full attitude_target message) */ - - uint16_t ignore; /**< if field i is set to true, the value should be ignored, see definition at top of file - for mapping */ - - bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */ - - float override_mode_switch; - - float aux1_cam_pan_flaps; - float aux2_cam_tilt; - float aux3_cam_zoom; - float aux4_cam_roll; - -}; /**< offboard control inputs */ -/** - * @} - */ - -/** - * Returns true if the position setpoint at index should be ignored - */ -inline bool offboard_control_sp_ignore_position(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { - return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_POS_X + index))); -} - -/** - * Returns true if all position setpoints should be ignored - */ -inline bool offboard_control_sp_ignore_position_all(const struct offboard_control_setpoint_s &offboard_control_sp) { - for (int i = 0; i < 3; i++) { - if (!offboard_control_sp_ignore_position(offboard_control_sp, i)) { - return false; - } - } - return true; -} - -/** - * Returns true if some position setpoints should be ignored - */ -inline bool offboard_control_sp_ignore_position_some(const struct offboard_control_setpoint_s &offboard_control_sp) { - for (int i = 0; i < 3; i++) { - if (offboard_control_sp_ignore_position(offboard_control_sp, i)) { - return true; - } - } - return false; -} - -/** - * Returns true if the velocity setpoint at index should be ignored - */ -inline bool offboard_control_sp_ignore_velocity(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { - return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_VEL_X + index))); -} - -/** - * Returns true if all velocity setpoints should be ignored - */ -inline bool offboard_control_sp_ignore_velocity_all(const struct offboard_control_setpoint_s &offboard_control_sp) { - for (int i = 0; i < 3; i++) { - if (!offboard_control_sp_ignore_velocity(offboard_control_sp, i)) { - return false; - } - } - return true; -} - -/** - * Returns true if some velocity setpoints should be ignored - */ -inline bool offboard_control_sp_ignore_velocity_some(const struct offboard_control_setpoint_s &offboard_control_sp) { - for (int i = 0; i < 3; i++) { - if (offboard_control_sp_ignore_velocity(offboard_control_sp, i)) { - return true; - } - } - return false; -} - -/** - * Returns true if the acceleration setpoint at index should be ignored - */ -inline bool offboard_control_sp_ignore_acceleration(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { - return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_ACC_X + index))); -} - -/** - * Returns true if all acceleration setpoints should be ignored - */ -inline bool offboard_control_sp_ignore_acceleration_all(const struct offboard_control_setpoint_s &offboard_control_sp) { - for (int i = 0; i < 3; i++) { - if (!offboard_control_sp_ignore_acceleration(offboard_control_sp, i)) { - return false; - } - } - return true; -} - -/** - * Returns true if some acceleration setpoints should be ignored - */ -inline bool offboard_control_sp_ignore_acceleration_some(const struct offboard_control_setpoint_s &offboard_control_sp) { - for (int i = 0; i < 3; i++) { - if (offboard_control_sp_ignore_acceleration(offboard_control_sp, i)) { - return true; - } - } - return false; -} - -/** - * Returns true if the bodyrate setpoint at index should be ignored - */ -inline bool offboard_control_sp_ignore_bodyrates(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { - return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_BODYRATE_X + index))); -} - -/** - * Returns true if some of the bodyrate setpoints should be ignored - */ -inline bool offboard_control_sp_ignore_bodyrates_some(const struct offboard_control_setpoint_s &offboard_control_sp) { - for (int i = 0; i < 3; i++) { - if (offboard_control_sp_ignore_bodyrates(offboard_control_sp, i)) { - return true; - } - } - return false; -} - -/** - * Returns true if the attitude setpoint should be ignored - */ -inline bool offboard_control_sp_ignore_attitude(const struct offboard_control_setpoint_s &offboard_control_sp) { - return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_ATT)); -} - -/** - * Returns true if the thrust setpoint should be ignored - */ -inline bool offboard_control_sp_ignore_thrust(const struct offboard_control_setpoint_s &offboard_control_sp) { - return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_THRUST)); -} - -/** - * Returns true if the yaw setpoint should be ignored - */ -inline bool offboard_control_sp_ignore_yaw(const struct offboard_control_setpoint_s &offboard_control_sp) { - return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_YAW)); -} - -/** - * Returns true if the yaw rate setpoint should be ignored - */ -inline bool offboard_control_sp_ignore_yawrate(const struct offboard_control_setpoint_s &offboard_control_sp) { - return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_YAWRATE)); -} - - -/* register this as object request broker structure */ -ORB_DECLARE(offboard_control_setpoint); - -#endif From 3d29fa5f4e387457ad425425a70c3a0792f808e7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 15 Feb 2015 19:22:47 +0100 Subject: [PATCH 016/112] mavlink receiver: fix indentation --- src/modules/mavlink/mavlink_receiver.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3a5ae47abb..3e09e5c816 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -580,10 +580,10 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the local pos values */ if (!offboard_control_mode.ignore_position) { - pos_sp_triplet.current.position_valid = true; - pos_sp_triplet.current.x = set_position_target_local_ned.x; - pos_sp_triplet.current.y = set_position_target_local_ned.y; - pos_sp_triplet.current.z = set_position_target_local_ned.z; + pos_sp_triplet.current.position_valid = true; + pos_sp_triplet.current.x = set_position_target_local_ned.x; + pos_sp_triplet.current.y = set_position_target_local_ned.y; + pos_sp_triplet.current.z = set_position_target_local_ned.z; } else { pos_sp_triplet.current.position_valid = false; } From 0389d30e0ee07b7d3d81da99dacfc1d5854eed26 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 16 Feb 2015 21:47:11 +0100 Subject: [PATCH 017/112] offboard attitude sp: handle thrust only messages if attitude/rates haven been used previously do not set the ignore flags even if the message asks us to do so to keep the controllers running --- src/modules/mavlink/mavlink_receiver.cpp | 23 ++++++++++++++++++----- 1 file changed, 18 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3e09e5c816..abaec51e43 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -693,7 +693,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) mavlink_set_attitude_target_t set_attitude_target; mavlink_msg_set_attitude_target_decode(msg, &set_attitude_target); - struct offboard_control_mode_s offboard_control_mode; + static struct offboard_control_mode_s offboard_control_mode = {}; memset(&offboard_control_mode, 0, sizeof(offboard_control_mode)); /* Only accept messages which are intended for this system */ @@ -702,12 +702,25 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) (mavlink_system.compid == set_attitude_target.target_component || set_attitude_target.target_component == 0)) { - /* set correct ignore flags for body rate fields: copy from mavlink message */ - offboard_control_mode.ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7); /* set correct ignore flags for thrust field: copy from mavlink message */ offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); - /* set correct ignore flags for attitude field: copy from mavlink message */ - offboard_control_mode.ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7)); + + /* + * if attitude or body rate have been used (not ignored) previously and this message only sends + * throttle and has the ignore bits set for attitude and rates don't change the flags for attitude and + * body rates to keep the controllers running + */ + bool ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7); + bool ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7)); + + if (ignore_bodyrate && ignore_attitude && !offboard_control_mode.ignore_thrust) { + /* Message want's us to ignore everything except thrust: only ignore if previously ignored */ + offboard_control_mode.ignore_bodyrate = ignore_bodyrate && offboard_control_mode.ignore_bodyrate; + offboard_control_mode.ignore_attitude = ignore_attitude && offboard_control_mode.ignore_attitude; + } else { + offboard_control_mode.ignore_bodyrate = ignore_bodyrate; + offboard_control_mode.ignore_attitude = ignore_attitude; + } offboard_control_mode.timestamp = hrt_absolute_time(); From ed16bd6fc6c2d4e8c080c9c9f1e43eda854acc4d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 17 Feb 2015 20:35:28 +0100 Subject: [PATCH 018/112] mavlinkreceiver: set att target: remove memset --- src/modules/mavlink/mavlink_receiver.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index abaec51e43..70bd390d8a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -694,7 +694,6 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) mavlink_msg_set_attitude_target_decode(msg, &set_attitude_target); static struct offboard_control_mode_s offboard_control_mode = {}; - memset(&offboard_control_mode, 0, sizeof(offboard_control_mode)); /* Only accept messages which are intended for this system */ if ((mavlink_system.sysid == set_attitude_target.target_system || From ee6dc51ef252751cccbc5018576a27d8a1bb5cf4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 21 Feb 2015 11:08:34 +0100 Subject: [PATCH 019/112] improve offboard attitude setpoint handling --- src/modules/mavlink/mavlink_receiver.cpp | 29 +++++++++++++++++------- 1 file changed, 21 insertions(+), 8 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 70bd390d8a..67942912f0 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -704,6 +704,13 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) /* set correct ignore flags for thrust field: copy from mavlink message */ offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); + /* + * The tricky part in pasrsing this message is that the offboard sender can set attitude and thrust + * using different messages. Eg.: First send set_attitude_target containing the attitude and ignore + * bits set for everything else and then send set_attitude_target containing the thrust and ignore bits + * set for everything else. + */ + /* * if attitude or body rate have been used (not ignored) previously and this message only sends * throttle and has the ignore bits set for attitude and rates don't change the flags for attitude and @@ -721,6 +728,10 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) offboard_control_mode.ignore_attitude = ignore_attitude; } + offboard_control_mode.ignore_position = true; + offboard_control_mode.ignore_velocity = true; + offboard_control_mode.ignore_acceleration_force = true; + offboard_control_mode.timestamp = hrt_absolute_time(); if (_offboard_control_mode_pub < 0) { @@ -742,15 +753,16 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) if (_control_mode.flag_control_offboard_enabled) { /* Publish attitude setpoint if attitude and thrust ignore bits are not set */ - if (!(offboard_control_mode.ignore_attitude || - offboard_control_mode.ignore_thrust)) { - struct vehicle_attitude_setpoint_s att_sp; + if (!(offboard_control_mode.ignore_attitude)) { + static struct vehicle_attitude_setpoint_s att_sp = {}; att_sp.timestamp = hrt_absolute_time(); mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body); att_sp.R_valid = true; - att_sp.thrust = set_attitude_target.thrust; + if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid + att_sp.thrust = set_attitude_target.thrust; + } att_sp.yaw_sp_move_rate = 0.0; memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d)); att_sp.q_d_valid = true; @@ -763,14 +775,15 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) /* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */ ///XXX add support for ignoring individual axes - if (!(offboard_control_mode.ignore_bodyrate || - offboard_control_mode.ignore_thrust)) { - struct vehicle_rates_setpoint_s rates_sp; + if (!(offboard_control_mode.ignore_bodyrate)) { + static struct vehicle_rates_setpoint_s rates_sp = {}; rates_sp.timestamp = hrt_absolute_time(); rates_sp.roll = set_attitude_target.body_roll_rate; rates_sp.pitch = set_attitude_target.body_pitch_rate; rates_sp.yaw = set_attitude_target.body_yaw_rate; - rates_sp.thrust = set_attitude_target.thrust; + if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid + rates_sp.thrust = set_attitude_target.thrust; + } if (_att_sp_pub < 0) { _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); From 9be3cd4a55767ba7748a565c508a9a7592d25c60 Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Thu, 19 Feb 2015 14:59:36 -0700 Subject: [PATCH 020/112] Updated mavlink to master --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 68a8882474..7ae438b86e 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 68a88824741ac26838ec0c8c38d67b51dc84b799 +Subproject commit 7ae438b86ea983e95af5f092e45a5d0f9d093c74 From 40ae0ebdaca797ef9265c00ebb7f38eb82df7336 Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Thu, 19 Feb 2015 15:17:52 -0700 Subject: [PATCH 021/112] Set up to receive mavlink actuator control messages and publish to uorb --- src/modules/mavlink/mavlink_receiver.cpp | 29 ++++++++++++++++++++++++ src/modules/mavlink/mavlink_receiver.h | 2 ++ 2 files changed, 31 insertions(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 67942912f0..86dff69c54 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -109,6 +109,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _flow_pub(-1), _range_pub(-1), _offboard_control_mode_pub(-1), + _actuator_controls_pub(-1), _global_vel_sp_pub(-1), _att_sp_pub(-1), _rates_sp_pub(-1), @@ -170,6 +171,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_set_attitude_target(msg); break; + case MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET: + handle_message_set_actuator_control_target(msg); + break; + case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: handle_message_vision_position_estimate(msg); break; @@ -648,6 +653,30 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } } +void +MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *msg) +{ + mavlink_set_actuator_control_target_t set_actuator_control_target; + mavlink_msg_set_actuator_control_target_decode(msg, &set_actuator_control_target); + + struct actuator_controls_s actuator_controls; + memset(&actuator_controls, 0, sizeof(actuator_controls));//XXX breaks compatibility with multiple setpoints + + if ((mavlink_system.sysid == set_actuator_control_target.target_system || + set_actuator_control_target.target_system == 0) && + (mavlink_system.compid == set_actuator_control_target.target_component || + set_actuator_control_target.target_component == 0)){ + + actuator_controls.timestamp = hrt_absolute_time(); + + for(size_t i = 0; i < 8 ; i++){ + actuator_controls.control[i] = set_actuator_control_target.controls[i]; + } + _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls); + } + +} + void MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 15943753d0..4886bbd0a4 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -122,6 +122,7 @@ private: void handle_message_vision_position_estimate(mavlink_message_t *msg); void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); void handle_message_set_position_target_local_ned(mavlink_message_t *msg); + void handle_message_set_actuator_control_target(mavlink_message_t *msg); void handle_message_set_attitude_target(mavlink_message_t *msg); void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); @@ -163,6 +164,7 @@ private: orb_advert_t _flow_pub; orb_advert_t _range_pub; orb_advert_t _offboard_control_mode_pub; + orb_advert_t _actuator_controls_pub; orb_advert_t _global_vel_sp_pub; orb_advert_t _att_sp_pub; orb_advert_t _rates_sp_pub; From 877232119fe78af70a6a096f7d3a3deec2d825fe Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Tue, 24 Feb 2015 12:29:51 -0700 Subject: [PATCH 022/112] Small changes --- src/modules/fw_att_control/fw_att_control_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 6e49bd1d15..5052b41c37 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -520,7 +520,7 @@ FixedwingAttitudeControl::vehicle_control_mode_poll() orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated); if (vcontrol_mode_updated) { - + //vehicle_control_mode_s orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode); } } @@ -795,10 +795,10 @@ FixedwingAttitudeControl::task_main() /* Simple handling of failsafe: deploy parachute if failsafe is on */ if (_vcontrol_mode.flag_control_termination_enabled) { _actuators_airframe.control[7] = 1.0f; -// warnx("_actuators_airframe.control[1] = 1.0f;"); + //warnx("_actuators_airframe.control[1] = 1.0f;"); } else { _actuators_airframe.control[7] = 0.0f; -// warnx("_actuators_airframe.control[1] = -1.0f;"); + //warnx("_actuators_airframe.control[1] = -1.0f;"); } /* decide if in stabilized or full manual control */ From 5e199b3984e35e6a8078abbc36d9c9440e1cd7af Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Tue, 24 Feb 2015 13:31:22 -0700 Subject: [PATCH 023/112] Set ignore flags to true --- src/modules/mavlink/mavlink_receiver.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 86dff69c54..7c506e0056 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -667,6 +667,24 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m (mavlink_system.compid == set_actuator_control_target.target_component || set_actuator_control_target.target_component == 0)){ + /* ignore all since we are setting raw actuators here */ + offboard_control_mode.ignore_thrust = true; + offboard_control_mode.ignore_attitude = true; + offboard_control_mode.ignore_bodyrate = true; + offboard_control_mode.ignore_position = true; + offboard_control_mode.ignore_velocity = true; + offboard_control_mode.ignore_acceleration_force = true; + + offboard_control_mode.timestamp = hrt_absolute_time(); + + if (_offboard_control_mode_pub < 0) { + _offboard_control_mode_pub = orb_advertise(O + actuator_controls.timestamp = RB_ID(offboard_control_mode), &offboard_control_mode); + + } else { + orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode); + } + actuator_controls.timestamp = hrt_absolute_time(); for(size_t i = 0; i < 8 ; i++){ From e2de72b882d7c74cdaafcfc74ab7176ef94a4455 Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Tue, 24 Feb 2015 16:43:49 -0700 Subject: [PATCH 024/112] Added offboard actuator controls flags to offboard control mode and vehicle control mode to disable controls in att_control apps --- msg/vehicle_control_mode.msg | 1 + src/modules/commander/commander.cpp | 9 ++++ .../fw_att_control/fw_att_control_main.cpp | 28 +++++++----- src/modules/mavlink/mavlink_receiver.cpp | 7 ++- .../mc_att_control/mc_att_control_main.cpp | 2 +- .../uORB/topics/offboard_control_mode.h | 1 + .../vtol_att_control_main.cpp | 44 +++++++++++-------- 7 files changed, 57 insertions(+), 35 deletions(-) diff --git a/msg/vehicle_control_mode.msg b/msg/vehicle_control_mode.msg index 153a642bb4..44f0186421 100644 --- a/msg/vehicle_control_mode.msg +++ b/msg/vehicle_control_mode.msg @@ -12,6 +12,7 @@ bool flag_system_hil_enabled bool flag_control_manual_enabled # true if manual input is mixed in bool flag_control_auto_enabled # true if onboard autopilot should act bool flag_control_offboard_enabled # true if offboard control should be used +bool flag_control_offboard_actuator_control_enabled #true if raw actuator control in offboard used bool flag_control_rates_enabled # true if rates are stabilized bool flag_control_attitude_enabled # true if attitude stabilization is mixed in bool flag_control_force_enabled # true if force control is mixed in diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5a362666ca..e688b8fa52 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2302,6 +2302,7 @@ set_control_mode() control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol); control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON; control_mode.flag_control_offboard_enabled = false; + control_mode.flag_control_offboard_actuator_control_enabled = false; switch (status.nav_state) { case vehicle_status_s::NAVIGATION_STATE_MANUAL: @@ -2452,6 +2453,14 @@ set_control_mode() control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_position; + control_mode.flag_control_offboard_actuator_control_enabled = offboard_control_mode.ignore_thrust && + offboard_control_mode.ignore_attitude && + offboard_control_mode.ignore_bodyrate && + offboard_control_mode.ignore_position && + offboard_control_mode.ignore_velocity && + offboard_control_mode.ignore_acceleration_force && + offboard_control_mode.actuator_control_mode; + break; default: diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 5052b41c37..a7b563b0f1 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -1077,20 +1077,24 @@ FixedwingAttitudeControl::task_main() _actuators_airframe.timestamp = hrt_absolute_time(); _actuators_airframe.timestamp_sample = _att.timestamp; - /* publish the actuator controls */ - if (_actuators_0_pub > 0) { - orb_publish(_actuators_id, _actuators_0_pub, &_actuators); - } else if (_actuators_id) { - _actuators_0_pub= orb_advertise(_actuators_id, &_actuators); - } + /* Only publish if actuator_control mode is not enabled */ + if(!_vcontrol_mode.flag_control_offboard_actuator_control_enabled) + { + /* publish the actuator controls */ + if (_actuators_0_pub > 0) { + orb_publish(_actuators_id, _actuators_0_pub, &_actuators); + } else if (_actuators_id) { + _actuators_0_pub= orb_advertise(_actuators_id, &_actuators); + } - if (_actuators_2_pub > 0) { - /* publish the actuator controls*/ - orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe); + if (_actuators_2_pub > 0) { + /* publish the actuator controls*/ + orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe); - } else { - /* advertise and publish */ - _actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe); + } else { + /* advertise and publish */ + _actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe); + } } } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7c506e0056..48866f0365 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -540,7 +540,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400); /* yawrate ignore flag mapps to ignore_bodyrate */ offboard_control_mode.ignore_bodyrate = (bool)(set_position_target_local_ned.type_mask & 0x800); - + offboard_control_mode.actuator_control_mode = false; offboard_control_mode.timestamp = hrt_absolute_time(); @@ -678,9 +678,7 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m offboard_control_mode.timestamp = hrt_absolute_time(); if (_offboard_control_mode_pub < 0) { - _offboard_control_mode_pub = orb_advertise(O - actuator_controls.timestamp = RB_ID(offboard_control_mode), &offboard_control_mode); - + _offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode); } else { orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode); } @@ -774,6 +772,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) offboard_control_mode.ignore_bodyrate = ignore_bodyrate; offboard_control_mode.ignore_attitude = ignore_attitude; } + offboard_control_mode.actuator_control_mode = false; offboard_control_mode.ignore_position = true; offboard_control_mode.ignore_velocity = true; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index a0d76b0a60..099b92404e 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -804,7 +804,7 @@ MulticopterAttitudeControl::task_main() _actuators.timestamp = hrt_absolute_time(); _actuators.timestamp_sample = _v_att.timestamp; - if (!_actuators_0_circuit_breaker_enabled) { + if (!_actuators_0_circuit_breaker_enabled || !_v_control_mode.flag_control_offboard_actuator_control_enabled) { if (_actuators_0_pub > 0) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); perf_end(_controller_latency_perf); diff --git a/src/modules/uORB/topics/offboard_control_mode.h b/src/modules/uORB/topics/offboard_control_mode.h index 559659a1d4..956c3881ee 100644 --- a/src/modules/uORB/topics/offboard_control_mode.h +++ b/src/modules/uORB/topics/offboard_control_mode.h @@ -61,6 +61,7 @@ struct offboard_control_mode_s { bool ignore_position; bool ignore_velocity; bool ignore_acceleration_force; + bool actuator_control_mode; }; /**< offboard control inputs */ /** diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 288ba2ebe0..53a08f97a5 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -782,18 +782,22 @@ void VtolAttitudeControl::task_main() fill_mc_att_control_output(); fill_mc_att_rates_sp(); - if (_actuators_0_pub > 0) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); + /* Only publish if actuator_control mode is not enabled */ + if(!_v_control_mode.flag_control_offboard_actuator_control_enabled) + { + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); - } + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); + } - if (_actuators_1_pub > 0) { - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); + if (_actuators_1_pub > 0) { + orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); - } else { - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); + } else { + _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); + } } } } @@ -813,18 +817,22 @@ void VtolAttitudeControl::task_main() fill_fw_att_control_output(); fill_fw_att_rates_sp(); - if (_actuators_0_pub > 0) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); + /* Only publish if actuator_control mode is not enabled */ + if(!_v_control_mode.flag_control_offboard_actuator_control_enabled) + { + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); - } + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); + } - if (_actuators_1_pub > 0) { - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); + if (_actuators_1_pub > 0) { + orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); - } else { - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); + } else { + _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); + } } } } From fd4fd4eaaa39330d4a1cf1154100e6d1201e847f Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Tue, 24 Feb 2015 16:52:50 -0700 Subject: [PATCH 025/112] Set the actuator control flag in receiver --- src/modules/mavlink/mavlink_receiver.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 48866f0365..8291f20068 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -674,6 +674,8 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m offboard_control_mode.ignore_position = true; offboard_control_mode.ignore_velocity = true; offboard_control_mode.ignore_acceleration_force = true; + + offboard_control_mode.actuator_control_mode = false; offboard_control_mode.timestamp = hrt_absolute_time(); From 7d6723aa2d088e7527d043704a2fc6b9297d248f Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Tue, 24 Feb 2015 16:53:47 -0700 Subject: [PATCH 026/112] small change --- src/modules/mavlink/mavlink_receiver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8291f20068..eb1af79d65 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -674,8 +674,8 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m offboard_control_mode.ignore_position = true; offboard_control_mode.ignore_velocity = true; offboard_control_mode.ignore_acceleration_force = true; - - offboard_control_mode.actuator_control_mode = false; + + offboard_control_mode.actuator_control_mode = true; offboard_control_mode.timestamp = hrt_absolute_time(); From b8aa79cae4da9343a0ab301c6383c6ed0dfc61db Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Wed, 25 Feb 2015 11:54:41 -0700 Subject: [PATCH 027/112] Made changes to have actuator controls mirror other syntax more closely --- src/modules/mavlink/mavlink_receiver.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index eb1af79d65..c551fdfea3 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -690,7 +690,13 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m for(size_t i = 0; i < 8 ; i++){ actuator_controls.control[i] = set_actuator_control_target.controls[i]; } - _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls); + + if (_offboard_control_mode_pub < 0) { + _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls); + } else { + orb_publish(ORB_ID(actuator_controls_0), _actuator_controls_pub, &actuator_controls); + } + } } From b0e878dbae1f471ddf0d3c063f238260124f9d5a Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Wed, 25 Feb 2015 12:00:23 -0700 Subject: [PATCH 028/112] Compiler error --- src/modules/mavlink/mavlink_receiver.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c551fdfea3..a4c6c3da44 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -659,9 +659,12 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m mavlink_set_actuator_control_target_t set_actuator_control_target; mavlink_msg_set_actuator_control_target_decode(msg, &set_actuator_control_target); + struct offboard_control_mode_s offboard_control_mode; + memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));//XXX breaks compatibility with multiple setpoints + struct actuator_controls_s actuator_controls; memset(&actuator_controls, 0, sizeof(actuator_controls));//XXX breaks compatibility with multiple setpoints - + if ((mavlink_system.sysid == set_actuator_control_target.target_system || set_actuator_control_target.target_system == 0) && (mavlink_system.compid == set_actuator_control_target.target_component || From d7dc3a3ee85e3a3c28d97ef49d53026f254d3650 Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Wed, 25 Feb 2015 12:12:14 -0700 Subject: [PATCH 029/112] Cleaned up some chunky code --- src/modules/commander/commander.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e688b8fa52..024e6c18a4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2453,13 +2453,13 @@ set_control_mode() control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_position; - control_mode.flag_control_offboard_actuator_control_enabled = offboard_control_mode.ignore_thrust && - offboard_control_mode.ignore_attitude && - offboard_control_mode.ignore_bodyrate && - offboard_control_mode.ignore_position && - offboard_control_mode.ignore_velocity && - offboard_control_mode.ignore_acceleration_force && - offboard_control_mode.actuator_control_mode; + control_mode.flag_control_offboard_actuator_control_enabled = offboard_control_mode.ignore_thrust && + offboard_control_mode.ignore_attitude && + offboard_control_mode.ignore_bodyrate && + offboard_control_mode.ignore_position && + offboard_control_mode.ignore_velocity && + offboard_control_mode.ignore_acceleration_force && + offboard_control_mode.actuator_control_mode; break; From 220fb19eb74b330bd3e97efc628aeb1bda510dcf Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Wed, 25 Feb 2015 14:25:28 -0700 Subject: [PATCH 030/112] Removed actuator_control_mode flags...Using pre-existing flags instead --- msg/vehicle_control_mode.msg | 1 - src/modules/commander/commander.cpp | 10 ---------- .../fw_att_control/fw_att_control_main.cpp | 7 ++++--- src/modules/mavlink/mavlink_receiver.cpp | 18 +++++++----------- .../mc_att_control/mc_att_control_main.cpp | 2 +- .../uORB/topics/offboard_control_mode.h | 1 - .../vtol_att_control/vtol_att_control_main.cpp | 10 ++++++---- 7 files changed, 18 insertions(+), 31 deletions(-) diff --git a/msg/vehicle_control_mode.msg b/msg/vehicle_control_mode.msg index 44f0186421..153a642bb4 100644 --- a/msg/vehicle_control_mode.msg +++ b/msg/vehicle_control_mode.msg @@ -12,7 +12,6 @@ bool flag_system_hil_enabled bool flag_control_manual_enabled # true if manual input is mixed in bool flag_control_auto_enabled # true if onboard autopilot should act bool flag_control_offboard_enabled # true if offboard control should be used -bool flag_control_offboard_actuator_control_enabled #true if raw actuator control in offboard used bool flag_control_rates_enabled # true if rates are stabilized bool flag_control_attitude_enabled # true if attitude stabilization is mixed in bool flag_control_force_enabled # true if force control is mixed in diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 024e6c18a4..2c5e0aa886 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2302,7 +2302,6 @@ set_control_mode() control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol); control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON; control_mode.flag_control_offboard_enabled = false; - control_mode.flag_control_offboard_actuator_control_enabled = false; switch (status.nav_state) { case vehicle_status_s::NAVIGATION_STATE_MANUAL: @@ -2442,7 +2441,6 @@ set_control_mode() !offboard_control_mode.ignore_velocity || !offboard_control_mode.ignore_acceleration_force; - control_mode.flag_control_velocity_enabled = !offboard_control_mode.ignore_velocity || !offboard_control_mode.ignore_position; @@ -2453,14 +2451,6 @@ set_control_mode() control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_position; - control_mode.flag_control_offboard_actuator_control_enabled = offboard_control_mode.ignore_thrust && - offboard_control_mode.ignore_attitude && - offboard_control_mode.ignore_bodyrate && - offboard_control_mode.ignore_position && - offboard_control_mode.ignore_velocity && - offboard_control_mode.ignore_acceleration_force && - offboard_control_mode.actuator_control_mode; - break; default: diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index a7b563b0f1..5daeae477e 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -520,7 +520,7 @@ FixedwingAttitudeControl::vehicle_control_mode_poll() orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated); if (vcontrol_mode_updated) { - //vehicle_control_mode_s + orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode); } } @@ -1077,8 +1077,9 @@ FixedwingAttitudeControl::task_main() _actuators_airframe.timestamp = hrt_absolute_time(); _actuators_airframe.timestamp_sample = _att.timestamp; - /* Only publish if actuator_control mode is not enabled */ - if(!_vcontrol_mode.flag_control_offboard_actuator_control_enabled) + /* Only publish if any of the proper modes are enabled */ + if(_vcontrol_mode.flag_control_rates_enabled || + _vcontrol_mode.flag_control_attitude_enabled) { /* publish the actuator controls */ if (_actuators_0_pub > 0) { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index a4c6c3da44..27a6d4bcac 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -540,8 +540,6 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400); /* yawrate ignore flag mapps to ignore_bodyrate */ offboard_control_mode.ignore_bodyrate = (bool)(set_position_target_local_ned.type_mask & 0x800); - offboard_control_mode.actuator_control_mode = false; - offboard_control_mode.timestamp = hrt_absolute_time(); @@ -665,10 +663,10 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m struct actuator_controls_s actuator_controls; memset(&actuator_controls, 0, sizeof(actuator_controls));//XXX breaks compatibility with multiple setpoints - if ((mavlink_system.sysid == set_actuator_control_target.target_system || - set_actuator_control_target.target_system == 0) && - (mavlink_system.compid == set_actuator_control_target.target_component || - set_actuator_control_target.target_component == 0)){ + if ((mavlink_system.sysid == set_actuator_control_target.target_system || + set_actuator_control_target.target_system == 0) && + (mavlink_system.compid == set_actuator_control_target.target_component || + set_actuator_control_target.target_component == 0)) { /* ignore all since we are setting raw actuators here */ offboard_control_mode.ignore_thrust = true; @@ -678,8 +676,6 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m offboard_control_mode.ignore_velocity = true; offboard_control_mode.ignore_acceleration_force = true; - offboard_control_mode.actuator_control_mode = true; - offboard_control_mode.timestamp = hrt_absolute_time(); if (_offboard_control_mode_pub < 0) { @@ -690,11 +686,12 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m actuator_controls.timestamp = hrt_absolute_time(); - for(size_t i = 0; i < 8 ; i++){ + /* Set duty cycles for the servos in actuator_controls_0 */ + for(size_t i = 0; i < 8; i++) { actuator_controls.control[i] = set_actuator_control_target.controls[i]; } - if (_offboard_control_mode_pub < 0) { + if (_actuator_controls_pub < 0) { _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls); } else { orb_publish(ORB_ID(actuator_controls_0), _actuator_controls_pub, &actuator_controls); @@ -783,7 +780,6 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) offboard_control_mode.ignore_bodyrate = ignore_bodyrate; offboard_control_mode.ignore_attitude = ignore_attitude; } - offboard_control_mode.actuator_control_mode = false; offboard_control_mode.ignore_position = true; offboard_control_mode.ignore_velocity = true; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 099b92404e..a0d76b0a60 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -804,7 +804,7 @@ MulticopterAttitudeControl::task_main() _actuators.timestamp = hrt_absolute_time(); _actuators.timestamp_sample = _v_att.timestamp; - if (!_actuators_0_circuit_breaker_enabled || !_v_control_mode.flag_control_offboard_actuator_control_enabled) { + if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub > 0) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); perf_end(_controller_latency_perf); diff --git a/src/modules/uORB/topics/offboard_control_mode.h b/src/modules/uORB/topics/offboard_control_mode.h index 956c3881ee..559659a1d4 100644 --- a/src/modules/uORB/topics/offboard_control_mode.h +++ b/src/modules/uORB/topics/offboard_control_mode.h @@ -61,7 +61,6 @@ struct offboard_control_mode_s { bool ignore_position; bool ignore_velocity; bool ignore_acceleration_force; - bool actuator_control_mode; }; /**< offboard control inputs */ /** diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 53a08f97a5..74e1efd6cc 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -782,8 +782,9 @@ void VtolAttitudeControl::task_main() fill_mc_att_control_output(); fill_mc_att_rates_sp(); - /* Only publish if actuator_control mode is not enabled */ - if(!_v_control_mode.flag_control_offboard_actuator_control_enabled) + /* Only publish if the proper mode(s) are enabled */ + if(_v_control_mode.flag_control_attitude_enabled || + _v_control_mode.flag_control_rates_enabled) { if (_actuators_0_pub > 0) { orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); @@ -817,8 +818,9 @@ void VtolAttitudeControl::task_main() fill_fw_att_control_output(); fill_fw_att_rates_sp(); - /* Only publish if actuator_control mode is not enabled */ - if(!_v_control_mode.flag_control_offboard_actuator_control_enabled) + /* Only publish if the proper mode(s) are enabled */ + if(_v_control_mode.flag_control_attitude_enabled || + _v_control_mode.flag_control_rates_enabled) { if (_actuators_0_pub > 0) { orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); From 0f51907dd662ae6ebc9ab997dfeda273769cffce Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Wed, 25 Feb 2015 17:31:56 -0700 Subject: [PATCH 031/112] Check if offboard mode was activated before publishing controls --- src/modules/mavlink/mavlink_receiver.cpp | 28 ++++++++++++++++-------- 1 file changed, 19 insertions(+), 9 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 27a6d4bcac..bce93cc6a5 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -684,19 +684,29 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode); } - actuator_controls.timestamp = hrt_absolute_time(); - /* Set duty cycles for the servos in actuator_controls_0 */ - for(size_t i = 0; i < 8; i++) { - actuator_controls.control[i] = set_actuator_control_target.controls[i]; + /* If we are in offboard control mode, publish the actuator controls */ + bool updated; + orb_check(_control_mode_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } - if (_actuator_controls_pub < 0) { - _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls); - } else { - orb_publish(ORB_ID(actuator_controls_0), _actuator_controls_pub, &actuator_controls); + if (_control_mode.flag_control_offboard_enabled) { + + actuator_controls.timestamp = hrt_absolute_time(); + + /* Set duty cycles for the servos in actuator_controls_0 */ + for(size_t i = 0; i < 8; i++) { + actuator_controls.control[i] = set_actuator_control_target.controls[i]; + } + + if (_actuator_controls_pub < 0) { + _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls); + } else { + orb_publish(ORB_ID(actuator_controls_0), _actuator_controls_pub, &actuator_controls); + } } - } } From 2d0c5616cbc89f0621e2e8eb56b06635ecedfd90 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 10 Feb 2015 22:35:32 +0100 Subject: [PATCH 032/112] ros: add skeleton for mavlink node --- CMakeLists.txt | 9 ++++ src/platforms/ros/nodes/mavlink/mavlink.cpp | 60 +++++++++++++++++++++ src/platforms/ros/nodes/mavlink/mavlink.h | 60 +++++++++++++++++++++ 3 files changed, 129 insertions(+) create mode 100644 src/platforms/ros/nodes/mavlink/mavlink.cpp create mode 100644 src/platforms/ros/nodes/mavlink/mavlink.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 0c81607b00..1434b43961 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -240,6 +240,15 @@ target_link_libraries(commander px4 ) +## Mavlink +add_executable(mavlink + src/platforms/ros/nodes/mavlink/mavlink.cpp) +add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(mavlink + ${catkin_LIBRARIES} + px4 +) + ############# ## Install ## ############# diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp new file mode 100644 index 0000000000..e64e576bde --- /dev/null +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink.cpp + * Dummy mavlink node that interfaces to a mavros node via UDP + * This simulates the onboard mavlink app to some degree. It should be possible to + * send offboard setpoints via mavros to the SITL setup the same way as on the real system + * + * @author Thomas Gubler +*/ + +#include "mavlink.h" + +#include + +using namespace px4; + +Mavlink::Mavlink() : + _n() +{ +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "mavlink"); + Mavlink m; + ros::spin(); + return 0; +} diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h new file mode 100644 index 0000000000..2723df35c4 --- /dev/null +++ b/src/platforms/ros/nodes/mavlink/mavlink.h @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink.h + * Dummy mavlink node that interfaces to a mavros node via UDP + * This simulates the onboard mavlink app to some degree. It should be possible to + * send offboard setpoints via mavros to the SITL setup the same way as on the real system + * + * @author Thomas Gubler +*/ + +#include "ros/ros.h" + +namespace px4 +{ + +class Mavlink +{ +public: + Mavlink(); + + ~Mavlink() {} + +protected: + + ros::NodeHandle _n; +}; + +} From 001575e740261acf9de68023ab8e8bd59a478ce3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 10 Feb 2015 23:34:26 +0100 Subject: [PATCH 033/112] ros: mavlink node: add mavconn link --- CMakeLists.txt | 4 +++- package.xml | 2 ++ src/platforms/ros/nodes/mavlink/mavlink.cpp | 2 ++ src/platforms/ros/nodes/mavlink/mavlink.h | 2 ++ 4 files changed, 9 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1434b43961..491c2c7ed2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,6 +3,7 @@ project(px4) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") add_definitions(-D__PX4_ROS) add_definitions(-D__EXPORT=) +add_definitions(-DMAVLINK_DIALECT=common) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) @@ -16,6 +17,7 @@ find_package(catkin REQUIRED COMPONENTS gazebo_msgs sensor_msgs mav_msgs + libmavconn ) find_package(Eigen REQUIRED) @@ -109,7 +111,7 @@ generate_messages( catkin_package( INCLUDE_DIRS src/include LIBRARIES px4 - CATKIN_DEPENDS message_runtime roscpp rospy std_msgs + CATKIN_DEPENDS message_runtime roscpp rospy std_msgs libmavconn DEPENDS system_lib ) diff --git a/package.xml b/package.xml index 6662003907..96d622a682 100644 --- a/package.xml +++ b/package.xml @@ -44,10 +44,12 @@ rospy std_msgs eigen + libmavconn roscpp rospy std_msgs eigen + libmavconn diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index e64e576bde..b6413c3b4e 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -49,6 +49,8 @@ using namespace px4; Mavlink::Mavlink() : _n() { + + _link = mavconn::MAVConnInterface::open_url("udp://localhost:14551@localhost:14552"); } int main(int argc, char **argv) diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h index 2723df35c4..5b39468870 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.h +++ b/src/platforms/ros/nodes/mavlink/mavlink.h @@ -41,6 +41,7 @@ */ #include "ros/ros.h" +#include namespace px4 { @@ -55,6 +56,7 @@ public: protected: ros::NodeHandle _n; + mavconn::MAVConnInterface::Ptr _link; }; } From 3e5cbfcf77939d5f650885c7a82aaf527d40a094 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 11 Feb 2015 23:02:58 +0100 Subject: [PATCH 034/112] ros: mavlink onboard node: send attitude via mavlink --- NuttX | 2 +- launch/multicopter.launch | 1 + src/platforms/ros/nodes/mavlink/mavlink.cpp | 24 +++++++++++++++++++-- src/platforms/ros/nodes/mavlink/mavlink.h | 4 ++++ 4 files changed, 28 insertions(+), 3 deletions(-) diff --git a/NuttX b/NuttX index 11afcdfee6..787aca971a 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 11afcdfee6a3961952dd92f02c1abaa4756b115f +Subproject commit 787aca971a86219d4e791100646b54ed8245a733 diff --git a/launch/multicopter.launch b/launch/multicopter.launch index 95400bd82d..bc0e377715 100644 --- a/launch/multicopter.launch +++ b/launch/multicopter.launch @@ -9,6 +9,7 @@ + diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index b6413c3b4e..131a4930f2 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -47,10 +47,11 @@ using namespace px4; Mavlink::Mavlink() : - _n() + _n(), + _v_att_sub(_n.subscribe("vehicle_attitude", 10, &Mavlink::VehicleAttitudeCallback, this)) { - _link = mavconn::MAVConnInterface::open_url("udp://localhost:14551@localhost:14552"); + _link = mavconn::MAVConnInterface::open_url("udp://localhost:14565@localhost:14560"); } int main(int argc, char **argv) @@ -60,3 +61,22 @@ int main(int argc, char **argv) ros::spin(); return 0; } + +void Mavlink::VehicleAttitudeCallback(const px4::vehicle_attitudeConstPtr &msg) +{ + mavlink_message_t msg_m; + mavlink_msg_attitude_quaternion_pack_chan( + _link->get_system_id(), + _link->get_component_id(), + _link->get_channel(), + &msg_m, //XXX hardcoded + get_time_micros() / 1000, + msg->q[0], + msg->q[1], + msg->q[2], + msg->q[3], + msg->rollspeed, + msg->pitchspeed, + msg->yawspeed); + _link->send_message(&msg_m); +} diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h index 5b39468870..e683597a94 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.h +++ b/src/platforms/ros/nodes/mavlink/mavlink.h @@ -42,6 +42,7 @@ #include "ros/ros.h" #include +#include namespace px4 { @@ -57,6 +58,9 @@ protected: ros::NodeHandle _n; mavconn::MAVConnInterface::Ptr _link; + ros::Subscriber _v_att_sub; + + void VehicleAttitudeCallback(const px4::vehicle_attitudeConstPtr &msg); }; } From 93f8fc33c890bb961f0fba03537cc54bf8a88d1f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 12 Feb 2015 20:43:52 +0100 Subject: [PATCH 035/112] ros mavlink node: handle set_attitude_target --- src/platforms/ros/nodes/mavlink/mavlink.cpp | 43 +++++++++++++++++++-- src/platforms/ros/nodes/mavlink/mavlink.h | 26 ++++++++++++- 2 files changed, 65 insertions(+), 4 deletions(-) diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index 131a4930f2..a7830e97fb 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -48,10 +48,12 @@ using namespace px4; Mavlink::Mavlink() : _n(), - _v_att_sub(_n.subscribe("vehicle_attitude", 10, &Mavlink::VehicleAttitudeCallback, this)) + _v_att_sub(_n.subscribe("vehicle_attitude", 1, &Mavlink::VehicleAttitudeCallback, this)), + _v_att_sp_pub(_n.advertise("vehicle_attitude_setpoint", 1)) { - _link = mavconn::MAVConnInterface::open_url("udp://localhost:14565@localhost:14560"); + _link->message_received.connect(boost::bind(&Mavlink::handle_msg, this, _1, _2, _3)); + } int main(int argc, char **argv) @@ -62,7 +64,7 @@ int main(int argc, char **argv) return 0; } -void Mavlink::VehicleAttitudeCallback(const px4::vehicle_attitudeConstPtr &msg) +void Mavlink::VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg) { mavlink_message_t msg_m; mavlink_msg_attitude_quaternion_pack_chan( @@ -80,3 +82,38 @@ void Mavlink::VehicleAttitudeCallback(const px4::vehicle_attitudeConstPtr &msg) msg->yawspeed); _link->send_message(&msg_m); } + +void Mavlink::handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid) { + (void)sysid; + (void)compid; + + switch(mmsg->msgid) { + case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: + handle_msg_set_attitude_target(mmsg); + break; + default: + break; + } + +} + +void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) +{ + mavlink_set_attitude_target_t set_att_target; + mavlink_msg_set_attitude_target_decode(mmsg, &set_att_target); + + vehicle_attitude_setpoint msg; + + msg.timestamp = get_time_micros(); + mavlink_quaternion_to_euler(set_att_target.q, &msg.roll_body, &msg.pitch_body, &msg.yaw_body); + mavlink_quaternion_to_dcm(set_att_target.q, (float(*)[3])msg.R_body.data()); + msg.R_valid = true; + msg.thrust = set_att_target.thrust; + for (ssize_t i = 0; i < 4; i++) { + msg.q_d[i] = set_att_target.q[i]; + } + msg.q_d_valid = true; + + _v_att_sp_pub.publish(msg); + +} diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h index e683597a94..6f75364363 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.h +++ b/src/platforms/ros/nodes/mavlink/mavlink.h @@ -43,6 +43,7 @@ #include "ros/ros.h" #include #include +#include namespace px4 { @@ -59,8 +60,31 @@ protected: ros::NodeHandle _n; mavconn::MAVConnInterface::Ptr _link; ros::Subscriber _v_att_sub; + ros::Publisher _v_att_sp_pub; + + /** + * + * Simulates output of attitude data from the FCU + * Equivalent to the mavlink stream ATTITUDE + * + * */ + void VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg); + + + /** + * + * Handle incoming mavlink messages ant publish them to ROS ("Mavlink Receiver") + * + * */ + void handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid); + + /** + * + * Handle SET_ATTITUDE_TARGET mavlink messages + * + * */ + void handle_msg_set_attitude_target(const mavlink_message_t *mmsg); - void VehicleAttitudeCallback(const px4::vehicle_attitudeConstPtr &msg); }; } From 1c9509c235fe9b9d36612768a9935163c7b18cfa Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 14 Feb 2015 16:45:05 +0100 Subject: [PATCH 036/112] move offboard_control_mode topic to msg mode --- msg/offboard_control_mode.msg | 9 +++ .../uORB/topics/offboard_control_mode.h | 73 ------------------- 2 files changed, 9 insertions(+), 73 deletions(-) create mode 100644 msg/offboard_control_mode.msg delete mode 100644 src/modules/uORB/topics/offboard_control_mode.h diff --git a/msg/offboard_control_mode.msg b/msg/offboard_control_mode.msg new file mode 100644 index 0000000000..e2d06963b6 --- /dev/null +++ b/msg/offboard_control_mode.msg @@ -0,0 +1,9 @@ +# Off-board control mode +uint64 timestamp + +bool ignore_thrust +bool ignore_attitude +bool ignore_bodyrate +bool ignore_position +bool ignore_velocity +bool ignore_acceleration_force diff --git a/src/modules/uORB/topics/offboard_control_mode.h b/src/modules/uORB/topics/offboard_control_mode.h deleted file mode 100644 index 559659a1d4..0000000000 --- a/src/modules/uORB/topics/offboard_control_mode.h +++ /dev/null @@ -1,73 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2015 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * - * 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 offboard_control_mode.h - * Definition of the manual_control_setpoint uORB topic. - */ - -#ifndef TOPIC_OFFBOARD_CONTROL_MODE_H_ -#define TOPIC_OFFBOARD_CONTROL_MODE_H_ - -#include -#include "../uORB.h" - -/** - * Off-board control mode - */ - -/** - * @addtogroup topics - * @{ - */ - -struct offboard_control_mode_s { - uint64_t timestamp; - - bool ignore_thrust; - bool ignore_attitude; - bool ignore_bodyrate; - bool ignore_position; - bool ignore_velocity; - bool ignore_acceleration_force; - -}; /**< offboard control inputs */ -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(offboard_control_mode); - -#endif From 3475d8883be075b8fb1e476154a88870ca2bd5e5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 14 Feb 2015 16:45:46 +0100 Subject: [PATCH 037/112] enable offboard control mode topic for multiplatform --- CMakeLists.txt | 1 + src/platforms/px4_includes.h | 2 ++ 2 files changed, 3 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 491c2c7ed2..4e91fb3bee 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -76,6 +76,7 @@ add_message_files( position_setpoint_triplet.msg vehicle_local_position_setpoint.msg vehicle_global_velocity_setpoint.msg + offboard_control_mode.msg ) ## Generate services in the 'srv' folder diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index f8561fa3b7..364a5f31b9 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -66,6 +66,7 @@ #include #include #include +#include #endif #else @@ -93,6 +94,7 @@ #include #include #include +#include #endif #include #include From 4869f9f3d46babb53cdde47fabda4d1b22399565 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 14 Feb 2015 16:47:24 +0100 Subject: [PATCH 038/112] ros:manual input dummy node: add offboard button --- .../ros/nodes/manual_input/manual_input.cpp | 15 +++++++++++++-- .../ros/nodes/manual_input/manual_input.h | 2 ++ 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp index 72f6e252f9..d5d14a21fb 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.cpp +++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp @@ -76,7 +76,8 @@ ManualInput::ManualInput() : _n.param("map_posctl", _param_buttons_map[2], 2); _n.param("map_auto_mission", _param_buttons_map[3], 3); _n.param("map_auto_loiter", _param_buttons_map[4], 4); - _n.param("map_auto_rtl", _param_buttons_map[5], 4); + _n.param("map_auto_rtl", _param_buttons_map[5], 5); + _n.param("map_offboard", _param_buttons_map[6], 6); /* Default to manual */ _msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; @@ -120,7 +121,6 @@ void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, do void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_control_setpoint &msg_mc_sp) { msg_mc_sp.acro_switch = px4::manual_control_setpoint::SWITCH_POS_NONE; - msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_NONE; if (_buttons_state[MAIN_STATE_MANUAL] != msg->buttons[_param_buttons_map[MAIN_STATE_MANUAL]] == true) { msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; @@ -128,6 +128,7 @@ void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_co msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; return; } else if (_buttons_state[MAIN_STATE_ALTCTL] != msg->buttons[_param_buttons_map[MAIN_STATE_ALTCTL]] == true) { @@ -136,6 +137,7 @@ void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_co msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; return; } else if (_buttons_state[MAIN_STATE_POSCTL] != msg->buttons[_param_buttons_map[MAIN_STATE_POSCTL]] == true) { @@ -144,6 +146,15 @@ void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_co msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_ON; msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + return; + } else if (_buttons_state[MAIN_STATE_OFFBOARD] != msg->buttons[_param_buttons_map[MAIN_STATE_OFFBOARD]] == true) { + msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_MIDDLE; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_ON; + msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_ON; return; } diff --git a/src/platforms/ros/nodes/manual_input/manual_input.h b/src/platforms/ros/nodes/manual_input/manual_input.h index bf704f6757..2bafcca2e8 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.h +++ b/src/platforms/ros/nodes/manual_input/manual_input.h @@ -77,6 +77,8 @@ protected: MAIN_STATE_AUTO_MISSION, MAIN_STATE_AUTO_LOITER, MAIN_STATE_AUTO_RTL, + // MAIN_STATE_ACRO, + MAIN_STATE_OFFBOARD, MAIN_STATE_MAX }; From 01b8a18ad520a9d7bfecd3eea9a2e1dfc76b0ab1 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 14 Feb 2015 16:48:25 +0100 Subject: [PATCH 039/112] ros: mavlink dummy node: parse attitude target messages --- src/platforms/ros/nodes/mavlink/mavlink.cpp | 36 +++++++++++++++------ src/platforms/ros/nodes/mavlink/mavlink.h | 2 ++ 2 files changed, 28 insertions(+), 10 deletions(-) diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index a7830e97fb..8d658caa57 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -49,7 +49,7 @@ using namespace px4; Mavlink::Mavlink() : _n(), _v_att_sub(_n.subscribe("vehicle_attitude", 1, &Mavlink::VehicleAttitudeCallback, this)), - _v_att_sp_pub(_n.advertise("vehicle_attitude_setpoint", 1)) + _offboard_control_mode_pub(_n.advertise("offboard_control_mode", 1)) { _link = mavconn::MAVConnInterface::open_url("udp://localhost:14565@localhost:14560"); _link->message_received.connect(boost::bind(&Mavlink::handle_msg, this, _1, _2, _3)); @@ -102,18 +102,34 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) mavlink_set_attitude_target_t set_att_target; mavlink_msg_set_attitude_target_decode(mmsg, &set_att_target); - vehicle_attitude_setpoint msg; + offboard_control_mode offboard_control_mode_msg; + /* set correct ignore flags for body rate fields: copy from mavlink message */ + offboard_control_mode_msg.ignore_bodyrate = (bool)(set_att_target.type_mask & 0x7); + /* set correct ignore flags for thrust field: copy from mavlink message */ + offboard_control_mode_msg.ignore_thrust = (bool)(set_att_target.type_mask & (1 << 6)); + /* set correct ignore flags for attitude field: copy from mavlink message */ + offboard_control_mode_msg.ignore_attitude = (bool)(set_att_target.type_mask & (1 << 7)); - msg.timestamp = get_time_micros(); - mavlink_quaternion_to_euler(set_att_target.q, &msg.roll_body, &msg.pitch_body, &msg.yaw_body); - mavlink_quaternion_to_dcm(set_att_target.q, (float(*)[3])msg.R_body.data()); - msg.R_valid = true; - msg.thrust = set_att_target.thrust; + offboard_control_mode_msg.timestamp = get_time_micros(); + _offboard_control_mode_pub.publish(offboard_control_mode_msg); + + vehicle_attitude_setpoint v_att_sp_msg; + + /* The real mavlink app has a ckeck at this location which makes sure that the attitude setpoint + * gets published only if in offboard mode. We leave that out for now. + * */ + + v_att_sp_msg.timestamp = get_time_micros(); + mavlink_quaternion_to_euler(set_att_target.q, &v_att_sp_msg.roll_body, &v_att_sp_msg.pitch_body, + &v_att_sp_msg.yaw_body); + mavlink_quaternion_to_dcm(set_att_target.q, (float(*)[3])v_att_sp_msg.R_body.data()); + v_att_sp_msg.R_valid = true; + v_att_sp_msg.thrust = set_att_target.thrust; for (ssize_t i = 0; i < 4; i++) { - msg.q_d[i] = set_att_target.q[i]; + v_att_sp_msg.q_d[i] = set_att_target.q[i]; } - msg.q_d_valid = true; + v_att_sp_msg.q_d_valid = true; - _v_att_sp_pub.publish(msg); + _v_att_sp_pub.publish(v_att_sp_msg); } diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h index 6f75364363..2ee383b4f2 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.h +++ b/src/platforms/ros/nodes/mavlink/mavlink.h @@ -44,6 +44,7 @@ #include #include #include +#include namespace px4 { @@ -61,6 +62,7 @@ protected: mavconn::MAVConnInterface::Ptr _link; ros::Subscriber _v_att_sub; ros::Publisher _v_att_sp_pub; + ros::Publisher _offboard_control_mode_pub; /** * From 582c664a9c61e3b6cb4762e90ce437e5843c5d14 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 14 Feb 2015 16:49:06 +0100 Subject: [PATCH 040/112] ros: commander dummy node: set control flags in offboard mode --- .../ros/nodes/commander/commander.cpp | 36 ++++++++++++++++++- src/platforms/ros/nodes/commander/commander.h | 8 +++++ 2 files changed, 43 insertions(+), 1 deletion(-) diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index 2673122c70..b0f905d23f 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -45,13 +45,15 @@ Commander::Commander() : _n(), _man_ctrl_sp_sub(_n.subscribe("manual_control_setpoint", 10, &Commander::ManualControlInputCallback, this)), + _offboard_control_mode_sub(_n.subscribe("offboard_control_mode", 10, &Commander::OffboardControlModeCallback, this)), _vehicle_control_mode_pub(_n.advertise("vehicle_control_mode", 10)), _actuator_armed_pub(_n.advertise("actuator_armed", 10)), _vehicle_status_pub(_n.advertise("vehicle_status", 10)), _parameter_update_pub(_n.advertise("parameter_update", 10)), _msg_parameter_update(), _msg_actuator_armed(), - _msg_vehicle_control_mode() + _msg_vehicle_control_mode(), + _msg_offboard_control_mode() { } @@ -107,6 +109,33 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, // XXX this is a minimal implementation. If more advanced functionalities are // needed consider a full port of the commander + + if (msg->offboard_switch) + { + msg_vehicle_control_mode.flag_control_rates_enabled = !_msg_offboard_control_mode.ignore_bodyrate || + !_msg_offboard_control_mode.ignore_attitude || + !_msg_offboard_control_mode.ignore_position || + !_msg_offboard_control_mode.ignore_velocity || + !_msg_offboard_control_mode.ignore_acceleration_force; + + msg_vehicle_control_mode.flag_control_attitude_enabled = !_msg_offboard_control_mode.ignore_attitude || + !_msg_offboard_control_mode.ignore_position || + !_msg_offboard_control_mode.ignore_velocity || + !_msg_offboard_control_mode.ignore_acceleration_force; + + + msg_vehicle_control_mode.flag_control_velocity_enabled = !_msg_offboard_control_mode.ignore_velocity || + !_msg_offboard_control_mode.ignore_position; + + msg_vehicle_control_mode.flag_control_climb_rate_enabled = !_msg_offboard_control_mode.ignore_velocity || + !_msg_offboard_control_mode.ignore_position; + + msg_vehicle_control_mode.flag_control_position_enabled = !_msg_offboard_control_mode.ignore_position; + + msg_vehicle_control_mode.flag_control_altitude_enabled = !_msg_offboard_control_mode.ignore_position; + return; + } + switch (msg->mode_switch) { case px4::manual_control_setpoint::SWITCH_POS_NONE: ROS_WARN("Joystick button mapping error, main mode not set"); @@ -152,6 +181,11 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, } +void Commander::OffboardControlModeCallback(const px4::offboard_control_modeConstPtr &msg) +{ + _msg_offboard_control_mode = *msg; +} + int main(int argc, char **argv) { ros::init(argc, argv, "commander"); diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h index 58b7257b72..3152055aee 100644 --- a/src/platforms/ros/nodes/commander/commander.h +++ b/src/platforms/ros/nodes/commander/commander.h @@ -44,6 +44,7 @@ #include #include #include +#include class Commander { @@ -58,6 +59,11 @@ protected: */ void ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg); + /** + * Stores the offboard control mode + */ + void OffboardControlModeCallback(const px4::offboard_control_modeConstPtr &msg); + /** * Set control mode flags based on stick positions (equiv to code in px4 commander) */ @@ -67,6 +73,7 @@ protected: ros::NodeHandle _n; ros::Subscriber _man_ctrl_sp_sub; + ros::Subscriber _offboard_control_mode_sub; ros::Publisher _vehicle_control_mode_pub; ros::Publisher _actuator_armed_pub; ros::Publisher _vehicle_status_pub; @@ -75,5 +82,6 @@ protected: px4::parameter_update _msg_parameter_update; px4::actuator_armed _msg_actuator_armed; px4::vehicle_control_mode _msg_vehicle_control_mode; + px4::offboard_control_mode _msg_offboard_control_mode; }; From 8d36305f8b5d9393003f6074327ba279c98622ce Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 14 Feb 2015 18:05:43 +0100 Subject: [PATCH 041/112] add mavros sitl launch file --- launch/mavros_sitl.launch | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) create mode 100644 launch/mavros_sitl.launch diff --git a/launch/mavros_sitl.launch b/launch/mavros_sitl.launch new file mode 100644 index 0000000000..17b6a7035b --- /dev/null +++ b/launch/mavros_sitl.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + From ca250d21eb13b9887773422e63ff664447dfe264 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 15 Feb 2015 11:40:07 +0100 Subject: [PATCH 042/112] ros: mavlink dummy node: listen to vehicle local position and publish to mavlink (LOCAL_POSITION_NED) --- src/platforms/ros/nodes/mavlink/mavlink.cpp | 21 ++++++++++++++++++++- src/platforms/ros/nodes/mavlink/mavlink.h | 12 +++++++++++- 2 files changed, 31 insertions(+), 2 deletions(-) diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index 8d658caa57..3485b1f4ec 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -49,6 +49,7 @@ using namespace px4; Mavlink::Mavlink() : _n(), _v_att_sub(_n.subscribe("vehicle_attitude", 1, &Mavlink::VehicleAttitudeCallback, this)), + _v_local_pos_sub(_n.subscribe("vehicle_local_position", 1, &Mavlink::VehicleLocalPositionCallback, this)), _offboard_control_mode_pub(_n.advertise("offboard_control_mode", 1)) { _link = mavconn::MAVConnInterface::open_url("udp://localhost:14565@localhost:14560"); @@ -71,7 +72,7 @@ void Mavlink::VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg) _link->get_system_id(), _link->get_component_id(), _link->get_channel(), - &msg_m, //XXX hardcoded + &msg_m, get_time_micros() / 1000, msg->q[0], msg->q[1], @@ -83,6 +84,24 @@ void Mavlink::VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg) _link->send_message(&msg_m); } +void Mavlink::VehicleLocalPositionCallback(const vehicle_local_positionConstPtr &msg) +{ + mavlink_message_t msg_m; + mavlink_msg_local_position_ned_pack_chan( + _link->get_system_id(), + _link->get_component_id(), + _link->get_channel(), + &msg_m, + get_time_micros() / 1000, + msg->x, + msg->y, + msg->z, + msg->vx, + msg->vy, + msg->vz); + _link->send_message(&msg_m); +} + void Mavlink::handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid) { (void)sysid; (void)compid; diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h index 2ee383b4f2..a246af4a4b 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.h +++ b/src/platforms/ros/nodes/mavlink/mavlink.h @@ -43,6 +43,7 @@ #include "ros/ros.h" #include #include +#include #include #include @@ -61,17 +62,26 @@ protected: ros::NodeHandle _n; mavconn::MAVConnInterface::Ptr _link; ros::Subscriber _v_att_sub; + ros::Subscriber _v_local_pos_sub; ros::Publisher _v_att_sp_pub; ros::Publisher _offboard_control_mode_pub; /** * * Simulates output of attitude data from the FCU - * Equivalent to the mavlink stream ATTITUDE + * Equivalent to the mavlink stream ATTITUDE_QUATERNION * * */ void VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg); + /** + * + * Simulates output of local position data from the FCU + * Equivalent to the mavlink stream LOCAL_POSITION_NED + * + * */ + void VehicleLocalPositionCallback(const vehicle_local_positionConstPtr &msg); + /** * From 6e69558b42243a2b661d6fc48fc07a22961d4e9e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 15 Feb 2015 12:39:32 +0100 Subject: [PATCH 043/112] enable force setpoint message for multiplatform --- CMakeLists.txt | 1 + msg/vehicle_force_setpoint.msg | 8 ++++++++ src/platforms/px4_includes.h | 2 ++ 3 files changed, 11 insertions(+) create mode 100644 msg/vehicle_force_setpoint.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index 4e91fb3bee..c3e8943886 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -77,6 +77,7 @@ add_message_files( vehicle_local_position_setpoint.msg vehicle_global_velocity_setpoint.msg offboard_control_mode.msg + vehicle_force_setpoint.msg ) ## Generate services in the 'srv' folder diff --git a/msg/vehicle_force_setpoint.msg b/msg/vehicle_force_setpoint.msg new file mode 100644 index 0000000000..9e2322005d --- /dev/null +++ b/msg/vehicle_force_setpoint.msg @@ -0,0 +1,8 @@ +# Definition of force (NED) setpoint uORB topic. Typically this can be used +# by a position control app together with an attitude control app. + + +float32 x # in N NED +float32 y # in N NED +float32 z # in N NED +float32 yaw # right-hand rotation around downward axis (rad, equivalent to Tait-Bryan yaw) diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 364a5f31b9..0e98783fda 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -67,6 +67,7 @@ #include #include #include +#include #endif #else @@ -95,6 +96,7 @@ #include #include #include +#include #endif #include #include From 5beafd25e6947b5f6ac33fe66521fb462a1be1b0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 15 Feb 2015 12:40:46 +0100 Subject: [PATCH 044/112] ros: mavlink dummy node: handle position target local ned mavlink messages and forward them to position_setpoint_triplet --- src/platforms/ros/nodes/mavlink/mavlink.cpp | 149 +++++++++++++++++--- src/platforms/ros/nodes/mavlink/mavlink.h | 11 ++ 2 files changed, 142 insertions(+), 18 deletions(-) diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index 3485b1f4ec..dbb3dac7af 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -50,7 +50,10 @@ Mavlink::Mavlink() : _n(), _v_att_sub(_n.subscribe("vehicle_attitude", 1, &Mavlink::VehicleAttitudeCallback, this)), _v_local_pos_sub(_n.subscribe("vehicle_local_position", 1, &Mavlink::VehicleLocalPositionCallback, this)), - _offboard_control_mode_pub(_n.advertise("offboard_control_mode", 1)) + _v_att_sp_pub(_n.advertise("vehicle_attitude_setpoint", 1)), + _pos_sp_triplet_pub(_n.advertise("position_setpoint_triplet", 1)), + _offboard_control_mode_pub(_n.advertise("offboard_control_mode", 1)), + _force_sp_pub(_n.advertise("vehicle_force_setpoint", 1)) { _link = mavconn::MAVConnInterface::open_url("udp://localhost:14565@localhost:14560"); _link->message_received.connect(boost::bind(&Mavlink::handle_msg, this, _1, _2, _3)); @@ -121,34 +124,144 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) mavlink_set_attitude_target_t set_att_target; mavlink_msg_set_attitude_target_decode(mmsg, &set_att_target); - offboard_control_mode offboard_control_mode_msg; + offboard_control_mode offboard_control_mode; /* set correct ignore flags for body rate fields: copy from mavlink message */ - offboard_control_mode_msg.ignore_bodyrate = (bool)(set_att_target.type_mask & 0x7); + offboard_control_mode.ignore_bodyrate = (bool)(set_att_target.type_mask & 0x7); /* set correct ignore flags for thrust field: copy from mavlink message */ - offboard_control_mode_msg.ignore_thrust = (bool)(set_att_target.type_mask & (1 << 6)); + offboard_control_mode.ignore_thrust = (bool)(set_att_target.type_mask & (1 << 6)); /* set correct ignore flags for attitude field: copy from mavlink message */ - offboard_control_mode_msg.ignore_attitude = (bool)(set_att_target.type_mask & (1 << 7)); + offboard_control_mode.ignore_attitude = (bool)(set_att_target.type_mask & (1 << 7)); - offboard_control_mode_msg.timestamp = get_time_micros(); - _offboard_control_mode_pub.publish(offboard_control_mode_msg); + offboard_control_mode.timestamp = get_time_micros(); + _offboard_control_mode_pub.publish(offboard_control_mode); - vehicle_attitude_setpoint v_att_sp_msg; + vehicle_attitude_setpoint att_sp; /* The real mavlink app has a ckeck at this location which makes sure that the attitude setpoint * gets published only if in offboard mode. We leave that out for now. - * */ + */ - v_att_sp_msg.timestamp = get_time_micros(); - mavlink_quaternion_to_euler(set_att_target.q, &v_att_sp_msg.roll_body, &v_att_sp_msg.pitch_body, - &v_att_sp_msg.yaw_body); - mavlink_quaternion_to_dcm(set_att_target.q, (float(*)[3])v_att_sp_msg.R_body.data()); - v_att_sp_msg.R_valid = true; - v_att_sp_msg.thrust = set_att_target.thrust; + att_sp.timestamp = get_time_micros(); + mavlink_quaternion_to_euler(set_att_target.q, &att_sp.roll_body, &att_sp.pitch_body, + &att_sp.yaw_body); + mavlink_quaternion_to_dcm(set_att_target.q, (float(*)[3])att_sp.R_body.data()); + att_sp.R_valid = true; + att_sp.thrust = set_att_target.thrust; for (ssize_t i = 0; i < 4; i++) { - v_att_sp_msg.q_d[i] = set_att_target.q[i]; + att_sp.q_d[i] = set_att_target.q[i]; } - v_att_sp_msg.q_d_valid = true; + att_sp.q_d_valid = true; - _v_att_sp_pub.publish(v_att_sp_msg); + _v_att_sp_pub.publish(att_sp); } + +void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t *mmsg) +{ + + mavlink_set_position_target_local_ned_t set_position_target_local_ned; + mavlink_msg_set_position_target_local_ned_decode(mmsg, &set_position_target_local_ned); + + offboard_control_mode offboard_control_mode; + // memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));//XXX breaks compatibility with multiple setpoints + + /* Only accept messages which are intended for this system */ + // XXX removed for sitl, makes maybe sense to re-introduce at some point + // if ((mavlink_system.sysid == set_position_target_local_ned.target_system || + // set_position_target_local_ned.target_system == 0) && + // (mavlink_system.compid == set_position_target_local_ned.target_component || + // set_position_target_local_ned.target_component == 0)) { + + /* convert mavlink type (local, NED) to uORB offboard control struct */ + offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7); + offboard_control_mode.ignore_velocity = (bool)(set_position_target_local_ned.type_mask & 0x38); + offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_local_ned.type_mask & 0x1C0); + bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9)); + /* yaw ignore flag mapps to ignore_attitude */ + offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400); + /* yawrate ignore flag mapps to ignore_bodyrate */ + offboard_control_mode.ignore_bodyrate = (bool)(set_position_target_local_ned.type_mask & 0x800); + + + + offboard_control_mode.timestamp = get_time_micros(); + _offboard_control_mode_pub.publish(offboard_control_mode); + + /* The real mavlink app has a ckeck at this location which makes sure that the position setpoint triplet + * gets published only if in offboard mode. We leave that out for now. + */ + if (is_force_sp && offboard_control_mode.ignore_position && + offboard_control_mode.ignore_velocity) { + /* The offboard setpoint is a force setpoint only, directly writing to the force + * setpoint topic and not publishing the setpoint triplet topic */ + vehicle_force_setpoint force_sp; + force_sp.x = set_position_target_local_ned.afx; + force_sp.y = set_position_target_local_ned.afy; + force_sp.z = set_position_target_local_ned.afz; + //XXX: yaw + + _force_sp_pub.publish(force_sp); + } else { + /* It's not a pure force setpoint: publish to setpoint triplet topic */ + position_setpoint_triplet pos_sp_triplet; + pos_sp_triplet.previous.valid = false; + pos_sp_triplet.next.valid = false; + pos_sp_triplet.current.valid = true; + pos_sp_triplet.current.type = position_setpoint::SETPOINT_TYPE_POSITION; //XXX support others + + /* set the local pos values */ + if (!offboard_control_mode.ignore_position) { + pos_sp_triplet.current.position_valid = true; + pos_sp_triplet.current.x = set_position_target_local_ned.x; + pos_sp_triplet.current.y = set_position_target_local_ned.y; + pos_sp_triplet.current.z = set_position_target_local_ned.z; + } else { + pos_sp_triplet.current.position_valid = false; + } + + /* set the local vel values */ + if (!offboard_control_mode.ignore_velocity) { + pos_sp_triplet.current.velocity_valid = true; + pos_sp_triplet.current.vx = set_position_target_local_ned.vx; + pos_sp_triplet.current.vy = set_position_target_local_ned.vy; + pos_sp_triplet.current.vz = set_position_target_local_ned.vz; + } else { + pos_sp_triplet.current.velocity_valid = false; + } + + /* set the local acceleration values if the setpoint type is 'local pos' and none + * of the accelerations fields is set to 'ignore' */ + if (!offboard_control_mode.ignore_acceleration_force) { + pos_sp_triplet.current.acceleration_valid = true; + pos_sp_triplet.current.a_x = set_position_target_local_ned.afx; + pos_sp_triplet.current.a_y = set_position_target_local_ned.afy; + pos_sp_triplet.current.a_z = set_position_target_local_ned.afz; + pos_sp_triplet.current.acceleration_is_force = + is_force_sp; + + } else { + pos_sp_triplet.current.acceleration_valid = false; + } + + /* set the yaw sp value */ + if (!offboard_control_mode.ignore_attitude) { + pos_sp_triplet.current.yaw_valid = true; + pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw; + + } else { + pos_sp_triplet.current.yaw_valid = false; + } + + /* set the yawrate sp value */ + if (!offboard_control_mode.ignore_bodyrate) { + pos_sp_triplet.current.yawspeed_valid = true; + pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate; + + } else { + pos_sp_triplet.current.yawspeed_valid = false; + } + //XXX handle global pos setpoints (different MAV frames) + + _pos_sp_triplet_pub.publish(pos_sp_triplet); + } +} diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h index a246af4a4b..acb2408f30 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.h +++ b/src/platforms/ros/nodes/mavlink/mavlink.h @@ -45,6 +45,8 @@ #include #include #include +#include +#include #include namespace px4 @@ -64,7 +66,9 @@ protected: ros::Subscriber _v_att_sub; ros::Subscriber _v_local_pos_sub; ros::Publisher _v_att_sp_pub; + ros::Publisher _pos_sp_triplet_pub; ros::Publisher _offboard_control_mode_pub; + ros::Publisher _force_sp_pub; /** * @@ -97,6 +101,13 @@ protected: * */ void handle_msg_set_attitude_target(const mavlink_message_t *mmsg); + /** + * + * Handle SET_POSITION_TARGET_LOCAL_NED mavlink messages + * + * */ + void handle_msg_set_position_target_local_ned(const mavlink_message_t *mmsg); + }; } From ae64e4e05c2a8f6d179f817c9d591881aafcfee6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 15 Feb 2015 18:29:57 +0100 Subject: [PATCH 045/112] ros: manual input (joystick) node: correctly initilize switches --- src/platforms/ros/nodes/manual_input/manual_input.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp index d5d14a21fb..8488c518f5 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.cpp +++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp @@ -85,6 +85,8 @@ ManualInput::ManualInput() : _msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; _msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; _msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + _msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + _msg_mc_sp.acro_switch = px4::manual_control_setpoint::SWITCH_POS_NONE; } From 8b40112e9f6ad25a41cdef73f3d3001be49c0271 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 15 Feb 2015 18:31:49 +0100 Subject: [PATCH 046/112] ros: commander dummy node: fix offboard support --- .../ros/nodes/commander/commander.cpp | 70 +++++++++++++------ src/platforms/ros/nodes/commander/commander.h | 8 +++ 2 files changed, 55 insertions(+), 23 deletions(-) diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index b0f905d23f..9ca54339df 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -53,12 +53,17 @@ Commander::Commander() : _msg_parameter_update(), _msg_actuator_armed(), _msg_vehicle_control_mode(), - _msg_offboard_control_mode() + _msg_offboard_control_mode(), + _got_manual_control(false) { + + /* Default to offboard control: when no joystick is connected offboard control should just work */ + } void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg) { + _got_manual_control = true; px4::vehicle_status msg_vehicle_status; /* fill vehicle control mode based on (faked) stick positions*/ @@ -103,6 +108,36 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon } } +void Commander::SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode, + px4::vehicle_control_mode &msg_vehicle_control_mode) +{ + msg_vehicle_control_mode.flag_control_manual_enabled = false; + msg_vehicle_control_mode.flag_control_offboard_enabled = true; + msg_vehicle_control_mode.flag_control_auto_enabled = false; + + msg_vehicle_control_mode.flag_control_rates_enabled = !msg_offboard_control_mode.ignore_bodyrate || + !msg_offboard_control_mode.ignore_attitude || + !msg_offboard_control_mode.ignore_position || + !msg_offboard_control_mode.ignore_velocity || + !msg_offboard_control_mode.ignore_acceleration_force; + + msg_vehicle_control_mode.flag_control_attitude_enabled = !msg_offboard_control_mode.ignore_attitude || + !msg_offboard_control_mode.ignore_position || + !msg_offboard_control_mode.ignore_velocity || + !msg_offboard_control_mode.ignore_acceleration_force; + + + msg_vehicle_control_mode.flag_control_velocity_enabled = !msg_offboard_control_mode.ignore_velocity || + !msg_offboard_control_mode.ignore_position; + + msg_vehicle_control_mode.flag_control_climb_rate_enabled = !msg_offboard_control_mode.ignore_velocity || + !msg_offboard_control_mode.ignore_position; + + msg_vehicle_control_mode.flag_control_position_enabled = !msg_offboard_control_mode.ignore_position; + + msg_vehicle_control_mode.flag_control_altitude_enabled = !msg_offboard_control_mode.ignore_position; +} + void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, px4::vehicle_status &msg_vehicle_status, px4::vehicle_control_mode &msg_vehicle_control_mode) { @@ -110,32 +145,14 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, // needed consider a full port of the commander - if (msg->offboard_switch) + if (msg->offboard_switch == px4::manual_control_setpoint::SWITCH_POS_ON) { - msg_vehicle_control_mode.flag_control_rates_enabled = !_msg_offboard_control_mode.ignore_bodyrate || - !_msg_offboard_control_mode.ignore_attitude || - !_msg_offboard_control_mode.ignore_position || - !_msg_offboard_control_mode.ignore_velocity || - !_msg_offboard_control_mode.ignore_acceleration_force; - - msg_vehicle_control_mode.flag_control_attitude_enabled = !_msg_offboard_control_mode.ignore_attitude || - !_msg_offboard_control_mode.ignore_position || - !_msg_offboard_control_mode.ignore_velocity || - !_msg_offboard_control_mode.ignore_acceleration_force; - - - msg_vehicle_control_mode.flag_control_velocity_enabled = !_msg_offboard_control_mode.ignore_velocity || - !_msg_offboard_control_mode.ignore_position; - - msg_vehicle_control_mode.flag_control_climb_rate_enabled = !_msg_offboard_control_mode.ignore_velocity || - !_msg_offboard_control_mode.ignore_position; - - msg_vehicle_control_mode.flag_control_position_enabled = !_msg_offboard_control_mode.ignore_position; - - msg_vehicle_control_mode.flag_control_altitude_enabled = !_msg_offboard_control_mode.ignore_position; + SetOffboardControl(_msg_offboard_control_mode, msg_vehicle_control_mode); return; } + msg_vehicle_control_mode.flag_control_offboard_enabled = false; + switch (msg->mode_switch) { case px4::manual_control_setpoint::SWITCH_POS_NONE: ROS_WARN("Joystick button mapping error, main mode not set"); @@ -184,6 +201,13 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, void Commander::OffboardControlModeCallback(const px4::offboard_control_modeConstPtr &msg) { _msg_offboard_control_mode = *msg; + + /* Force system into offboard control mode */ + if (!_got_manual_control) { + SetOffboardControl(_msg_offboard_control_mode, _msg_vehicle_control_mode); + _msg_vehicle_control_mode.timestamp = px4::get_time_micros(); + _vehicle_control_mode_pub.publish(_msg_vehicle_control_mode); + } } int main(int argc, char **argv) diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h index 3152055aee..c537c84193 100644 --- a/src/platforms/ros/nodes/commander/commander.h +++ b/src/platforms/ros/nodes/commander/commander.h @@ -71,6 +71,12 @@ protected: px4::vehicle_status &msg_vehicle_status, px4::vehicle_control_mode &msg_vehicle_control_mode); + /** + * Sets offboard controll flags in msg_vehicle_control_mode + */ + void SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode, + px4::vehicle_control_mode &msg_vehicle_control_mode); + ros::NodeHandle _n; ros::Subscriber _man_ctrl_sp_sub; ros::Subscriber _offboard_control_mode_sub; @@ -84,4 +90,6 @@ protected: px4::vehicle_control_mode _msg_vehicle_control_mode; px4::offboard_control_mode _msg_offboard_control_mode; + bool _got_manual_control; + }; From b829b553f4a3dcf2357e482dc8b1c868c779e29d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 15 Feb 2015 18:33:17 +0100 Subject: [PATCH 047/112] ros: mavlink dummy node: actually call handle_msg_set_position_target_local_ned --- src/platforms/ros/nodes/mavlink/mavlink.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index dbb3dac7af..013788ecd8 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -113,6 +113,9 @@ void Mavlink::handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t c case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: handle_msg_set_attitude_target(mmsg); break; + case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: + handle_msg_set_position_target_local_ned(mmsg); + break; default: break; } From 1a6cbe170cba5e90a9082df0d46a2de39857f967 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 15 Feb 2015 18:33:55 +0100 Subject: [PATCH 048/112] ros: demo node for offboard position setpoints --- .../demo_offboard_position_setpoints.cpp | 77 +++++++++++++++++++ .../demo_offboard_position_setpoints.h | 57 ++++++++++++++ 2 files changed, 134 insertions(+) create mode 100644 src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp create mode 100644 src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.h diff --git a/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp new file mode 100644 index 0000000000..c7dc358dd5 --- /dev/null +++ b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * 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 demo_offboard_position_Setpoints.cpp + * + * Demo for sending offboard position setpoints to mavros to show offboard position control in SITL + * + * @author Thomas Gubler +*/ + +#include "demo_offboard_position_setpoints.h" + +#include +#include + +DemoOffboardPositionSetpoints::DemoOffboardPositionSetpoints() : + _n(), + _local_position_sp_pub(_n.advertise("mavros/setpoint/local_position", 1)) +{ +} + + +int DemoOffboardPositionSetpoints::main() +{ + px4::Rate loop_rate(10); + + while (ros::ok()) { + loop_rate.sleep(); + ros::spinOnce(); + + /* Publish example offboard position setpoint */ + geometry_msgs::PoseStamped pose; + pose.pose.position.x = 0; + pose.pose.position.y = 0; + pose.pose.position.z = 1; + _local_position_sp_pub.publish(pose); + } + return 0; +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "demo_offboard_position_setpoints"); + DemoOffboardPositionSetpoints d; + return d.main(); +} diff --git a/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.h b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.h new file mode 100644 index 0000000000..14187a9cf8 --- /dev/null +++ b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.h @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * 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 demo_offboard_position_Setpoints.h + * + * Demo for sending offboard position setpoints to mavros to show offboard position control in SITL + * + * @author Thomas Gubler +*/ + +#include "ros/ros.h" +#include + +class DemoOffboardPositionSetpoints +{ +public: + DemoOffboardPositionSetpoints(); + + ~DemoOffboardPositionSetpoints() {} + + int main(); + +protected: + ros::NodeHandle _n; + ros::Publisher _local_position_sp_pub; +}; From 39a105df73efb193009273e40ae3ab21d8deb7ff Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 15 Feb 2015 18:34:27 +0100 Subject: [PATCH 049/112] ros: launch files for offboard postion setpoints demo --- ...o_ardrone_empty_world_offboard_demo.launch | 10 +++++++ launch/mavros_sitl.launch | 26 ++++++++++--------- 2 files changed, 24 insertions(+), 12 deletions(-) create mode 100644 launch/gazebo_ardrone_empty_world_offboard_demo.launch diff --git a/launch/gazebo_ardrone_empty_world_offboard_demo.launch b/launch/gazebo_ardrone_empty_world_offboard_demo.launch new file mode 100644 index 0000000000..ce2386920f --- /dev/null +++ b/launch/gazebo_ardrone_empty_world_offboard_demo.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/launch/mavros_sitl.launch b/launch/mavros_sitl.launch index 17b6a7035b..29f7dd857f 100644 --- a/launch/mavros_sitl.launch +++ b/launch/mavros_sitl.launch @@ -2,18 +2,20 @@ - - - - + + + + + - - - + + + - - - - - + + + + + + From dae7c698b0356643b15fafe8553d84b2084328f2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 15 Feb 2015 19:05:47 +0100 Subject: [PATCH 050/112] ros: CMakeLists: small fixes and added offboard demo node --- CMakeLists.txt | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c3e8943886..f7c694955d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs + geometry_msgs message_generation cmake_modules gazebo_msgs @@ -235,24 +236,33 @@ target_link_libraries(mc_mixer px4 ) -## Commander +## Commander dummy add_executable(commander src/platforms/ros/nodes/commander/commander.cpp) -add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp) +add_dependencies(commander ${PROJECT_NAME}_generate_messages_cpp_cpp) target_link_libraries(commander ${catkin_LIBRARIES} px4 ) -## Mavlink +## Mavlink dummy add_executable(mavlink src/platforms/ros/nodes/mavlink/mavlink.cpp) -add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp) +add_dependencies(mavlink ${PROJECT_NAME}_generate_messages_cpp_cpp) target_link_libraries(mavlink ${catkin_LIBRARIES} px4 ) +## Offboard Position Setpoint Demo +add_executable(demo_offboard_position_setpoints + src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp) +add_dependencies(demo_offboard_position_setpoints ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(demo_offboard_position_setpoints + ${catkin_LIBRARIES} + px4 +) + ############# ## Install ## ############# From c4b4c5fa41af70b3328486cfa91eb4cf041a1fdb Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 15 Feb 2015 20:55:23 +0100 Subject: [PATCH 051/112] fix year in file header --- .../demo_offboard_position_setpoints.cpp | 2 +- .../demo_offboard_position_setpoints.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp index c7dc358dd5..7366d7fc61 100644 --- a/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp +++ b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.h b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.h index 14187a9cf8..7d39690f4c 100644 --- a/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.h +++ b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions From 03e900d3f18c164a7693d3a66e2b69d6353fe733 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 17 Feb 2015 20:14:58 +0100 Subject: [PATCH 052/112] rename ros launch file --- ...ch => gazebo_ardrone_empty_world_offboard_positiondemo.launch} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename launch/{gazebo_ardrone_empty_world_offboard_demo.launch => gazebo_ardrone_empty_world_offboard_positiondemo.launch} (100%) diff --git a/launch/gazebo_ardrone_empty_world_offboard_demo.launch b/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch similarity index 100% rename from launch/gazebo_ardrone_empty_world_offboard_demo.launch rename to launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch From 80fbb512c98f7a026dacd8da1da0a319496db4ca Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 17 Feb 2015 22:02:41 +0100 Subject: [PATCH 053/112] ros: mavlink node: update to latest offboard code --- src/platforms/ros/nodes/mavlink/mavlink.cpp | 36 ++++++++++++++------- 1 file changed, 25 insertions(+), 11 deletions(-) diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index 013788ecd8..29ffe68ed4 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -124,16 +124,30 @@ void Mavlink::handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t c void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) { - mavlink_set_attitude_target_t set_att_target; - mavlink_msg_set_attitude_target_decode(mmsg, &set_att_target); + mavlink_set_attitude_target_t set_attitude_target; + mavlink_msg_set_attitude_target_decode(mmsg, &set_attitude_target); offboard_control_mode offboard_control_mode; - /* set correct ignore flags for body rate fields: copy from mavlink message */ - offboard_control_mode.ignore_bodyrate = (bool)(set_att_target.type_mask & 0x7); + /* set correct ignore flags for thrust field: copy from mavlink message */ - offboard_control_mode.ignore_thrust = (bool)(set_att_target.type_mask & (1 << 6)); - /* set correct ignore flags for attitude field: copy from mavlink message */ - offboard_control_mode.ignore_attitude = (bool)(set_att_target.type_mask & (1 << 7)); + offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); + + /* + * if attitude or body rate have been used (not ignored) previously and this message only sends + * throttle and has the ignore bits set for attitude and rates don't change the flags for attitude and + * body rates to keep the controllers running + */ + bool ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7); + bool ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7)); + + if (ignore_bodyrate && ignore_attitude && !offboard_control_mode.ignore_thrust) { + /* Message want's us to ignore everything except thrust: only ignore if previously ignored */ + offboard_control_mode.ignore_bodyrate = ignore_bodyrate && offboard_control_mode.ignore_bodyrate; + offboard_control_mode.ignore_attitude = ignore_attitude && offboard_control_mode.ignore_attitude; + } else { + offboard_control_mode.ignore_bodyrate = ignore_bodyrate; + offboard_control_mode.ignore_attitude = ignore_attitude; + } offboard_control_mode.timestamp = get_time_micros(); _offboard_control_mode_pub.publish(offboard_control_mode); @@ -145,13 +159,13 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) */ att_sp.timestamp = get_time_micros(); - mavlink_quaternion_to_euler(set_att_target.q, &att_sp.roll_body, &att_sp.pitch_body, + mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); - mavlink_quaternion_to_dcm(set_att_target.q, (float(*)[3])att_sp.R_body.data()); + mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body.data()); att_sp.R_valid = true; - att_sp.thrust = set_att_target.thrust; + att_sp.thrust = set_attitude_target.thrust; for (ssize_t i = 0; i < 4; i++) { - att_sp.q_d[i] = set_att_target.q[i]; + att_sp.q_d[i] = set_attitude_target.q[i]; } att_sp.q_d_valid = true; From 27511324ff41caa069505524f646c25835fb796d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 17 Feb 2015 22:03:20 +0100 Subject: [PATCH 054/112] ros: add offboard attitude sp demo (WIP) --- CMakeLists.txt | 9 ++ ...e_empty_world_offboard_attitudedemo.launch | 10 +++ .../demo_offboard_attitude_setpoints.cpp | 83 +++++++++++++++++++ .../demo_offboard_attitude_setpoints.h | 58 +++++++++++++ 4 files changed, 160 insertions(+) create mode 100644 launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch create mode 100644 src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp create mode 100644 src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.h diff --git a/CMakeLists.txt b/CMakeLists.txt index f7c694955d..01b52a436d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -263,6 +263,15 @@ target_link_libraries(demo_offboard_position_setpoints px4 ) +## Offboard Attitude Setpoint Demo +add_executable(demo_offboard_attitude_setpoints + src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp) +add_dependencies(demo_offboard_attitude_setpoints ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(demo_offboard_attitude_setpoints + ${catkin_LIBRARIES} + px4 +) + ############# ## Install ## ############# diff --git a/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch b/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch new file mode 100644 index 0000000000..3998409de8 --- /dev/null +++ b/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp new file mode 100644 index 0000000000..402d477832 --- /dev/null +++ b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file demo_offboard_position_Setpoints.cpp + * + * Demo for sending offboard position setpoints to mavros to show offboard position control in SITL + * + * @author Thomas Gubler +*/ + +#include "demo_offboard_attitude_setpoints.h" + +#include +#include +#include + +DemoOffboardAttitudeSetpoints::DemoOffboardAttitudeSetpoints() : + _n(), + _attitude_sp_pub(_n.advertise("mavros/setpoint/attitude", 1)), + _thrust_sp_pub(_n.advertise("mavros/setpoint/att_throttle", 1)) +{ +} + + +int DemoOffboardAttitudeSetpoints::main() +{ + px4::Rate loop_rate(10); + + while (ros::ok()) { + loop_rate.sleep(); + ros::spinOnce(); + + /* Publish example offboard attitude setpoint */ + geometry_msgs::PoseStamped pose; + pose.pose.position.x = 0; + pose.pose.position.y = 0; + pose.pose.position.z = 1; + _attitude_sp_pub.publish(pose); + + std_msgs::Float64 thrust; + thrust.data = 0.5; + _thrust_sp_pub.publish(thrust); + } + return 0; +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "demo_offboard_position_setpoints"); + DemoOffboardAttitudeSetpoints d; + return d.main(); +} diff --git a/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.h b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.h new file mode 100644 index 0000000000..d7b7a37ba0 --- /dev/null +++ b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.h @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file demo_offboard_attitude_Setpoints.h + * + * Demo for sending offboard position setpoints to mavros to show offboard position control in SITL + * + * @author Thomas Gubler +*/ + +#include "ros/ros.h" +#include + +class DemoOffboardAttitudeSetpoints +{ +public: + DemoOffboardAttitudeSetpoints(); + + ~DemoOffboardAttitudeSetpoints() {} + + int main(); + +protected: + ros::NodeHandle _n; + ros::Publisher _attitude_sp_pub; + ros::Publisher _thrust_sp_pub; +}; From edbf6204588657b72408efb83c6874b4f7ae6c1e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 21 Feb 2015 11:09:41 +0100 Subject: [PATCH 055/112] ros: fix offboard attitude demo launch file --- launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch b/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch index 3998409de8..2b9d797f6e 100644 --- a/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch +++ b/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch @@ -4,7 +4,7 @@ - + From 5b0423109f631763f7e71311d4af7ffa4805edda Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 21 Feb 2015 11:11:36 +0100 Subject: [PATCH 056/112] ros mavlink dummy node: improve offboard attitude setpoint handling --- src/platforms/ros/nodes/mavlink/mavlink.cpp | 24 +++++++++++++++------ 1 file changed, 18 insertions(+), 6 deletions(-) diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index 29ffe68ed4..5459fcffdc 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -127,7 +127,7 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) mavlink_set_attitude_target_t set_attitude_target; mavlink_msg_set_attitude_target_decode(mmsg, &set_attitude_target); - offboard_control_mode offboard_control_mode; + static offboard_control_mode offboard_control_mode; /* set correct ignore flags for thrust field: copy from mavlink message */ offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); @@ -148,11 +148,14 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) offboard_control_mode.ignore_bodyrate = ignore_bodyrate; offboard_control_mode.ignore_attitude = ignore_attitude; } + offboard_control_mode.ignore_position = true; + offboard_control_mode.ignore_velocity = true; + offboard_control_mode.ignore_acceleration_force = true; offboard_control_mode.timestamp = get_time_micros(); _offboard_control_mode_pub.publish(offboard_control_mode); - vehicle_attitude_setpoint att_sp; + static vehicle_attitude_setpoint att_sp = {}; /* The real mavlink app has a ckeck at this location which makes sure that the attitude setpoint * gets published only if in offboard mode. We leave that out for now. @@ -163,14 +166,23 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) &att_sp.yaw_body); mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body.data()); att_sp.R_valid = true; - att_sp.thrust = set_attitude_target.thrust; - for (ssize_t i = 0; i < 4; i++) { - att_sp.q_d[i] = set_attitude_target.q[i]; + + if (!offboard_control_mode.ignore_thrust) { + att_sp.thrust = set_attitude_target.thrust; + } + + if (!offboard_control_mode.ignore_attitude) { + for (ssize_t i = 0; i < 4; i++) { + att_sp.q_d[i] = set_attitude_target.q[i]; + } + att_sp.q_d_valid = true; } - att_sp.q_d_valid = true; _v_att_sp_pub.publish(att_sp); + + //XXX real mavlink publishes rate sp here + } void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t *mmsg) From 7ff84c0dcf48e128ae98c52ae6affd43aa46d341 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 21 Feb 2015 11:12:04 +0100 Subject: [PATCH 057/112] ros: offboard attitude demo node: make quad jump --- .../demo_offboard_attitude_setpoints.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp index 402d477832..027b29a875 100644 --- a/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp +++ b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp @@ -44,6 +44,7 @@ #include #include #include +#include DemoOffboardAttitudeSetpoints::DemoOffboardAttitudeSetpoints() : _n(), @@ -69,7 +70,7 @@ int DemoOffboardAttitudeSetpoints::main() _attitude_sp_pub.publish(pose); std_msgs::Float64 thrust; - thrust.data = 0.5; + thrust.data = 0.4f + 0.25 * (sinf(2.0f * (float)px4::get_time_micros() / 1000000.0f)); // just some example throttle input that makes the quad 'jump' _thrust_sp_pub.publish(thrust); } return 0; From 4bf3107faf50f821cda8715252357754b0ee844a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 21 Feb 2015 11:54:04 +0100 Subject: [PATCH 058/112] mavros sitl: make it listen to the attitude setpoint topic --- launch/mavros_sitl.launch | 2 ++ 1 file changed, 2 insertions(+) diff --git a/launch/mavros_sitl.launch b/launch/mavros_sitl.launch index 29f7dd857f..582fdaa7d2 100644 --- a/launch/mavros_sitl.launch +++ b/launch/mavros_sitl.launch @@ -8,6 +8,8 @@ + + From e5d54a487fbca638a52d8c5a12ef4374680cdf96 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 21 Feb 2015 11:54:31 +0100 Subject: [PATCH 059/112] ros offboard attitude sp demo: move attitude --- CMakeLists.txt | 1 + package.xml | 2 ++ .../demo_offboard_attitude_setpoints.cpp | 9 +++++---- 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 01b52a436d..f015e56184 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(catkin REQUIRED COMPONENTS sensor_msgs mav_msgs libmavconn + tf ) find_package(Eigen REQUIRED) diff --git a/package.xml b/package.xml index 96d622a682..28b682c008 100644 --- a/package.xml +++ b/package.xml @@ -45,11 +45,13 @@ std_msgs eigen libmavconn + tf roscpp rospy std_msgs eigen libmavconn + tf diff --git a/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp index 027b29a875..fb0b09de1e 100644 --- a/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp +++ b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp @@ -45,6 +45,7 @@ #include #include #include +#include DemoOffboardAttitudeSetpoints::DemoOffboardAttitudeSetpoints() : _n(), @@ -64,13 +65,13 @@ int DemoOffboardAttitudeSetpoints::main() /* Publish example offboard attitude setpoint */ geometry_msgs::PoseStamped pose; - pose.pose.position.x = 0; - pose.pose.position.y = 0; - pose.pose.position.z = 1; + tf::Quaternion q = tf::createQuaternionFromRPY(0.0, 0.1 * (sinf(0.5 * (float)px4::get_time_micros() / 1000000.0f)) , 0.0); + quaternionTFToMsg(q, pose.pose.orientation); + _attitude_sp_pub.publish(pose); std_msgs::Float64 thrust; - thrust.data = 0.4f + 0.25 * (sinf(2.0f * (float)px4::get_time_micros() / 1000000.0f)); // just some example throttle input that makes the quad 'jump' + thrust.data = 0.4f + 0.25 * (sinf((float)px4::get_time_micros() / 1000000.0f)); // just some example throttle input that makes the quad 'jump' _thrust_sp_pub.publish(thrust); } return 0; From cbbc660b88917fd0c4cf3fe54ca5c769775e7ae9 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 22 Feb 2015 12:35:45 +0100 Subject: [PATCH 060/112] arm automatically when offboard control mode is set --- src/platforms/ros/nodes/commander/commander.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index 9ca54339df..0c32026f39 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -205,8 +205,25 @@ void Commander::OffboardControlModeCallback(const px4::offboard_control_modeCons /* Force system into offboard control mode */ if (!_got_manual_control) { SetOffboardControl(_msg_offboard_control_mode, _msg_vehicle_control_mode); + + px4::vehicle_status msg_vehicle_status; + msg_vehicle_status.timestamp = px4::get_time_micros(); + msg_vehicle_status.hil_state = msg_vehicle_status.HIL_STATE_OFF; + msg_vehicle_status.hil_state = msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; + msg_vehicle_status.is_rotary_wing = true; + msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED; + + + _msg_actuator_armed.armed = true; + _msg_actuator_armed.timestamp = px4::get_time_micros(); + _msg_vehicle_control_mode.timestamp = px4::get_time_micros(); + _msg_vehicle_control_mode.flag_armed = true; + + _vehicle_control_mode_pub.publish(_msg_vehicle_control_mode); + _actuator_armed_pub.publish(_msg_actuator_armed); + _vehicle_status_pub.publish(msg_vehicle_status); } } From a54849eeffde2cfefd5b8274fc2d7ef2da4e92e6 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 22 Feb 2015 14:54:32 +0100 Subject: [PATCH 061/112] adding previous integration demo tests --- CMakeLists.txt | 8 ++ integrationtests/demo_tests/arm_test.py | 37 +++++++ integrationtests/demo_tests/demo_tests.launch | 18 ++++ integrationtests/demo_tests/manual_input.py | 59 ++++++++++++ integrationtests/demo_tests/posctl_test.py | 96 +++++++++++++++++++ integrationtests/integrationtests.launch | 15 +++ package.xml | 1 + 7 files changed, 234 insertions(+) create mode 100755 integrationtests/demo_tests/arm_test.py create mode 100644 integrationtests/demo_tests/demo_tests.launch create mode 100755 integrationtests/demo_tests/manual_input.py create mode 100755 integrationtests/demo_tests/posctl_test.py create mode 100644 integrationtests/integrationtests.launch diff --git a/CMakeLists.txt b/CMakeLists.txt index f015e56184..ca8e114883 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -134,6 +134,7 @@ include_directories( src/ src/lib ${EIGEN_INCLUDE_DIRS} + integrationtests ) ## generate multiplatform wrapper headers @@ -320,3 +321,10 @@ install(TARGETS ${PROJECT_NAME} ## Add folders to be run by python nosetests # catkin_add_nosetests(test) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest(integrationtests/integrationtests.launch) +endif() + + diff --git a/integrationtests/demo_tests/arm_test.py b/integrationtests/demo_tests/arm_test.py new file mode 100755 index 0000000000..569e0af7ce --- /dev/null +++ b/integrationtests/demo_tests/arm_test.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python +PKG = 'px4' + +import sys +import unittest +import rospy + +from px4.msg import actuator_armed +from manual_input import ManualInput + +class ArmTest(unittest.TestCase): + + # + # General callback functions used in tests + # + def actuator_armed_callback(self, data): + self.actuatorStatus = data + + # + # Test arming + # + def test_arm(self): + rospy.init_node('test_node', anonymous=True) + sub = rospy.Subscriber('px4_multicopter/actuator_armed', actuator_armed, self.actuator_armed_callback) + + # method to test + arm = ManualInput() + arm.arm() + + self.assertEquals(self.actuatorStatus.armed, True, "not armed") + + + + +if __name__ == '__main__': + import rostest + rostest.rosrun(PKG, 'arm_test', ArmTest) diff --git a/integrationtests/demo_tests/demo_tests.launch b/integrationtests/demo_tests/demo_tests.launch new file mode 100644 index 0000000000..b5377659f9 --- /dev/null +++ b/integrationtests/demo_tests/demo_tests.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/integrationtests/demo_tests/manual_input.py b/integrationtests/demo_tests/manual_input.py new file mode 100755 index 0000000000..4bf5085091 --- /dev/null +++ b/integrationtests/demo_tests/manual_input.py @@ -0,0 +1,59 @@ +#!/usr/bin/env python +import sys +import rospy + +from sensor_msgs.msg import Joy +from std_msgs.msg import Header + + +# +# Manual input control helper, fakes joystick input +# > needs to correspond to default mapping in manual_input node +# +class ManualInput: + + def __init__(self): + rospy.init_node('test_node', anonymous=True) + self.joyPx4 = rospy.Publisher('px4_multicopter/joy', Joy, queue_size=10) + self.joyIris = rospy.Publisher('iris/joy', Joy, queue_size=10) + + def arm(self): + rate = rospy.Rate(10) # 10hz + + msg = Joy() + msg.header = Header() + msg.buttons = [0, 0, 0, 0, 0] + msg.axes = [-0.0, -0.0, 1.0, -0.0, -0.0, 0.0, 0.0] + count = 0 + while not rospy.is_shutdown() and count < 10: + rospy.loginfo("zeroing") + self.joyPx4.publish(msg) + self.joyIris.publish(msg) + rate.sleep() + count = count + 1 + + msg.buttons = [0, 0, 0, 0, 0] + msg.axes = [-1.0, -0.0, 1.0, -0.0, -0.0, 0.0, 0.0] + count = 0 + while not rospy.is_shutdown() and count < 10: + rospy.loginfo("arming") + self.joyPx4.publish(msg) + self.joyIris.publish(msg) + rate.sleep() + count = count + 1 + + def posctl(self): + rate = rospy.Rate(10) # 10hz + + # triggers posctl + msg = Joy() + msg.header = Header() + msg.buttons = [0, 0, 1, 0, 0] + msg.axes = [-0.0, -0.0, 1.0, -0.0, -0.0, 0.0, 0.0] + count = 0 + while not rospy.is_shutdown() and count < 10: + rospy.loginfo("triggering posctl") + self.joyPx4.publish(msg) + self.joyIris.publish(msg) + rate.sleep() + count = count + 1 diff --git a/integrationtests/demo_tests/posctl_test.py b/integrationtests/demo_tests/posctl_test.py new file mode 100755 index 0000000000..1cbf09cf73 --- /dev/null +++ b/integrationtests/demo_tests/posctl_test.py @@ -0,0 +1,96 @@ +#!/usr/bin/env python +PKG = 'px4' + +import sys +import unittest +import rospy + +from px4.msg import vehicle_local_position +from px4.msg import vehicle_control_mode +from px4.msg import actuator_armed +from px4.msg import position_setpoint_triplet +from px4.msg import position_setpoint +from sensor_msgs.msg import Joy +from std_msgs.msg import Header + +from manual_input import ManualInput + + +class PosctlTest(unittest.TestCase): + + # + # General callback functions used in tests + # + def position_callback(self, data): + self.hasPos = True + self.localPosition = data + + def vehicle_control_mode_callback(self, data): + self.controlMode = data + + + # + # Helper methods + # + def is_at_position(self, x, y, z, offset): + rospy.loginfo("current position %f, %f, %f" % (self.localPosition.x, self.localPosition.y, self.localPosition.z)) + return self.localPosition.z > (z - offset) and self.localPosition.z < (z + offset) + + # + # Test POSCTL + # + def test_posctl(self): + rospy.init_node('test_node', anonymous=True) + rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback) + pubSpt = rospy.Publisher('px4_multicopter/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) + rate = rospy.Rate(10) # 10hz + + manIn = ManualInput() + + # arm and go into POSCTL + manIn.arm() + manIn.posctl() + self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set") + self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") + self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set") + + # set a position setpoint + pos = position_setpoint() + pos.valid = True + pos.x = 2 + pos.z = -2 + pos.y = 2 + pos.position_valid = True + stp = position_setpoint_triplet() + stp.current = pos + pubSpt.publish(stp) + + # does it reach the position in X seconds? + count = 0 + timeout = 120 + while(count < timeout): + if(self.is_at_position(pos.x, pos.y, pos.z, 0.5)): + break + count = count + 1 + rate.sleep() + + self.assertTrue(count < timeout, "took too long to get to position") + + # does it hold the position for Y seconds? + positionHeld = True + count = 0 + timeout = 50 + while(count < timeout): + if(not self.is_at_position(pos.x, pos.y, pos.z, 0.5)): + positionHeld = False + break + count = count + 1 + rate.sleep() + + self.assertTrue(count == timeout, "position could not be held") + + +if __name__ == '__main__': + import rostest + rostest.rosrun(PKG, 'posctl_test', PosctlTest) diff --git a/integrationtests/integrationtests.launch b/integrationtests/integrationtests.launch new file mode 100644 index 0000000000..2e59b56155 --- /dev/null +++ b/integrationtests/integrationtests.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/package.xml b/package.xml index 28b682c008..2da49096f9 100644 --- a/package.xml +++ b/package.xml @@ -46,6 +46,7 @@ eigen libmavconn tf + rostest roscpp rospy std_msgs From e0e7f8c5177b813b3e6e8eb0477f9ee32c06c407 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 22 Feb 2015 18:02:43 +0100 Subject: [PATCH 062/112] - updated manual input to publish directly to px4 and not over joystick topics - updated tests to work with current setup --- integrationtests/demo_tests/demo_tests.launch | 9 +- integrationtests/demo_tests/manual_input.py | 95 ++++++++++++++----- ...posctl_test.py => offboard_posctl_test.py} | 10 +- launch/gazebo_ardrone_empty_world.launch | 2 +- 4 files changed, 84 insertions(+), 32 deletions(-) rename integrationtests/demo_tests/{posctl_test.py => offboard_posctl_test.py} (93%) diff --git a/integrationtests/demo_tests/demo_tests.launch b/integrationtests/demo_tests/demo_tests.launch index b5377659f9..34281b781a 100644 --- a/integrationtests/demo_tests/demo_tests.launch +++ b/integrationtests/demo_tests/demo_tests.launch @@ -1,8 +1,8 @@ - - + + - + @@ -12,7 +12,8 @@ + - + diff --git a/integrationtests/demo_tests/manual_input.py b/integrationtests/demo_tests/manual_input.py index 4bf5085091..eb9144bbcb 100755 --- a/integrationtests/demo_tests/manual_input.py +++ b/integrationtests/demo_tests/manual_input.py @@ -2,43 +2,59 @@ import sys import rospy -from sensor_msgs.msg import Joy +from px4.msg import manual_control_setpoint +from mav_msgs.msg import CommandAttitudeThrust from std_msgs.msg import Header - # -# Manual input control helper, fakes joystick input -# > needs to correspond to default mapping in manual_input node +# Manual input control helper +# +# Note: this is not the way to do it. ATM it fakes input to iris/command/attitude because else +# the simulator does not instantiate our controller. # class ManualInput: def __init__(self): rospy.init_node('test_node', anonymous=True) - self.joyPx4 = rospy.Publisher('px4_multicopter/joy', Joy, queue_size=10) - self.joyIris = rospy.Publisher('iris/joy', Joy, queue_size=10) + self.pubMcsp = rospy.Publisher('px4_multicopter/manual_control_setpoint', manual_control_setpoint, queue_size=10) + self.pubAtt = rospy.Publisher('iris/command/attitude', CommandAttitudeThrust, queue_size=10) def arm(self): rate = rospy.Rate(10) # 10hz - msg = Joy() - msg.header = Header() - msg.buttons = [0, 0, 0, 0, 0] - msg.axes = [-0.0, -0.0, 1.0, -0.0, -0.0, 0.0, 0.0] + att = CommandAttitudeThrust() + att.header = Header() + + pos = manual_control_setpoint() + pos.x = 0 + pos.z = 0 + pos.y = 0 + pos.r = 0 + pos.mode_switch = 3 + pos.return_switch = 3 + pos.posctl_switch = 3 + pos.loiter_switch = 3 + pos.acro_switch = 0 + pos.offboard_switch = 3 + count = 0 while not rospy.is_shutdown() and count < 10: rospy.loginfo("zeroing") - self.joyPx4.publish(msg) - self.joyIris.publish(msg) + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) + # Fake input to iris commander + self.pubAtt.publish(att) rate.sleep() count = count + 1 - msg.buttons = [0, 0, 0, 0, 0] - msg.axes = [-1.0, -0.0, 1.0, -0.0, -0.0, 0.0, 0.0] + pos.r = 1 count = 0 while not rospy.is_shutdown() and count < 10: rospy.loginfo("arming") - self.joyPx4.publish(msg) - self.joyIris.publish(msg) + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) rate.sleep() count = count + 1 @@ -46,14 +62,49 @@ class ManualInput: rate = rospy.Rate(10) # 10hz # triggers posctl - msg = Joy() - msg.header = Header() - msg.buttons = [0, 0, 1, 0, 0] - msg.axes = [-0.0, -0.0, 1.0, -0.0, -0.0, 0.0, 0.0] + pos = manual_control_setpoint() + pos.x = 0 + pos.z = 0 + pos.y = 0 + pos.r = 0 + pos.mode_switch = 2 + pos.return_switch = 3 + pos.posctl_switch = 1 + pos.loiter_switch = 3 + pos.acro_switch = 0 + pos.offboard_switch = 3 + count = 0 while not rospy.is_shutdown() and count < 10: rospy.loginfo("triggering posctl") - self.joyPx4.publish(msg) - self.joyIris.publish(msg) + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) rate.sleep() count = count + 1 + + def offboard(self): + rate = rospy.Rate(10) # 10hz + + # triggers posctl + pos = manual_control_setpoint() + pos.x = 0 + pos.z = 0 + pos.y = 0 + pos.r = 0 + pos.mode_switch = 3 + pos.return_switch = 3 + pos.posctl_switch = 3 + pos.loiter_switch = 3 + pos.acro_switch = 0 + pos.offboard_switch = 1 + + count = 0 + while not rospy.is_shutdown() and count < 10: + rospy.loginfo("triggering posctl") + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) + rate.sleep() + count = count + 1 + diff --git a/integrationtests/demo_tests/posctl_test.py b/integrationtests/demo_tests/offboard_posctl_test.py similarity index 93% rename from integrationtests/demo_tests/posctl_test.py rename to integrationtests/demo_tests/offboard_posctl_test.py index 1cbf09cf73..0d56082459 100755 --- a/integrationtests/demo_tests/posctl_test.py +++ b/integrationtests/demo_tests/offboard_posctl_test.py @@ -16,7 +16,7 @@ from std_msgs.msg import Header from manual_input import ManualInput -class PosctlTest(unittest.TestCase): +class OffboardPosctlTest(unittest.TestCase): # # General callback functions used in tests @@ -37,7 +37,7 @@ class PosctlTest(unittest.TestCase): return self.localPosition.z > (z - offset) and self.localPosition.z < (z + offset) # - # Test POSCTL + # Test offboard POSCTL # def test_posctl(self): rospy.init_node('test_node', anonymous=True) @@ -48,9 +48,9 @@ class PosctlTest(unittest.TestCase): manIn = ManualInput() - # arm and go into POSCTL + # arm and go into offboard manIn.arm() - manIn.posctl() + manIn.offboard() self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set") self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set") @@ -93,4 +93,4 @@ class PosctlTest(unittest.TestCase): if __name__ == '__main__': import rostest - rostest.rosrun(PKG, 'posctl_test', PosctlTest) + rostest.rosrun(PKG, 'posctl_test', OffboardPosctlTest) diff --git a/launch/gazebo_ardrone_empty_world.launch b/launch/gazebo_ardrone_empty_world.launch index 9d19fe5f9a..22bfb0c799 100644 --- a/launch/gazebo_ardrone_empty_world.launch +++ b/launch/gazebo_ardrone_empty_world.launch @@ -3,7 +3,7 @@ - + From 75f1678047e2beb4ec4e1cf7fd383685175d3694 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 23 Feb 2015 18:18:08 +0100 Subject: [PATCH 063/112] - updated position control test - added flight path assertion helper --- .../demo_tests/flight_path_assertion.py | 122 ++++++++++++++++++ .../demo_tests/offboard_posctl_test.py | 85 +++++++----- 2 files changed, 177 insertions(+), 30 deletions(-) create mode 100644 integrationtests/demo_tests/flight_path_assertion.py diff --git a/integrationtests/demo_tests/flight_path_assertion.py b/integrationtests/demo_tests/flight_path_assertion.py new file mode 100644 index 0000000000..fb78f89298 --- /dev/null +++ b/integrationtests/demo_tests/flight_path_assertion.py @@ -0,0 +1,122 @@ +#!/usr/bin/env python +import sys +import rospy +import threading + +from px4.msg import vehicle_local_position +from gazebo_msgs.srv import SpawnModel +from gazebo_msgs.srv import SetModelState +from geometry_msgs.msg import Pose +from geometry_msgs.msg import Twist + +from numpy import linalg +import numpy as np +import math + +# +# Helper to test if vehicle stays in expected flight path. +# +class FlightPathAssertion(threading.Thread): + + def __init__(self, positions, tunnelRadius = 1, yawOffset = 0.2): + threading.Thread.__init__(self) + rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback) + self.spawn = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel) + self.setModelState = rospy.ServiceProxy('gazebo/set_model_state', SetModelState) + self.positions = positions + self.tunnelRadius = tunnelRadius + self.yawOffset = yawOffset + self.hasPos = False + self.shouldStop = False + self.center = positions[0] + self.endOfSegment = False + + def position_callback(self, data): + self.hasPos = True + self.localPosition = data + + def spawn_indicator(self): + xml = "true0.7%f1 0 0 0.51 0 0 0.5" % self.tunnelRadius + self.spawn("indicator", xml, "", Pose(), "") + + def position_indicator(self): + state = SetModelState() + state.model_name = "indicator" + pose = Pose() + pose.position.x = self.center[0] + pose.position.y = (-1) * self.center[1] + pose.position.z = (-1) * self.center[2] + state.pose = pose + state.twist = Twist() + state.reference_frame = "" + self.setModelState(state) + + def distance_to_line(self, a, b, pos): + v = b - a + w = pos - a + + c1 = np.dot(w, v) + if c1 <= 0: # before a + self.center = a + return linalg.norm(pos - a) + + c2 = np.dot(v, v) + if c2 <= c1: # after b + self.center = b + self.endOfSegment = True + return linalg.norm(pos - b) + + x = c1 / c2 + l = a + x * v + self.center = l + return linalg.norm(pos - l) + + def stop(self): + self.shouldStop = True + + def run(self): + rate = rospy.Rate(10) # 10hz + self.spawn_indicator() + + current = 0 + + while not self.shouldStop: + if (self.hasPos): + # calculate distance to line segment between first two points + # if distances > tunnelRadius + # exit with error + # advance current pos if not on the line anymore or distance to next point < tunnelRadius + # exit if current pos is now the last position + + self.position_indicator() + + pos = np.array((self.localPosition.x, + self.localPosition.y, + self.localPosition.z)) + aPos = np.array((self.positions[current][0], + self.positions[current][1], + self.positions[current][2])) + bPos = np.array((self.positions[current + 1][0], + self.positions[current + 1][1], + self.positions[current + 1][2])) + + dist = self.distance_to_line(aPos, bPos, pos) + bDist = linalg.norm(pos - bPos) + + rospy.loginfo("distance to line: %f, distance to end: %f" % (dist, bDist)) + + if (dist > self.tunnelRadius): + rospy.logerr("left tunnel at position (%f, %f, %f)" % (self.localPosition.x, self.localPosition.y, self.localPosition.z)) + # FIXME: assertion + break + + if (self.endOfSegment or bDist < self.tunnelRadius): + rospy.loginfo("next segment") + self.endOfSegment = False + current = current + 1 + + if (current == len(self.positions) - 1): + rospy.loginfo("no more positions") + break + + rate.sleep() diff --git a/integrationtests/demo_tests/offboard_posctl_test.py b/integrationtests/demo_tests/offboard_posctl_test.py index 0d56082459..af8d4b821b 100755 --- a/integrationtests/demo_tests/offboard_posctl_test.py +++ b/integrationtests/demo_tests/offboard_posctl_test.py @@ -5,6 +5,9 @@ import sys import unittest import rospy +from numpy import linalg +import numpy as np + from px4.msg import vehicle_local_position from px4.msg import vehicle_control_mode from px4.msg import actuator_armed @@ -14,10 +17,18 @@ from sensor_msgs.msg import Joy from std_msgs.msg import Header from manual_input import ManualInput +from flight_path_assertion import FlightPathAssertion class OffboardPosctlTest(unittest.TestCase): + def setUp(self): + rospy.init_node('test_node', anonymous=True) + rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback) + self.pubSpt = rospy.Publisher('px4_multicopter/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) + self.rate = rospy.Rate(10) # 10hz + # # General callback functions used in tests # @@ -33,19 +44,37 @@ class OffboardPosctlTest(unittest.TestCase): # Helper methods # def is_at_position(self, x, y, z, offset): - rospy.loginfo("current position %f, %f, %f" % (self.localPosition.x, self.localPosition.y, self.localPosition.z)) - return self.localPosition.z > (z - offset) and self.localPosition.z < (z + offset) + rospy.logdebug("current position %f, %f, %f" % (self.localPosition.x, self.localPosition.y, self.localPosition.z)) + desired = np.array((x, y, z)) + pos = np.array((self.localPosition.x, self.localPosition.y, self.localPosition.z)) + return linalg.norm(desired - pos) < offset + + def reach_position(self, x, y, z, timeout): + # set a position setpoint + pos = position_setpoint() + pos.valid = True + pos.x = x + pos.y = y + pos.z = z + pos.position_valid = True + stp = position_setpoint_triplet() + stp.current = pos + self.pubSpt.publish(stp) + + # does it reach the position in X seconds? + count = 0 + while(count < timeout): + if(self.is_at_position(pos.x, pos.y, pos.z, 0.5)): + break + count = count + 1 + self.rate.sleep() + + self.assertTrue(count < timeout, "took too long to get to position") # # Test offboard POSCTL # def test_posctl(self): - rospy.init_node('test_node', anonymous=True) - rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback) - pubSpt = rospy.Publisher('px4_multicopter/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) - rate = rospy.Rate(10) # 10hz - manIn = ManualInput() # arm and go into offboard @@ -55,42 +84,38 @@ class OffboardPosctlTest(unittest.TestCase): self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set") - # set a position setpoint - pos = position_setpoint() - pos.valid = True - pos.x = 2 - pos.z = -2 - pos.y = 2 - pos.position_valid = True - stp = position_setpoint_triplet() - stp.current = pos - pubSpt.publish(stp) + # prepare flight path assertion + fpa = FlightPathAssertion( + ( + (0,0,0), + (2,2,-2), + (2,-2,-2), + (-2,-2,-2), + (2,2,-2), + ), 0.5, 0) + fpa.start() - # does it reach the position in X seconds? - count = 0 - timeout = 120 - while(count < timeout): - if(self.is_at_position(pos.x, pos.y, pos.z, 0.5)): - break - count = count + 1 - rate.sleep() - - self.assertTrue(count < timeout, "took too long to get to position") + self.reach_position(2, 2, -2, 120) + self.reach_position(2, -2, -2, 120) + self.reach_position(-2, -2, -2, 120) + self.reach_position(2, 2, -2, 120) # does it hold the position for Y seconds? positionHeld = True count = 0 timeout = 50 while(count < timeout): - if(not self.is_at_position(pos.x, pos.y, pos.z, 0.5)): + if(not self.is_at_position(2, 2, -2, 0.5)): positionHeld = False break count = count + 1 - rate.sleep() + self.rate.sleep() self.assertTrue(count == timeout, "position could not be held") + fpa.stop() if __name__ == '__main__': import rostest rostest.rosrun(PKG, 'posctl_test', OffboardPosctlTest) + #unittest.main() From 85ac3e3515bc214a770074182617208b24ee0209 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 23 Feb 2015 18:33:10 +0100 Subject: [PATCH 064/112] renamed tests, added placeholder for mavros test --- integrationtests/demo_tests/demo_tests.launch | 19 ------------------- .../{arm_test.py => direct_arm_test.py} | 0 ...test.py => direct_offboard_posctl_test.py} | 0 .../demo_tests/direct_tests.launch | 18 ++++++++++++++++++ .../demo_tests/mavros_tests.launch | 18 ++++++++++++++++++ integrationtests/integrationtests.launch | 15 ++------------- 6 files changed, 38 insertions(+), 32 deletions(-) delete mode 100644 integrationtests/demo_tests/demo_tests.launch rename integrationtests/demo_tests/{arm_test.py => direct_arm_test.py} (100%) rename integrationtests/demo_tests/{offboard_posctl_test.py => direct_offboard_posctl_test.py} (100%) create mode 100644 integrationtests/demo_tests/direct_tests.launch create mode 100644 integrationtests/demo_tests/mavros_tests.launch diff --git a/integrationtests/demo_tests/demo_tests.launch b/integrationtests/demo_tests/demo_tests.launch deleted file mode 100644 index 34281b781a..0000000000 --- a/integrationtests/demo_tests/demo_tests.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - - - diff --git a/integrationtests/demo_tests/arm_test.py b/integrationtests/demo_tests/direct_arm_test.py similarity index 100% rename from integrationtests/demo_tests/arm_test.py rename to integrationtests/demo_tests/direct_arm_test.py diff --git a/integrationtests/demo_tests/offboard_posctl_test.py b/integrationtests/demo_tests/direct_offboard_posctl_test.py similarity index 100% rename from integrationtests/demo_tests/offboard_posctl_test.py rename to integrationtests/demo_tests/direct_offboard_posctl_test.py diff --git a/integrationtests/demo_tests/direct_tests.launch b/integrationtests/demo_tests/direct_tests.launch new file mode 100644 index 0000000000..d871c085cf --- /dev/null +++ b/integrationtests/demo_tests/direct_tests.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/integrationtests/demo_tests/mavros_tests.launch b/integrationtests/demo_tests/mavros_tests.launch new file mode 100644 index 0000000000..f5ad3412d0 --- /dev/null +++ b/integrationtests/demo_tests/mavros_tests.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/integrationtests/integrationtests.launch b/integrationtests/integrationtests.launch index 2e59b56155..7f6368d12b 100644 --- a/integrationtests/integrationtests.launch +++ b/integrationtests/integrationtests.launch @@ -1,15 +1,4 @@ - - - - - - - - - - - - - + + From 952f91738ea0704fe5dbbb1598a17ca53f3a70a0 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 23 Feb 2015 21:45:01 +0100 Subject: [PATCH 065/112] update flight path assertion and error handling --- .../demo_tests/direct_offboard_posctl_test.py | 35 ++++++++++--------- .../demo_tests/flight_path_assertion.py | 23 +++++++++--- 2 files changed, 37 insertions(+), 21 deletions(-) diff --git a/integrationtests/demo_tests/direct_offboard_posctl_test.py b/integrationtests/demo_tests/direct_offboard_posctl_test.py index af8d4b821b..6b28e0a18d 100755 --- a/integrationtests/demo_tests/direct_offboard_posctl_test.py +++ b/integrationtests/demo_tests/direct_offboard_posctl_test.py @@ -29,6 +29,10 @@ class OffboardPosctlTest(unittest.TestCase): self.pubSpt = rospy.Publisher('px4_multicopter/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) self.rate = rospy.Rate(10) # 10hz + def tearDown(self): + if (self.fpa): + self.fpa.stop() + # # General callback functions used in tests # @@ -83,22 +87,21 @@ class OffboardPosctlTest(unittest.TestCase): self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set") self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set") - - # prepare flight path assertion - fpa = FlightPathAssertion( - ( - (0,0,0), - (2,2,-2), - (2,-2,-2), - (-2,-2,-2), - (2,2,-2), - ), 0.5, 0) - fpa.start() - self.reach_position(2, 2, -2, 120) - self.reach_position(2, -2, -2, 120) - self.reach_position(-2, -2, -2, 120) - self.reach_position(2, 2, -2, 120) + # prepare flight path assertion + positions = ( + (0,0,0), + (2,2,-2), + (2,-2,-2), + (-2,-2,-2), + (2,2,-2)) + + self.fpa = FlightPathAssertion(positions, 1, 0) + self.fpa.start() + + for i in range(0, len(positions)): + self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120) + self.assertFalse(self.fpa.failed, "breached flight path tunnel (%d)" % i) # does it hold the position for Y seconds? positionHeld = True @@ -112,7 +115,7 @@ class OffboardPosctlTest(unittest.TestCase): self.rate.sleep() self.assertTrue(count == timeout, "position could not be held") - fpa.stop() + self.fpa.stop() if __name__ == '__main__': diff --git a/integrationtests/demo_tests/flight_path_assertion.py b/integrationtests/demo_tests/flight_path_assertion.py index fb78f89298..1d99b7e5ac 100644 --- a/integrationtests/demo_tests/flight_path_assertion.py +++ b/integrationtests/demo_tests/flight_path_assertion.py @@ -6,6 +6,7 @@ import threading from px4.msg import vehicle_local_position from gazebo_msgs.srv import SpawnModel from gazebo_msgs.srv import SetModelState +from gazebo_msgs.srv import DeleteModel from geometry_msgs.msg import Pose from geometry_msgs.msg import Twist @@ -18,11 +19,20 @@ import math # class FlightPathAssertion(threading.Thread): + # + # Arguments + # - positions: tuple of tuples in the form (x, y, z, heading) + # + # TODO: yaw validation + # TODO: fail main test thread + # def __init__(self, positions, tunnelRadius = 1, yawOffset = 0.2): threading.Thread.__init__(self) rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback) - self.spawn = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel) + self.spawnModel = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel) self.setModelState = rospy.ServiceProxy('gazebo/set_model_state', SetModelState) + self.deleteModel = rospy.ServiceProxy('gazebo/delete_model', DeleteModel) + self.positions = positions self.tunnelRadius = tunnelRadius self.yawOffset = yawOffset @@ -30,14 +40,16 @@ class FlightPathAssertion(threading.Thread): self.shouldStop = False self.center = positions[0] self.endOfSegment = False + self.failed = False def position_callback(self, data): self.hasPos = True self.localPosition = data def spawn_indicator(self): + self.deleteModel("indicator") xml = "true0.7%f1 0 0 0.51 0 0 0.5" % self.tunnelRadius - self.spawn("indicator", xml, "", Pose(), "") + self.spawnModel("indicator", xml, "", Pose(), "") def position_indicator(self): state = SetModelState() @@ -103,11 +115,12 @@ class FlightPathAssertion(threading.Thread): dist = self.distance_to_line(aPos, bPos, pos) bDist = linalg.norm(pos - bPos) - rospy.loginfo("distance to line: %f, distance to end: %f" % (dist, bDist)) + rospy.logdebug("distance to line: %f, distance to end: %f" % (dist, bDist)) if (dist > self.tunnelRadius): - rospy.logerr("left tunnel at position (%f, %f, %f)" % (self.localPosition.x, self.localPosition.y, self.localPosition.z)) - # FIXME: assertion + msg = "left tunnel at position (%f, %f, %f)" % (self.localPosition.x, self.localPosition.y, self.localPosition.z) + rospy.logerr(msg) + self.failed = True break if (self.endOfSegment or bDist < self.tunnelRadius): From 638022f7ad6cd3bff61e264e8a80940ff23d084c Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 23 Feb 2015 22:02:26 +0100 Subject: [PATCH 066/112] fix headers and indentation --- .../demo_tests/direct_arm_test.py | 74 +++++-- .../demo_tests/direct_offboard_posctl_test.py | 204 ++++++++++-------- .../demo_tests/flight_path_assertion.py | 36 ++++ integrationtests/demo_tests/manual_input.py | 200 ++++++++++------- 4 files changed, 329 insertions(+), 185 deletions(-) diff --git a/integrationtests/demo_tests/direct_arm_test.py b/integrationtests/demo_tests/direct_arm_test.py index 569e0af7ce..238f2d7e0f 100755 --- a/integrationtests/demo_tests/direct_arm_test.py +++ b/integrationtests/demo_tests/direct_arm_test.py @@ -1,4 +1,40 @@ #!/usr/bin/env python +#*************************************************************************** +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +#***************************************************************************/ + +# +# @author Andreas Antener +# PKG = 'px4' import sys @@ -10,28 +46,28 @@ from manual_input import ManualInput class ArmTest(unittest.TestCase): - # - # General callback functions used in tests - # - def actuator_armed_callback(self, data): - self.actuatorStatus = data - - # - # Test arming - # - def test_arm(self): - rospy.init_node('test_node', anonymous=True) - sub = rospy.Subscriber('px4_multicopter/actuator_armed', actuator_armed, self.actuator_armed_callback) + # + # General callback functions used in tests + # + def actuator_armed_callback(self, data): + self.actuatorStatus = data + + # + # Test arming + # + def test_arm(self): + rospy.init_node('test_node', anonymous=True) + sub = rospy.Subscriber('px4_multicopter/actuator_armed', actuator_armed, self.actuator_armed_callback) - # method to test - arm = ManualInput() - arm.arm() + # method to test + arm = ManualInput() + arm.arm() - self.assertEquals(self.actuatorStatus.armed, True, "not armed") + self.assertEquals(self.actuatorStatus.armed, True, "not armed") - + if __name__ == '__main__': - import rostest - rostest.rosrun(PKG, 'arm_test', ArmTest) + import rostest + rostest.rosrun(PKG, 'arm_test', ArmTest) diff --git a/integrationtests/demo_tests/direct_offboard_posctl_test.py b/integrationtests/demo_tests/direct_offboard_posctl_test.py index 6b28e0a18d..42667757b0 100755 --- a/integrationtests/demo_tests/direct_offboard_posctl_test.py +++ b/integrationtests/demo_tests/direct_offboard_posctl_test.py @@ -1,4 +1,40 @@ #!/usr/bin/env python +#*************************************************************************** +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +#***************************************************************************/ + +# +# @author Andreas Antener +# PKG = 'px4' import sys @@ -22,103 +58,103 @@ from flight_path_assertion import FlightPathAssertion class OffboardPosctlTest(unittest.TestCase): - def setUp(self): - rospy.init_node('test_node', anonymous=True) - rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback) - self.pubSpt = rospy.Publisher('px4_multicopter/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) - self.rate = rospy.Rate(10) # 10hz + def setUp(self): + rospy.init_node('test_node', anonymous=True) + rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback) + self.pubSpt = rospy.Publisher('px4_multicopter/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) + self.rate = rospy.Rate(10) # 10hz - def tearDown(self): - if (self.fpa): - self.fpa.stop() + def tearDown(self): + if (self.fpa): + self.fpa.stop() - # - # General callback functions used in tests - # - def position_callback(self, data): - self.hasPos = True - self.localPosition = data + # + # General callback functions used in tests + # + def position_callback(self, data): + self.hasPos = True + self.localPosition = data - def vehicle_control_mode_callback(self, data): - self.controlMode = data + def vehicle_control_mode_callback(self, data): + self.controlMode = data - # - # Helper methods - # - def is_at_position(self, x, y, z, offset): - rospy.logdebug("current position %f, %f, %f" % (self.localPosition.x, self.localPosition.y, self.localPosition.z)) - desired = np.array((x, y, z)) - pos = np.array((self.localPosition.x, self.localPosition.y, self.localPosition.z)) - return linalg.norm(desired - pos) < offset + # + # Helper methods + # + def is_at_position(self, x, y, z, offset): + rospy.logdebug("current position %f, %f, %f" % (self.localPosition.x, self.localPosition.y, self.localPosition.z)) + desired = np.array((x, y, z)) + pos = np.array((self.localPosition.x, self.localPosition.y, self.localPosition.z)) + return linalg.norm(desired - pos) < offset - def reach_position(self, x, y, z, timeout): - # set a position setpoint - pos = position_setpoint() - pos.valid = True - pos.x = x - pos.y = y - pos.z = z - pos.position_valid = True - stp = position_setpoint_triplet() - stp.current = pos - self.pubSpt.publish(stp) + def reach_position(self, x, y, z, timeout): + # set a position setpoint + pos = position_setpoint() + pos.valid = True + pos.x = x + pos.y = y + pos.z = z + pos.position_valid = True + stp = position_setpoint_triplet() + stp.current = pos + self.pubSpt.publish(stp) - # does it reach the position in X seconds? - count = 0 - while(count < timeout): - if(self.is_at_position(pos.x, pos.y, pos.z, 0.5)): - break - count = count + 1 - self.rate.sleep() + # does it reach the position in X seconds? + count = 0 + while(count < timeout): + if(self.is_at_position(pos.x, pos.y, pos.z, 0.5)): + break + count = count + 1 + self.rate.sleep() - self.assertTrue(count < timeout, "took too long to get to position") + self.assertTrue(count < timeout, "took too long to get to position") - # - # Test offboard POSCTL - # - def test_posctl(self): - manIn = ManualInput() + # + # Test offboard POSCTL + # + def test_posctl(self): + manIn = ManualInput() - # arm and go into offboard - manIn.arm() - manIn.offboard() - self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set") - self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") - self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set") + # arm and go into offboard + manIn.arm() + manIn.offboard() + self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set") + self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") + self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set") - # prepare flight path assertion - positions = ( - (0,0,0), - (2,2,-2), - (2,-2,-2), - (-2,-2,-2), - (2,2,-2)) + # prepare flight path assertion + positions = ( + (0,0,0), + (2,2,-2), + (2,-2,-2), + (-2,-2,-2), + (2,2,-2)) - self.fpa = FlightPathAssertion(positions, 1, 0) - self.fpa.start() + self.fpa = FlightPathAssertion(positions, 1, 0) + self.fpa.start() - for i in range(0, len(positions)): - self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120) - self.assertFalse(self.fpa.failed, "breached flight path tunnel (%d)" % i) - - # does it hold the position for Y seconds? - positionHeld = True - count = 0 - timeout = 50 - while(count < timeout): - if(not self.is_at_position(2, 2, -2, 0.5)): - positionHeld = False - break - count = count + 1 - self.rate.sleep() + for i in range(0, len(positions)): + self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120) + self.assertFalse(self.fpa.failed, "breached flight path tunnel (%d)" % i) + + # does it hold the position for Y seconds? + positionHeld = True + count = 0 + timeout = 50 + while(count < timeout): + if(not self.is_at_position(2, 2, -2, 0.5)): + positionHeld = False + break + count = count + 1 + self.rate.sleep() - self.assertTrue(count == timeout, "position could not be held") - self.fpa.stop() - + self.assertTrue(count == timeout, "position could not be held") + self.fpa.stop() + if __name__ == '__main__': - import rostest - rostest.rosrun(PKG, 'posctl_test', OffboardPosctlTest) - #unittest.main() + import rostest + rostest.rosrun(PKG, 'posctl_test', OffboardPosctlTest) + #unittest.main() diff --git a/integrationtests/demo_tests/flight_path_assertion.py b/integrationtests/demo_tests/flight_path_assertion.py index 1d99b7e5ac..1f5bf01fc8 100644 --- a/integrationtests/demo_tests/flight_path_assertion.py +++ b/integrationtests/demo_tests/flight_path_assertion.py @@ -1,4 +1,40 @@ #!/usr/bin/env python +#*************************************************************************** +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +#***************************************************************************/ + +# +# @author Andreas Antener +# import sys import rospy import threading diff --git a/integrationtests/demo_tests/manual_input.py b/integrationtests/demo_tests/manual_input.py index eb9144bbcb..55911bede6 100755 --- a/integrationtests/demo_tests/manual_input.py +++ b/integrationtests/demo_tests/manual_input.py @@ -1,4 +1,40 @@ #!/usr/bin/env python +#*************************************************************************** +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +#***************************************************************************/ + +# +# @author Andreas Antener +# import sys import rospy @@ -14,97 +50,97 @@ from std_msgs.msg import Header # class ManualInput: - def __init__(self): - rospy.init_node('test_node', anonymous=True) - self.pubMcsp = rospy.Publisher('px4_multicopter/manual_control_setpoint', manual_control_setpoint, queue_size=10) - self.pubAtt = rospy.Publisher('iris/command/attitude', CommandAttitudeThrust, queue_size=10) + def __init__(self): + rospy.init_node('test_node', anonymous=True) + self.pubMcsp = rospy.Publisher('px4_multicopter/manual_control_setpoint', manual_control_setpoint, queue_size=10) + self.pubAtt = rospy.Publisher('iris/command/attitude', CommandAttitudeThrust, queue_size=10) - def arm(self): - rate = rospy.Rate(10) # 10hz + def arm(self): + rate = rospy.Rate(10) # 10hz - att = CommandAttitudeThrust() - att.header = Header() + att = CommandAttitudeThrust() + att.header = Header() - pos = manual_control_setpoint() - pos.x = 0 - pos.z = 0 - pos.y = 0 - pos.r = 0 - pos.mode_switch = 3 - pos.return_switch = 3 - pos.posctl_switch = 3 - pos.loiter_switch = 3 - pos.acro_switch = 0 - pos.offboard_switch = 3 + pos = manual_control_setpoint() + pos.x = 0 + pos.z = 0 + pos.y = 0 + pos.r = 0 + pos.mode_switch = 3 + pos.return_switch = 3 + pos.posctl_switch = 3 + pos.loiter_switch = 3 + pos.acro_switch = 0 + pos.offboard_switch = 3 - count = 0 - while not rospy.is_shutdown() and count < 10: - rospy.loginfo("zeroing") - time = rospy.get_rostime().now() - pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubMcsp.publish(pos) - # Fake input to iris commander - self.pubAtt.publish(att) - rate.sleep() - count = count + 1 + count = 0 + while not rospy.is_shutdown() and count < 10: + rospy.loginfo("zeroing") + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) + # Fake input to iris commander + self.pubAtt.publish(att) + rate.sleep() + count = count + 1 - pos.r = 1 - count = 0 - while not rospy.is_shutdown() and count < 10: - rospy.loginfo("arming") - time = rospy.get_rostime().now() - pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubMcsp.publish(pos) - rate.sleep() - count = count + 1 + pos.r = 1 + count = 0 + while not rospy.is_shutdown() and count < 10: + rospy.loginfo("arming") + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) + rate.sleep() + count = count + 1 - def posctl(self): - rate = rospy.Rate(10) # 10hz + def posctl(self): + rate = rospy.Rate(10) # 10hz - # triggers posctl - pos = manual_control_setpoint() - pos.x = 0 - pos.z = 0 - pos.y = 0 - pos.r = 0 - pos.mode_switch = 2 - pos.return_switch = 3 - pos.posctl_switch = 1 - pos.loiter_switch = 3 - pos.acro_switch = 0 - pos.offboard_switch = 3 + # triggers posctl + pos = manual_control_setpoint() + pos.x = 0 + pos.z = 0 + pos.y = 0 + pos.r = 0 + pos.mode_switch = 2 + pos.return_switch = 3 + pos.posctl_switch = 1 + pos.loiter_switch = 3 + pos.acro_switch = 0 + pos.offboard_switch = 3 - count = 0 - while not rospy.is_shutdown() and count < 10: - rospy.loginfo("triggering posctl") - time = rospy.get_rostime().now() - pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubMcsp.publish(pos) - rate.sleep() - count = count + 1 + count = 0 + while not rospy.is_shutdown() and count < 10: + rospy.loginfo("triggering posctl") + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) + rate.sleep() + count = count + 1 - def offboard(self): - rate = rospy.Rate(10) # 10hz + def offboard(self): + rate = rospy.Rate(10) # 10hz - # triggers posctl - pos = manual_control_setpoint() - pos.x = 0 - pos.z = 0 - pos.y = 0 - pos.r = 0 - pos.mode_switch = 3 - pos.return_switch = 3 - pos.posctl_switch = 3 - pos.loiter_switch = 3 - pos.acro_switch = 0 - pos.offboard_switch = 1 + # triggers posctl + pos = manual_control_setpoint() + pos.x = 0 + pos.z = 0 + pos.y = 0 + pos.r = 0 + pos.mode_switch = 3 + pos.return_switch = 3 + pos.posctl_switch = 3 + pos.loiter_switch = 3 + pos.acro_switch = 0 + pos.offboard_switch = 1 - count = 0 - while not rospy.is_shutdown() and count < 10: - rospy.loginfo("triggering posctl") - time = rospy.get_rostime().now() - pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubMcsp.publish(pos) - rate.sleep() - count = count + 1 + count = 0 + while not rospy.is_shutdown() and count < 10: + rospy.loginfo("triggering posctl") + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) + rate.sleep() + count = count + 1 From b955b9391d611706d1fd022c527426cab6a54924 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 23 Feb 2015 23:35:06 +0100 Subject: [PATCH 067/112] added mavros offboard test --- .../demo_tests/mavros_offboard_posctl_test.py | 146 ++++++++++++++++++ .../demo_tests/mavros_tests.launch | 4 +- 2 files changed, 148 insertions(+), 2 deletions(-) create mode 100755 integrationtests/demo_tests/mavros_offboard_posctl_test.py diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py new file mode 100755 index 0000000000..7468ad53f0 --- /dev/null +++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py @@ -0,0 +1,146 @@ +#!/usr/bin/env python +#*************************************************************************** +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +#***************************************************************************/ + +# +# @author Andreas Antener +# +PKG = 'px4' + +import sys +import unittest +import rospy +import math + +from numpy import linalg +import numpy as np + +from px4.msg import vehicle_control_mode +from std_msgs.msg import Header +from geometry_msgs.msg import PoseStamped, Quaternion +from tf.transformations import quaternion_from_euler + +class OffboardPosctlTest(unittest.TestCase): + + def setUp(self): + rospy.init_node('test_node', anonymous=True) + rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber("px4_multicopter/mavros/position/local", PoseStamped, self.position_callback) + self.pubSpt = rospy.Publisher('px4_multicopter/mavros/setpoint/local_position', PoseStamped, queue_size=10) + self.rate = rospy.Rate(10) # 10hz + self.hasPos = False + + # + # General callback functions used in tests + # + def position_callback(self, data): + self.hasPos = True + self.localPosition = data + + def vehicle_control_mode_callback(self, data): + self.controlMode = data + + + # + # Helper methods + # + def is_at_position(self, x, y, z, offset): + if(not self.hasPos): + return False + + rospy.logdebug("current position %f, %f, %f" % (self.localPosition.pose.position.x, self.localPosition.pose.position.y, self.localPosition.pose.position.z)) + desired = np.array((x, y, z)) + pos = np.array((self.localPosition.pose.position.x, self.localPosition.pose.position.y, self.localPosition.pose.position.z)) + return linalg.norm(desired - pos) < offset + + def reach_position(self, x, y, z, timeout): + # set a position setpoint + pos = PoseStamped() + pos.header = Header() + pos.header.frame_id = "base_footprint" + pos.header.stamp = rospy.Time.now() + pos.pose.position.x = x + pos.pose.position.y = y + pos.pose.position.z = z + + # For demo purposes we will lock yaw/heading to north. + yaw_degrees = 0 # North + yaw = math.radians(yaw_degrees) + quaternion = quaternion_from_euler(0, 0, yaw) + pos.pose.orientation = Quaternion(*quaternion) + + # does it reach the position in X seconds? + count = 0 + while(count < timeout): + self.pubSpt.publish(pos) + + if(self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5)): + break + count = count + 1 + self.rate.sleep() + + self.assertTrue(count < timeout, "took too long to get to position") + + # + # Test offboard POSCTL + # + def test_posctl(self): + # prepare flight path assertion + positions = ( + (0,0,0), + (2,2,2), + (2,-2,2), + (-2,-2,2), + (2,2,2)) + + for i in range(0, len(positions)): + self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120) + + # does it hold the position for Y seconds? + positionHeld = True + count = 0 + timeout = 50 + while(count < timeout): + if(not self.is_at_position(2, 2, 2, 0.5)): + positionHeld = False + break + count = count + 1 + self.rate.sleep() + + self.assertTrue(count == timeout, "position could not be held") + + +if __name__ == '__main__': + import rostest + rostest.rosrun(PKG, 'posctl_test', OffboardPosctlTest) + #unittest.main() diff --git a/integrationtests/demo_tests/mavros_tests.launch b/integrationtests/demo_tests/mavros_tests.launch index f5ad3412d0..8c1ad7b4db 100644 --- a/integrationtests/demo_tests/mavros_tests.launch +++ b/integrationtests/demo_tests/mavros_tests.launch @@ -1,5 +1,5 @@ - + @@ -14,5 +14,5 @@ - + From 482f2c94424f32e45aaee68b8b515e1eab40b6de Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 23 Feb 2015 23:45:54 +0100 Subject: [PATCH 068/112] added integration tests to cmake list --- CMakeLists.txt | 3 ++- integrationtests/integrationtests.launch | 4 ---- 2 files changed, 2 insertions(+), 5 deletions(-) delete mode 100644 integrationtests/integrationtests.launch diff --git a/CMakeLists.txt b/CMakeLists.txt index ca8e114883..457a0bfa74 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -324,7 +324,8 @@ install(TARGETS ${PROJECT_NAME} if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) - add_rostest(integrationtests/integrationtests.launch) + add_rostest(integrationtests/demo_tests/direct_tests.launch) + add_rostest(integrationtests/demo_tests/mavros_tests.launch) endif() diff --git a/integrationtests/integrationtests.launch b/integrationtests/integrationtests.launch deleted file mode 100644 index 7f6368d12b..0000000000 --- a/integrationtests/integrationtests.launch +++ /dev/null @@ -1,4 +0,0 @@ - - - - From 7fb82e74c89eeb7f542080305a95b94098a548ab Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 28 Feb 2015 18:39:36 +0100 Subject: [PATCH 069/112] tiny comment improvement --- src/modules/mavlink/mavlink_receiver.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index bce93cc6a5..573d5aecf2 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -659,10 +659,10 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m struct offboard_control_mode_s offboard_control_mode; memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));//XXX breaks compatibility with multiple setpoints - + struct actuator_controls_s actuator_controls; memset(&actuator_controls, 0, sizeof(actuator_controls));//XXX breaks compatibility with multiple setpoints - + if ((mavlink_system.sysid == set_actuator_control_target.target_system || set_actuator_control_target.target_system == 0) && (mavlink_system.compid == set_actuator_control_target.target_component || @@ -768,7 +768,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); /* - * The tricky part in pasrsing this message is that the offboard sender can set attitude and thrust + * The tricky part in parsing this message is that the offboard sender *can* set attitude and thrust * using different messages. Eg.: First send set_attitude_target containing the attitude and ignore * bits set for everything else and then send set_attitude_target containing the thrust and ignore bits * set for everything else. From 5706e1d7789afdab9316cc056b74d23a30676909 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 27 Feb 2015 16:55:19 +0100 Subject: [PATCH 070/112] use mavros arming service to arm --- integrationtests/demo_tests/mavros_offboard_posctl_test.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py index 7468ad53f0..59f8276b16 100755 --- a/integrationtests/demo_tests/mavros_offboard_posctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py @@ -49,6 +49,7 @@ from px4.msg import vehicle_control_mode from std_msgs.msg import Header from geometry_msgs.msg import PoseStamped, Quaternion from tf.transformations import quaternion_from_euler +from mavros.srv import CommandBool class OffboardPosctlTest(unittest.TestCase): @@ -57,6 +58,7 @@ class OffboardPosctlTest(unittest.TestCase): rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) rospy.Subscriber("px4_multicopter/mavros/position/local", PoseStamped, self.position_callback) self.pubSpt = rospy.Publisher('px4_multicopter/mavros/setpoint/local_position', PoseStamped, queue_size=10) + self.cmdArm = rospy.ServiceProxy("px4_multicopter/mavros/cmd/arming", CommandBool) self.rate = rospy.Rate(10) # 10hz self.hasPos = False @@ -111,10 +113,15 @@ class OffboardPosctlTest(unittest.TestCase): self.assertTrue(count < timeout, "took too long to get to position") + def arm(self): + return self.cmdArm(value=True) + # # Test offboard POSCTL # def test_posctl(self): + self.assertTrue(self.arm(), "Could not arm") + # prepare flight path assertion positions = ( (0,0,0), From 5d8516b356a58c865de2336c211e1974f2f453e2 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 27 Feb 2015 17:32:01 +0100 Subject: [PATCH 071/112] wait on armin service and wait 2 seconds after calling it --- integrationtests/demo_tests/mavros_offboard_posctl_test.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py index 59f8276b16..533d9ad3c3 100755 --- a/integrationtests/demo_tests/mavros_offboard_posctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py @@ -55,12 +55,15 @@ class OffboardPosctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) + rospy.wait_for_service('px4_multicopter/mavros/cmd/arming', 30) rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) rospy.Subscriber("px4_multicopter/mavros/position/local", PoseStamped, self.position_callback) self.pubSpt = rospy.Publisher('px4_multicopter/mavros/setpoint/local_position', PoseStamped, queue_size=10) self.cmdArm = rospy.ServiceProxy("px4_multicopter/mavros/cmd/arming", CommandBool) self.rate = rospy.Rate(10) # 10hz + self.rateSec = rospy.Rate(1) self.hasPos = False + self.controlMode = vehicle_control_mode() # # General callback functions used in tests @@ -121,6 +124,9 @@ class OffboardPosctlTest(unittest.TestCase): # def test_posctl(self): self.assertTrue(self.arm(), "Could not arm") + self.rateSec.sleep() + self.rateSec.sleep() + self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set after 2 seconds") # prepare flight path assertion positions = ( From c4b938fee625d1c9aabfd5c75d63635384f5d52f Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 27 Feb 2015 18:01:16 +0100 Subject: [PATCH 072/112] moved mavros to root node --- .../demo_tests/mavros_offboard_posctl_test.py | 10 +++---- launch/mavros_sitl.launch | 28 +++++++++---------- 2 files changed, 18 insertions(+), 20 deletions(-) diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py index 533d9ad3c3..14c7d6c19d 100755 --- a/integrationtests/demo_tests/mavros_offboard_posctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py @@ -55,11 +55,11 @@ class OffboardPosctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) - rospy.wait_for_service('px4_multicopter/mavros/cmd/arming', 30) - rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - rospy.Subscriber("px4_multicopter/mavros/position/local", PoseStamped, self.position_callback) - self.pubSpt = rospy.Publisher('px4_multicopter/mavros/setpoint/local_position', PoseStamped, queue_size=10) - self.cmdArm = rospy.ServiceProxy("px4_multicopter/mavros/cmd/arming", CommandBool) + rospy.wait_for_service('mavros/cmd/arming', 30) + rospy.Subscriber('vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber("mavros/position/local", PoseStamped, self.position_callback) + self.pubSpt = rospy.Publisher('mavros/setpoint/local_position', PoseStamped, queue_size=10) + self.cmdArm = rospy.ServiceProxy("mavros/cmd/arming", CommandBool) self.rate = rospy.Rate(10) # 10hz self.rateSec = rospy.Rate(1) self.hasPos = False diff --git a/launch/mavros_sitl.launch b/launch/mavros_sitl.launch index 582fdaa7d2..40010a2735 100644 --- a/launch/mavros_sitl.launch +++ b/launch/mavros_sitl.launch @@ -2,22 +2,20 @@ - - - - - + + + + - + - - - + + + - - - - - - + + + + + From edfbde1505456e89d79c11be5c8780a1219cce30 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sat, 28 Feb 2015 15:27:23 +0100 Subject: [PATCH 073/112] move demo nodes out of px4 namespace --- .../gazebo_ardrone_empty_world_offboard_attitudedemo.launch | 5 ++--- .../gazebo_ardrone_empty_world_offboard_positiondemo.launch | 5 ++--- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch b/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch index 2b9d797f6e..717244abf5 100644 --- a/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch +++ b/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch @@ -3,8 +3,7 @@ - - - + + diff --git a/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch b/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch index ce2386920f..9ff7f7d1fa 100644 --- a/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch +++ b/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch @@ -3,8 +3,7 @@ - - - + + From 675b86550fe15a7c7cbd94c030f4907c5eb7ef18 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Feb 2015 20:49:54 +0100 Subject: [PATCH 074/112] Sensors: Clear old mag rotation param once new values is present --- src/modules/sensors/sensors.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 1d392b0daa..527ca22100 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1543,6 +1543,9 @@ Sensors::parameter_update_poll(bool forced) if ((deprecated_mag_rot != 0) && (mag_rot <= 0)) { mag_rot = deprecated_mag_rot; param_set_no_notification(param_find(str), &mag_rot); + /* clear the old param, not supported in GUI anyway */ + deprecated_mag_rot = 0; + param_set_no_notification(param_find("SENS_EXT_MAG_ROT"), &deprecated_mag_rot); } /* handling of transition from internal to external */ From 6635d4113eaf29c278045d74f9a4c6e2974ad485 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Mar 2015 11:38:11 +0100 Subject: [PATCH 075/112] Remove header which is now auto-generated --- .../uORB/topics/vehicle_force_setpoint.h | 65 ------------------- 1 file changed, 65 deletions(-) delete mode 100644 src/modules/uORB/topics/vehicle_force_setpoint.h diff --git a/src/modules/uORB/topics/vehicle_force_setpoint.h b/src/modules/uORB/topics/vehicle_force_setpoint.h deleted file mode 100644 index e3a7360b2b..0000000000 --- a/src/modules/uORB/topics/vehicle_force_setpoint.h +++ /dev/null @@ -1,65 +0,0 @@ -/**************************************************************************** - * - * 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 vehicle_force_setpoint.h - * @author Thomas Gubler - * Definition of force (NED) setpoint uORB topic. Typically this can be used - * by a position control app together with an attitude control app. - */ - -#ifndef TOPIC_VEHICLE_FORCE_SETPOINT_H_ -#define TOPIC_VEHICLE_FORCE_SETPOINT_H_ - -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -struct vehicle_force_setpoint_s { - float x; /**< in N NED */ - float y; /**< in N NED */ - float z; /**< in N NED */ - float yaw; /**< right-hand rotation around downward axis (rad, equivalent to Tait-Bryan yaw) */ -}; /**< Desired force in NED frame */ - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_force_setpoint); - -#endif From 2b71bff85879fa0264877e9391f579d53071ea13 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Mar 2015 11:38:53 +0100 Subject: [PATCH 076/112] Generate messages first, then check submodules --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index ab8c614e14..97e25c54dc 100644 --- a/Makefile +++ b/Makefile @@ -124,7 +124,7 @@ $(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4 .PHONY: $(FIRMWARES) $(BUILD_DIR)%.build/firmware.px4: config = $(patsubst $(BUILD_DIR)%.build/firmware.px4,%,$@) $(BUILD_DIR)%.build/firmware.px4: work_dir = $(BUILD_DIR)$(config).build/ -$(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: checksubmodules generateuorbtopicheaders +$(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: generateuorbtopicheaders checksubmodules @$(ECHO) %%%% @$(ECHO) %%%% Building $(config) in $(work_dir) @$(ECHO) %%%% From f10bcb037778ec47e947e724cd354a7d3de55473 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Mar 2015 12:32:46 +0100 Subject: [PATCH 077/112] mc pos ctrl multiplatform: do not publish att sp in offboard && no position/velocity control --- .../mc_pos_control.cpp | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp index 2e14b744f6..40268358ad 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -1015,12 +1015,19 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti reset_yaw_sp = true; } - /* publish attitude setpoint */ - if (_att_sp_pub != nullptr) { - _att_sp_pub->publish(_att_sp_msg); + /* publish attitude setpoint + * Do not publish if offboard is enabled but position/velocity control is disabled, in this case the attitude setpoint + * is published by the mavlink app + */ + if (!(_control_mode->data().flag_control_offboard_enabled && + !(_control_mode->data().flag_control_position_enabled || + _control_mode->data().flag_control_velocity_enabled))) { + if (_att_sp_pub != nullptr) { + _att_sp_pub->publish(_att_sp_msg); - } else { - _att_sp_pub = _n.advertise(); + } else { + _att_sp_pub = _n.advertise(); + } } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ From 0dec33526b3d372950733c9a3588d88cc622fc08 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Mar 2015 12:33:27 +0100 Subject: [PATCH 078/112] mc pos ctrl: do not publish att sp in offboard && no position/velocity control --- .../mc_pos_control/mc_pos_control_main.cpp | 25 ++++++++++++------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 7f87e3532b..e29b0c66f2 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -761,8 +761,8 @@ void MulticopterPositionControl::control_auto(float dt) orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); //Make sure that the position setpoint is valid - if (!isfinite(_pos_sp_triplet.current.lat) || - !isfinite(_pos_sp_triplet.current.lon) || + if (!isfinite(_pos_sp_triplet.current.lat) || + !isfinite(_pos_sp_triplet.current.lon) || !isfinite(_pos_sp_triplet.current.alt)) { _pos_sp_triplet.current.valid = false; } @@ -1367,7 +1367,7 @@ MulticopterPositionControl::task_main() } else if (yaw_offs > YAW_OFFSET_MAX) { _att_sp.yaw_body = _wrap_pi(_att.yaw + YAW_OFFSET_MAX); - } + } } //Control roll and pitch directly if we no aiding velocity controller is active @@ -1388,15 +1388,22 @@ MulticopterPositionControl::task_main() _att_sp.timestamp = hrt_absolute_time(); } else { - reset_yaw_sp = true; + reset_yaw_sp = true; } - // publish attitude setpoint - if (_att_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + /* publish attitude setpoint + * Do not publish if offboard is enabled but position/velocity control is disabled, + * in this case the attitude setpoint is published by the mavlink app + */ + if (!(_control_mode.flag_control_offboard_enabled && + !(_control_mode.flag_control_position_enabled || + _control_mode.flag_control_velocity_enabled))) { + if (_att_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); - } else { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } else { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ From 100fb07142f58c72bd3d5f49846cadb94ad35a41 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Mar 2015 18:06:00 +0100 Subject: [PATCH 079/112] Fix ordering of check submodule execution --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index 97e25c54dc..7620099f98 100644 --- a/Makefile +++ b/Makefile @@ -236,7 +236,7 @@ GENMSG_PYTHONPATH = $(PX4_BASE)Tools/genmsg/src GENCPP_PYTHONPATH = $(PX4_BASE)Tools/gencpp/src .PHONY: generateuorbtopicheaders -generateuorbtopicheaders: +generateuorbtopicheaders: checksubmodules @$(ECHO) "Generating uORB topic headers" $(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH):$(PYTHONPATH) $(PYTHON) \ $(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \ From 030348eddd4546c3b7d0e6bbbe473e8821f03d94 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Mar 2015 18:10:14 +0100 Subject: [PATCH 080/112] commander: Improve status feedback to make status parsing simpler in UI --- src/modules/commander/accelerometer_calibration.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 87065b56f3..d70e050006 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -358,7 +358,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se if (orient < 0) { mavlink_and_console_log_info(mavlink_fd, "invalid motion, hold still..."); - sleep(3); + sleep(2); continue; } @@ -372,6 +372,8 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se mavlink_and_console_log_info(mavlink_fd, "Hold still, starting to measure %s side", orientation_strs[orient]); sleep(1); read_accelerometer_avg(subs, accel_ref, orient, samples_num); + mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]); + usleep(100000); mavlink_and_console_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", orientation_strs[orient], (double)accel_ref[0][orient][0], (double)accel_ref[0][orient][1], From 29aabbd762878aa2882a538b763a247a9f2ad33f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Feb 2015 11:22:43 +0100 Subject: [PATCH 081/112] MC: Use combined att / pos filter --- ROMFS/px4fmu_common/init.d/rc.mc_apps | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 2ecc104df7..e2fe67946f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -4,9 +4,10 @@ # att & pos estimator, att & pos control. # -attitude_estimator_ekf start -#ekf_att_pos_estimator start -position_estimator_inav start +# previously (2014) the system was relying on +#attitude_estimator_ekf start +#position_estimator_inav start +ekf_att_pos_estimator start if mc_att_control start then From d0b4b2157a3c57ed5652f098299c59f8e1d5689f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Feb 2015 11:23:01 +0100 Subject: [PATCH 082/112] mkblctrl: Use less RAM --- src/drivers/mkblctrl/mkblctrl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index db53b2647f..d3b8d4e9d5 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -296,7 +296,7 @@ MK::init(unsigned motors) _task = task_spawn_cmd("mkblctrl", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, - 2048, + 1500, (main_t)&MK::task_main_trampoline, nullptr); From f9e525bbab886f3c6227c7dd286a60dc27f0d0db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Feb 2015 11:23:16 +0100 Subject: [PATCH 083/112] commander: Do not allocate excessive stack --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2c5e0aa886..74ebe0ae43 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -966,7 +966,7 @@ int commander_thread_main(int argc, char *argv[]) pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, 2900); + pthread_attr_setstacksize(&commander_low_prio_attr, 2600); struct sched_param param; (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); From da5b06a5df475b853368a895e92d80a1c1406d28 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Feb 2015 11:23:29 +0100 Subject: [PATCH 084/112] land detector: Use less RAM --- src/modules/land_detector/land_detector_main.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 1e43e7ad5d..011567e577 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -139,7 +139,7 @@ static int land_detector_start(const char *mode) _landDetectorTaskID = task_spawn_cmd("land_detector", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1200, + 1000, (main_t)&land_detector_deamon_thread, nullptr); @@ -179,8 +179,7 @@ int land_detector_main(int argc, char *argv[]) { if (argc < 1) { - warnx("usage: land_detector {start|stop|status} [mode]\nmode can either be 'fixedwing' or 'multicopter'"); - exit(0); + goto exiterr; } if (argc >= 2 && !strcmp(argv[1], "start")) { @@ -209,6 +208,8 @@ int land_detector_main(int argc, char *argv[]) } } - warn("usage: land_detector {start|stop|status} [mode]\nmode can either be 'fixedwing' or 'multicopter'"); +exiterr: + warnx("usage: land_detector {start|stop|status} [mode]"); + warnx("mode can either be 'fixedwing' or 'multicopter'"); return 1; } From 0ebbb5f533b969a1de32dcb4d2a87a0e9fea74d2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Feb 2015 11:23:44 +0100 Subject: [PATCH 085/112] MAVLink: Use less RAM --- src/modules/mavlink/mavlink_main.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 024dfd6fb7..0132a3193d 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1621,7 +1621,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2800, + 2700, (main_t)&Mavlink::start_helper, (char * const *)argv); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 573d5aecf2..0c34fc58a8 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1549,7 +1549,7 @@ MavlinkReceiver::receive_start(Mavlink *parent) param.sched_priority = SCHED_PRIORITY_MAX - 80; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); - pthread_attr_setstacksize(&receiveloop_attr, 2900); + pthread_attr_setstacksize(&receiveloop_attr, 2100); pthread_t thread; pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent); From acd54728794d1087c282b5c5e4e1d32ecda423ee Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Feb 2015 11:23:58 +0100 Subject: [PATCH 086/112] MC att control: Use less RAM --- src/modules/mc_att_control/mc_att_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index a0d76b0a60..0243dc2f7c 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -835,7 +835,7 @@ MulticopterAttitudeControl::start() _control_task = task_spawn_cmd("mc_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2000, + 1800, (main_t)&MulticopterAttitudeControl::task_main_trampoline, nullptr); From 8831e258b3f178fb3ceed10de5a788f725fe3f6f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Feb 2015 11:24:11 +0100 Subject: [PATCH 087/112] MC pos control: Use less RAM --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 7f87e3532b..247c8715c1 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1419,7 +1419,7 @@ MulticopterPositionControl::start() _control_task = task_spawn_cmd("mc_pos_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2000, + 1800, (main_t)&MulticopterPositionControl::task_main_trampoline, nullptr); From f3b0b41a0d3848c37b2533424b4501a84ef821ec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Mar 2015 18:05:22 +0100 Subject: [PATCH 088/112] Make INAV configurable --- ROMFS/px4fmu_common/init.d/rc.mc_apps | 13 +++++++++---- .../position_estimator_inav_params.c | 14 ++++++++++++++ 2 files changed, 23 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index e2fe67946f..6517e026ab 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -4,10 +4,15 @@ # att & pos estimator, att & pos control. # -# previously (2014) the system was relying on -#attitude_estimator_ekf start -#position_estimator_inav start -ekf_att_pos_estimator start +# previously (2014) the system was relying on +# INAV, which defaults to 0 now. +if param compare INAV_ENABLED 1 +then + attitude_estimator_ekf start + position_estimator_inav start +else + ekf_att_pos_estimator start +fi if mc_att_control start then diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 5387b7e87f..91915fb532 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -288,6 +288,20 @@ PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f); */ PARAM_DEFINE_INT32(CBRK_NO_VISION, 0); +/** + * INAV enabled + * + * If set to 1, use INAV for position estimation + * the system uses the compined attitude / position + * filter framework. + * + * @min 0.0 + * @max 1.0 + * @unit s + * @group Position Estimator INAV + */ +PARAM_DEFINE_INT32(INAV_ENABLED, 0); + int parameters_init(struct position_estimator_inav_param_handles *h) { h->w_z_baro = param_find("INAV_W_Z_BARO"); From 243d2bc4548a137a5fbd240ad47c5826f085d407 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Mar 2015 18:32:02 +0100 Subject: [PATCH 089/112] commander: Shorten mag cal interval --- src/modules/commander/mag_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index a94b478c46..b633dc6017 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) int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id) { /* 45 seconds */ - uint64_t calibration_interval = 45 * 1000 * 1000; + uint64_t calibration_interval = 25 * 1000 * 1000; /* maximum 500 values */ const unsigned int calibration_maxcount = 240; From 163c25268b2c41c566d7d99da2fddda719c79d49 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Mar 2015 12:52:05 +0100 Subject: [PATCH 090/112] mc att multi: reduce stack size --- .../mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp index c2b8470757..40eb498b49 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp @@ -71,7 +71,7 @@ int mc_att_control_m_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("mc_att_control_m", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 3000, + 1900, main, (argv) ? (char* const*)&argv[2] : (char* const*)NULL); From 1da2dc7e34d56eeed16bf034358a1e7e18ebcfcc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Mar 2015 12:52:15 +0100 Subject: [PATCH 091/112] mc pos multi: reduce stack size --- .../mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp index 87996d93ba..1082061f64 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp @@ -71,7 +71,7 @@ int mc_pos_control_m_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("mc_pos_control_m", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 3000, + 2500, main, (argv) ? (char* const*)&argv[2] : (char* const*)NULL); From dc678342226e2b200e7c4c030b9d2a456a497d19 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Mar 2015 19:04:14 +0100 Subject: [PATCH 092/112] Mag cal: Add 100% message --- src/modules/commander/mag_calibration.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index b633dc6017..e0786db79d 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -137,6 +137,9 @@ int do_mag_calibration(int mavlink_fd) } if (calibrated_ok) { + + mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100); + usleep(100000); mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); /* auto-save to EEPROM */ From 9252124cfc84ad31f4a88d937343c7e5682e3339 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 1 Mar 2015 23:56:05 +0100 Subject: [PATCH 093/112] - updated test names, fixed lying comments and updated notes, misc. cleanup - added mavros attitude control test - using manual arming for mavros tests so they succeed for the moment, arming should be implemented soon --- ...rm_test.py => direct_manual_input_test.py} | 30 +++- .../demo_tests/direct_offboard_posctl_test.py | 15 +- .../demo_tests/direct_tests.launch | 4 +- .../demo_tests/flight_path_assertion.py | 2 +- integrationtests/demo_tests/manual_input.py | 14 +- .../demo_tests/mavros_offboard_attctl_test.py | 142 ++++++++++++++++++ .../demo_tests/mavros_offboard_posctl_test.py | 25 ++- .../demo_tests/mavros_tests.launch | 1 + 8 files changed, 206 insertions(+), 27 deletions(-) rename integrationtests/demo_tests/{direct_arm_test.py => direct_manual_input_test.py} (67%) create mode 100755 integrationtests/demo_tests/mavros_offboard_attctl_test.py diff --git a/integrationtests/demo_tests/direct_arm_test.py b/integrationtests/demo_tests/direct_manual_input_test.py similarity index 67% rename from integrationtests/demo_tests/direct_arm_test.py rename to integrationtests/demo_tests/direct_manual_input_test.py index 238f2d7e0f..6d115316b2 100755 --- a/integrationtests/demo_tests/direct_arm_test.py +++ b/integrationtests/demo_tests/direct_manual_input_test.py @@ -42,32 +42,46 @@ import unittest import rospy from px4.msg import actuator_armed +from px4.msg import vehicle_control_mode from manual_input import ManualInput -class ArmTest(unittest.TestCase): +# +# Tests if commander reacts to manual input and sets control flags accordingly +# +class ManualInputTest(unittest.TestCase): # # General callback functions used in tests # def actuator_armed_callback(self, data): self.actuatorStatus = data + + def vehicle_control_mode_callback(self, data): + self.controlMode = data # # Test arming # - def test_arm(self): + def test_manual_input(self): rospy.init_node('test_node', anonymous=True) - sub = rospy.Subscriber('px4_multicopter/actuator_armed', actuator_armed, self.actuator_armed_callback) + rospy.Subscriber('px4_multicopter/actuator_armed', actuator_armed, self.actuator_armed_callback) + rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - # method to test - arm = ManualInput() - arm.arm() + man = ManualInput() - self.assertEquals(self.actuatorStatus.armed, True, "not armed") + # Test arming + man.arm() + self.assertEquals(self.actuatorStatus.armed, True, "did not arm") + # Test posctl + man.posctl() + self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set") + # Test offboard + man.offboard() + self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") if __name__ == '__main__': import rostest - rostest.rosrun(PKG, 'arm_test', ArmTest) + rostest.rosrun(PKG, 'direct_manual_input_test', ManualInputTest) diff --git a/integrationtests/demo_tests/direct_offboard_posctl_test.py b/integrationtests/demo_tests/direct_offboard_posctl_test.py index 42667757b0..0db4fd1312 100755 --- a/integrationtests/demo_tests/direct_offboard_posctl_test.py +++ b/integrationtests/demo_tests/direct_offboard_posctl_test.py @@ -55,7 +55,13 @@ from std_msgs.msg import Header from manual_input import ManualInput from flight_path_assertion import FlightPathAssertion - +# +# Tests flying a path in offboard control by directly sending setpoints +# to the position controller (position_setpoint_triplet). +# +# For the test to be successful it needs to stay on the predefined path +# and reach all setpoints in a certain time. +# class OffboardPosctlTest(unittest.TestCase): def setUp(self): @@ -112,7 +118,7 @@ class OffboardPosctlTest(unittest.TestCase): self.assertTrue(count < timeout, "took too long to get to position") # - # Test offboard POSCTL + # Test offboard position control # def test_posctl(self): manIn = ManualInput() @@ -124,7 +130,7 @@ class OffboardPosctlTest(unittest.TestCase): self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set") - # prepare flight path assertion + # prepare flight path positions = ( (0,0,0), (2,2,-2), @@ -132,6 +138,7 @@ class OffboardPosctlTest(unittest.TestCase): (-2,-2,-2), (2,2,-2)) + # flight path assertion self.fpa = FlightPathAssertion(positions, 1, 0) self.fpa.start() @@ -156,5 +163,5 @@ class OffboardPosctlTest(unittest.TestCase): if __name__ == '__main__': import rostest - rostest.rosrun(PKG, 'posctl_test', OffboardPosctlTest) + rostest.rosrun(PKG, 'direct_offboard_posctl_test', OffboardPosctlTest) #unittest.main() diff --git a/integrationtests/demo_tests/direct_tests.launch b/integrationtests/demo_tests/direct_tests.launch index d871c085cf..1a7d843fdc 100644 --- a/integrationtests/demo_tests/direct_tests.launch +++ b/integrationtests/demo_tests/direct_tests.launch @@ -13,6 +13,6 @@ - - + + diff --git a/integrationtests/demo_tests/flight_path_assertion.py b/integrationtests/demo_tests/flight_path_assertion.py index 1f5bf01fc8..485de8c416 100644 --- a/integrationtests/demo_tests/flight_path_assertion.py +++ b/integrationtests/demo_tests/flight_path_assertion.py @@ -51,7 +51,7 @@ import numpy as np import math # -# Helper to test if vehicle stays in expected flight path. +# Helper to test if vehicle stays on expected flight path. # class FlightPathAssertion(threading.Thread): diff --git a/integrationtests/demo_tests/manual_input.py b/integrationtests/demo_tests/manual_input.py index 55911bede6..cf139bb1da 100755 --- a/integrationtests/demo_tests/manual_input.py +++ b/integrationtests/demo_tests/manual_input.py @@ -45,7 +45,7 @@ from std_msgs.msg import Header # # Manual input control helper # -# Note: this is not the way to do it. ATM it fakes input to iris/command/attitude because else +# FIXME: this is not the way to do it! ATM it fakes input to iris/command/attitude because else # the simulator does not instantiate our controller. # class ManualInput: @@ -74,7 +74,7 @@ class ManualInput: pos.offboard_switch = 3 count = 0 - while not rospy.is_shutdown() and count < 10: + while not rospy.is_shutdown() and count < 5: rospy.loginfo("zeroing") time = rospy.get_rostime().now() pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 @@ -86,7 +86,7 @@ class ManualInput: pos.r = 1 count = 0 - while not rospy.is_shutdown() and count < 10: + while not rospy.is_shutdown() and count < 5: rospy.loginfo("arming") time = rospy.get_rostime().now() pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 @@ -111,7 +111,7 @@ class ManualInput: pos.offboard_switch = 3 count = 0 - while not rospy.is_shutdown() and count < 10: + while not rospy.is_shutdown() and count < 5: rospy.loginfo("triggering posctl") time = rospy.get_rostime().now() pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 @@ -122,7 +122,7 @@ class ManualInput: def offboard(self): rate = rospy.Rate(10) # 10hz - # triggers posctl + # triggers offboard pos = manual_control_setpoint() pos.x = 0 pos.z = 0 @@ -136,8 +136,8 @@ class ManualInput: pos.offboard_switch = 1 count = 0 - while not rospy.is_shutdown() and count < 10: - rospy.loginfo("triggering posctl") + while not rospy.is_shutdown() and count < 5: + rospy.loginfo("triggering offboard") time = rospy.get_rostime().now() pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 self.pubMcsp.publish(pos) diff --git a/integrationtests/demo_tests/mavros_offboard_attctl_test.py b/integrationtests/demo_tests/mavros_offboard_attctl_test.py new file mode 100755 index 0000000000..2f568adc94 --- /dev/null +++ b/integrationtests/demo_tests/mavros_offboard_attctl_test.py @@ -0,0 +1,142 @@ +#!/usr/bin/env python +#*************************************************************************** +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +#***************************************************************************/ + +# +# @author Andreas Antener +# +PKG = 'px4' + +import sys +import unittest +import rospy +import math + +from numpy import linalg +import numpy as np + +from px4.msg import vehicle_control_mode +from std_msgs.msg import Header +from std_msgs.msg import Float64 +from geometry_msgs.msg import PoseStamped, Quaternion +from tf.transformations import quaternion_from_euler +from mavros.srv import CommandBool + +from manual_input import ManualInput + +# +# Tests flying a path in offboard control by sending position setpoints +# over MAVROS. +# +# For the test to be successful it needs to reach all setpoints in a certain time. +# FIXME: add flight path assertion (needs transformation from ROS frame to NED) +# +class OffboardAttctlTest(unittest.TestCase): + + def setUp(self): + rospy.init_node('test_node', anonymous=True) + rospy.wait_for_service('mavros/cmd/arming', 30) + rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber("mavros/position/local", PoseStamped, self.position_callback) + self.pubAtt = rospy.Publisher('mavros/setpoint/attitude', PoseStamped, queue_size=10) + self.pubThr = rospy.Publisher('mavros/setpoint/att_throttle', Float64, queue_size=10) + self.cmdArm = rospy.ServiceProxy("mavros/cmd/arming", CommandBool) + self.rate = rospy.Rate(10) # 10hz + self.rateSec = rospy.Rate(1) + self.hasPos = False + self.controlMode = vehicle_control_mode() + + # + # General callback functions used in tests + # + def position_callback(self, data): + self.hasPos = True + self.localPosition = data + + def vehicle_control_mode_callback(self, data): + self.controlMode = data + + + # + # Helper methods + # + def arm(self): + return self.cmdArm(value=True) + + # + # Test offboard position control + # + def test_attctl(self): + # FIXME: this must go ASAP when arming is implemented + manIn = ManualInput() + manIn.arm() + + self.assertTrue(self.arm(), "Could not arm") + self.rateSec.sleep() + self.rateSec.sleep() + self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set after 2 seconds") + + # set some attitude and thrust + att = PoseStamped() + att.header = Header() + att.header.frame_id = "base_footprint" + att.header.stamp = rospy.Time.now() + quaternion = quaternion_from_euler(0.2, 0.2, 0) + att.pose.orientation = Quaternion(*quaternion) + + throttle = Float64() + throttle.data = 0.6 + + # does it cross expected boundaries in X seconds? + count = 0 + timeout = 120 + while(count < timeout): + # update timestamp for each published SP + att.header.stamp = rospy.Time.now() + self.pubAtt.publish(att) + self.pubThr.publish(throttle) + + if (self.localPosition.pose.position.x > 5 + and self.localPosition.pose.position.z > 5 + and self.localPosition.pose.position.y < -5): + break + count = count + 1 + self.rate.sleep() + + self.assertTrue(count < timeout, "took too long to cross boundaries") + + +if __name__ == '__main__': + import rostest + rostest.rosrun(PKG, 'mavros_offboard_attctl_test', OffboardAttctlTest) + #unittest.main() diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py index 14c7d6c19d..09ca5d9fd5 100755 --- a/integrationtests/demo_tests/mavros_offboard_posctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py @@ -51,12 +51,21 @@ from geometry_msgs.msg import PoseStamped, Quaternion from tf.transformations import quaternion_from_euler from mavros.srv import CommandBool +from manual_input import ManualInput + +# +# Tests flying a path in offboard control by sending position setpoints +# over MAVROS. +# +# For the test to be successful it needs to reach all setpoints in a certain time. +# FIXME: add flight path assertion (needs transformation from ROS frame to NED) +# class OffboardPosctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) rospy.wait_for_service('mavros/cmd/arming', 30) - rospy.Subscriber('vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) rospy.Subscriber("mavros/position/local", PoseStamped, self.position_callback) self.pubSpt = rospy.Publisher('mavros/setpoint/local_position', PoseStamped, queue_size=10) self.cmdArm = rospy.ServiceProxy("mavros/cmd/arming", CommandBool) @@ -93,7 +102,6 @@ class OffboardPosctlTest(unittest.TestCase): pos = PoseStamped() pos.header = Header() pos.header.frame_id = "base_footprint" - pos.header.stamp = rospy.Time.now() pos.pose.position.x = x pos.pose.position.y = y pos.pose.position.z = z @@ -107,6 +115,8 @@ class OffboardPosctlTest(unittest.TestCase): # does it reach the position in X seconds? count = 0 while(count < timeout): + # update timestamp for each published SP + pos.header.stamp = rospy.Time.now() self.pubSpt.publish(pos) if(self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5)): @@ -120,15 +130,20 @@ class OffboardPosctlTest(unittest.TestCase): return self.cmdArm(value=True) # - # Test offboard POSCTL + # Test offboard position control # def test_posctl(self): + # FIXME: this must go ASAP when arming is implemented + manIn = ManualInput() + manIn.arm() + manIn.offboard() + self.assertTrue(self.arm(), "Could not arm") self.rateSec.sleep() self.rateSec.sleep() self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set after 2 seconds") - # prepare flight path assertion + # prepare flight path positions = ( (0,0,0), (2,2,2), @@ -155,5 +170,5 @@ class OffboardPosctlTest(unittest.TestCase): if __name__ == '__main__': import rostest - rostest.rosrun(PKG, 'posctl_test', OffboardPosctlTest) + rostest.rosrun(PKG, 'mavros_offboard_posctl_test', OffboardPosctlTest) #unittest.main() diff --git a/integrationtests/demo_tests/mavros_tests.launch b/integrationtests/demo_tests/mavros_tests.launch index 8c1ad7b4db..4651f0dc9c 100644 --- a/integrationtests/demo_tests/mavros_tests.launch +++ b/integrationtests/demo_tests/mavros_tests.launch @@ -15,4 +15,5 @@ + From 3f8c011e8c05bcfc668bb1aff8b3227c47cf8884 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 2 Mar 2015 00:28:53 +0100 Subject: [PATCH 094/112] updated test class names to be unique --- integrationtests/demo_tests/direct_offboard_posctl_test.py | 4 ++-- integrationtests/demo_tests/mavros_offboard_attctl_test.py | 4 ++-- integrationtests/demo_tests/mavros_offboard_posctl_test.py | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/integrationtests/demo_tests/direct_offboard_posctl_test.py b/integrationtests/demo_tests/direct_offboard_posctl_test.py index 0db4fd1312..e09550bbc2 100755 --- a/integrationtests/demo_tests/direct_offboard_posctl_test.py +++ b/integrationtests/demo_tests/direct_offboard_posctl_test.py @@ -62,7 +62,7 @@ from flight_path_assertion import FlightPathAssertion # For the test to be successful it needs to stay on the predefined path # and reach all setpoints in a certain time. # -class OffboardPosctlTest(unittest.TestCase): +class DirectOffboardPosctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) @@ -163,5 +163,5 @@ class OffboardPosctlTest(unittest.TestCase): if __name__ == '__main__': import rostest - rostest.rosrun(PKG, 'direct_offboard_posctl_test', OffboardPosctlTest) + rostest.rosrun(PKG, 'direct_offboard_posctl_test', DirectOffboardPosctlTest) #unittest.main() diff --git a/integrationtests/demo_tests/mavros_offboard_attctl_test.py b/integrationtests/demo_tests/mavros_offboard_attctl_test.py index 2f568adc94..27885635a1 100755 --- a/integrationtests/demo_tests/mavros_offboard_attctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_attctl_test.py @@ -61,7 +61,7 @@ from manual_input import ManualInput # For the test to be successful it needs to reach all setpoints in a certain time. # FIXME: add flight path assertion (needs transformation from ROS frame to NED) # -class OffboardAttctlTest(unittest.TestCase): +class MavrosOffboardAttctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) @@ -138,5 +138,5 @@ class OffboardAttctlTest(unittest.TestCase): if __name__ == '__main__': import rostest - rostest.rosrun(PKG, 'mavros_offboard_attctl_test', OffboardAttctlTest) + rostest.rosrun(PKG, 'mavros_offboard_attctl_test', MavrosOffboardAttctlTest) #unittest.main() diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py index 09ca5d9fd5..a1f1cf3c55 100755 --- a/integrationtests/demo_tests/mavros_offboard_posctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py @@ -60,7 +60,7 @@ from manual_input import ManualInput # For the test to be successful it needs to reach all setpoints in a certain time. # FIXME: add flight path assertion (needs transformation from ROS frame to NED) # -class OffboardPosctlTest(unittest.TestCase): +class MavrosOffboardPosctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) @@ -170,5 +170,5 @@ class OffboardPosctlTest(unittest.TestCase): if __name__ == '__main__': import rostest - rostest.rosrun(PKG, 'mavros_offboard_posctl_test', OffboardPosctlTest) + rostest.rosrun(PKG, 'mavros_offboard_posctl_test', MavrosOffboardPosctlTest) #unittest.main() From ca4adebe117af84f77141f6fc7b2f865cba58086 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 2 Mar 2015 14:30:46 +0900 Subject: [PATCH 095/112] batt_smbus: remove debug --- src/drivers/batt_smbus/batt_smbus.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 604ce35c57..a958fc60d3 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -452,10 +452,7 @@ BATT_SMBUS::read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_ uint8_t pec = get_PEC(reg, true, buff, bufflen + 1); if (pec != buff[bufflen + 1]) { - // debug - warnx("CurrPEC:%x vs MyPec:%x", (int)buff[bufflen + 1], (int)pec); return 0; - } // copy data From 48bf84ff3754109fe6cf8e0e161eb70ae0987bfe Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 2 Mar 2015 14:30:01 +0900 Subject: [PATCH 096/112] oreoled: support send_bytes ioctl Also increase maximum command length to 24 bytes --- src/drivers/drv_oreoled.h | 5 ++++- src/drivers/oreoled/oreoled.cpp | 25 +++++++++++++++++++++++++ 2 files changed, 29 insertions(+), 1 deletion(-) diff --git a/src/drivers/drv_oreoled.h b/src/drivers/drv_oreoled.h index bc53f04a54..0dcb10a7b1 100644 --- a/src/drivers/drv_oreoled.h +++ b/src/drivers/drv_oreoled.h @@ -58,6 +58,9 @@ /** run macro */ #define OREOLED_RUN_MACRO _OREOLEDIOC(2) +/** send bytes */ +#define OREOLED_SEND_BYTES _OREOLEDIOC(3) + /* Oreo LED driver supports up to 4 leds */ #define OREOLED_NUM_LEDS 4 @@ -65,7 +68,7 @@ #define OREOLED_ALL_INSTANCES 0xff /* maximum command length that can be sent to LEDs */ -#define OREOLED_CMD_LENGTH_MAX 10 +#define OREOLED_CMD_LENGTH_MAX 24 /* enum passed to OREOLED_SET_MODE ioctl() * defined by hardware */ diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index 2f5bf75bc2..b44c4b720b 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -368,6 +368,31 @@ OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) return ret; + case OREOLED_SEND_BYTES: + /* send bytes */ + new_cmd = *((oreoled_cmd_t *) arg); + + /* special handling for request to set all instances */ + if (new_cmd.led_num == OREOLED_ALL_INSTANCES) { + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + /* add command to queue for all healthy leds */ + if (_healthy[i]) { + new_cmd.led_num = i; + _cmd_queue->force(&new_cmd); + ret = OK; + } + } + + } else if (new_cmd.led_num < OREOLED_NUM_LEDS) { + /* request to set individual instance's rgb value */ + if (_healthy[new_cmd.led_num]) { + _cmd_queue->force(&new_cmd); + ret = OK; + } + } + + return ret; + default: /* see if the parent class can make any use of it */ ret = CDev::ioctl(filp, cmd, arg); From 5a402ed3f16c3bb989f9732be4efa9565a1c5170 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 2 Mar 2015 15:29:43 +0100 Subject: [PATCH 097/112] - publish offboard mode with manual input so the vehicle control status is correct - trigger offboard in mavros attitude test --- integrationtests/demo_tests/manual_input.py | 33 +++++++++++++++++-- .../demo_tests/mavros_offboard_attctl_test.py | 3 +- .../demo_tests/mavros_offboard_posctl_test.py | 2 +- 3 files changed, 34 insertions(+), 4 deletions(-) diff --git a/integrationtests/demo_tests/manual_input.py b/integrationtests/demo_tests/manual_input.py index cf139bb1da..9b2471a00e 100755 --- a/integrationtests/demo_tests/manual_input.py +++ b/integrationtests/demo_tests/manual_input.py @@ -39,6 +39,7 @@ import sys import rospy from px4.msg import manual_control_setpoint +from px4.msg import offboard_control_mode from mav_msgs.msg import CommandAttitudeThrust from std_msgs.msg import Header @@ -46,13 +47,15 @@ from std_msgs.msg import Header # Manual input control helper # # FIXME: this is not the way to do it! ATM it fakes input to iris/command/attitude because else -# the simulator does not instantiate our controller. +# the simulator does not instantiate our controller. Probably this whole class will disappear once +# arming works correctly. # class ManualInput: def __init__(self): rospy.init_node('test_node', anonymous=True) self.pubMcsp = rospy.Publisher('px4_multicopter/manual_control_setpoint', manual_control_setpoint, queue_size=10) + self.pubOff = rospy.Publisher('px4_multicopter/offboard_control_mode', offboard_control_mode, queue_size=10) self.pubAtt = rospy.Publisher('iris/command/attitude', CommandAttitudeThrust, queue_size=10) def arm(self): @@ -119,9 +122,35 @@ class ManualInput: rate.sleep() count = count + 1 - def offboard(self): + + def offboard_attctl(self): + self.offboard(False, False, True, True, True, True) + + def offboard_posctl(self): + self.offboard(False, False, True, False, True, True) + + # Trigger offboard and set offboard control mode before + def offboard(self, ignore_thrust=False, ignore_attitude=False, ignore_bodyrate=True, + ignore_position=False, ignore_velocity=True, ignore_acceleration_force=True): rate = rospy.Rate(10) # 10hz + mode = offboard_control_mode() + mode.ignore_thrust = ignore_thrust + mode.ignore_attitude = ignore_attitude + mode.ignore_bodyrate = ignore_bodyrate + mode.ignore_position = ignore_position + mode.ignore_velocity = ignore_velocity + mode.ignore_acceleration_force = ignore_acceleration_force + + count = 0 + while not rospy.is_shutdown() and count < 5: + rospy.loginfo("setting offboard mode") + time = rospy.get_rostime().now() + mode.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubOff.publish(mode) + rate.sleep() + count = count + 1 + # triggers offboard pos = manual_control_setpoint() pos.x = 0 diff --git a/integrationtests/demo_tests/mavros_offboard_attctl_test.py b/integrationtests/demo_tests/mavros_offboard_attctl_test.py index 27885635a1..a52f7ffc1b 100755 --- a/integrationtests/demo_tests/mavros_offboard_attctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_attctl_test.py @@ -100,6 +100,7 @@ class MavrosOffboardAttctlTest(unittest.TestCase): # FIXME: this must go ASAP when arming is implemented manIn = ManualInput() manIn.arm() + manIn.offboard_attctl() self.assertTrue(self.arm(), "Could not arm") self.rateSec.sleep() @@ -111,7 +112,7 @@ class MavrosOffboardAttctlTest(unittest.TestCase): att.header = Header() att.header.frame_id = "base_footprint" att.header.stamp = rospy.Time.now() - quaternion = quaternion_from_euler(0.2, 0.2, 0) + quaternion = quaternion_from_euler(0.15, 0.15, 0) att.pose.orientation = Quaternion(*quaternion) throttle = Float64() diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py index a1f1cf3c55..a3739ae5ce 100755 --- a/integrationtests/demo_tests/mavros_offboard_posctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py @@ -136,7 +136,7 @@ class MavrosOffboardPosctlTest(unittest.TestCase): # FIXME: this must go ASAP when arming is implemented manIn = ManualInput() manIn.arm() - manIn.offboard() + manIn.offboard_posctl() self.assertTrue(self.arm(), "Could not arm") self.rateSec.sleep() From b1dcb10f25862d7d833d91518a007b618236ad64 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 5 Jan 2015 23:16:30 -0500 Subject: [PATCH 098/112] add make check_format to check astyle code formatting --- .gitignore | 2 ++ Makefile | 4 ++++ Tools/check_code_style.sh | 28 ++++++++++++++++++++++++++++ Tools/fix_code_style.sh | 1 + 4 files changed, 35 insertions(+) create mode 100755 Tools/check_code_style.sh diff --git a/.gitignore b/.gitignore index 611325444e..0e553fa365 100644 --- a/.gitignore +++ b/.gitignore @@ -46,3 +46,5 @@ Firmware.zip unittests/build *.generated.h .vagrant +*.pretty + diff --git a/Makefile b/Makefile index 7620099f98..201187e021 100644 --- a/Makefile +++ b/Makefile @@ -262,6 +262,10 @@ testbuild: tests: generateuorbtopicheaders $(Q) (mkdir -p $(PX4_BASE)/unittests/build && cd $(PX4_BASE)/unittests/build && cmake .. && $(MAKE) unittests) +.PHONY: format check_format +check_format: + $(Q) (./Tools/check_code_style.sh | sort -n) + # # Cleanup targets. 'clean' should remove all built products and force # a complete re-compilation, 'distclean' should remove everything diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh new file mode 100755 index 0000000000..491fbb0ce2 --- /dev/null +++ b/Tools/check_code_style.sh @@ -0,0 +1,28 @@ +#!/usr/bin/env bash +set -eu +failed=0 +for fn in $(find . -path './src/lib/uavcan' -prune -o \ + -path './NuttX' -prune -o \ + -path './Build' -prune -o \ + -path './mavlink' -prune -o \ + -path './unittests/gtest' -prune -o \ + -name '*.c' -o -name '*.cpp' -o -name '*.hpp' -o -name '*.h' -type f); do + if [ -f "$fn" ]; + then + ./Tools/fix_code_style.sh --quiet < $fn > $fn.pretty + diffsize=$(diff -y --suppress-common-lines $fn $fn.pretty | wc -l) + rm -f $fn.pretty + if [ $diffsize -ne 0 ]; then + failed=1 + echo $diffsize $fn + fi + fi +done + +if [ $failed -eq 0 ]; then + echo "Format checks passed" + exit 0 +else + echo "Format checks failed; please run ./Tools/fix_code_style.sh on each file" + exit 1 +fi diff --git a/Tools/fix_code_style.sh b/Tools/fix_code_style.sh index 5995d428ea..e73a5a8af2 100755 --- a/Tools/fix_code_style.sh +++ b/Tools/fix_code_style.sh @@ -18,4 +18,5 @@ astyle \ --exclude=EASTL \ --add-brackets \ --max-code-length=120 \ + --preserve-date \ $* From 92a3b9ce1fad65f6c0b941b7c77c6b286f547ede Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 7 Jan 2015 23:01:02 -0500 Subject: [PATCH 099/112] add astyle pre-commit git hook --- Tools/pre-commit | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) create mode 100755 Tools/pre-commit diff --git a/Tools/pre-commit b/Tools/pre-commit new file mode 100755 index 0000000000..db5863d3eb --- /dev/null +++ b/Tools/pre-commit @@ -0,0 +1,29 @@ +#!/bin/sh +echo "hello world" + +if git rev-parse --verify HEAD >/dev/null 2>&1 +then + against=HEAD +else + # Initial commit: diff against an empty tree object + against=4b825dc642cb6eb9a060e54bf8d69288fbee4904 +fi + +# Redirect output to stderr. +exec 1>&2 + +CHANGED_FILES=`git diff --cached --name-only --diff-filter=ACM $against | grep '\.c\|\.cpp\|\.h\|\.hpp'` +FAILED=0 +if [ ! -z "$CHANGED_FILES" -a "$CHANGED_FILES" != " " ]; then + echo $CHANGED_FILES + for FILE in $CHANGED_FILES; do + ./Tools/fix_code_style.sh --quiet < $FILE > $FILE.pretty + diff -u $FILE $FILE.pretty || FAILED=1 + rm -f $FILE.pretty + if [ $FAILED -ne 0 ]; then + echo "There are code formatting errors. Please fix them by running ./Tools/fix_code_style.sh $FILE" + exit $FAILED + fi + done +fi +exit 0 From b5a2c8708ceea21bdea0c61f6999b5b3272d3f09 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 22 Feb 2015 17:41:02 -0500 Subject: [PATCH 100/112] check_code_style.sh ignore unittests/build directory --- Tools/check_code_style.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh index 491fbb0ce2..df6afced45 100755 --- a/Tools/check_code_style.sh +++ b/Tools/check_code_style.sh @@ -6,6 +6,7 @@ for fn in $(find . -path './src/lib/uavcan' -prune -o \ -path './Build' -prune -o \ -path './mavlink' -prune -o \ -path './unittests/gtest' -prune -o \ + -path './unittests/build' -prune -o \ -name '*.c' -o -name '*.cpp' -o -name '*.hpp' -o -name '*.h' -type f); do if [ -f "$fn" ]; then From b2082c124aed3f6e90b4925e98dfadf877b1636e Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 22 Feb 2015 17:50:55 -0500 Subject: [PATCH 101/112] git pre-commit hook don't echo files changed --- Tools/pre-commit | 3 --- 1 file changed, 3 deletions(-) diff --git a/Tools/pre-commit b/Tools/pre-commit index db5863d3eb..13cd4aaddb 100755 --- a/Tools/pre-commit +++ b/Tools/pre-commit @@ -1,6 +1,4 @@ #!/bin/sh -echo "hello world" - if git rev-parse --verify HEAD >/dev/null 2>&1 then against=HEAD @@ -15,7 +13,6 @@ exec 1>&2 CHANGED_FILES=`git diff --cached --name-only --diff-filter=ACM $against | grep '\.c\|\.cpp\|\.h\|\.hpp'` FAILED=0 if [ ! -z "$CHANGED_FILES" -a "$CHANGED_FILES" != " " ]; then - echo $CHANGED_FILES for FILE in $CHANGED_FILES; do ./Tools/fix_code_style.sh --quiet < $FILE > $FILE.pretty diff -u $FILE $FILE.pretty || FAILED=1 From 8de2b8af146826ff0495f495ffef509a988f85f8 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 22 Feb 2015 18:55:05 -0500 Subject: [PATCH 102/112] check_code_style.sh ignore mathlib/CMSIS --- Tools/check_code_style.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh index df6afced45..387aab92bf 100755 --- a/Tools/check_code_style.sh +++ b/Tools/check_code_style.sh @@ -2,6 +2,7 @@ set -eu failed=0 for fn in $(find . -path './src/lib/uavcan' -prune -o \ + -path './src/lib/mathlib/CMSIS' -prune -o \ -path './NuttX' -prune -o \ -path './Build' -prune -o \ -path './mavlink' -prune -o \ From 265147ce7f64d40124c615623afdde86fb2bb8f9 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 23 Feb 2015 13:10:12 -0500 Subject: [PATCH 103/112] make check_format ignore src/modules/attitude_estimator_ekf/codegen --- Tools/check_code_style.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh index 387aab92bf..8d1ab6363b 100755 --- a/Tools/check_code_style.sh +++ b/Tools/check_code_style.sh @@ -3,6 +3,7 @@ set -eu failed=0 for fn in $(find . -path './src/lib/uavcan' -prune -o \ -path './src/lib/mathlib/CMSIS' -prune -o \ + -path './src/modules/attitude_estimator_ekf/codegen/' -prune -o \ -path './NuttX' -prune -o \ -path './Build' -prune -o \ -path './mavlink' -prune -o \ From a31fccb0b0d2182b20e6d4b001a710f98f1d84ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 2 Mar 2015 21:11:21 +0100 Subject: [PATCH 104/112] Uploader: Fix flashing if multiple board types are connected --- Tools/px_uploader.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index f4e317cfa8..91ac084ef4 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -565,8 +565,12 @@ try: except RuntimeError as ex: - # print the error - print("\nERROR: %s" % ex.args) + if "not suitable" in ex.args: + up.close() + continue + else: + # print the error + print("\nERROR: %s" % ex.args) finally: # always close the port From 7a0db340f7363a678963e5761a006ac099d94c54 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 2 Mar 2015 12:56:12 +0100 Subject: [PATCH 105/112] added quaternion methods for inverse and vector rotation --- src/lib/mathlib/math/Quaternion.hpp | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index 38400beef5..d28966fca6 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -115,6 +115,35 @@ public: return Vector<3>(&data[1]); } + /** + * inverse of quaternion + */ + math::Quaternion inverse() { + Quaternion res; + memcpy(res.data,data,sizeof(res.data)); + res.data[1] = -res.data[1]; + res.data[2] = -res.data[2]; + res.data[3] = -res.data[3]; + return res; + } + + + /** + * rotate vector by quaternion + */ + Vector<3> rotate(const Vector<3> &w) { + Quaternion q_w; // extend vector to quaternion + Quaternion q = {data[0],data[1],data[2],data[3]}; + Quaternion q_rotated; // quaternion representation of rotated vector + q_w(0) = 0; + q_w(1) = w.data[0]; + q_w(2) = w.data[1]; + q_w(3) = w.data[2]; + q_rotated = q*q_w*q.inverse(); + Vector<3> res = {q_rotated.data[1],q_rotated.data[2],q_rotated.data[3]}; + return res; + } + /** * set quaternion to rotation defined by euler angles */ From 200f0e9a93ec9039d2fb5a2e519f2dd424a9d814 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 2 Mar 2015 23:05:03 +0100 Subject: [PATCH 106/112] Better error handling for multi-board setups --- Tools/px_uploader.py | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 91ac084ef4..859c821e5b 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -416,7 +416,7 @@ class uploader(object): def upload(self, fw): # Make sure we are doing the right thing if self.board_type != fw.property('board_id'): - raise RuntimeError("Firmware not suitable for this board") + raise IOError("Firmware not suitable for this board") if self.fw_maxsize < fw.property('image_size'): raise RuntimeError("Firmware image is too large for this board") @@ -564,13 +564,12 @@ try: up.upload(fw) except RuntimeError as ex: + # print the error + print("\nERROR: %s" % ex.args) - if "not suitable" in ex.args: - up.close() - continue - else: - # print the error - print("\nERROR: %s" % ex.args) + except IOError as e: + up.close(); + continue finally: # always close the port From 6ad5243a516af1b3d12c826d4bf4dc30c6f6b11d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Mar 2015 00:19:48 +0100 Subject: [PATCH 107/112] Fixed NuttX submodule to correct version --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 787aca971a..11afcdfee6 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 787aca971a86219d4e791100646b54ed8245a733 +Subproject commit 11afcdfee6a3961952dd92f02c1abaa4756b115f From 3e5b8ded8cdd650e961008ce65c93dd64a326554 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Mar 2015 17:27:50 +0100 Subject: [PATCH 108/112] Increase rate of MAVLink output on companion link --- ROMFS/px4fmu_common/init.d/rcS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index ea3f721f34..9118f26013 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -436,7 +436,7 @@ then then if param compare SYS_COMPANION 921600 then - mavlink start -d /dev/ttyS2 -b 921600 -m onboard + mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 20000 fi fi From f7ef6e19502aabbb4b8b1131b88854669885b178 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Mar 2015 17:28:14 +0100 Subject: [PATCH 109/112] Fix stack sizes based on observed usage. Frees some more RAM --- nuttx-configs/aerocore/nsh/defconfig | 6 +++--- nuttx-configs/px4fmu-v1/nsh/defconfig | 7 ++++--- nuttx-configs/px4fmu-v2/nsh/defconfig | 6 +++--- 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/nuttx-configs/aerocore/nsh/defconfig b/nuttx-configs/aerocore/nsh/defconfig index 29dff64aad..08ff4a9885 100644 --- a/nuttx-configs/aerocore/nsh/defconfig +++ b/nuttx-configs/aerocore/nsh/defconfig @@ -332,7 +332,7 @@ CONFIG_BOARD_LOOPSPERMSEC=16717 CONFIG_DRAM_START=0x20000000 CONFIG_DRAM_SIZE=262144 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y -CONFIG_ARCH_INTERRUPTSTACK=4096 +CONFIG_ARCH_INTERRUPTSTACK=1500 # # Boot options @@ -415,8 +415,8 @@ CONFIG_PREALLOC_TIMERS=50 # # Stack and heap information # -CONFIG_IDLETHREAD_STACKSIZE=6000 -CONFIG_USERMAIN_STACKSIZE=4096 +CONFIG_IDLETHREAD_STACKSIZE=1000 +CONFIG_USERMAIN_STACKSIZE=3000 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=2048 diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index 86ed041fff..cd410051c7 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -325,7 +325,8 @@ CONFIG_BOARD_LOOPSPERMSEC=16717 CONFIG_DRAM_START=0x20000000 CONFIG_DRAM_SIZE=196608 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y -CONFIG_ARCH_INTERRUPTSTACK=4096 +# The actual usage is 420 bytes +CONFIG_ARCH_INTERRUPTSTACK=1500 # # Boot options @@ -416,8 +417,8 @@ CONFIG_PREALLOC_TIMERS=50 # # Stack and heap information # -CONFIG_IDLETHREAD_STACKSIZE=3500 -CONFIG_USERMAIN_STACKSIZE=2600 +CONFIG_IDLETHREAD_STACKSIZE=1000 +CONFIG_USERMAIN_STACKSIZE=3000 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=2048 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 6a1aec22bd..4ccc5dacb6 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -367,7 +367,7 @@ CONFIG_BOARD_LOOPSPERMSEC=16717 CONFIG_DRAM_START=0x20000000 CONFIG_DRAM_SIZE=262144 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y -CONFIG_ARCH_INTERRUPTSTACK=4096 +CONFIG_ARCH_INTERRUPTSTACK=1500 # # Boot options @@ -451,8 +451,8 @@ CONFIG_PREALLOC_TIMERS=50 # # Stack and heap information # -CONFIG_IDLETHREAD_STACKSIZE=3500 -CONFIG_USERMAIN_STACKSIZE=2600 +CONFIG_IDLETHREAD_STACKSIZE=1000 +CONFIG_USERMAIN_STACKSIZE=3000 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=2048 From 1d90e86ec463bdcfed49ef09dd27206e1451b8c4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Mar 2015 01:08:03 +0100 Subject: [PATCH 110/112] Commander and MAVLink: Adjust stack sizes as required --- src/modules/commander/commander.cpp | 2 +- src/modules/mavlink/module.mk | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 74ebe0ae43..f832f761ef 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -966,7 +966,7 @@ int commander_thread_main(int argc, char *argv[]) pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, 2600); + pthread_attr_setstacksize(&commander_low_prio_attr, 2400); struct sched_param param; (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index f9d30dcbea..e82b8bd935 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -51,7 +51,7 @@ INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink MAXOPTIMIZATION = -Os -MODULE_STACKSIZE = 1024 +MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ -Wno-attributes -Wno-packed From 0261f5dfbee1ee498c2b40acb6a2651664a367c1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Mar 2015 01:27:35 +0100 Subject: [PATCH 111/112] MAVLink: Reduce stack usage --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0132a3193d..f8e819ce7a 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1621,7 +1621,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2700, + 2400, (main_t)&Mavlink::start_helper, (char * const *)argv); From dfd6be78bf4165d388bd073b28a54f6b7fb3300d Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 3 Mar 2015 09:22:01 -0800 Subject: [PATCH 112/112] Add RC_CHAN_CNT, RC_TH_USER These are used by ground station software --- src/modules/sensors/sensor_params.c | 30 +++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 5ec6780331..272e4b14f3 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -993,6 +993,36 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f); PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124); /* scaling for 3DR power brick */ +/** + * RC channel count + * + * This parameter is used by Ground Station software to save the number + * of channels which were used during RC calibration. It is only meant + * for ground station use. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ + +PARAM_DEFINE_INT32(RC_CHAN_CNT, 0); + +/** + * RC mode switch threshold automaic distribution + * + * This parameter is used by Ground Station software to specify whether + * the threshold values for flight mode switches were automatically calculated. + * 0 indicates that the threshold values were set by the user. Any other value + * indicates that the threshold value where automatically set by the ground + * station software. It is only meant for ground station use. + * + * @min 0 + * @max 1 + * @group Radio Calibration + */ + +PARAM_DEFINE_INT32(RC_TH_USER, 1); + /** * Roll control channel mapping. *