From bbd5f33a22f6371a0dd6c0c6ae4a585101e81398 Mon Sep 17 00:00:00 2001 From: Sebastian Verling Date: Mon, 23 Feb 2015 18:38:55 +0100 Subject: [PATCH 001/151] 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/151] 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/151] 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 653b3e71be7059707a0963e15dfb12e5403025aa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Feb 2015 01:17:47 +0100 Subject: [PATCH 004/151] PWM command: Do not modify disarmed PWM values --- src/systemcmds/pwm/pwm.c | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 0c984d69f8..f8da686cd0 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -337,13 +337,19 @@ pwm_main(int argc, char *argv[]) if (pwm_value == 0) warnx("reading disarmed value of zero, disabling disarmed PWM"); - struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; + struct pwm_output_values pwm_values; + /* first get current state before modifying it */ + ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (long unsigned int)&pwm_values.values); + if (ret != OK) { + errx(ret, "failed get disarmed values"); + } for (unsigned i = 0; i < servo_count; i++) { if (set_mask & 1< Date: Sat, 21 Feb 2015 11:08:08 +0100 Subject: [PATCH 005/151] PWM command: Fix min/max/disarmed/failsafe commands to also read the current settings first before modifying them. --- src/systemcmds/pwm/pwm.c | 35 +++++++++++++++++++++++++++-------- 1 file changed, 27 insertions(+), 8 deletions(-) diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index f8da686cd0..620b0a6f67 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -279,7 +279,14 @@ pwm_main(int argc, char *argv[]) if (pwm_value == 0) usage("no PWM value provided"); - struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + pwm_values.channel_count = servo_count; + /* first get current state before modifying it */ + ret = ioctl(fd, PWM_SERVO_GET_MIN_PWM, (long unsigned int)&pwm_values); + if (ret != OK) { + errx(ret, "failed get min values"); + } for (unsigned i = 0; i < servo_count; i++) { if (set_mask & 1< Date: Fri, 20 Feb 2015 11:53:42 +1100 Subject: [PATCH 006/151] drivers: added header for pwm_input --- src/drivers/drv_pwm_input.h | 51 +++++++++++++++++++++++++++++++++++++ 1 file changed, 51 insertions(+) create mode 100644 src/drivers/drv_pwm_input.h diff --git a/src/drivers/drv_pwm_input.h b/src/drivers/drv_pwm_input.h new file mode 100644 index 0000000000..778d9fcf5b --- /dev/null +++ b/src/drivers/drv_pwm_input.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +#include +#include + +#define PWMIN0_DEVICE_PATH "/dev/pwmin0" + +__BEGIN_DECLS + +/* + * Initialise the timer + */ +__EXPORT extern int pwm_input_main(int argc, char * argv[]); + +__END_DECLS From 086caddf73555700724d98e5ac17b34e1cc6726e Mon Sep 17 00:00:00 2001 From: Evan Slatyer Date: Fri, 20 Feb 2015 12:11:07 +1100 Subject: [PATCH 007/151] pwm_input: added uORB message structure --- msg/pwm_input.msg | 5 +++++ 1 file changed, 5 insertions(+) create mode 100644 msg/pwm_input.msg diff --git a/msg/pwm_input.msg b/msg/pwm_input.msg new file mode 100644 index 0000000000..beb82021d2 --- /dev/null +++ b/msg/pwm_input.msg @@ -0,0 +1,5 @@ + +uint64 timestamp # Microseconds since system boot +uint64 error_count +uint32 pulse_width # Pulse width, timer counts +uint32 period # Period, timer counts From 322392d85331a69b96a0ce6cd3db19bfbe02f592 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 21 Feb 2015 18:15:08 +1100 Subject: [PATCH 008/151] fmuv2: setup for PWM input on timer4, channel 2 --- src/drivers/boards/px4fmu-v2/board_config.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 2fd8bc724a..273af1023a 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -207,6 +207,11 @@ __BEGIN_DECLS #define HRT_TIMER 8 /* use timer8 for the HRT */ #define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ +/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ +#define PWMIN_TIMER 4 +#define PWMIN_TIMER_CHANNEL 2 +#define GPIO_PWM_IN GPIO_TIM4_CH2IN_2 + /**************************************************************************************************** * Public Types ****************************************************************************************************/ From 41cc04c06485f40c2219b8585d50138cd2e5fe37 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 21 Feb 2015 18:15:57 +1100 Subject: [PATCH 009/151] pwm_input: added PWM input driver this allows for input of PWM signals on the FMUv2 aux5 pins --- src/drivers/pwm_input/module.mk | 41 ++ src/drivers/pwm_input/pwm_input.cpp | 603 ++++++++++++++++++++++++++++ 2 files changed, 644 insertions(+) create mode 100644 src/drivers/pwm_input/module.mk create mode 100644 src/drivers/pwm_input/pwm_input.cpp diff --git a/src/drivers/pwm_input/module.mk b/src/drivers/pwm_input/module.mk new file mode 100644 index 0000000000..04f04d6cbc --- /dev/null +++ b/src/drivers/pwm_input/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 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. +# +############################################################################ + +# +# Makefile to build the PWM input driver. +# + +MODULE_COMMAND = pwm_input + +SRCS = pwm_input.cpp + diff --git a/src/drivers/pwm_input/pwm_input.cpp b/src/drivers/pwm_input/pwm_input.cpp new file mode 100644 index 0000000000..2d771266c0 --- /dev/null +++ b/src/drivers/pwm_input/pwm_input.cpp @@ -0,0 +1,603 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Andrew Tridgell. 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 pwm_input.cpp + * + * PWM input driver based on earlier driver from Evan Slatyer, + * which in turn was based on drv_hrt.c + */ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "chip.h" +#include "up_internal.h" +#include "up_arch.h" + +#include "stm32.h" +#include "stm32_gpio.h" +#include "stm32_tim.h" +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#if HRT_TIMER == PWMIN_TIMER +#error cannot share timer between HRT and PWMIN +#endif + +#if !defined(GPIO_PWM_IN) || !defined(PWMIN_TIMER) || !defined(PWMIN_TIMER_CHANNEL) +#error PWMIN defines are needed in board_config.h for this board +#endif + +/* PWMIN configuration */ +#if PWMIN_TIMER == 1 +# define PWMIN_TIMER_BASE STM32_TIM1_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB2ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM1EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM1CC +# define PWMIN_TIMER_CLOCK STM32_APB2_TIM1_CLKIN +#elif PWMIN_TIMER == 2 +# define PWMIN_TIMER_BASE STM32_TIM2_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM2EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM2 +# define PWMIN_TIMER_CLOCK STM32_APB1_TIM2_CLKIN +#elif PWMIN_TIMER == 3 +# define PWMIN_TIMER_BASE STM32_TIM3_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB1ENR_TIM3EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM3 +# define PWMIN_TIMER_CLOCK STM32_APB1_TIM3_CLKIN +#elif PWMIN_TIMER == 4 +# define PWMIN_TIMER_BASE STM32_TIM4_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB1ENR_TIM4EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM4 +# define PWMIN_TIMER_CLOCK STM32_APB1_TIM4_CLKIN +#elif PWMIN_TIMER == 5 +# define PWMIN_TIMER_BASE STM32_TIM5_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM5EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM5 +# define PWMIN_TIMER_CLOCK STM32_APB1_TIM5_CLKIN +#elif PWMIN_TIMER == 8 +# define PWMIN_TIMER_BASE STM32_TIM8_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB2ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM8EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM8CC +# define PWMIN_TIMER_CLOCK STM32_APB2_TIM8_CLKIN +#elif PWMIN_TIMER == 9 +# define PWMIN_TIMER_BASE STM32_TIM9_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM9EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM1BRK +# define PWMIN_TIMER_CLOCK STM32_APB1_TIM9_CLKIN +#elif PWMIN_TIMER == 10 +# define PWMIN_TIMER_BASE STM32_TIM10_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM10EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM1UP +# define PWMIN_TIMER_CLOCK STM32_APB2_TIM10_CLKIN +#elif PWMIN_TIMER == 11 +# define PWMIN_TIMER_BASE STM32_TIM11_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM11EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM1TRGCOM +# define PWMIN_TIMER_CLOCK STM32_APB2_TIM11_CLKIN +#elif PWMIN_TIMER == 12 +# define PWMIN_TIMER_BASE STM32_TIM12_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM12EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM1TRGCOM +# define PWMIN_TIMER_CLOCK STM32_APB2_TIM12_CLKIN +#else +# error PWMIN_TIMER must be a value between 1 and 12 +#endif + +/* + * HRT clock must be at least 1MHz + */ +#if PWMIN_TIMER_CLOCK <= 1000000 +# error PWMIN_TIMER_CLOCK must be greater than 1MHz +#endif + +/* + * Timer register accessors + */ +#define REG(_reg) (*(volatile uint32_t *)(PWMIN_TIMER_BASE + _reg)) + +#define rCR1 REG(STM32_GTIM_CR1_OFFSET) +#define rCR2 REG(STM32_GTIM_CR2_OFFSET) +#define rSMCR REG(STM32_GTIM_SMCR_OFFSET) +#define rDIER REG(STM32_GTIM_DIER_OFFSET) +#define rSR REG(STM32_GTIM_SR_OFFSET) +#define rEGR REG(STM32_GTIM_EGR_OFFSET) +#define rCCMR1 REG(STM32_GTIM_CCMR1_OFFSET) +#define rCCMR2 REG(STM32_GTIM_CCMR2_OFFSET) +#define rCCER REG(STM32_GTIM_CCER_OFFSET) +#define rCNT REG(STM32_GTIM_CNT_OFFSET) +#define rPSC REG(STM32_GTIM_PSC_OFFSET) +#define rARR REG(STM32_GTIM_ARR_OFFSET) +#define rCCR1 REG(STM32_GTIM_CCR1_OFFSET) +#define rCCR2 REG(STM32_GTIM_CCR2_OFFSET) +#define rCCR3 REG(STM32_GTIM_CCR3_OFFSET) +#define rCCR4 REG(STM32_GTIM_CCR4_OFFSET) +#define rDCR REG(STM32_GTIM_DCR_OFFSET) +#define rDMAR REG(STM32_GTIM_DMAR_OFFSET) + +/* + * Specific registers and bits used by HRT sub-functions + */ +#if PWMIN_TIMER_CHANNEL == 1 + #define rCCR_PWMIN_A rCCR1 /* compare register for PWMIN */ + #define DIER_PWMIN_A (GTIM_DIER_CC1IE) /* interrupt enable for PWMIN */ + #define SR_INT_PWMIN_A GTIM_SR_CC1IF /* interrupt status for PWMIN */ + #define rCCR_PWMIN_B rCCR2 /* compare register for PWMIN */ + #define SR_INT_PWMIN_B GTIM_SR_CC2IF /* interrupt status for PWMIN */ + #define CCMR1_PWMIN ((0x02 << GTIM_CCMR1_CC2S_SHIFT) | (0x01 << GTIM_CCMR1_CC1S_SHIFT)) + #define CCMR2_PWMIN 0 + #define CCER_PWMIN (GTIM_CCER_CC2P | GTIM_CCER_CC1E | GTIM_CCER_CC2E) + #define SR_OVF_PWMIN (GTIM_SR_CC1OF | GTIM_SR_CC2OF) + #define SMCR_PWMIN_1 (0x05 << GTIM_SMCR_TS_SHIFT) + #define SMCR_PWMIN_2 ((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1) +#elif PWMIN_TIMER_CHANNEL == 2 + #define rCCR_PWMIN_A rCCR2 /* compare register for PWMIN */ + #define DIER_PWMIN_A (GTIM_DIER_CC2IE) /* interrupt enable for PWMIN */ + #define SR_INT_PWMIN_A GTIM_SR_CC2IF /* interrupt status for PWMIN */ + #define rCCR_PWMIN_B rCCR1 /* compare register for PWMIN */ + #define DIER_PWMIN_B GTIM_DIER_CC1IE /* interrupt enable for PWMIN */ + #define SR_INT_PWMIN_B GTIM_SR_CC1IF /* interrupt status for PWMIN */ + #define CCMR1_PWMIN ((0x01 << GTIM_CCMR1_CC2S_SHIFT) | (0x02 << GTIM_CCMR1_CC1S_SHIFT)) + #define CCMR2_PWMIN 0 + #define CCER_PWMIN (GTIM_CCER_CC1P | GTIM_CCER_CC1E | GTIM_CCER_CC2E) + #define SR_OVF_PWMIN (GTIM_SR_CC1OF | GTIM_SR_CC2OF) + #define SMCR_PWMIN_1 (0x06 << GTIM_SMCR_TS_SHIFT) + #define SMCR_PWMIN_2 ((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1) +#else + #error PWMIN_TIMER_CHANNEL must be either 1 and 2. +#endif + + +class PWMIN : device::CDev +{ +public: + PWMIN(); + virtual ~PWMIN(); + + virtual int init(); + virtual int open(struct file *filp); + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + void _publish(uint16_t status, uint32_t period, uint32_t pulse_width); + void _print_info(void); + +private: + uint32_t error_count; + uint32_t pulses_captured; + uint32_t last_period; + uint32_t last_width; + RingBuffer *reports; + bool timer_started; + + void timer_init(void); +}; + +static int pwmin_tim_isr(int irq, void *context); +static void pwmin_start(void); +static void pwmin_info(void); +static void pwmin_test(void); +static void pwmin_reset(void); + +static PWMIN *g_dev; + +PWMIN::PWMIN() : + CDev("pwmin", PWMIN0_DEVICE_PATH), + error_count(0), + pulses_captured(0), + last_period(0), + last_width(0), + reports(nullptr), + timer_started(false) +{ +} + +PWMIN::~PWMIN() +{ + if (reports != nullptr) + delete reports; +} + +/* + initialise the driver. This doesn't actually start the timer (that + is done on open). We don't start the timer to allow for this driver + to be started in init scripts when the user may be using the input + pin as PWM output + */ +int +PWMIN::init() +{ + // we just register the device in /dev, and only actually + // activate the timer when requested to when the device is opened + CDev::init(); + + reports = new RingBuffer(2, sizeof(struct pwm_input_s)); + if (reports == nullptr) { + return -ENOMEM; + } + + return OK; +} + +/* + * Initialise the timer we are going to use. + */ +void PWMIN::timer_init(void) +{ + // run with interrupts disabled in case the timer is already + // setup. We don't want it firing while we are doing the setup + irqstate_t flags = irqsave(); + stm32_configgpio(GPIO_PWM_IN); + + /* claim our interrupt vector */ + irq_attach(PWMIN_TIMER_VECTOR, pwmin_tim_isr); + + /* Clear no bits, set timer enable bit.*/ + modifyreg32(PWMIN_TIMER_POWER_REG, 0, PWMIN_TIMER_POWER_BIT); + + /* disable and configure the timer */ + rCR1 = 0; + rCR2 = 0; + rSMCR = 0; + rDIER = DIER_PWMIN_A; + rCCER = 0; /* unlock CCMR* registers */ + rCCMR1 = CCMR1_PWMIN; + rCCMR2 = CCMR2_PWMIN; + rSMCR = SMCR_PWMIN_1; /* Set up mode */ + rSMCR = SMCR_PWMIN_2; /* Enable slave mode controller */ + rCCER = CCER_PWMIN; + rDCR = 0; + + // for simplicity scale by the clock in MHz. This gives us + // readings in microseconds which is typically what is needed + // for a PWM input driver + uint32_t prescaler = PWMIN_TIMER_CLOCK/1000000UL; + + /* + * define the clock speed. We want the highest possible clock + * speed that avoids overflows. + */ + rPSC = prescaler - 1; + + /* run the full span of the counter. All timers can handle + * uint16 */ + rARR = UINT16_MAX; + + /* generate an update event; reloads the counter, all registers */ + rEGR = GTIM_EGR_UG; + + /* enable the timer */ + rCR1 = GTIM_CR1_CEN; + + /* enable interrupts */ + up_enable_irq(PWMIN_TIMER_VECTOR); + + irqrestore(flags); + + timer_started = true; +} + +/* + hook for open of the driver. We start the timer at this point, then + leave it running + */ +int +PWMIN::open(struct file *filp) +{ + if (g_dev == nullptr) { + return -EIO; + } + int ret = CDev::open(filp); + if (ret == OK && !timer_started) { + g_dev->timer_init(); + } + return ret; +} + + +/* + handle ioctl requests + */ +int +PWMIN::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 500)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + irqrestore(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return reports->size(); + + case SENSORIOCRESET: + /* user has asked for the timer to be reset. This may + be needed if the pin was used for a different + purpose (such as PWM output) + */ + timer_init(); + return OK; + + default: + /* give it to the superclass */ + return CDev::ioctl(filp, cmd, arg); + } +} + + +/* + read some samples from the device + */ +ssize_t +PWMIN::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct pwm_input_s); + struct pwm_input_s *buf = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + while (count--) { + if (reports->get(buf)) { + ret += sizeof(struct pwm_input_s); + buf++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; +} + +/* + publish some data from the ISR in the ring buffer + */ +void PWMIN::_publish(uint16_t status, uint32_t period, uint32_t pulse_width) +{ + /* if we missed an edge, we have to give up */ + if (status & SR_OVF_PWMIN) { + error_count++; + return; + } + + struct pwm_input_s pwmin_report; + pwmin_report.timestamp = hrt_absolute_time(); + pwmin_report.error_count = error_count; + pwmin_report.period = period; + pwmin_report.pulse_width = pulse_width; + + reports->force(&pwmin_report); + + last_period = period; + last_width = pulse_width; + pulses_captured++; +} + +/* + print information on the last captured + */ +void PWMIN::_print_info(void) +{ + if (!timer_started) { + printf("timer not started - try the 'test' command\n"); + } else { + printf("count=%u period=%u width=%u\n", + (unsigned)pulses_captured, + (unsigned)last_period, + (unsigned)last_width); + } +} + + +/* + Handle the interupt, gathering pulse data + */ +static int pwmin_tim_isr(int irq, void *context) +{ + uint16_t status = rSR; + uint32_t period = rCCR_PWMIN_A; + uint32_t pulse_width = rCCR_PWMIN_B; + + /* ack the interrupts we just read */ + rSR = 0; + + if (g_dev != nullptr) { + g_dev->_publish(status, period, pulse_width); + } + + return OK; +} + +/* + start the driver + */ +static void pwmin_start(void) +{ + if (g_dev != nullptr) { + printf("driver already started\n"); + exit(1); + } + g_dev = new PWMIN(); + if (g_dev == nullptr) { + errx(1, "driver allocation failed"); + } + if (g_dev->init() != OK) { + errx(1, "driver init failed"); + } + exit(0); +} + +/* + test the driver + */ +static void pwmin_test(void) +{ + int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY); + if (fd == -1) { + errx(1, "Failed to open device"); + } + uint64_t start_time = hrt_absolute_time(); + + printf("Showing samples for 5 seconds\n"); + + while (hrt_absolute_time() < start_time+5U*1000UL*1000UL) { + struct pwm_input_s buf; + if (::read(fd, &buf, sizeof(buf)) == sizeof(buf)) { + printf("period=%u width=%u error_count=%u\n", + (unsigned)buf.period, + (unsigned)buf.pulse_width, + (unsigned)buf.error_count); + } + } + close(fd); + exit(0); +} + +/* + reset the timer + */ +static void pwmin_reset(void) +{ + int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY); + if (fd == -1) { + errx(1, "Failed to open device"); + } + if (ioctl(fd, SENSORIOCRESET, 0) != OK) { + errx(1, "reset failed"); + } + close(fd); + exit(0); +} + +/* + show some information on the driver + */ +static void pwmin_info(void) +{ + if (g_dev == nullptr) { + printf("driver not started\n"); + exit(1); + } + g_dev->_print_info(); + exit(0); +} + + +/* + driver entry point + */ +int pwm_input_main(int argc, char * argv[]) +{ + const char *verb = argv[1]; + + /* + * Start/load the driver. + */ + if (!strcmp(verb, "start")) { + pwmin_start(); + } + + /* + * Print driver information. + */ + if (!strcmp(verb, "info")) { + pwmin_info(); + } + + /* + * print test results + */ + if (!strcmp(verb, "test")) { + pwmin_test(); + } + + /* + * reset the timer + */ + if (!strcmp(verb, "reset")) { + pwmin_reset(); + } + + errx(1, "unrecognized command, try 'start', 'info', 'reset' or 'test'"); + return 0; +} From 9a875c53af750e115529b1bb64269d2ded712c46 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 23 Feb 2015 13:22:30 +1100 Subject: [PATCH 010/151] px4fmu: added "mode_pwm4" startup mode this is the default mode ArduPilot uses, and by setting it in the init script we avoid any pin activity on the two GPIO pins during boot --- src/drivers/px4fmu/fmu.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 3a9246bf29..8e7e93679b 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1529,6 +1529,7 @@ enum PortMode { PORT_GPIO_AND_SERIAL, PORT_PWM_AND_SERIAL, PORT_PWM_AND_GPIO, + PORT_PWM4, }; PortMode g_port_mode; @@ -1564,6 +1565,13 @@ fmu_new_mode(PortMode new_mode) #endif break; +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + case PORT_PWM4: + /* select 4-pin PWM mode */ + servo_mode = PX4FMU::MODE_4PWM; + break; +#endif + /* mixed modes supported on v1 board only */ #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) @@ -1836,6 +1844,10 @@ fmu_main(int argc, char *argv[]) } else if (!strcmp(verb, "mode_pwm")) { new_mode = PORT_FULL_PWM; +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + } else if (!strcmp(verb, "mode_pwm4")) { + new_mode = PORT_PWM4; +#endif #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) } else if (!strcmp(verb, "mode_serial")) { From 41377709e630943593bca810295df9f488602831 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 23 Feb 2015 22:15:47 +0100 Subject: [PATCH 011/151] added test for quaternion methods --- src/systemcmds/tests/test_mathlib.cpp | 67 +++++++++++++++++++++++++++ 1 file changed, 67 insertions(+) diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index 9fa52aaaa0..f56667d673 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -205,5 +205,72 @@ int test_mathlib(int argc, char *argv[]) } + { + // test conversion rotation matrix to quaternion and back + math::Matrix<3,3> R_orig; + math::Matrix<3,3> R; + math::Quaternion q; + float diff = 0.1f; + float tol = 0.00001f; + + for(float roll = -M_PI_F;roll <= M_PI_F;roll+=diff) { + for(float pitch = -M_PI_2_F;pitch <= M_PI_2_F;pitch+=diff) { + for(float yaw = -M_PI_F;yaw <= M_PI_F;yaw+=diff) { + R_orig.from_euler(roll,pitch,yaw); + q.from_dcm(R_orig); + R = q.to_dcm(); + for(int i = 0;i< 3;i++) { + for(int j=0;j<3;j++) { + if(fabsf(R_orig.data[i][j] - R.data[i][j]) > 0.00001f) { + warnx("Quaternion method 'from_dcm' or 'to_dcm' outside tolerance!"); + rc = 1; + } + } + } + } + } + } + + // test against some known values + tol = 0.0001f; + math::Quaternion q_true = {1.0f,0.0f,0.0f,0.0f}; + R_orig.identity(); + q.from_dcm(R_orig); + for(unsigned i = 0;i<4;i++) { + if(fabsf(q.data[i] - q_true.data[i]) > tol) { + warnx("Quaternion method 'from_dcm()' outside tolerance!"); + rc = 1; + } + } + + q_true.from_euler(0.3f,0.2f,0.1f); + q = {0.9833f,0.1436f,0.1060f,0.0343f}; + for(unsigned i = 0;i<4;i++) { + if(fabsf(q.data[i] - q_true.data[i]) > tol) { + warnx("Quaternion method 'from_euler()' outside tolerance!"); + rc = 1; + } + } + + q_true.from_euler(-1.5f,-0.2f,0.5f); + q = {0.7222f,-0.6391f,-0.2386f,0.1142f}; + for(unsigned i = 0;i<4;i++) { + if(fabsf(q.data[i] - q_true.data[i]) > tol) { + warnx("Quaternion method 'from_euler()' outside tolerance!"); + rc = 1; + } + } + + q_true.from_euler(M_PI_2_F,-M_PI_2_F,-M_PI_F/3); + q = {0.6830f,0.1830f,-0.6830f,0.1830f}; + for(unsigned i = 0;i<4;i++) { + if(fabsf(q.data[i] - q_true.data[i]) > tol) { + warnx("Quaternion method 'from_euler()' outside tolerance!"); + rc = 1; + } + } + + } + return rc; } From a252f4a99111597babd447c962208287e3f244ba Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 23 Feb 2015 22:26:41 +0100 Subject: [PATCH 012/151] fixed quaternion method from_dcm() --- src/lib/mathlib/math/Quaternion.hpp | 34 ++++++++++++++++++++++++----- 1 file changed, 28 insertions(+), 6 deletions(-) diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index bf6e3365dd..6ea4cf557a 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -135,12 +135,34 @@ public: data[3] = static_cast(cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2); } - void from_dcm(const Matrix<3, 3> &m) { - // avoiding singularities by not using division equations - data[0] = 0.5f * sqrtf(1.0f + m.data[0][0] + m.data[1][1] + m.data[2][2]); - data[1] = 0.5f * sqrtf(1.0f + m.data[0][0] - m.data[1][1] - m.data[2][2]); - data[2] = 0.5f * sqrtf(1.0f - m.data[0][0] + m.data[1][1] - m.data[2][2]); - data[3] = 0.5f * sqrtf(1.0f - m.data[0][0] - m.data[1][1] + m.data[2][2]); + void from_dcm(const Matrix<3, 3> &dcm) { + float tr = dcm.data[0][0] + dcm.data[1][1] + dcm.data[2][2]; + if (tr > 0.0f) { + float s = sqrtf(tr + 1.0f); + data[0] = s * 0.5f; + s = 0.5f / s; + data[1] = (dcm.data[2][1] - dcm.data[1][2]) * s; + data[2] = (dcm.data[0][2] - dcm.data[2][0]) * s; + data[3] = (dcm.data[1][0] - dcm.data[0][1]) * s; + } else { + /* Find maximum diagonal element in dcm + * store index in dcm_i */ + int dcm_i = 0; + for (int i = 1; i < 3; i++) { + if (dcm.data[i][i] > dcm.data[dcm_i][dcm_i]) { + dcm_i = i; + } + } + int dcm_j = (dcm_i + 1) % 3; + int dcm_k = (dcm_i + 2) % 3; + float s = sqrtf((dcm.data[dcm_i][dcm_i] - dcm.data[dcm_j][dcm_j] - + dcm.data[dcm_k][dcm_k]) + 1.0f); + data[dcm_i + 1] = s * 0.5f; + s = 0.5f / s; + data[dcm_j + 1] = (dcm.data[dcm_i][dcm_j] + dcm.data[dcm_j][dcm_i]) * s; + data[dcm_k + 1] = (dcm.data[dcm_k][dcm_i] + dcm.data[dcm_i][dcm_k]) * s; + data[0] = (dcm.data[dcm_k][dcm_j] - dcm.data[dcm_j][dcm_k]) * s; + } } /** From 71a5351deb4662e28f5b684d61aede930ba524e6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Feb 2015 23:24:38 +0100 Subject: [PATCH 013/151] mathlib test: Code style fixes --- src/systemcmds/tests/test_mathlib.cpp | 65 +++++++++++++++------------ 1 file changed, 36 insertions(+), 29 deletions(-) diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index f56667d673..d78be48263 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -160,10 +160,10 @@ int test_mathlib(int argc, char *argv[]) { // test nonsymmetric +, -, +=, -= - float data1[2][3] = {{1,2,3},{4,5,6}}; - float data2[2][3] = {{2,4,6},{8,10,12}}; - float data3[2][3] = {{3,6,9},{12,15,18}}; - + float data1[2][3] = {{1, 2, 3}, {4, 5, 6}}; + float data2[2][3] = {{2, 4, 6}, {8, 10, 12}}; + float data3[2][3] = {{3, 6, 9}, {12, 15, 18}}; + Matrix<2, 3> m1(data1); Matrix<2, 3> m2(data2); Matrix<2, 3> m3(data3); @@ -185,6 +185,7 @@ int test_mathlib(int argc, char *argv[]) } m1 += m2; + if (m1 != m3) { warnx("Matrix<2, 3> += Matrix<2, 3> failed!"); m1.print(); @@ -195,6 +196,7 @@ int test_mathlib(int argc, char *argv[]) m1 -= m2; Matrix<2, 3> m1_orig(data1); + if (m1 != m1_orig) { warnx("Matrix<2, 3> -= Matrix<2, 3> failed!"); m1.print(); @@ -202,26 +204,27 @@ int test_mathlib(int argc, char *argv[]) m1_orig.print(); rc = 1; } - + } { // test conversion rotation matrix to quaternion and back - math::Matrix<3,3> R_orig; - math::Matrix<3,3> R; + math::Matrix<3, 3> R_orig; + math::Matrix<3, 3> R; math::Quaternion q; float diff = 0.1f; float tol = 0.00001f; - for(float roll = -M_PI_F;roll <= M_PI_F;roll+=diff) { - for(float pitch = -M_PI_2_F;pitch <= M_PI_2_F;pitch+=diff) { - for(float yaw = -M_PI_F;yaw <= M_PI_F;yaw+=diff) { - R_orig.from_euler(roll,pitch,yaw); + for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) { + for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) { + for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) { + R_orig.from_euler(roll, pitch, yaw); q.from_dcm(R_orig); R = q.to_dcm(); - for(int i = 0;i< 3;i++) { - for(int j=0;j<3;j++) { - if(fabsf(R_orig.data[i][j] - R.data[i][j]) > 0.00001f) { + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + if (fabsf(R_orig.data[i][j] - R.data[i][j]) > 0.00001f) { warnx("Quaternion method 'from_dcm' or 'to_dcm' outside tolerance!"); rc = 1; } @@ -233,38 +236,42 @@ int test_mathlib(int argc, char *argv[]) // test against some known values tol = 0.0001f; - math::Quaternion q_true = {1.0f,0.0f,0.0f,0.0f}; + math::Quaternion q_true = {1.0f, 0.0f, 0.0f, 0.0f}; R_orig.identity(); q.from_dcm(R_orig); - for(unsigned i = 0;i<4;i++) { - if(fabsf(q.data[i] - q_true.data[i]) > tol) { + + for (unsigned i = 0; i < 4; i++) { + if (fabsf(q.data[i] - q_true.data[i]) > tol) { warnx("Quaternion method 'from_dcm()' outside tolerance!"); rc = 1; } } - q_true.from_euler(0.3f,0.2f,0.1f); - q = {0.9833f,0.1436f,0.1060f,0.0343f}; - for(unsigned i = 0;i<4;i++) { - if(fabsf(q.data[i] - q_true.data[i]) > tol) { + q_true.from_euler(0.3f, 0.2f, 0.1f); + q = {0.9833f, 0.1436f, 0.1060f, 0.0343f}; + + for (unsigned i = 0; i < 4; i++) { + if (fabsf(q.data[i] - q_true.data[i]) > tol) { warnx("Quaternion method 'from_euler()' outside tolerance!"); rc = 1; } } - q_true.from_euler(-1.5f,-0.2f,0.5f); - q = {0.7222f,-0.6391f,-0.2386f,0.1142f}; - for(unsigned i = 0;i<4;i++) { - if(fabsf(q.data[i] - q_true.data[i]) > tol) { + q_true.from_euler(-1.5f, -0.2f, 0.5f); + q = {0.7222f, -0.6391f, -0.2386f, 0.1142f}; + + for (unsigned i = 0; i < 4; i++) { + if (fabsf(q.data[i] - q_true.data[i]) > tol) { warnx("Quaternion method 'from_euler()' outside tolerance!"); rc = 1; } } - q_true.from_euler(M_PI_2_F,-M_PI_2_F,-M_PI_F/3); - q = {0.6830f,0.1830f,-0.6830f,0.1830f}; - for(unsigned i = 0;i<4;i++) { - if(fabsf(q.data[i] - q_true.data[i]) > tol) { + q_true.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3); + q = {0.6830f, 0.1830f, -0.6830f, 0.1830f}; + + for (unsigned i = 0; i < 4; i++) { + if (fabsf(q.data[i] - q_true.data[i]) > tol) { warnx("Quaternion method 'from_euler()' outside tolerance!"); rc = 1; } From 9d8651c10c7cd431e78757dd465ba30f723d16e2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Feb 2015 23:27:51 +0100 Subject: [PATCH 014/151] mathlib test: Print all test types which are actually performed --- src/systemcmds/tests/test_mathlib.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index d78be48263..0519a55a97 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 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 @@ -158,6 +158,7 @@ int test_mathlib(int argc, char *argv[]) } { + warnx("Nonsymmetric matrix operations test") // test nonsymmetric +, -, +=, -= float data1[2][3] = {{1, 2, 3}, {4, 5, 6}}; @@ -215,6 +216,8 @@ int test_mathlib(int argc, char *argv[]) float diff = 0.1f; float tol = 0.00001f; + warnx("Quaternion transformation methods test."); + for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) { for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) { for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) { From 6e07c8c9e4564f3f9677ddee0a023d0003913212 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Feb 2015 23:56:22 +0100 Subject: [PATCH 015/151] mathlib tests: Fix typo --- src/systemcmds/tests/test_mathlib.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index 0519a55a97..682514cb79 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -158,7 +158,7 @@ int test_mathlib(int argc, char *argv[]) } { - warnx("Nonsymmetric matrix operations test") + warnx("Nonsymmetric matrix operations test"); // test nonsymmetric +, -, +=, -= float data1[2][3] = {{1, 2, 3}, {4, 5, 6}}; From 4f385123340ce88fed4d441f14986d4769fa97cd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 22 Feb 2015 12:21:29 +0100 Subject: [PATCH 016/151] port #1703 to multiplatform --- .../mc_pos_control_multiplatform/mc_pos_control.cpp | 12 +++++++++++- .../mc_pos_control_multiplatform/mc_pos_control.h | 1 + 2 files changed, 12 insertions(+), 1 deletion(-) 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 391558dcce..e6a7ee8a64 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -106,7 +106,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _manual_control_sp = _n.subscribe(0); _armed = _n.subscribe(0); _local_pos = _n.subscribe(0); - _pos_sp_triplet = _n.subscribe(0); + _pos_sp_triplet = _n.subscribe(&MulticopterPositionControl::handle_position_setpoint_triplet, this, 0); _local_pos_sp = _n.subscribe(0); _global_vel_sp = _n.subscribe(0); @@ -554,6 +554,16 @@ void MulticopterPositionControl::handle_parameter_update(const px4_parameter_upd parameters_update(); } +void MulticopterPositionControl::handle_position_setpoint_triplet(const px4_position_setpoint_triplet &msg) +{ + /* Make sure that the position setpoint is valid */ + if (!isfinite(_pos_sp_triplet->data().current.lat) || + !isfinite(_pos_sp_triplet->data().current.lon) || + !isfinite(_pos_sp_triplet->data().current.alt)) { + _pos_sp_triplet->data().current.valid = false; + } +} + void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_attitude &msg) { static bool reset_int_z = true; diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h index 245b5f5a91..b754ccf01e 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h @@ -75,6 +75,7 @@ public: /* Callbacks for topics */ void handle_vehicle_attitude(const px4_vehicle_attitude &msg); void handle_parameter_update(const px4_parameter_update &msg); + void handle_position_setpoint_triplet(const px4_position_setpoint_triplet &msg); void spin() { _n.spin(); } From 6c3f57f87e1dc3e7fd36c049599e1fdcd51f0a26 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 22 Feb 2015 12:32:51 +0100 Subject: [PATCH 017/151] port #1752 to multiplatform --- .../mc_pos_control.cpp | 40 +++++++++++-------- 1 file changed, 24 insertions(+), 16 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 e6a7ee8a64..1fe75b90c9 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -620,22 +620,6 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti control_auto(dt); } - /* fill local position setpoint */ - _local_pos_sp_msg.data().timestamp = get_time_micros(); - _local_pos_sp_msg.data().x = _pos_sp(0); - _local_pos_sp_msg.data().y = _pos_sp(1); - _local_pos_sp_msg.data().z = _pos_sp(2); - _local_pos_sp_msg.data().yaw = _att_sp_msg.data().yaw_body; - - /* publish local position setpoint */ - if (_local_pos_sp_pub != nullptr) { - _local_pos_sp_pub->publish(_local_pos_sp_msg); - - } else { - _local_pos_sp_pub = _n.advertise(); - } - - if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid && _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ _R.identity(); @@ -935,6 +919,11 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti _att_sp_msg.data().thrust = thrust_abs; + /* save thrust setpoint for logging */ + _local_pos_sp_msg.data().acc_x = thrust_sp(0); + _local_pos_sp_msg.data().acc_x = thrust_sp(1); + _local_pos_sp_msg.data().acc_x = thrust_sp(2); + _att_sp_msg.data().timestamp = get_time_micros(); /* publish attitude setpoint */ @@ -950,6 +939,25 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti } } + /* fill local position setpoint */ + _local_pos_sp_msg.data().timestamp = get_time_micros(); + _local_pos_sp_msg.data().x = _pos_sp(0); + _local_pos_sp_msg.data().y = _pos_sp(1); + _local_pos_sp_msg.data().z = _pos_sp(2); + _local_pos_sp_msg.data().yaw = _att_sp_msg.data().yaw_body; + _local_pos_sp_msg.data().vx = _vel_sp(0); + _local_pos_sp_msg.data().vy = _vel_sp(1); + _local_pos_sp_msg.data().vz = _vel_sp(2); + + /* publish local position setpoint */ + if (_local_pos_sp_pub != nullptr) { + _local_pos_sp_pub->publish(_local_pos_sp_msg); + + } else { + _local_pos_sp_pub = _n.advertise(); + } + + } else { /* position controller disabled, reset setpoints */ _reset_alt_sp = true; From 7b57092a9dce92aca4187f2564a8534f6a95d590 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 22 Feb 2015 12:49:20 +0100 Subject: [PATCH 018/151] multiplatform port of #1741: remove attitude sp generation from mc_att_control_multiplatform --- .../mc_att_control.cpp | 12 --- .../mc_att_control_base.cpp | 98 +------------------ .../mc_att_control_base.h | 2 - 3 files changed, 4 insertions(+), 108 deletions(-) diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp index ecbf98c9ec..9af4b32630 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp @@ -186,18 +186,6 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const px4_vehicle_atti if (_v_control_mode->data().flag_control_attitude_enabled) { control_attitude(dt); - /* publish the attitude setpoint if needed */ - if (_publish_att_sp && _v_status->data().is_rotary_wing) { - _v_att_sp_mod.data().timestamp = px4::get_time_micros(); - - if (_att_sp_pub != nullptr) { - _att_sp_pub->publish(_v_att_sp_mod); - - } else { - _att_sp_pub = _n.advertise(); - } - } - /* publish attitude rates setpoint */ _v_rates_sp_mod.data().roll = _rates_sp(0); _v_rates_sp_mod.data().pitch = _rates_sp(1); diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp index e779239ba7..16f1b79bf8 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp @@ -55,7 +55,6 @@ using namespace std; #endif MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : - _v_att_sp_mod(), _v_rates_sp_mod(), _actuators(), _publish_att_sp(false) @@ -87,101 +86,11 @@ MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() void MulticopterAttitudeControlBase::control_attitude(float dt) { - float yaw_sp_move_rate = 0.0f; - _publish_att_sp = false; - - - if (_v_control_mode->data().flag_control_manual_enabled) { - /* manual input, set or modify attitude setpoint */ - - if (_v_control_mode->data().flag_control_velocity_enabled - || _v_control_mode->data().flag_control_climb_rate_enabled) { - /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ - //XXX get rid of memcpy - memcpy(&(_v_att_sp_mod.data()), &_v_att_sp->data(), sizeof(_v_att_sp_mod.data())); - } - - if (!_v_control_mode->data().flag_control_climb_rate_enabled) { - /* pass throttle directly if not in altitude stabilized mode */ - _v_att_sp_mod.data().thrust = _manual_control_sp->data().z; - _publish_att_sp = true; - } - - if (!_armed->data().armed) { - /* reset yaw setpoint when disarmed */ - _reset_yaw_sp = true; - } - - /* reset yaw setpoint to current position if needed */ - if (_reset_yaw_sp) { - _reset_yaw_sp = false; - _v_att_sp_mod.data().yaw_body = _v_att->data().yaw; - _v_att_sp_mod.data().R_valid = false; - // _publish_att_sp = true; - } - - if (!_v_control_mode->data().flag_control_velocity_enabled) { - - if (_v_att_sp_mod.data().thrust < 0.1f) { - // TODO - //if (_status.condition_landed) { - /* reset yaw setpoint if on ground */ - // reset_yaw_sp = true; - //} - } else { - /* move yaw setpoint */ - yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max; - _v_att_sp_mod.data().yaw_body = _wrap_pi( - _v_att_sp_mod.data().yaw_body + yaw_sp_move_rate * dt); - float yaw_offs_max = _params.man_yaw_max / _params.att_p(2); - float yaw_offs = _wrap_pi(_v_att_sp_mod.data().yaw_body - _v_att->data().yaw); - - if (yaw_offs < -yaw_offs_max) { - _v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw - yaw_offs_max); - - } else if (yaw_offs > yaw_offs_max) { - _v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw + yaw_offs_max); - } - - _v_att_sp_mod.data().R_valid = false; - // _publish_att_sp = true; - } - /* update attitude setpoint if not in position control mode */ - _v_att_sp_mod.data().roll_body = _manual_control_sp->data().y * _params.man_roll_max; - _v_att_sp_mod.data().pitch_body = -_manual_control_sp->data().x - * _params.man_pitch_max; - _v_att_sp_mod.data().R_valid = false; - // _publish_att_sp = true; - } - - } else { - /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ - //XXX get rid of memcpy - memcpy(&(_v_att_sp_mod.data()), &_v_att_sp->data(), sizeof(_v_att_sp_mod.data())); - - /* reset yaw setpoint after non-manual control mode */ - _reset_yaw_sp = true; - } - - _thrust_sp = _v_att_sp_mod.data().thrust; + _thrust_sp = _v_att_sp->data().thrust; /* construct attitude setpoint rotation matrix */ math::Matrix<3, 3> R_sp; - - if (_v_att_sp_mod.data().R_valid) { - /* rotation matrix in _att_sp is valid, use it */ - R_sp.set(&_v_att_sp_mod.data().R_body[0]); - - } else { - /* rotation matrix in _att_sp is not valid, use euler angles instead */ - R_sp.from_euler(_v_att_sp_mod.data().roll_body, _v_att_sp_mod.data().pitch_body, - _v_att_sp_mod.data().yaw_body); - - /* copy rotation matrix back to setpoint struct */ - memcpy(&_v_att_sp_mod.data().R_body[0], &R_sp.data[0][0], - sizeof(_v_att_sp_mod.data().R_body)); - _v_att_sp_mod.data().R_valid = true; - } + R_sp.set(_v_att_sp->data().R_body); /* rotation matrix for current state */ math::Matrix<3, 3> R; @@ -260,7 +169,8 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) _params.yaw_rate_max); /* feed forward yaw setpoint rate */ - _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; + _rates_sp(2) += _v_att_sp->data().yaw_sp_move_rate * yaw_w * _params.yaw_ff; + } void MulticopterAttitudeControlBase::control_attitude_rates(float dt) diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.h b/src/modules/mc_att_control_multiplatform/mc_att_control_base.h index 124147079a..5e26b47e27 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_base.h +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.h @@ -97,8 +97,6 @@ protected: px4::Subscriber *_armed; /**< actuator arming status */ px4::Subscriber *_v_status; /**< vehicle status */ - px4_vehicle_attitude_setpoint _v_att_sp_mod; /**< modified vehicle attitude setpoint - that gets published eventually */ px4_vehicle_rates_setpoint _v_rates_sp_mod; /**< vehicle rates setpoint that gets published eventually*/ px4_actuator_controls_0 _actuators; /**< actuator controls */ From f1c6b24f7a80f88baf272bc0b947f7e9649bee27 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 22 Feb 2015 12:50:26 +0100 Subject: [PATCH 019/151] multiplatform port of #1741: att control: remove yaw sp reset after acro --- src/modules/mc_att_control_multiplatform/mc_att_control.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp index 9af4b32630..9ec6f18641 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp @@ -212,9 +212,6 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const px4_vehicle_atti _manual_control_sp->data().r).emult(_params.acro_rate_max); _thrust_sp = _manual_control_sp->data().z; - /* reset yaw setpoint after ACRO */ - _reset_yaw_sp = true; - /* publish attitude rates setpoint */ _v_rates_sp_mod.data().roll = _rates_sp(0); _v_rates_sp_mod.data().pitch = _rates_sp(1); From a56c16dfab948517007ad570d67f02dd4e2edb60 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 22 Feb 2015 13:02:22 +0100 Subject: [PATCH 020/151] multiplatform port of #1741: move params from att control to pos control --- .../mc_att_control.cpp | 8 ----- .../mc_att_control.h | 3 -- .../mc_att_control_base.cpp | 3 -- .../mc_att_control_base.h | 3 -- .../mc_att_control_params.c | 29 ------------------- .../mc_att_control_params.h | 3 -- .../mc_pos_control.cpp | 10 ++++++- .../mc_pos_control.h | 6 ++++ .../mc_pos_control_params.c | 29 +++++++++++++++++++ .../mc_pos_control_params.h | 3 ++ 10 files changed, 47 insertions(+), 50 deletions(-) diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp index 9ec6f18641..b5a551109f 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp @@ -89,9 +89,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : .yaw_rate_d = px4::ParameterFloat("MC_YAWRATE_D", PARAM_MC_YAWRATE_D_DEFAULT), .yaw_ff = px4::ParameterFloat("MC_YAW_FF", PARAM_MC_YAW_FF_DEFAULT), .yaw_rate_max = px4::ParameterFloat("MC_YAWRATE_MAX", PARAM_MC_YAWRATE_MAX_DEFAULT), - .man_roll_max = px4::ParameterFloat("MC_MAN_R_MAX", PARAM_MC_MAN_R_MAX_DEFAULT), - .man_pitch_max = px4::ParameterFloat("MC_MAN_P_MAX", PARAM_MC_MAN_P_MAX_DEFAULT), - .man_yaw_max = px4::ParameterFloat("MC_MAN_Y_MAX", PARAM_MC_MAN_Y_MAX_DEFAULT), .acro_roll_max = px4::ParameterFloat("MC_ACRO_R_MAX", PARAM_MC_ACRO_R_MAX_DEFAULT), .acro_pitch_max = px4::ParameterFloat("MC_ACRO_P_MAX", PARAM_MC_ACRO_P_MAX_DEFAULT), .acro_yaw_max = px4::ParameterFloat("MC_ACRO_Y_MAX", PARAM_MC_ACRO_Y_MAX_DEFAULT) @@ -146,11 +143,6 @@ MulticopterAttitudeControl::parameters_update() _params.yaw_ff = _params_handles.yaw_ff.update(); _params.yaw_rate_max = math::radians(_params_handles.yaw_rate_max.update()); - /* manual control scale */ - _params.man_roll_max = math::radians(_params_handles.man_roll_max.update()); - _params.man_pitch_max = math::radians(_params_handles.man_pitch_max.update()); - _params.man_yaw_max = math::radians(_params_handles.man_yaw_max.update()); - /* acro control scale */ _params.acro_rate_max(0) = math::radians(_params_handles.acro_roll_max.update()); _params.acro_rate_max(1) = math::radians(_params_handles.acro_pitch_max.update()); diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.h b/src/modules/mc_att_control_multiplatform/mc_att_control.h index e44317316d..52ba369c1a 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.h +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.h @@ -110,9 +110,6 @@ private: px4::ParameterFloat yaw_ff; px4::ParameterFloat yaw_rate_max; - px4::ParameterFloat man_roll_max; - px4::ParameterFloat man_pitch_max; - px4::ParameterFloat man_yaw_max; px4::ParameterFloat acro_roll_max; px4::ParameterFloat acro_pitch_max; px4::ParameterFloat acro_yaw_max; diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp index 16f1b79bf8..18e5986360 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp @@ -66,9 +66,6 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : _params.rate_d.zero(); _params.yaw_ff = 0.0f; _params.yaw_rate_max = 0.0f; - _params.man_roll_max = 0.0f; - _params.man_pitch_max = 0.0f; - _params.man_yaw_max = 0.0f; _params.acro_rate_max.zero(); _rates_prev.zero(); diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.h b/src/modules/mc_att_control_multiplatform/mc_att_control_base.h index 5e26b47e27..4e33018b46 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_base.h +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.h @@ -119,9 +119,6 @@ protected: float yaw_ff; /**< yaw control feed-forward */ float yaw_rate_max; /**< max yaw rate */ - float man_roll_max; - float man_pitch_max; - float man_yaw_max; math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ int32_t autostart_id; diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_params.c b/src/modules/mc_att_control_multiplatform/mc_att_control_params.c index a0b1706f2d..395ae83b02 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_params.c +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_params.c @@ -189,35 +189,6 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAW_FF); */ PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX); -/** - * Max manual roll - * - * @unit deg - * @min 0.0 - * @max 90.0 - * @group Multicopter Attitude Control - */ -PX4_PARAM_DEFINE_FLOAT(MC_MAN_R_MAX); - -/** - * Max manual pitch - * - * @unit deg - * @min 0.0 - * @max 90.0 - * @group Multicopter Attitude Control - */ -PX4_PARAM_DEFINE_FLOAT(MC_MAN_P_MAX); - -/** - * Max manual yaw rate - * - * @unit deg/s - * @min 0.0 - * @group Multicopter Attitude Control - */ -PX4_PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX); - /** * Max acro roll rate * diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_params.h b/src/modules/mc_att_control_multiplatform/mc_att_control_params.h index 59dd5240f6..ff962dbf1f 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_params.h +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_params.h @@ -57,9 +57,6 @@ #define PARAM_MC_YAWRATE_D_DEFAULT 0.0f #define PARAM_MC_YAW_FF_DEFAULT 0.5f #define PARAM_MC_YAWRATE_MAX_DEFAULT 120.0f -#define PARAM_MC_MAN_R_MAX_DEFAULT 35.0f -#define PARAM_MC_MAN_P_MAX_DEFAULT 35.0f -#define PARAM_MC_MAN_Y_MAX_DEFAULT 120.0f #define PARAM_MC_ACRO_R_MAX_DEFAULT 35.0f #define PARAM_MC_ACRO_P_MAX_DEFAULT 35.0f #define PARAM_MC_ACRO_Y_MAX_DEFAULT 120.0f 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 1fe75b90c9..1c0298a077 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -82,7 +82,10 @@ MulticopterPositionControl::MulticopterPositionControl() : .xy_ff = px4::ParameterFloat("MPC_XY_FF", PARAM_MPC_XY_FF_DEFAULT), .tilt_max_air = px4::ParameterFloat("MPC_TILTMAX_AIR", PARAM_MPC_TILTMAX_AIR_DEFAULT), .land_speed = px4::ParameterFloat("MPC_LAND_SPEED", PARAM_MPC_LAND_SPEED_DEFAULT), - .tilt_max_land = px4::ParameterFloat("MPC_TILTMAX_LND", PARAM_MPC_TILTMAX_LND_DEFAULT) + .tilt_max_land = px4::ParameterFloat("MPC_TILTMAX_LND", PARAM_MPC_TILTMAX_LND_DEFAULT), + .man_roll_max = px4::ParameterFloat("MC_MAN_R_MAX", PARAM_MPC_MAN_R_MAX_DEFAULT), + .man_pitch_max = px4::ParameterFloat("MC_MAN_P_MAX", PARAM_MPC_MAN_P_MAX_DEFAULT), + .man_yaw_max = px4::ParameterFloat("MC_MAN_Y_MAX", PARAM_MPC_MAN_Y_MAX_DEFAULT) }), _ref_alt(0.0f), _ref_timestamp(0), @@ -146,6 +149,11 @@ MulticopterPositionControl::parameters_update() _params.land_speed = _params_handles.land_speed.update(); _params.tilt_max_land = math::radians(_params_handles.tilt_max_land.update()); + /* manual control scale */ + _params.man_roll_max = math::radians(_params_handles.man_roll_max.update()); + _params.man_pitch_max = math::radians(_params_handles.man_pitch_max.update()); + _params.man_yaw_max = math::radians(_params_handles.man_yaw_max.update()); + _params.pos_p(0) = _params.pos_p(1) = _params_handles.xy_p.update(); _params.pos_p(2) = _params_handles.z_p.update(); _params.vel_p(0) = _params.vel_p(1) = _params_handles.xy_vel_p.update(); diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h index b754ccf01e..59b663b9fb 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h @@ -126,6 +126,9 @@ protected: px4::ParameterFloat tilt_max_air; px4::ParameterFloat land_speed; px4::ParameterFloat tilt_max_land; + px4::ParameterFloat man_roll_max; + px4::ParameterFloat man_pitch_max; + px4::ParameterFloat man_yaw_max; } _params_handles; /**< handles for interesting parameters */ struct { @@ -134,6 +137,9 @@ protected: float tilt_max_air; float land_speed; float tilt_max_land; + float man_roll_max; + float man_pitch_max; + float man_yaw_max; math::Vector<3> pos_p; math::Vector<3> vel_p; diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c index c741a7f0a6..709085662a 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c @@ -210,3 +210,32 @@ PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND); */ PX4_PARAM_DEFINE_FLOAT(MPC_LAND_SPEED); +/** + * Max manual roll + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_MAN_R_MAX); + +/** + * Max manual pitch + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_MAN_P_MAX); + +/** + * Max manual yaw rate + * + * @unit deg/s + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_MAN_Y_MAX); + diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h index fec3bcb7cc..8c8b707ae0 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h @@ -58,4 +58,7 @@ #define PARAM_MPC_TILTMAX_AIR_DEFAULT 45.0f #define PARAM_MPC_TILTMAX_LND_DEFAULT 15.0f #define PARAM_MPC_LAND_SPEED_DEFAULT 1.0f +#define PARAM_MPC_MAN_R_MAX_DEFAULT 35.0f +#define PARAM_MPC_MAN_P_MAX_DEFAULT 35.0f +#define PARAM_MPC_MAN_Y_MAX_DEFAULT 120.0f From 61398f5c03ad8c894b4c3933adb0272dd9c7d8e6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 22 Feb 2015 13:37:14 +0100 Subject: [PATCH 021/151] multiplatform port of #1741 and #1801: port attitude setpoint generation in manual to mc_pos_control_multiplatform --- .../mc_pos_control.cpp | 70 +++++++++++++++++-- .../mc_pos_control.h | 23 +++--- 2 files changed, 78 insertions(+), 15 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 1c0298a077..303123202b 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -41,6 +41,9 @@ #include "mc_pos_control.h" #include "mc_pos_control_params.h" +/* The following inclue is needed because the pos controller depens on a parameter from attitude control to set a + * reasonable yaw setpoint in manual mode */ +#include #define TILT_COS_MAX 0.7f #define SIGMA 0.000001f @@ -83,9 +86,10 @@ MulticopterPositionControl::MulticopterPositionControl() : .tilt_max_air = px4::ParameterFloat("MPC_TILTMAX_AIR", PARAM_MPC_TILTMAX_AIR_DEFAULT), .land_speed = px4::ParameterFloat("MPC_LAND_SPEED", PARAM_MPC_LAND_SPEED_DEFAULT), .tilt_max_land = px4::ParameterFloat("MPC_TILTMAX_LND", PARAM_MPC_TILTMAX_LND_DEFAULT), - .man_roll_max = px4::ParameterFloat("MC_MAN_R_MAX", PARAM_MPC_MAN_R_MAX_DEFAULT), - .man_pitch_max = px4::ParameterFloat("MC_MAN_P_MAX", PARAM_MPC_MAN_P_MAX_DEFAULT), - .man_yaw_max = px4::ParameterFloat("MC_MAN_Y_MAX", PARAM_MPC_MAN_Y_MAX_DEFAULT) + .man_roll_max = px4::ParameterFloat("MPC_MAN_R_MAX", PARAM_MPC_MAN_R_MAX_DEFAULT), + .man_pitch_max = px4::ParameterFloat("MPC_MAN_P_MAX", PARAM_MPC_MAN_P_MAX_DEFAULT), + .man_yaw_max = px4::ParameterFloat("MPC_MAN_Y_MAX", PARAM_MPC_MAN_Y_MAX_DEFAULT), + .mc_att_yaw_p = px4::ParameterFloat("MC_YAW_P", PARAM_MC_YAW_P_DEFAULT) }), _ref_alt(0.0f), _ref_timestamp(0), @@ -102,7 +106,6 @@ MulticopterPositionControl::MulticopterPositionControl() : * Do subscriptions */ _att = _n.subscribe(&MulticopterPositionControl::handle_vehicle_attitude, this, 0); - _v_att_sp = _n.subscribe(0); _control_mode = _n.subscribe(0); _parameter_update = _n.subscribe( &MulticopterPositionControl::handle_parameter_update, this, 1000); @@ -154,6 +157,8 @@ MulticopterPositionControl::parameters_update() _params.man_pitch_max = math::radians(_params_handles.man_pitch_max.update()); _params.man_yaw_max = math::radians(_params_handles.man_yaw_max.update()); + _params.mc_att_yaw_p = _params_handles.mc_att_yaw_p.update(); + _params.pos_p(0) = _params.pos_p(1) = _params_handles.xy_p.update(); _params.pos_p(2) = _params_handles.z_p.update(); _params.vel_p(0) = _params.vel_p(1) = _params_handles.xy_vel_p.update(); @@ -577,6 +582,7 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti static bool reset_int_z = true; static bool reset_int_z_manual = false; static bool reset_int_xy = true; + static bool reset_yaw_sp = true; static bool was_armed = false; static uint64_t t_prev = 0; @@ -590,8 +596,10 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti _reset_alt_sp = true; reset_int_z = true; reset_int_xy = true; + reset_yaw_sp = true; } + /* Update previous arming state */ was_armed = _control_mode->data().flag_armed; update_ref(); @@ -975,6 +983,60 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti reset_int_xy = true; } + /* generate attitude setpoint from manual controls */ + if(_control_mode->data().flag_control_manual_enabled && _control_mode->data().flag_control_attitude_enabled) { + + /* reset yaw setpoint to current position if needed */ + if (reset_yaw_sp) { + reset_yaw_sp = false; + _att_sp_msg.data().yaw_body = _att->data().yaw; + } + + // do not move yaw while arming + else if (_manual_control_sp->data().z > 0.1f) + { + const float YAW_OFFSET_MAX = _params.man_yaw_max / _params.mc_att_yaw_p; + + _att_sp_msg.data().yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max; + _att_sp_msg.data().yaw_body = _wrap_pi(_att_sp_msg.data().yaw_body + _att_sp_msg.data().yaw_sp_move_rate * dt); + float yaw_offs = _wrap_pi(_att_sp_msg.data().yaw_body - _att->data().yaw); + if (yaw_offs < - YAW_OFFSET_MAX) { + _att_sp_msg.data().yaw_body = _wrap_pi(_att->data().yaw - YAW_OFFSET_MAX); + + } else if (yaw_offs > YAW_OFFSET_MAX) { + _att_sp_msg.data().yaw_body = _wrap_pi(_att->data().yaw + YAW_OFFSET_MAX); + } + } + + /* Control roll and pitch directly if we no aiding velocity controller is active */ + if(!_control_mode->data().flag_control_velocity_enabled) { + _att_sp_msg.data().roll_body = _manual_control_sp->data().y * _params.man_roll_max; + _att_sp_msg.data().pitch_body = -_manual_control_sp->data().x * _params.man_pitch_max; + } + + /* Control climb rate directly if no aiding altitude controller is active */ + if(!_control_mode->data().flag_control_climb_rate_enabled) { + _att_sp_msg.data().thrust = _manual_control_sp->data().z; + } + + /* Construct attitude setpoint rotation matrix */ + math::Matrix<3,3> R_sp; + R_sp.from_euler(_att_sp_msg.data().roll_body,_att_sp_msg.data().pitch_body,_att_sp_msg.data().yaw_body); + memcpy(&_att_sp_msg.data().R_body[0], R_sp.data, sizeof(_att_sp_msg.data().R_body)); + _att_sp_msg.data().timestamp = get_time_micros(); + } + else { + reset_yaw_sp = true; + } + + /* publish attitude setpoint */ + if (_att_sp_pub != nullptr) { + _att_sp_pub->publish(_att_sp_msg); + + } else { + _att_sp_pub = _n.advertise(); + } + /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ reset_int_z_manual = _control_mode->data().flag_armed && _control_mode->data().flag_control_manual_enabled && !_control_mode->data().flag_control_climb_rate_enabled; } diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h index 59b663b9fb..54c6de155c 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h @@ -88,19 +88,18 @@ protected: Publisher *_att_sp_pub; /**< attitude setpoint publication */ Publisher *_local_pos_sp_pub; /**< vehicle local position setpoint publication */ - Publisher *_global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ + Publisher *_global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ - Subscriber *_att; /**< vehicle attitude */ - Subscriber *_v_att_sp; /**< vehicle attitude setpoint */ - Subscriber *_control_mode; /**< vehicle control mode */ - Subscriber *_parameter_update; /**< parameter update */ - Subscriber *_manual_control_sp; /**< manual control setpoint */ - Subscriber *_armed; /**< actuator arming status */ - Subscriber *_local_pos; /**< local position */ - Subscriber *_pos_sp_triplet; /**< local position */ - Subscriber *_local_pos_sp; /**< local position */ - Subscriber *_global_vel_sp; /**< local position */ + Subscriber *_att; /**< vehicle attitude */ + Subscriber *_control_mode; /**< vehicle control mode */ + Subscriber *_parameter_update; /**< parameter update */ + Subscriber *_manual_control_sp; /**< manual control setpoint */ + Subscriber *_armed; /**< actuator arming status */ + Subscriber *_local_pos; /**< local position */ + Subscriber *_pos_sp_triplet; /**< local position */ + Subscriber *_local_pos_sp; /**< local position */ + Subscriber *_global_vel_sp; /**< local position */ px4_vehicle_attitude_setpoint _att_sp_msg; px4_vehicle_local_position_setpoint _local_pos_sp_msg; @@ -129,6 +128,7 @@ protected: px4::ParameterFloat man_roll_max; px4::ParameterFloat man_pitch_max; px4::ParameterFloat man_yaw_max; + px4::ParameterFloat mc_att_yaw_p; // needed for calculating reasonable attitude setpoints in manual } _params_handles; /**< handles for interesting parameters */ struct { @@ -140,6 +140,7 @@ protected: float man_roll_max; float man_pitch_max; float man_yaw_max; + float mc_att_yaw_p; math::Vector<3> pos_p; math::Vector<3> vel_p; From 5d2b78abeff771644293c732f3bee9bf6bc60e44 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 22 Feb 2015 13:49:02 +0100 Subject: [PATCH 022/151] Revert "multiplat pos ctrl: also set yaw sp in manual modes" This reverts commit c0f1d841afd7f2e6aae83b705f25a21727fb184e. --- .../mc_pos_control.cpp | 19 ++----------------- 1 file changed, 2 insertions(+), 17 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 303123202b..e649c28a57 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -300,22 +300,6 @@ MulticopterPositionControl::control_manual(float dt) _sp_move_rate /= sp_move_norm; } - /* move yaw setpoint */ - //XXX hardcoded hack until #1741 is in/ported (the values stem - //from default param values, see how yaw setpoint is moved in the attitude controller) - float yaw_sp_move_rate = _manual_control_sp->data().r * 120.0f * M_DEG_TO_RAD_F; - _att_sp_msg.data().yaw_body = _wrap_pi( - _att_sp_msg.data().yaw_body + yaw_sp_move_rate * dt); - float yaw_offs_max = 120.0f * M_DEG_TO_RAD_F / 2.0f; - float yaw_offs = _wrap_pi(_att_sp_msg.data().yaw_body - _att->data().yaw); - - if (yaw_offs < -yaw_offs_max) { - _att_sp_msg.data().yaw_body = _wrap_pi(_att->data().yaw - yaw_offs_max); - - } else if (yaw_offs > yaw_offs_max) { - _att_sp_msg.data().yaw_body = _wrap_pi(_att->data().yaw + yaw_offs_max); - } - /* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */ math::Matrix<3, 3> R_yaw_sp; R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp_msg.data().yaw_body); @@ -643,7 +627,8 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti _att_sp_msg.data().R_valid = true; _att_sp_msg.data().roll_body = 0.0f; - // _att_sp_msg.data().yaw_body = _att->data().yaw; + _att_sp_msg.data().pitch_body = 0.0f; + _att_sp_msg.data().yaw_body = _att->data().yaw; _att_sp_msg.data().thrust = 0.0f; _att_sp_msg.data().timestamp = get_time_micros(); From 4d26c2164e9a1f37b1e28afa05748a6cbf5a0e51 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 22 Feb 2015 13:52:40 +0100 Subject: [PATCH 023/151] fix comment style --- src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 e649c28a57..c220b05c47 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -977,7 +977,7 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti _att_sp_msg.data().yaw_body = _att->data().yaw; } - // do not move yaw while arming + /* do not move yaw while arming */ else if (_manual_control_sp->data().z > 0.1f) { const float YAW_OFFSET_MAX = _params.man_yaw_max / _params.mc_att_yaw_p; From 4bc2d5f6879c320a3a9b7e86617ae718f156c7d3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 22 Feb 2015 14:17:32 +0100 Subject: [PATCH 024/151] mc att multi: style fixes to be consistent with old controller --- .../mc_att_control_base.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp index 18e5986360..5db8f77ac4 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp @@ -96,11 +96,11 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) /* all input data is ready, run controller itself */ /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ - math::Vector < 3 > R_z(R(0, 2), R(1, 2), R(2, 2)); - math::Vector < 3 > R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); + math::Vector <3> R_z(R(0, 2), R(1, 2), R(2, 2)); + math::Vector <3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); /* axis and sin(angle) of desired rotation */ - math::Vector < 3 > e_R = R.transposed() * (R_z % R_sp_z); + math::Vector <3> e_R = R.transposed() * (R_z % R_sp_z); /* calculate angle error */ float e_R_z_sin = e_R.length(); @@ -115,7 +115,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) if (e_R_z_sin > 0.0f) { /* get axis-angle representation */ float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); - math::Vector < 3 > e_R_z_axis = e_R / e_R_z_sin; + math::Vector <3> e_R_z_axis = e_R / e_R_z_sin; e_R = e_R_z_axis * e_R_z_angle; @@ -130,9 +130,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) e_R_cp(2, 1) = e_R_z_axis(0); /* rotation matrix for roll/pitch only rotation */ - R_rp = R - * (_I + e_R_cp * e_R_z_sin - + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); + R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); } else { /* zero roll/pitch rotation */ @@ -140,8 +138,8 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) } /* R_rp and R_sp has the same Z axis, calculate yaw error */ - math::Vector < 3 > R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); - math::Vector < 3 > R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); + math::Vector <3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); + math::Vector <3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; if (e_R_z_cos < 0.0f) { @@ -149,7 +147,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) * calculate angle and axis for R -> R_sp rotation directly */ math::Quaternion q; q.from_dcm(R.transposed() * R_sp); - math::Vector < 3 > e_R_d = q.imag(); + math::Vector <3> e_R_d = q.imag(); e_R_d.normalize(); e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); From a284a9838380f26a6d3d3eb6537e6debaf623ac5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 22 Feb 2015 14:18:06 +0100 Subject: [PATCH 025/151] mc pos multi: set R valid in attitude sp in manual mode --- src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp | 1 + 1 file changed, 1 insertion(+) 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 c220b05c47..2e14b744f6 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -1007,6 +1007,7 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti /* Construct attitude setpoint rotation matrix */ math::Matrix<3,3> R_sp; R_sp.from_euler(_att_sp_msg.data().roll_body,_att_sp_msg.data().pitch_body,_att_sp_msg.data().yaw_body); + _att_sp_msg.data().R_valid = true; memcpy(&_att_sp_msg.data().R_body[0], R_sp.data, sizeof(_att_sp_msg.data().R_body)); _att_sp_msg.data().timestamp = get_time_micros(); } From 9224738804ed7c605d8e89d14b84c53e0956a9c5 Mon Sep 17 00:00:00 2001 From: Stefan Date: Tue, 24 Feb 2015 16:01:41 +0100 Subject: [PATCH 026/151] update dead link to Ublox Documentation actualise dead link to Ublox M8 V15-17 Receiver Description Protocol Spec ( new R08 from 4 December 2014) --- src/drivers/gps/ubx.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index e197059db1..f42c968d37 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -45,7 +45,7 @@ * (rework, add ubx7+ compatibility) * * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf - * @see http://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8-V15_ReceiverDescriptionProtocolSpec_Public_%28UBX-13003221%29.pdf + * @see http://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8_ReceiverDescriptionProtocolSpec_%28UBX-13003221%29_Public.pdf */ #include From 66a8905357b98dd50155ad1cafbf9fdc5340832f Mon Sep 17 00:00:00 2001 From: Stefan Date: Tue, 24 Feb 2015 16:12:52 +0100 Subject: [PATCH 027/151] update Firmware Download link --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 9441b341da..34c2e880ea 100644 --- a/README.md +++ b/README.md @@ -13,7 +13,7 @@ This repository contains the PX4 Flight Core, with the main applications located * [Fixed wing](http://px4.io/platforms/planes/start) * [VTOL](http://px4.io/platforms/vtol/start) * Binaries (always up-to-date from master): - * [Downloads](http://px4.io/downloads) + * [Downloads](http://px4.io/firmware/downloads) * Mailing list: [Google Groups](http://groups.google.com/group/px4users) ### Users ### From 0ac3c80542c09c59187666f1143a4ad83ce56403 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 24 Dec 2014 15:40:08 +0900 Subject: [PATCH 028/151] OreoLED: driver for attiny88 based LED controller --- src/drivers/drv_oreoled.h | 146 +++++++ src/drivers/oreoled/module.mk | 8 + src/drivers/oreoled/oreoled.cpp | 651 ++++++++++++++++++++++++++++++++ 3 files changed, 805 insertions(+) create mode 100644 src/drivers/drv_oreoled.h create mode 100644 src/drivers/oreoled/module.mk create mode 100644 src/drivers/oreoled/oreoled.cpp diff --git a/src/drivers/drv_oreoled.h b/src/drivers/drv_oreoled.h new file mode 100644 index 0000000000..bc53f04a54 --- /dev/null +++ b/src/drivers/drv_oreoled.h @@ -0,0 +1,146 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 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 drv_oreoled.h + * + * Oreo led device API + */ + +#pragma once + +#include +#include + +/* oreoled device path */ +#define OREOLED0_DEVICE_PATH "/dev/oreoled0" + +/* + * ioctl() definitions + */ + +#define _OREOLEDIOCBASE (0x2d00) +#define _OREOLEDIOC(_n) (_IOC(_OREOLEDIOCBASE, _n)) + +/** set constant RGB values */ +#define OREOLED_SET_RGB _OREOLEDIOC(1) + +/** run macro */ +#define OREOLED_RUN_MACRO _OREOLEDIOC(2) + +/* Oreo LED driver supports up to 4 leds */ +#define OREOLED_NUM_LEDS 4 + +/* instance of 0xff means apply to all instances */ +#define OREOLED_ALL_INSTANCES 0xff + +/* maximum command length that can be sent to LEDs */ +#define OREOLED_CMD_LENGTH_MAX 10 + +/* enum passed to OREOLED_SET_MODE ioctl() + * defined by hardware */ +enum oreoled_pattern { + OREOLED_PATTERN_OFF = 0, + OREOLED_PATTERN_SINE = 1, + OREOLED_PATTERN_SOLID = 2, + OREOLED_PATTERN_SIREN = 3, + OREOLED_PATTERN_STROBE = 4, + OREOLED_PATTERN_FADEIN = 5, + OREOLED_PATTERN_FADEOUT = 6, + OREOLED_PATTERN_PARAMUPDATE = 7, + OREOLED_PATTERN_ENUM_COUNT +}; + +/* enum passed to OREOLED_SET_MODE ioctl() + * defined by hardware */ +enum oreoled_param { + OREOLED_PARAM_BIAS_RED = 0, + OREOLED_PARAM_BIAS_GREEN = 1, + OREOLED_PARAM_BIAS_BLUE = 2, + OREOLED_PARAM_AMPLITUDE_RED = 3, + OREOLED_PARAM_AMPLITUDE_GREEN = 4, + OREOLED_PARAM_AMPLITUDE_BLUE = 5, + OREOLED_PARAM_PERIOD = 6, + OREOLED_PARAM_REPEAT = 7, + OREOLED_PARAM_PHASEOFFSET = 8, + OREOLED_PARAM_MACRO = 9, + OREOLED_PARAM_ENUM_COUNT +}; + +/* enum of available macros + * defined by hardware */ +enum oreoled_macro { + OREOLED_PARAM_MACRO_RESET = 0, + OREOLED_PARAM_MACRO_COLOUR_CYCLE = 1, + OREOLED_PARAM_MACRO_BREATH = 2, + OREOLED_PARAM_MACRO_STROBE = 3, + OREOLED_PARAM_MACRO_FADEIN = 4, + OREOLED_PARAM_MACRO_FADEOUT = 5, + OREOLED_PARAM_MACRO_RED = 6, + OREOLED_PARAM_MACRO_GREEN = 7, + OREOLED_PARAM_MACRO_BLUE = 8, + OREOLED_PARAM_MACRO_YELLOW = 9, + OREOLED_PARAM_MACRO_WHITE = 10, + OREOLED_PARAM_MACRO_ENUM_COUNT +}; + +/* + structure passed to OREOLED_SET_RGB ioctl() + Note that the driver scales the brightness to 0 to 255, regardless + of the hardware scaling + */ +typedef struct { + uint8_t instance; + oreoled_pattern pattern; + uint8_t red; + uint8_t green; + uint8_t blue; +} oreoled_rgbset_t; + +/* + structure passed to OREOLED_RUN_MACRO ioctl() + */ +typedef struct { + uint8_t instance; + oreoled_macro macro; +} oreoled_macrorun_t; + +/* + structure passed to send_bytes method (only used for testing) + */ +typedef struct { + uint8_t led_num; + uint8_t num_bytes; + uint8_t buff[OREOLED_CMD_LENGTH_MAX]; +} oreoled_cmd_t; + diff --git a/src/drivers/oreoled/module.mk b/src/drivers/oreoled/module.mk new file mode 100644 index 0000000000..96afdd5fdc --- /dev/null +++ b/src/drivers/oreoled/module.mk @@ -0,0 +1,8 @@ +# +# Oreo LED driver +# + +MODULE_COMMAND = oreoled +SRCS = oreoled.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp new file mode 100644 index 0000000000..afd20275a9 --- /dev/null +++ b/src/drivers/oreoled/oreoled.cpp @@ -0,0 +1,651 @@ +/**************************************************************************** + * + * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Randy Mackay + * + * 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 oreoled.cpp + * + * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C. + * + */ + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include + +#define OREOLED_NUM_LEDS 4 ///< maximum number of LEDs the oreo led driver can support +#define OREOLED_BASE_I2C_ADDR 0x68 ///< base i2c address (7-bit) +#define OREOLED_TIMEOUT_MS 10000000U ///< timeout looking for battery 10seconds after startup +#define OREOLED_GENERALCALL_US 4000000U ///< general call sent every 4 seconds +#define OREOLED_GENERALCALL_CMD 0x00 ///< general call command sent at regular intervals + +#define OREOLED_STARTUP_INTERVAL_US (1000000U / 10U) ///< time in microseconds, measure at 10hz +#define OREOLED_UPDATE_INTERVAL_US (1000000U / 50U) ///< time in microseconds, measure at 10hz + +#define OREOLED_CMD_QUEUE_SIZE 10 ///< up to 10 messages can be queued up to send to the LEDs + +class OREOLED : public device::I2C +{ +public: + OREOLED(int bus, int i2c_addr); + virtual ~OREOLED(); + + virtual int init(); + virtual int probe(); + virtual int info(); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /* send general call on I2C bus to syncronise all LEDs */ + int send_general_call(); + + /* send cmd to an LEDs (used for testing only) */ + int send_cmd(oreoled_cmd_t sb); + +private: + + /** + * Start periodic updates to the LEDs + */ + void start(); + + /** + * Stop periodic updates to the LEDs + */ + void stop(); + + /** + * static function that is called by worker queue + */ + static void cycle_trampoline(void *arg); + + /** + * update the colours displayed by the LEDs + */ + void cycle(); + + /* internal variables */ + work_s _work; ///< work queue for scheduling reads + bool _healthy[OREOLED_NUM_LEDS]; ///< health of each LED + uint8_t _num_healthy; ///< number of healthy LEDs + RingBuffer *_cmd_queue; ///< buffer of commands to send to LEDs + uint64_t _last_gencall; + uint64_t _start_time; ///< system time we first attempt to communicate with battery +}; + +/* for now, we only support one OREOLED */ +namespace +{ +OREOLED *g_oreoled = nullptr; +} + +void oreoled_usage(); + +extern "C" __EXPORT int oreoled_main(int argc, char *argv[]); + +/* constructor */ +OREOLED::OREOLED(int bus, int i2c_addr) : + I2C("oreoled", OREOLED0_DEVICE_PATH, bus, i2c_addr, 100000), + _work{}, + _num_healthy(0), + _cmd_queue(nullptr), + _last_gencall(0) +{ + /* initialise to unhealthy */ + memset(_healthy, 0, sizeof(_healthy)); + + /* capture startup time */ + _start_time = hrt_absolute_time(); +} + +/* destructor */ +OREOLED::~OREOLED() +{ + /* make sure we are truly inactive */ + stop(); + + /* clear command queue */ + if (_cmd_queue != nullptr) { + delete _cmd_queue; + } +} + +int +OREOLED::init() +{ + int ret; + + /* initialise I2C bus */ + ret = I2C::init(); + if (ret != OK) { + return ret; + } + + /* allocate command queue */ + _cmd_queue = new RingBuffer(OREOLED_CMD_QUEUE_SIZE, sizeof(oreoled_cmd_t)); + if (_cmd_queue == nullptr) { + return ENOTTY; + } else { + /* start work queue */ + start(); + } + + return OK; +} + +int +OREOLED::probe() +{ + /* always return true */ + return OK; +} + +int +OREOLED::info() +{ + /* print health info on each LED */ + for (uint8_t i=0; icycle(); + } +} + +void +OREOLED::cycle() +{ + /* check time since startup */ + uint64_t now = hrt_absolute_time(); + bool startup_timeout = (now - _start_time > OREOLED_TIMEOUT_MS); + + /* if not leds found during start-up period, exit without rescheduling */ + if (startup_timeout && _num_healthy == 0) { + warnx("did not find oreoled"); + return; + } + + /* during startup period keep searching for unhealthy LEDs */ + if (!startup_timeout && _num_healthy < OREOLED_NUM_LEDS) { + /* prepare command to turn off LED*/ + uint8_t msg[] = {OREOLED_PATTERN_OFF}; + /* attempt to contact each unhealthy LED */ + for (uint8_t i=0; iget(&next_cmd,sizeof(oreoled_cmd_t))) { + /* send valid messages to healthy LEDs */ + if ((next_cmd.led_num < OREOLED_NUM_LEDS) && _healthy[next_cmd.led_num] && (next_cmd.num_bytes <= OREOLED_CMD_LENGTH_MAX)) { + /* set I2C address */ + set_address(OREOLED_BASE_I2C_ADDR+next_cmd.led_num); + /* send I2C command */ + transfer(next_cmd.buff, next_cmd.num_bytes, nullptr, 0); + } + } + + /* send general call every 4 seconds*/ + if ((now - _last_gencall) > OREOLED_GENERALCALL_US) { + send_general_call(); + } + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, + USEC2TICK(OREOLED_UPDATE_INTERVAL_US)); +} + +int +OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = -ENODEV; + oreoled_cmd_t new_cmd; + + switch (cmd) { + case OREOLED_SET_RGB: + /* set the specified color */ + new_cmd.led_num = ((oreoled_rgbset_t *) arg)->instance; + new_cmd.buff[0] = ((oreoled_rgbset_t *) arg)->pattern; + new_cmd.buff[1] = OREOLED_PARAM_BIAS_RED; + new_cmd.buff[2] = ((oreoled_rgbset_t *) arg)->red; + new_cmd.buff[3] = OREOLED_PARAM_BIAS_GREEN; + new_cmd.buff[4] = ((oreoled_rgbset_t *) arg)->green; + new_cmd.buff[5] = OREOLED_PARAM_BIAS_BLUE; + new_cmd.buff[6] = ((oreoled_rgbset_t *) arg)->blue; + new_cmd.num_bytes = 7; + + /* special handling for request to set all instances rgb values */ + if (new_cmd.led_num == OREOLED_ALL_INSTANCES) { + for (uint8_t i=0; iforce(&new_cmd); + ret = OK; + } + } + + /* request to set individual instance's rgb value */ + } else if (new_cmd.led_num < OREOLED_NUM_LEDS) { + if (_healthy[new_cmd.led_num]) { + _cmd_queue->force(&new_cmd); + ret = OK; + } + } + return ret; + + case OREOLED_RUN_MACRO: + /* run a macro */ + new_cmd.led_num = ((oreoled_macrorun_t *) arg)->instance; + new_cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE; + new_cmd.buff[1] = OREOLED_PARAM_MACRO; + new_cmd.buff[2] = ((oreoled_macrorun_t *) arg)->macro; + new_cmd.num_bytes = 3; + + /* special handling for request to set all instances */ + if (new_cmd.led_num == OREOLED_ALL_INSTANCES) { + for (uint8_t i=0; iforce(&new_cmd); + ret = OK; + } + } + + /* request to set individual instance's rgb value */ + } else if (new_cmd.led_num < OREOLED_NUM_LEDS) { + 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); + break; + } + + return ret; +} + +/* send general call on I2C bus to syncronise all LEDs */ +int +OREOLED::send_general_call() +{ + int ret = -ENODEV; + + /* set I2C address to zero */ + set_address(0); + + /* prepare command : 0x01 = general hardware call, 0x00 = I2C address of master (but we don't act as a slave so set to zero)*/ + uint8_t msg[] = {0x01,0x00}; + + /* send I2C command */ + if (transfer(msg, sizeof(msg), nullptr, 0) == OK) { + ret = OK; + } + + /* record time */ + _last_gencall = hrt_absolute_time(); + + return ret; +} + +/* send a cmd to an LEDs (used for testing only) */ +int +OREOLED::send_cmd(oreoled_cmd_t new_cmd) +{ + int ret = -ENODEV; + + /* sanity check led number, health and cmd length */ + if ((new_cmd.led_num < OREOLED_NUM_LEDS) && _healthy[new_cmd.led_num] && (new_cmd.num_bytes < OREOLED_CMD_LENGTH_MAX)) { + /* set I2C address */ + set_address(OREOLED_BASE_I2C_ADDR+new_cmd.led_num); + + /* add to queue */ + _cmd_queue->force(&new_cmd); + ret = OK; + } + + return ret; +} + +void +oreoled_usage() +{ + warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'rgb 30 40 50' 'macro 4' 'gencall' 'bytes 7 9 6'"); + warnx("options:"); + warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); + warnx(" -a addr (0x%x)", OREOLED_BASE_I2C_ADDR); +} + +int +oreoled_main(int argc, char *argv[]) +{ + int i2cdevice = -1; + int i2c_addr = OREOLED_BASE_I2C_ADDR; /* 7bit */ + + int ch; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "a:b:")) != EOF) { + switch (ch) { + case 'a': + i2c_addr = (int)strtol(optarg, NULL, 0); + break; + + case 'b': + i2cdevice = (int)strtol(optarg, NULL, 0); + break; + + default: + oreoled_usage(); + exit(0); + } + } + + if (optind >= argc) { + oreoled_usage(); + exit(1); + } + + const char *verb = argv[optind]; + + int ret; + + /* start driver */ + if (!strcmp(verb, "start")) { + if (g_oreoled != nullptr) { + errx(1, "already started"); + } + + /* by default use LED bus */ + if (i2cdevice == -1) { + i2cdevice = PX4_I2C_BUS_LED; + } + + /* instantiate driver */ + g_oreoled = new OREOLED(i2cdevice, i2c_addr); + + /* check if object was created */ + if (g_oreoled == nullptr) { + errx(1, "failed to allocated memory for driver"); + } + + /* check object was created successfully */ + if (g_oreoled->init() != OK) { + delete g_oreoled; + g_oreoled = nullptr; + errx(1, "failed to start driver"); + } + + exit(0); + } + + /* need the driver past this point */ + if (g_oreoled == nullptr) { + warnx("not started"); + oreoled_usage(); + exit(1); + } + + if (!strcmp(verb, "test")) { + int fd = open(OREOLED0_DEVICE_PATH, O_RDWR); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + /* structure to hold desired colour */ + oreoled_rgbset_t rgb_set_red = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_SOLID, 0xFF, 0x0, 0x0}; + oreoled_rgbset_t rgb_set_blue = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_SOLID, 0x0, 0x0, 0xFF}; + oreoled_rgbset_t rgb_set_off = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_OFF, 0x0, 0x0, 0x0}; + + /* flash red and blue for 3 seconds */ + for (uint8_t i=0; i<30; i++) { + /* red */ + if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_red)) != OK) { + errx(1," failed to update rgb"); + } + /* sleep for 0.05 seconds */ + usleep(50000); + /* blue */ + if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_blue)) != OK) { + errx(1," failed to update rgb"); + } + /* sleep for 0.05 seconds */ + usleep(50000); + } + /* turn off LED */ + if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_off)) != OK) { + errx(1," failed to turn off led"); + } + + close(fd); + exit(ret); + } + + /* display driver status */ + if (!strcmp(verb, "info")) { + g_oreoled->info(); + exit(0); + } + + if (!strcmp(verb, "off") || !strcmp(verb, "stop")) { + int fd = open(OREOLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + /* turn off LED */ + oreoled_rgbset_t rgb_set_off = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_OFF, 0x0, 0x0, 0x0}; + ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_off); + + close(fd); + /* delete the oreoled object if stop was requested, in addition to turning off the LED. */ + if (!strcmp(verb, "stop")) { + OREOLED *tmp_oreoled = g_oreoled; + g_oreoled = nullptr; + delete tmp_oreoled; + exit(0); + } + exit(ret); + } + + /* send rgb request to all LEDS */ + if (!strcmp(verb, "rgb")) { + if (argc < 5) { + errx(1, "Usage: oreoled rgb "); + } + + int fd = open(OREOLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + uint8_t red = (uint8_t)strtol(argv[2], NULL, 0); + uint8_t green = (uint8_t)strtol(argv[3], NULL, 0); + uint8_t blue = (uint8_t)strtol(argv[4], NULL, 0); + oreoled_rgbset_t rgb_set = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_SOLID, red, green, blue}; + if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set)) != OK) { + errx(1, "failed to set rgb"); + } + + close(fd); + exit(ret); + } + + /* send macro request to all LEDS */ + if (!strcmp(verb, "macro")) { + if (argc < 3) { + errx(1, "Usage: oreoled macro "); + } + + int fd = open(OREOLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + uint8_t macro = (uint8_t)strtol(argv[2], NULL, 0); + + /* sanity check macro number */ + if (macro > OREOLED_PARAM_MACRO_ENUM_COUNT) { + errx(1, "invalid macro number %d",(int)macro); + exit(ret); + } + + oreoled_macrorun_t macro_run = {OREOLED_ALL_INSTANCES, (enum oreoled_macro)macro}; + if ((ret = ioctl(fd, OREOLED_RUN_MACRO, (unsigned long)¯o_run)) != OK) { + errx(1, "failed to run macro"); + } + close(fd); + exit(ret); + } + + /* send general hardware call to all LEDS */ + if (!strcmp(verb, "gencall")) { + ret = g_oreoled->send_general_call(); + warnx("sent general call"); + exit(ret); + } + + /* send a string of bytes to an LED using send_bytes function */ + if (!strcmp(verb, "bytes")) { + if (argc < 3) { + errx(1, "Usage: oreoled bytes ..."); + } + + /* structure to be sent */ + oreoled_cmd_t sendb; + + /* maximum of 20 bytes can be sent */ + if (argc > 20+3) { + errx(1, "Max of 20 bytes can be sent"); + } + + /* check led num */ + sendb.led_num = (uint8_t)strtol(argv[2], NULL, 0); + if (sendb.led_num > 3) { + errx(1, "led number must be between 0 ~ 3"); + } + + /* get bytes */ + sendb.num_bytes = argc-3; + uint8_t byte_count; + for (byte_count=0; byte_countsend_cmd(sendb)) != OK) { + errx(1, "failed to send command"); + } else { + warnx("sent %d bytes",(int)sendb.num_bytes); + } + exit(ret); + } + + oreoled_usage(); + exit(0); +} From 90df6c6b58764bb8e5a35ef379c00e8112ecfa50 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 25 Feb 2015 21:46:31 +0900 Subject: [PATCH 029/151] OreoLED: fix formatting --- src/drivers/oreoled/oreoled.cpp | 74 ++++++++++++++++++++++----------- 1 file changed, 49 insertions(+), 25 deletions(-) diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index afd20275a9..2f5bf75bc2 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -170,14 +170,17 @@ OREOLED::init() /* initialise I2C bus */ ret = I2C::init(); + if (ret != OK) { return ret; } /* allocate command queue */ _cmd_queue = new RingBuffer(OREOLED_CMD_QUEUE_SIZE, sizeof(oreoled_cmd_t)); + if (_cmd_queue == nullptr) { return ENOTTY; + } else { /* start work queue */ start(); @@ -197,11 +200,12 @@ int OREOLED::info() { /* print health info on each LED */ - for (uint8_t i=0; iget(&next_cmd,sizeof(oreoled_cmd_t))) { + + while (_cmd_queue->get(&next_cmd, sizeof(oreoled_cmd_t))) { /* send valid messages to healthy LEDs */ - if ((next_cmd.led_num < OREOLED_NUM_LEDS) && _healthy[next_cmd.led_num] && (next_cmd.num_bytes <= OREOLED_CMD_LENGTH_MAX)) { + if ((next_cmd.led_num < OREOLED_NUM_LEDS) && _healthy[next_cmd.led_num] + && (next_cmd.num_bytes <= OREOLED_CMD_LENGTH_MAX)) { /* set I2C address */ - set_address(OREOLED_BASE_I2C_ADDR+next_cmd.led_num); + set_address(OREOLED_BASE_I2C_ADDR + next_cmd.led_num); /* send I2C command */ transfer(next_cmd.buff, next_cmd.num_bytes, nullptr, 0); } @@ -311,7 +320,7 @@ OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) /* special handling for request to set all instances rgb values */ if (new_cmd.led_num == OREOLED_ALL_INSTANCES) { - for (uint8_t i=0; iforce(&new_cmd); ret = OK; } } + return ret; case OREOLED_RUN_MACRO: @@ -339,7 +349,7 @@ OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) /* special handling for request to set all instances */ if (new_cmd.led_num == OREOLED_ALL_INSTANCES) { - for (uint8_t i=0; iforce(&new_cmd); ret = OK; } } + return ret; default: @@ -376,7 +387,7 @@ OREOLED::send_general_call() set_address(0); /* prepare command : 0x01 = general hardware call, 0x00 = I2C address of master (but we don't act as a slave so set to zero)*/ - uint8_t msg[] = {0x01,0x00}; + uint8_t msg[] = {0x01, 0x00}; /* send I2C command */ if (transfer(msg, sizeof(msg), nullptr, 0) == OK) { @@ -398,7 +409,7 @@ OREOLED::send_cmd(oreoled_cmd_t new_cmd) /* sanity check led number, health and cmd length */ if ((new_cmd.led_num < OREOLED_NUM_LEDS) && _healthy[new_cmd.led_num] && (new_cmd.num_bytes < OREOLED_CMD_LENGTH_MAX)) { /* set I2C address */ - set_address(OREOLED_BASE_I2C_ADDR+new_cmd.led_num); + set_address(OREOLED_BASE_I2C_ADDR + new_cmd.led_num); /* add to queue */ _cmd_queue->force(&new_cmd); @@ -500,23 +511,27 @@ oreoled_main(int argc, char *argv[]) oreoled_rgbset_t rgb_set_off = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_OFF, 0x0, 0x0, 0x0}; /* flash red and blue for 3 seconds */ - for (uint8_t i=0; i<30; i++) { + for (uint8_t i = 0; i < 30; i++) { /* red */ if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_red)) != OK) { - errx(1," failed to update rgb"); + errx(1, " failed to update rgb"); } + /* sleep for 0.05 seconds */ usleep(50000); + /* blue */ if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_blue)) != OK) { - errx(1," failed to update rgb"); + errx(1, " failed to update rgb"); } + /* sleep for 0.05 seconds */ usleep(50000); } + /* turn off LED */ if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_off)) != OK) { - errx(1," failed to turn off led"); + errx(1, " failed to turn off led"); } close(fd); @@ -541,6 +556,7 @@ oreoled_main(int argc, char *argv[]) ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_off); close(fd); + /* delete the oreoled object if stop was requested, in addition to turning off the LED. */ if (!strcmp(verb, "stop")) { OREOLED *tmp_oreoled = g_oreoled; @@ -548,6 +564,7 @@ oreoled_main(int argc, char *argv[]) delete tmp_oreoled; exit(0); } + exit(ret); } @@ -567,6 +584,7 @@ oreoled_main(int argc, char *argv[]) uint8_t green = (uint8_t)strtol(argv[3], NULL, 0); uint8_t blue = (uint8_t)strtol(argv[4], NULL, 0); oreoled_rgbset_t rgb_set = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_SOLID, red, green, blue}; + if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set)) != OK) { errx(1, "failed to set rgb"); } @@ -591,14 +609,16 @@ oreoled_main(int argc, char *argv[]) /* sanity check macro number */ if (macro > OREOLED_PARAM_MACRO_ENUM_COUNT) { - errx(1, "invalid macro number %d",(int)macro); + errx(1, "invalid macro number %d", (int)macro); exit(ret); } oreoled_macrorun_t macro_run = {OREOLED_ALL_INSTANCES, (enum oreoled_macro)macro}; + if ((ret = ioctl(fd, OREOLED_RUN_MACRO, (unsigned long)¯o_run)) != OK) { errx(1, "failed to run macro"); } + close(fd); exit(ret); } @@ -620,29 +640,33 @@ oreoled_main(int argc, char *argv[]) oreoled_cmd_t sendb; /* maximum of 20 bytes can be sent */ - if (argc > 20+3) { + if (argc > 20 + 3) { errx(1, "Max of 20 bytes can be sent"); } /* check led num */ sendb.led_num = (uint8_t)strtol(argv[2], NULL, 0); + if (sendb.led_num > 3) { errx(1, "led number must be between 0 ~ 3"); } /* get bytes */ - sendb.num_bytes = argc-3; + sendb.num_bytes = argc - 3; uint8_t byte_count; - for (byte_count=0; byte_countsend_cmd(sendb)) != OK) { errx(1, "failed to send command"); + } else { - warnx("sent %d bytes",(int)sendb.num_bytes); + warnx("sent %d bytes", (int)sendb.num_bytes); } + exit(ret); } From 9a3ea6156903a9d4f9883088a0f55145240dc077 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 25 Feb 2015 21:55:22 +0900 Subject: [PATCH 030/151] build: add oreoled to px4fmu-v2_default --- makefiles/config_px4fmu-v2_default.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 0347492eb8..86925f9ff4 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -43,6 +43,7 @@ MODULES += drivers/frsky_telemetry MODULES += modules/sensors MODULES += drivers/mkblctrl MODULES += drivers/px4flow +MODULES += drivers/oreoled # # System commands From 59e623a2ebe28139dfed2a3df4a63f1999c50b1a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 25 Feb 2015 17:48:55 +0100 Subject: [PATCH 031/151] quaternion from dcm: comment and reference --- src/lib/mathlib/math/Quaternion.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index 6ea4cf557a..38400beef5 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -135,6 +135,10 @@ public: data[3] = static_cast(cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2); } + /** + * set quaternion to rotation by DCM + * Reference: Shoemake, Quaternions, http://www.cs.ucr.edu/~vbz/resources/quatut.pdf + */ void from_dcm(const Matrix<3, 3> &dcm) { float tr = dcm.data[0][0] + dcm.data[1][1] + dcm.data[2][2]; if (tr > 0.0f) { From 8240e90d265b2d0572b1db8aee570da5912f050e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Feb 2015 10:19:15 +1100 Subject: [PATCH 032/151] bl_update: fixed stat() check --- src/systemcmds/bl_update/bl_update.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/bl_update/bl_update.c b/src/systemcmds/bl_update/bl_update.c index ec9269d39d..02d2d6f3cf 100644 --- a/src/systemcmds/bl_update/bl_update.c +++ b/src/systemcmds/bl_update/bl_update.c @@ -74,7 +74,7 @@ bl_update_main(int argc, char *argv[]) struct stat s; - if (!stat(argv[1], &s)) + if (stat(argv[1], &s) != 0) err(1, "stat %s", argv[1]); /* sanity-check file size */ @@ -214,4 +214,4 @@ setopt(void) errx(1, "option bits setting failed; readback 0x%04x", *optcr); -} \ No newline at end of file +} From a8f9caaaa5e76fa6d4790608fef73b81e201d977 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 26 Feb 2015 11:14:06 +0100 Subject: [PATCH 033/151] px4_uploader: Prevent spin-lock from hogging CPU --- Tools/px_uploader.py | 127 +++++++++++++++++++++++-------------------- 1 file changed, 68 insertions(+), 59 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 95a3d40469..8042ec34ab 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -501,75 +501,84 @@ print("Loaded firmware for %x,%x, size: %d bytes, waiting for the bootloader..." print("If the board does not respond within 1-2 seconds, unplug and re-plug the USB connector.") # Spin waiting for a device to show up -while True: - portlist = [] - patterns = args.port.split(",") - # on unix-like platforms use glob to support wildcard ports. This allows - # the use of /dev/serial/by-id/usb-3D_Robotics on Linux, which prevents the upload from - # causing modem hangups etc - if "linux" in _platform or "darwin" in _platform: - import glob - for pattern in patterns: - portlist += glob.glob(pattern) - else: - portlist = patterns +try: + while True: + portlist = [] + patterns = args.port.split(",") + # on unix-like platforms use glob to support wildcard ports. This allows + # the use of /dev/serial/by-id/usb-3D_Robotics on Linux, which prevents the upload from + # causing modem hangups etc + if "linux" in _platform or "darwin" in _platform: + import glob + for pattern in patterns: + portlist += glob.glob(pattern) + else: + portlist = patterns - for port in portlist: + for port in portlist: - #print("Trying %s" % port) + #print("Trying %s" % port) - # create an uploader attached to the port - try: - if "linux" in _platform: - # Linux, don't open Mac OS and Win ports - if not "COM" in port and not "tty.usb" in port: - up = uploader(port, args.baud) - elif "darwin" in _platform: - # OS X, don't open Windows and Linux ports - if not "COM" in port and not "ACM" in port: - up = uploader(port, args.baud) - elif "win" in _platform: - # Windows, don't open POSIX ports - if not "/" in port: - up = uploader(port, args.baud) - except Exception: - # open failed, rate-limit our attempts - time.sleep(0.05) + # create an uploader attached to the port + try: + if "linux" in _platform: + # Linux, don't open Mac OS and Win ports + if not "COM" in port and not "tty.usb" in port: + up = uploader(port, args.baud) + elif "darwin" in _platform: + # OS X, don't open Windows and Linux ports + if not "COM" in port and not "ACM" in port: + up = uploader(port, args.baud) + elif "win" in _platform: + # Windows, don't open POSIX ports + if not "/" in port: + up = uploader(port, args.baud) + except Exception: + # open failed, rate-limit our attempts + time.sleep(0.05) - # and loop to the next port - continue + # and loop to the next port + continue - # port is open, try talking to it - try: - # identify the bootloader - up.identify() - print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port)) + # port is open, try talking to it + try: + # identify the bootloader + up.identify() + print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port)) - except Exception: - # most probably a timeout talking to the port, no bootloader, try to reboot the board - print("attempting reboot on %s..." % port) - print("if the board does not respond, unplug and re-plug the USB connector.") - up.send_reboot() + except Exception: + # most probably a timeout talking to the port, no bootloader, try to reboot the board + print("attempting reboot on %s..." % port) + print("if the board does not respond, unplug and re-plug the USB connector.") + up.send_reboot() - # wait for the reboot, without we might run into Serial I/O Error 5 - time.sleep(0.5) + # wait for the reboot, without we might run into Serial I/O Error 5 + time.sleep(0.5) - # always close the port - up.close() - continue + # always close the port + up.close() + continue - try: - # ok, we have a bootloader, try flashing it - up.upload(fw) + try: + # ok, we have a bootloader, try flashing it + up.upload(fw) - except RuntimeError as ex: + except RuntimeError as ex: - # print the error - print("\nERROR: %s" % ex.args) + # print the error + print("\nERROR: %s" % ex.args) - finally: - # always close the port - up.close() + finally: + # always close the port + up.close() - # we could loop here if we wanted to wait for more boards... - sys.exit(0) + # we could loop here if we wanted to wait for more boards... + sys.exit(0) + + #Rate-limit retries to prevent spin-lock from hogging the CPU + time.sleep(0.5) + +#CTRL+C aborts the upload/spin-lock by interrupt mechanics +except KeyboardInterrupt: + print("\n Upload aborted by user.") + sys.exit(0) \ No newline at end of file From 74177a2688f1163ec9659f3847d7cd17d0eb325f Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 26 Feb 2015 11:14:54 +0100 Subject: [PATCH 034/151] px4_uploader: Push program bytes faster by using bigger blocks --- Tools/px_uploader.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 8042ec34ab..c9a142ddf4 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -169,8 +169,8 @@ class uploader(object): INFO_BOARD_REV = b'\x03' # board revision INFO_FLASH_SIZE = b'\x04' # max firmware size in bytes - PROG_MULTI_MAX = 60 # protocol max is 255, must be multiple of 4 - READ_MULTI_MAX = 60 # protocol max is 255, something overflows with >= 64 + PROG_MULTI_MAX = 252 # protocol max is 255, must be multiple of 4 + READ_MULTI_MAX = 252 # protocol max is 255 NSH_INIT = bytearray(b'\x0d\x0d\x0d') NSH_REBOOT_BL = b"reboot -b\n" From 6d2e8b9d08ec3ce811cd0fc4cc5a1c561dee2195 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Feb 2015 13:07:43 +0100 Subject: [PATCH 035/151] Reduce delay between board search attempts, code style --- Tools/px_uploader.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index c9a142ddf4..f4e317cfa8 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -552,7 +552,7 @@ try: print("if the board does not respond, unplug and re-plug the USB connector.") up.send_reboot() - # wait for the reboot, without we might run into Serial I/O Error 5 + # wait for the reboot, without we might run into Serial I/O Error 5 time.sleep(0.5) # always close the port @@ -575,10 +575,10 @@ try: # we could loop here if we wanted to wait for more boards... sys.exit(0) - #Rate-limit retries to prevent spin-lock from hogging the CPU - time.sleep(0.5) + # Delay retries to < 20 Hz to prevent spin-lock from hogging the CPU + time.sleep(0.05) -#CTRL+C aborts the upload/spin-lock by interrupt mechanics +# CTRL+C aborts the upload/spin-lock by interrupt mechanics except KeyboardInterrupt: print("\n Upload aborted by user.") - sys.exit(0) \ No newline at end of file + sys.exit(0) From 6d1df8f721f5410b566f616a807bfd9060e830bd Mon Sep 17 00:00:00 2001 From: Vasily Dybala Date: Wed, 25 Feb 2015 22:49:02 +0300 Subject: [PATCH 036/151] Update land detector parameters at startup. --- src/modules/land_detector/FixedwingLandDetector.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 8e5bcf7bae..e26954d1a6 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -66,6 +66,8 @@ void FixedwingLandDetector::initialize() // Subscribe to local position and airspeed data _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); _airspeedSub = orb_subscribe(ORB_ID(airspeed)); + + updateParameterCache(true); } void FixedwingLandDetector::updateSubscriptions() From 97d31f667e6fcfe88fb23220b8d1c0ff938a598b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marcel=20St=C3=BCttgen?= Date: Fri, 27 Feb 2015 18:21:57 +0100 Subject: [PATCH 037/151] modified default pwm values and added comments to the startup scripts --- ROMFS/px4fmu_common/init.d/50001_rover | 4 ++-- ROMFS/px4fmu_common/init.d/rc.rover_defaults | 24 ++++++++++++++------ 2 files changed, 19 insertions(+), 9 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/50001_rover b/ROMFS/px4fmu_common/init.d/50001_rover index e9df3904b9..c2f94705cd 100644 --- a/ROMFS/px4fmu_common/init.d/50001_rover +++ b/ROMFS/px4fmu_common/init.d/50001_rover @@ -3,8 +3,8 @@ # Generic rover # +#load some defaults e.g. PWM values sh /etc/init.d/rc.rover_defaults +#choose a mixer, for rover control we need a plain passthrough to the servos set MIXER IO_pass - -set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_defaults b/ROMFS/px4fmu_common/init.d/rc.rover_defaults index fc68472a6a..bda3a9a66e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.rover_defaults @@ -2,12 +2,22 @@ set VEHICLE_TYPE rover -if [ $AUTOCNF == yes ] -then - # param set MC_ROLL_P 7.0 -fi +# i dont think AUTOCNF is needed here, can it be removed? +#if [ $AUTOCNF == yes ] +#then +# # param set MC_ROLL_P 7.0 +#fi + +#PWM Hz set PWM_RATE 50 -set PWM_DISARMED 1100 -set PWM_MIN 1100 -set PWM_MAX 1900 + +#PWW default value for "disarmed" mode +set PWM_DISARMED 1500 + +#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 From 81648f84cd97ec865ba7f20cebeb6285f7ef6d18 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 27 Feb 2015 15:49:40 +0100 Subject: [PATCH 038/151] 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 039/151] 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 040/151] 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 041/151] 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 042/151] 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 043/151] 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 044/151] 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 045/151] 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 046/151] 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 047/151] 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 048/151] 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 049/151] 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 050/151] 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 051/151] 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 052/151] 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 053/151] 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 054/151] 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 055/151] 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 056/151] 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 057/151] 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 058/151] 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 059/151] 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 060/151] 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 061/151] 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 062/151] 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 063/151] 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 064/151] 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 065/151] 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 066/151] 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 067/151] 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 068/151] 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 069/151] 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 070/151] 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 071/151] 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 072/151] 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 073/151] 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 074/151] 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 075/151] 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 076/151] 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 077/151] 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 078/151] 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 079/151] 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 080/151] 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 081/151] 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 082/151] 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 083/151] 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 084/151] 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 085/151] 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 086/151] 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 087/151] 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 088/151] 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 089/151] 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 090/151] 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 091/151] 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 092/151] 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 093/151] 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 094/151] 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 095/151] 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 096/151] - 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 097/151] - 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 098/151] 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 099/151] 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 100/151] 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 101/151] 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 102/151] 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 103/151] 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 104/151] 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 105/151] 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 106/151] 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 107/151] 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 108/151] 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 109/151] 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 110/151] 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 111/151] 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 112/151] 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 113/151] 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 114/151] 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 115/151] 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 116/151] 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 117/151] 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 118/151] 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 119/151] 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 120/151] 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 121/151] 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 122/151] 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 123/151] 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 124/151] 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 125/151] 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 126/151] 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 127/151] - 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 128/151] 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 129/151] 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 130/151] 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 131/151] - 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 132/151] 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 133/151] 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 134/151] 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 135/151] 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 136/151] 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 137/151] 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 138/151] 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 139/151] 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 140/151] 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 141/151] 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 142/151] 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 143/151] 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 144/151] 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 145/151] 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 146/151] 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. * From a82bcbf1d70c71f0e2b282a86029ad614a02ba6d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Mar 2015 09:04:01 +0100 Subject: [PATCH 147/151] Fix IO update when safety can not be set to on. From @zottgrammes --- ROMFS/px4fmu_common/init.d/rcS | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 9118f26013..1d21d77727 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -200,8 +200,13 @@ then if px4io start then + # try to safe px4 io so motor outputs dont go crazy if px4io safety_on then + # success! no-op + else + # px4io did not respond to the safety command + px4io stop fi fi From 96ab880c61f2ade6bce25caa43b33c877fccb688 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Mar 2015 19:20:14 +0100 Subject: [PATCH 148/151] Set build environment variables to enable retrieval of binaries in reports --- .travis.yml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index a994d34715..5c07b2ae66 100644 --- a/.travis.yml +++ b/.travis.yml @@ -37,6 +37,8 @@ env: # Email address: $PX4_EMAIL - secure: "ei3hKAw6Pk+vEkQBI5Y2Ak74BRAaXcK2UHVnVadviBHI4EVPwn1YGP6A4Y0wnLe4U7ETTl0UiijRoVxyDW0Mq896Pv0siw02amNpjSZZYu+RfN1+//MChB48OxsLDirUdHVrULhl/bOARM02h2Bg28jDE2g7IqmJwg3em3oMbjU=" - PX4_REPORT=report.txt + - PX4_BUILD_DATE=`date "+%Y-%m-%d"` + - BUILD_URI=https://px4-travis.s3.amazonaws.com/archives/Firmware/$TRAVIS_BRANCH/$PX4_BUILD_DATE-$TRAVIS_BUILD_ID/Firmware.zip script: - arm-none-eabi-gcc --version @@ -58,7 +60,7 @@ after_script: # upload newest build for this branch with s3 index - ./CI-Tools/s3cmd-put Images/px4*.px4 CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/ # archive newest build by date with s3 index - - ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/`date "+%Y-%m-%d"`-$TRAVIS_BUILD_ID/ + - ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/$PX4_BUILD_DATE-$TRAVIS_BUILD_ID/ - ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Firmware/$TRAVIS_BRANCH/ # upload top level index.html and timestamp.html - ./CI-Tools/s3cmd-put CI-Tools/index.html index.html From 01b0270c56a63d4cb9ef4c159a6c0b626cea6fe1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Mar 2015 19:22:46 +0100 Subject: [PATCH 149/151] Upload latest binary to s3 storage --- .travis.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.travis.yml b/.travis.yml index 5c07b2ae66..b8d5be581e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -61,6 +61,7 @@ after_script: - ./CI-Tools/s3cmd-put Images/px4*.px4 CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/ # archive newest build by date with s3 index - ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/$PX4_BUILD_DATE-$TRAVIS_BUILD_ID/ + - ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/ - ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Firmware/$TRAVIS_BRANCH/ # upload top level index.html and timestamp.html - ./CI-Tools/s3cmd-put CI-Tools/index.html index.html From c5851632de06e982b7f6cd24755349f2e04617a1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Mar 2015 20:05:11 +0100 Subject: [PATCH 150/151] Simplify Travis build ID naming on server --- .travis.yml | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/.travis.yml b/.travis.yml index b8d5be581e..33e2c23508 100644 --- a/.travis.yml +++ b/.travis.yml @@ -37,8 +37,7 @@ env: # Email address: $PX4_EMAIL - secure: "ei3hKAw6Pk+vEkQBI5Y2Ak74BRAaXcK2UHVnVadviBHI4EVPwn1YGP6A4Y0wnLe4U7ETTl0UiijRoVxyDW0Mq896Pv0siw02amNpjSZZYu+RfN1+//MChB48OxsLDirUdHVrULhl/bOARM02h2Bg28jDE2g7IqmJwg3em3oMbjU=" - PX4_REPORT=report.txt - - PX4_BUILD_DATE=`date "+%Y-%m-%d"` - - BUILD_URI=https://px4-travis.s3.amazonaws.com/archives/Firmware/$TRAVIS_BRANCH/$PX4_BUILD_DATE-$TRAVIS_BUILD_ID/Firmware.zip + - BUILD_URI=https://px4-travis.s3.amazonaws.com/archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/Firmware.zip script: - arm-none-eabi-gcc --version @@ -60,8 +59,7 @@ after_script: # upload newest build for this branch with s3 index - ./CI-Tools/s3cmd-put Images/px4*.px4 CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/ # archive newest build by date with s3 index - - ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/$PX4_BUILD_DATE-$TRAVIS_BUILD_ID/ - - ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/ + - ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/ - ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Firmware/$TRAVIS_BRANCH/ # upload top level index.html and timestamp.html - ./CI-Tools/s3cmd-put CI-Tools/index.html index.html From 9d4b4ab0492c4fb2f42ee1e6940c8f85c473f2ad Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Mar 2015 22:43:42 +0100 Subject: [PATCH 151/151] Travis: better output of built binaries --- .travis.yml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.travis.yml b/.travis.yml index 33e2c23508..d659bf6508 100644 --- a/.travis.yml +++ b/.travis.yml @@ -64,6 +64,9 @@ after_script: # upload top level index.html and timestamp.html - ./CI-Tools/s3cmd-put CI-Tools/index.html index.html - ./CI-Tools/s3cmd-put CI-Tools/timestamp.html timestamp.html + - echo "" + - echo "Binaries have been posted to:" + - echo https://px4-travis.s3.amazonaws.com/archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/Firmware.zip deploy: provider: releases