fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 15:28:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'navigator_new' into navigator_new_vector

Browse Source
This commit is contained in:
Anton Babushkin
2014-01-06 08:08:35 +01:00
parent dae5c83842 40196275d0
commit 300d891d76
32 changed files with 1173 additions and 253 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Documentation/fw_landing.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 17 KiB

makefiles/config_px4fmu-v1_backside.mk
+1 -15
View File
@@ -25,14 +25,10 @@ MODULES += drivers/bma180
MODULES += drivers/mpu6000
MODULES += drivers/hmc5883
MODULES += drivers/ms5611
MODULES += drivers/mb12xx
MODULES += drivers/gps
MODULES += drivers/hil
MODULES += drivers/hott/hott_telemetry
MODULES += drivers/hott/hott_sensors
MODULES += drivers/blinkm
MODULES += drivers/rgbled
MODULES += drivers/mkblctrl
MODULES += drivers/roboclaw
MODULES += drivers/airspeed
MODULES += drivers/ets_airspeed
@@ -71,22 +67,11 @@ MODULES += modules/gpio_led
#
# Estimation modules (EKF/ SO3 / other filters)
#
#MODULES += modules/attitude_estimator_ekf
MODULES += modules/att_pos_estimator_ekf
#MODULES += modules/position_estimator_inav
MODULES += examples/flow_position_estimator
MODULES += modules/attitude_estimator_so3
#
# Vehicle Control
#
#MODULES += modules/segway # XXX Needs GCC 4.7 fix
#MODULES += modules/fw_pos_control_l1
#MODULES += modules/fw_att_control
#MODULES += modules/multirotor_att_control
#MODULES += modules/multirotor_pos_control
#MODULES += examples/flow_position_control
#MODULES += examples/flow_speed_control
MODULES += modules/fixedwing_backside
#
@@ -119,6 +104,7 @@ MODULES += lib/ecl
MODULES += lib/external_lgpl
MODULES += lib/geo
MODULES += lib/conversion
MODULES += lib/launchdetection
#
# Demo apps
makefiles/config_px4fmu-v1_default.mk
+5 -6
View File
@@ -21,7 +21,7 @@ MODULES += drivers/px4fmu
MODULES += drivers/boards/px4fmu-v1
MODULES += drivers/ardrone_interface
MODULES += drivers/l3gd20
MODULES += drivers/bma180
#MODULES += drivers/bma180
MODULES += drivers/mpu6000
MODULES += drivers/hmc5883
MODULES += drivers/ms5611
@@ -33,7 +33,6 @@ MODULES += drivers/hott/hott_sensors
MODULES += drivers/blinkm
MODULES += drivers/rgbled
MODULES += drivers/mkblctrl
MODULES += drivers/roboclaw
MODULES += drivers/airspeed
MODULES += drivers/ets_airspeed
MODULES += drivers/meas_airspeed
@@ -75,18 +74,17 @@ MODULES += modules/attitude_estimator_ekf
MODULES += modules/attitude_estimator_so3
MODULES += modules/att_pos_estimator_ekf
MODULES += modules/position_estimator_inav
MODULES += examples/flow_position_estimator
#MODULES += examples/flow_position_estimator
#
# Vehicle Control
#
#MODULES += modules/segway # XXX Needs GCC 4.7 fix
MODULES += modules/fw_pos_control_l1
MODULES += modules/fw_att_control
MODULES += modules/mc_att_control_vector
MODULES += modules/mc_pos_control
MODULES += examples/flow_position_control
MODULES += examples/flow_speed_control
#MODULES += examples/flow_position_control
#MODULES += examples/flow_speed_control
#
# Logging
@@ -118,6 +116,7 @@ MODULES += lib/ecl
MODULES += lib/external_lgpl
MODULES += lib/geo
MODULES += lib/conversion
MODULES += lib/launchdetection
#
# Demo apps
makefiles/config_px4fmu-v2_default.mk
+1
View File
@@ -116,6 +116,7 @@ MODULES += lib/ecl
MODULES += lib/external_lgpl
MODULES += lib/geo
MODULES += lib/conversion
MODULES += lib/launchdetection
#
# Demo apps
makefiles/toolchain_gnu-arm-eabi.mk
+1 -2
View File
@@ -138,8 +138,7 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \
-Wold-style-declaration \
-Wmissing-parameter-type \
-Wmissing-prototypes \
-Wnested-externs \
-Wunsuffixed-float-constants
-Wnested-externs
# C++-specific warnings
#
src/drivers/boards/px4fmu-v1/board_config.h
+3 -3
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-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
@@ -34,7 +34,7 @@
/**
* @file board_config.h
*
* PX4FMU internal definitions
* PX4FMUv1 internal definitions
*/
#pragma once
@@ -180,7 +180,7 @@ __BEGIN_DECLS
#define HRT_TIMER 1 /* use timer1 for the HRT */
#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */
#define HRT_PPM_CHANNEL 3 /* use capture/compare channel 3 */
#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10)
#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF1|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10)
/****************************************************************************************************
* Public Types
src/drivers/boards/px4fmu-v2/board_config.h
+13 -13
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2013, 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
@@ -34,7 +34,7 @@
/**
* @file board_config.h
*
* PX4FMU internal definitions
* PX4FMUv2 internal definitions
*/
#pragma once
@@ -82,21 +82,21 @@ __BEGIN_DECLS
#define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15)
/* Data ready pins off */
#define GPIO_GYRO_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN0)
#define GPIO_MAG_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN1)
#define GPIO_ACCEL_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN4)
#define GPIO_GYRO_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN0)
#define GPIO_MAG_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN1)
#define GPIO_ACCEL_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN4)
#define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15)
/* SPI1 off */
#define GPIO_SPI1_SCK_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN5)
#define GPIO_SPI1_MISO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN6)
#define GPIO_SPI1_MOSI_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN7)
#define GPIO_SPI1_SCK_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN5)
#define GPIO_SPI1_MISO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN6)
#define GPIO_SPI1_MOSI_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN7)
/* SPI1 chip selects off */
#define GPIO_SPI_CS_GYRO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN13)
#define GPIO_SPI_CS_ACCEL_MAG_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN15)
#define GPIO_SPI_CS_BARO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTD|GPIO_PIN7)
#define GPIO_SPI_CS_MPU (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2)
#define GPIO_SPI_CS_GYRO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN13)
#define GPIO_SPI_CS_ACCEL_MAG_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN15)
#define GPIO_SPI_CS_BARO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTD|GPIO_PIN7)
#define GPIO_SPI_CS_MPU (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2)
/* SPI chip selects */
#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
@@ -177,7 +177,7 @@ __BEGIN_DECLS
*
* PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED)
*/
#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
/* High-resolution timer */
#define HRT_TIMER 8 /* use timer8 for the HRT */
src/drivers/boards/px4io-v1/board_config.h
+2 -2
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-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
@@ -92,4 +92,4 @@
#define HRT_TIMER 1 /* use timer1 for the HRT */
#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */
#define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */
#define GPIO_PPM_IN GPIO_TIM1_CH1IN
#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_PORTE|GPIO_PIN9)
src/drivers/boards/px4io-v2/board_config.h
+3 -3
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2013, 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
@@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file px4iov2_internal.h
* @file board_config.h
*
* PX4IOV2 internal definitions
*/
@@ -122,7 +122,7 @@
#define HRT_TIMER 1 /* use timer1 for the HRT */
#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */
#define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */
#define GPIO_PPM_IN GPIO_TIM1_CH1IN
#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_PORTE|GPIO_PIN9)
/* LED definitions ******************************************************************/
/* PX4 has two LEDs that we will encode as: */
src/drivers/drv_rc_input.h
+1 -1
View File
@@ -60,7 +60,7 @@
/**
* Maximum number of R/C input channels in the system. S.Bus has up to 18 channels.
*/
#define RC_INPUT_MAX_CHANNELS 20
#define RC_INPUT_MAX_CHANNELS 18
/**
* Input signal type, value is a control position from zero to 100
src/lib/geo/geo.c
-24
View File
@@ -503,27 +503,3 @@ __EXPORT float _wrap_360(float bearing)
return bearing;
}
__EXPORT bool inside_geofence(const struct vehicle_global_position_s *vehicle, const struct fence_s *fence)
{
/* Adaptation of algorithm originally presented as
* PNPOLY - Point Inclusion in Polygon Test
* W. Randolph Franklin (WRF) */
unsigned int i, j, vertices = fence->count;
bool c = false;
double lat = vehicle->lat / 1e7d;
double lon = vehicle->lon / 1e7d;
// skip vertex 0 (return point)
for (i = 0, j = vertices - 1; i < vertices; j = i++)
if (((fence->vertices[i].lon) >= lon != (fence->vertices[j].lon >= lon)) &&
(lat <= (fence->vertices[j].lat - fence->vertices[i].lat) * (lon - fence->vertices[i].lon) /
(fence->vertices[j].lon - fence->vertices[i].lon) + fence->vertices[i].lat))
c = !c;
return c;
}
src/lib/geo/geo.h
-11
View File
@@ -143,15 +143,4 @@ __EXPORT float _wrap_360(float bearing);
__EXPORT float _wrap_pi(float bearing);
__EXPORT float _wrap_2pi(float bearing);
/**
* Return whether craft is inside geofence.
*
* Calculate whether point is inside arbitrary polygon
* @param craft pointer craft coordinates
* @param fence pointer to array of coordinates, one per vertex. First and last vertex are assumed connected
* @return true: craft is inside fence, false:craft is outside fence
*/
__EXPORT bool inside_geofence(const struct vehicle_global_position_s *craft, const struct fence_s *fence);
__END_DECLS
src/lib/launchdetection/CatapultLaunchMethod.cpp
+90
View File
@@ -0,0 +1,90 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). 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 documentation4 and/or other materials provided with the
* distribution.
* 3. Neither the name ECL 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 CatapultLaunchMethod.cpp
* Catpult Launch detection
*
* Authors and acknowledgements in header.
*/
#include "CatapultLaunchMethod.h"
#include <systemlib/err.h>
CatapultLaunchMethod::CatapultLaunchMethod() :
last_timestamp(0),
integrator(0.0f),
launchDetected(false),
threshold_accel(NULL, "LAUN_CAT_A", false),
threshold_time(NULL, "LAUN_CAT_T", false)
{
}
CatapultLaunchMethod::~CatapultLaunchMethod() {
}
void CatapultLaunchMethod::update(float accel_x)
{
float dt = (float)hrt_elapsed_time(&last_timestamp) * 1e-6f;
last_timestamp = hrt_absolute_time();
if (accel_x > threshold_accel.get()) {
integrator += accel_x * dt;
// warnx("*** integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f",
// (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt);
if (integrator > threshold_accel.get() * threshold_time.get()) {
launchDetected = true;
}
} else {
// warnx("integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f",
// (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt);
/* reset integrator */
integrator = 0.0f;
launchDetected = false;
}
}
bool CatapultLaunchMethod::getLaunchDetected()
{
return launchDetected;
}
void CatapultLaunchMethod::updateParams()
{
threshold_accel.update();
threshold_time.update();
}
src/lib/launchdetection/CatapultLaunchMethod.h
+71
View File
@@ -0,0 +1,71 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). 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 documentation4 and/or other materials provided with the
* distribution.
* 3. Neither the name ECL 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 CatapultLaunchMethod.h
* Catpult Launch detection
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#ifndef CATAPULTLAUNCHMETHOD_H_
#define CATAPULTLAUNCHMETHOD_H_
#include "LaunchMethod.h"
#include <drivers/drv_hrt.h>
#include <controllib/block/BlockParam.hpp>
class CatapultLaunchMethod : public LaunchMethod
{
public:
CatapultLaunchMethod();
~CatapultLaunchMethod();
void update(float accel_x);
bool getLaunchDetected();
void updateParams();
private:
hrt_abstime last_timestamp;
// float threshold_accel_raw;
// float threshold_time;
float integrator;
bool launchDetected;
control::BlockParamFloat threshold_accel;
control::BlockParamFloat threshold_time;
};
#endif /* CATAPULTLAUNCHMETHOD_H_ */
src/lib/launchdetection/LaunchDetector.cpp
+96
View File
@@ -0,0 +1,96 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 launchDetection.cpp
* Auto Detection for different launch methods (e.g. catapult)
*
* Authors and acknowledgements in header.
*/
#include "LaunchDetector.h"
#include "CatapultLaunchMethod.h"
#include <systemlib/err.h>
LaunchDetector::LaunchDetector() :
launchdetection_on(NULL, "LAUN_ALL_ON", false),
throttlePreTakeoff(NULL, "LAUN_THR_PRE", false)
{
/* init all detectors */
launchMethods[0] = new CatapultLaunchMethod();
/* update all parameters of all detectors */
updateParams();
}
LaunchDetector::~LaunchDetector()
{
}
void LaunchDetector::update(float accel_x)
{
if (launchdetection_on.get() == 1) {
for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
launchMethods[i]->update(accel_x);
}
}
}
bool LaunchDetector::getLaunchDetected()
{
if (launchdetection_on.get() == 1) {
for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
if(launchMethods[i]->getLaunchDetected()) {
return true;
}
}
}
return false;
}
void LaunchDetector::updateParams() {
warnx(" LaunchDetector::updateParams()");
launchdetection_on.update();
throttlePreTakeoff.update();
for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
launchMethods[i]->updateParams();
warnx("updating component %d", i);
}
warnx(" LaunchDetector::updateParams() ended");
}
src/lib/launchdetection/LaunchDetector.h
+75
View File
@@ -0,0 +1,75 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). 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 documentation4 and/or other materials provided with the
* distribution.
* 3. Neither the name ECL 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 LaunchDetector.h
* Auto Detection for different launch methods (e.g. catapult)
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#ifndef LAUNCHDETECTOR_H
#define LAUNCHDETECTOR_H
#include <stdbool.h>
#include <stdint.h>
#include "LaunchMethod.h"
#include <controllib/block/BlockParam.hpp>
class __EXPORT LaunchDetector
{
public:
LaunchDetector();
~LaunchDetector();
void update(float accel_x);
bool getLaunchDetected();
void updateParams();
bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); };
float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); }
// virtual bool getLaunchDetected();
protected:
private:
LaunchMethod* launchMethods[1];
control::BlockParamInt launchdetection_on;
control::BlockParamFloat throttlePreTakeoff;
};
#endif // LAUNCHDETECTOR_H
src/lib/launchdetection/LaunchMethod.h
+54
View File
@@ -0,0 +1,54 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). 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 documentation4 and/or other materials provided with the
* distribution.
* 3. Neither the name ECL 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 LaunchMethod.h
* Base class for different launch methods
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#ifndef LAUNCHMETHOD_H_
#define LAUNCHMETHOD_H_
class LaunchMethod
{
public:
virtual void update(float accel_x) = 0;
virtual bool getLaunchDetected() = 0;
virtual void updateParams() = 0;
protected:
private:
};
#endif /* LAUNCHMETHOD_H_ */
src/lib/launchdetection/launchdetection_params.c
+72
View File
@@ -0,0 +1,72 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* 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 launchdetection_params.c
*
* Parameters for launchdetection
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/*
* Launch detection parameters, accessible via MAVLink
*
*/
/* Catapult Launch detection */
// @DisplayName Switch to enable launchdetection
// @Description if set to 1 launchdetection is enabled
// @Range 0 or 1
PARAM_DEFINE_INT32(LAUN_ALL_ON, 0);
// @DisplayName Catapult Accelerometer Threshold
// @Description LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection
// @Range > 0
PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f);
// @DisplayName Catapult Time Threshold
// @Description LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection
// @Range > 0, in seconds
PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f);
// @DisplayName Throttle setting while detecting the launch
// @Description The throttle is set to this value while the system is waiting for the takeoff
// @Range 0 to 1
PARAM_DEFINE_FLOAT(LAUN_THR_PRE, 0.0f);
src/lib/launchdetection/module.mk
+40
View File
@@ -0,0 +1,40 @@
############################################################################
#
# Copyright (c) 2013 Estimation and Control Library (ECL). 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.
#
############################################################################
#
# Launchdetection Library
#
SRCS = LaunchDetector.cpp \
CatapultLaunchMethod.cpp \
launchdetection_params.c
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+89 -32
View File
@@ -76,6 +76,7 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/navigation_capabilities.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/parameter_update.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
@@ -85,11 +86,12 @@
#include <systemlib/systemlib.h>
#include <mathlib/mathlib.h>
#include <mavlink/mavlink_log.h>
#include <launchdetection/LaunchDetector.h>
#include <ecl/l1/ecl_l1_pos_controller.h>
#include <external_lgpl/tecs/tecs.h>
#include "landingslope.h"
/**
* L1 control app start / stop handling function
*
@@ -131,7 +133,7 @@ private:
int _control_mode_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */
int _accel_sub; /**< body frame accelerations */
int _sensor_combined_sub; /**< for body frame accelerations */
orb_advert_t _attitude_sp_pub; /**< attitude setpoint */
orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */
@@ -144,7 +146,7 @@ private:
struct vehicle_control_mode_s _control_mode; /**< vehicle status */
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */
struct accel_report _accel; /**< body frame accelerations */
struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */
perf_counter_t _loop_perf; /**< loop performance counter */
@@ -170,6 +172,11 @@ private:
bool land_motor_lim;
bool land_onslope;
/* takeoff/launch states */
bool launch_detected;
bool usePreTakeoffThrust;
bool launch_detection_message_sent;
/* Landingslope object */
Landingslope landingslope;
@@ -177,6 +184,9 @@ private:
/* heading hold */
float target_bearing;
/* Launch detection */
LaunchDetector launchDetector;
/* throttle and airspeed states */
float _airspeed_error; ///< airspeed error to setpoint in m/s
bool _airspeed_valid; ///< flag if a valid airspeed estimate exists
@@ -307,7 +317,7 @@ private:
/**
* Check for accel updates.
*/
void vehicle_accel_poll();
void vehicle_sensor_combined_poll();
/**
* Check for set triplet updates.
@@ -384,7 +394,11 @@ FixedwingPositionControl::FixedwingPositionControl() :
land_motor_lim(false),
land_onslope(false),
flare_curve_alt_last(0.0f),
_mavlink_fd(-1)
_mavlink_fd(-1),
launchDetector(),
launch_detected(false),
usePreTakeoffThrust(false),
launch_detection_message_sent(false)
{
/* safely initialize structs */
vehicle_attitude_s _att = {0};
@@ -395,7 +409,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
vehicle_control_mode_s _control_mode = {0};
vehicle_global_position_s _global_pos = {0};
mission_item_triplet_s _mission_item_triplet = {0};
accel_report _accel = {0};
sensor_combined_s _sensor_combined = {0};
@@ -554,6 +568,9 @@ FixedwingPositionControl::parameters_update()
_nav_capabilities.landing_flare_length = landingslope.flare_length();
navigation_capabilities_publish();
/* Update Launch Detector Parameters */
launchDetector.updateParams();
return OK;
}
@@ -623,14 +640,14 @@ FixedwingPositionControl::vehicle_attitude_poll()
}
void
FixedwingPositionControl::vehicle_accel_poll()
FixedwingPositionControl::vehicle_sensor_combined_poll()
{
/* check if there is a new position */
bool accel_updated;
orb_check(_accel_sub, &accel_updated);
bool sensors_updated;
orb_check(_sensor_combined_sub, &sensors_updated);
if (accel_updated) {
orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
if (sensors_updated) {
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
}
}
@@ -748,8 +765,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
float baro_altitude = _global_pos.alt;
/* filter speed and altitude for controller */
math::Vector<3> accel_body(_accel.x, _accel.y, _accel.z);
math::Vector<3> accel_earth = _R_nb.transposed() * accel_body;
math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
math::Vector<3> accel_earth = _R_nb * accel_body;
_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
float altitude_error = _mission_item_triplet.current.altitude - _global_pos.alt;
@@ -964,30 +981,57 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else if (mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
/* Perform launch detection */
// warnx("Launch detection running");
if(!launch_detected) { //do not do further checks once a launch was detected
if (launchDetector.launchDetectionEnabled()) {
// warnx("Launch detection enabled");
if(!launch_detection_message_sent) {
mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
launch_detection_message_sent = true;
}
launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
if (launchDetector.getLaunchDetected()) {
launch_detected = true;
mavlink_log_info(_mavlink_fd, "#audio: Takeoff");
}
} else {
/* no takeoff detection --> fly */
launch_detected = true;
}
}
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
if (altitude_error > 15.0f) {
if (launch_detected) {
usePreTakeoffThrust = false;
/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(1.3f * _parameters.airspeed_min),
_airspeed.indicated_airspeed_m_s, eas2tas,
true, math::max(math::radians(mission_item_triplet.current.pitch_min), math::radians(10.0f)),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
if (altitude_error > 15.0f) {
/* limit roll motion to ensure enough lift */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f));
/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(1.3f * _parameters.airspeed_min),
_airspeed.indicated_airspeed_m_s, eas2tas,
true, math::max(math::radians(mission_item_triplet.current.pitch_min), math::radians(10.0f)),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
/* limit roll motion to ensure enough lift */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f));
} else {
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, math::radians(_parameters.pitch_limit_min),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
}
} else {
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, math::radians(_parameters.pitch_limit_min),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
usePreTakeoffThrust = true;
}
}
@@ -1009,6 +1053,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
land_onslope = false;
}
/* reset takeoff/launch state */
if (mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) {
launch_detected = false;
usePreTakeoffThrust = false;
launch_detection_message_sent = false;
}
if (was_circle_mode && !_l1_control.circle_mode()) {
/* just kicked out of loiter, reset roll integrals */
_att_sp.roll_reset_integral = true;
@@ -1103,8 +1154,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
setpoint = false;
}
if (usePreTakeoffThrust) {
_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
}
else {
_att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max);
}
_att_sp.pitch_body = _tecs.get_pitch_demand();
_att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max);
return setpoint;
}
@@ -1123,7 +1180,7 @@ FixedwingPositionControl::task_main()
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_mission_item_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
@@ -1205,7 +1262,7 @@ FixedwingPositionControl::task_main()
vehicle_attitude_poll();
vehicle_setpoint_poll();
vehicle_accel_poll();
vehicle_sensor_combined_poll();
vehicle_airspeed_poll();
// vehicle_baro_poll();
src/modules/fw_pos_control_l1/landingslope.h
+8 -8
View File
@@ -46,16 +46,16 @@
class Landingslope
{
private:
//xxx: documentation of landing pending
float _landing_slope_angle_rad;
float _flare_relative_alt;
/* see Documentation/fw_landing.png for a plot of the landing slope */
float _landing_slope_angle_rad; /**< phi in the plot */
float _flare_relative_alt; /**< h_flare,rel in the plot */
float _motor_lim_horizontal_distance;
float _H1_virt;
float _H0;
float _d1;
float _H1_virt; /**< H1 in the plot */
float _H0; /**< h_flare,rel + H1 in the plot */
float _d1; /**< d1 in the plot */
float _flare_constant;
float _flare_length;
float _horizontal_slope_displacement;
float _flare_length; /**< d1 + delta d in the plot */
float _horizontal_slope_displacement; /**< delta d in the plot */
void calculateSlopeValues();
src/modules/navigator/geofence.cpp
+299
View File
@@ -0,0 +1,299 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Jean Cyr <jean.m.cyr@gmail.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
*
* 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 geofence.cpp
* Provides functions for handling the geofence
*/
#include "geofence.h"
#include <uORB/topics/vehicle_global_position.h>
#include <string.h>
#include <dataman/dataman.h>
#include <systemlib/err.h>
#include <stdlib.h>
#include <stdio.h>
#include <ctype.h>
#include <nuttx/config.h>
#include <unistd.h>
/* Oddly, ERROR is not defined for C++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
Geofence::Geofence() : _fence_pub(-1),
_altitude_min(0),
_altitude_max(0),
_verticesCount(0),
param_geofence_on(NULL, "GF_ON", false)
{
/* Load initial params */
updateParams();
}
Geofence::~Geofence()
{
}
bool Geofence::inside(const struct vehicle_global_position_s *vehicle)
{
double lat = vehicle->lat / 1e7d;
double lon = vehicle->lon / 1e7d;
float alt = vehicle->alt;
return inside(lat, lon, vehicle->alt);
}
bool Geofence::inside(double lat, double lon, float altitude)
{
/* Return true if geofence is disabled */
if (param_geofence_on.get() != 1)
return true;
if (valid()) {
if (!isEmpty()) {
/* Vertical check */
if (altitude > _altitude_max || altitude < _altitude_min)
return false;
/*Horizontal check */
/* Adaptation of algorithm originally presented as
* PNPOLY - Point Inclusion in Polygon Test
* W. Randolph Franklin (WRF) */
bool c = false;
struct fence_vertex_s temp_vertex_i;
struct fence_vertex_s temp_vertex_j;
/* Red until fence is finished */
for (unsigned i = 0, j = _verticesCount - 1; i < _verticesCount; j = i++) {
if (dm_read(DM_KEY_FENCE_POINTS, i, &temp_vertex_i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
break;
}
if (dm_read(DM_KEY_FENCE_POINTS, j, &temp_vertex_j, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
break;
}
// skip vertex 0 (return point)
if (((temp_vertex_i.lon) >= lon != (temp_vertex_j.lon >= lon)) &&
(lat <= (temp_vertex_j.lat - temp_vertex_i.lat) * (lon - temp_vertex_i.lon) /
(temp_vertex_j.lon - temp_vertex_i.lon) + temp_vertex_i.lat)) {
c = !c;
}
}
return c;
} else {
/* Empty fence --> accept all points */
return true;
}
} else {
/* Invalid fence --> accept all points */
return true;
}
}
bool
Geofence::valid()
{
// NULL fence is valid
if (isEmpty())
return true;
// Otherwise
if ((_verticesCount < 4) || (_verticesCount > GEOFENCE_MAX_VERTICES)) {
warnx("Fence must have at least 3 sides and not more than %d", GEOFENCE_MAX_VERTICES - 1);
return false;
}
return true;
}
void
Geofence::addPoint(int argc, char *argv[])
{
int ix, last;
double lon, lat;
struct fence_vertex_s vertex;
char *end;
if ((argc == 1) && (strcmp("-clear", argv[0]) == 0)) {
dm_clear(DM_KEY_FENCE_POINTS);
publishFence(0);
return;
}
if (argc < 3)
errx(1, "Specify: -clear | sequence latitude longitude [-publish]");
ix = atoi(argv[0]);
if (ix >= DM_KEY_FENCE_POINTS_MAX)
errx(1, "Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX);
lat = strtod(argv[1], &end);
lon = strtod(argv[2], &end);
last = 0;
if ((argc > 3) && (strcmp(argv[3], "-publish") == 0))
last = 1;
vertex.lat = (float)lat;
vertex.lon = (float)lon;
if (dm_write(DM_KEY_FENCE_POINTS, ix, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) == sizeof(vertex)) {
if (last)
publishFence((unsigned)ix + 1);
return;
}
errx(1, "can't store fence point");
}
void
Geofence::publishFence(unsigned vertices)
{
if (_fence_pub == -1)
_fence_pub = orb_advertise(ORB_ID(fence), &vertices);
else
orb_publish(ORB_ID(fence), _fence_pub, &vertices);
}
int
Geofence::loadFromFile(const char *filename)
{
FILE *fp;
char line[120];
int pointCounter = 0;
bool gotVertical = false;
const char commentChar = '#';
/* Make sure no data is left in the datamanager */
clearDm();
/* open the mixer definition file */
fp = fopen(GEOFENCE_FILENAME, "r");
if (fp == NULL) {
return ERROR;
}
/* create geofence points from valid lines and store in DM */
for (;;) {
/* get a line, bail on error/EOF */
if (fgets(line, sizeof(line), fp) == NULL)
break;
/* Trim leading whitespace */
size_t textStart = 0;
while((textStart < sizeof(line)/sizeof(char)) && isspace(line[textStart])) textStart++;
/* if the line starts with #, skip */
if (line[textStart] == commentChar)
continue;
if (gotVertical) {
/* Parse the line as a geofence point */
struct fence_vertex_s vertex;
/* if the line starts with DMS, this means that the coordinate is given as degree minute second instead of decimal degrees */
if (line[textStart] == 'D' && line[textStart + 1] == 'M' && line[textStart + 2] == 'S') {
/* Handle degree minute second format */
float lat_d, lat_m, lat_s, lon_d, lon_m, lon_s;
if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6)
return ERROR;
// warnx("Geofence DMS: %.5f %.5f %.5f ; %.5f %.5f %.5f", (double)lat_d, (double)lat_m, (double)lat_s, (double)lon_d, (double)lon_m, (double)lon_s);
vertex.lat = lat_d + lat_m/60.0f + lat_s/3600.0f;
vertex.lon = lon_d + lon_m/60.0f + lon_s/3600.0f;
} else {
/* Handle decimal degree format */
if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2)
return ERROR;
}
if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex))
return ERROR;
warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter, (double)vertex.lat, (double)vertex.lon);
pointCounter++;
} else {
/* Parse the line as the vertical limits */
if (sscanf(line, "%f %f", &_altitude_min, &_altitude_max) != 2)
return ERROR;
warnx("Geofence: alt min: %.4f, alt_max: %.4f", (double)_altitude_min, (double)_altitude_max);
gotVertical = true;
}
}
fclose(fp);
/* Check if import was successful */
if(gotVertical && pointCounter > 0)
{
_verticesCount = pointCounter;
warnx("Geofence: imported successfully");
} else {
warnx("Geofence: import error");
}
return ERROR;
}
int Geofence::clearDm()
{
dm_clear(DM_KEY_FENCE_POINTS);
}
void Geofence::updateParams()
{
param_geofence_on.update();
}
src/modules/navigator/geofence.h
+93
View File
@@ -0,0 +1,93 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Jean Cyr <jean.m.cyr@gmail.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
*
* 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 geofence.h
* Provides functions for handling the geofence
*/
#ifndef GEOFENCE_H_
#define GEOFENCE_H_
#include <uORB/topics/fence.h>
#include <controllib/block/BlockParam.hpp>
#define GEOFENCE_FILENAME "/fs/microsd/etc/geofence.txt"
class Geofence {
private:
orb_advert_t _fence_pub; /**< publish fence topic */
float _altitude_min;
float _altitude_max;
unsigned _verticesCount;
/* Params */
control::BlockParamInt param_geofence_on;
public:
Geofence();
~Geofence();
/**
* Return whether craft is inside geofence.
*
* Calculate whether point is inside arbitrary polygon
* @param craft pointer craft coordinates
* @param fence pointer to array of coordinates, one per vertex. First and last vertex are assumed connected
* @return true: craft is inside fence, false:craft is outside fence
*/
bool inside(const struct vehicle_global_position_s *craft);
bool inside(double lat, double lon, float altitude);
int clearDm();
bool valid();
/**
* Specify fence vertex position.
*/
void addPoint(int argc, char *argv[]);
void publishFence(unsigned vertices);
int loadFromFile(const char *filename);
bool isEmpty() {return _verticesCount == 0;}
void updateParams();
};
#endif /* GEOFENCE_H_ */
src/modules/navigator/geofence_params.c
+55
View File
@@ -0,0 +1,55 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* 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 geofence_params.c
*
* Parameters for geofence
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/*
* geofence parameters, accessible via MAVLink
*
*/
// @DisplayName Switch to enable geofence
// @Description if set to 1 geofence is enabled, defaults to 1 because geofence is only enabled when the geofence.txt file is present
// @Range 0 or 1
PARAM_DEFINE_INT32(GF_ON, 1);
src/modules/navigator/mission_feasibility_checker.cpp
+29 -11
View File
@@ -48,6 +48,7 @@
#include <stdio.h>
#include <fcntl.h>
#include <errno.h>
#include <uORB/topics/fence.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
@@ -61,7 +62,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab
}
bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nItems)
bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
{
/* Init if not done yet */
init();
@@ -74,39 +75,56 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_
if (isRotarywing)
return checkMissionFeasibleRotarywing(dm_current, nItems);
return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence);
else
return checkMissionFeasibleFixedwing(dm_current, nItems);
return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence);
}
bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nItems)
bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
{
return checkGeofence(dm_current, nItems);
return checkGeofence(dm_current, nMissionItems, geofence);
}
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nItems)
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
{
/* Update fixed wing navigation capabilites */
updateNavigationCapabilities();
// warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement);
return (checkFixedWingLanding(dm_current, nItems) && checkGeofence(dm_current, nItems));
return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence));
}
bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nItems)
bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
{
//xxx: check geofence
/* Check if all mission items are inside the geofence (if we have a valid geofence) */
if (geofence.valid()) {
for (size_t i = 0; i < nMissionItems; i++) {
static struct mission_item_s missionitem;
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(dm_current, i, &missionitem, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
}
if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) { //xxx: handle relative altitude
mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i);
return false;
}
}
}
return true;
}
bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nItems)
bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems)
{
/* Go through all mission items and search for a landing waypoint
* if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */
for (size_t i = 0; i < nItems; i++) {
for (size_t i = 0; i < nMissionItems; i++) {
static struct mission_item_s missionitem;
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(dm_current, i, &missionitem, len) != len) {
src/modules/navigator/mission_feasibility_checker.h
+6 -5
View File
@@ -43,6 +43,7 @@
#include <uORB/topics/mission.h>
#include <uORB/topics/navigation_capabilities.h>
#include <dataman/dataman.h>
#include "geofence.h"
class MissionFeasibilityChecker
@@ -57,15 +58,15 @@ private:
void init();
/* Checks for all airframes */
bool checkGeofence(dm_item_t dm_current, size_t nItems);
bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
/* Checks specific to fixedwing airframes */
bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nItems);
bool checkFixedWingLanding(dm_item_t dm_current, size_t nItems);
bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems);
void updateNavigationCapabilities();
/* Checks specific to rotarywing airframes */
bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nItems);
bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
public:
MissionFeasibilityChecker();
@@ -74,7 +75,7 @@ public:
/*
* Returns true if mission is feasible and false otherwise
*/
bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nItems);
bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
};
src/modules/navigator/module.mk
+3 -1
View File
@@ -40,6 +40,8 @@ MODULE_COMMAND = navigator
SRCS = navigator_main.cpp \
navigator_params.c \
navigator_mission.cpp \
mission_feasibility_checker.cpp
mission_feasibility_checker.cpp \
geofence.cpp \
geofence_params.c
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
src/modules/navigator/navigator_main.cpp
+61 -111
View File
@@ -75,9 +75,12 @@
#include <mathlib/mathlib.h>
#include <dataman/dataman.h>
#include <mavlink/mavlink_log.h>
#include <sys/types.h>
#include <sys/stat.h>
#include "navigator_mission.h"
#include "mission_feasibility_checker.h"
#include "geofence.h"
/* oddly, ERROR is not defined for c++ */
@@ -119,14 +122,14 @@ public:
void status();
/**
* Load fence parameters.
* Add point to geofence
*/
bool load_fence(unsigned vertices);
void add_fence_point(int argc, char *argv[]);
/**
* Specify fence vertex position.
* Load fence from file
*/
void fence_point(int argc, char *argv[]);
void load_fence_from_file(const char *filename);
private:
@@ -144,7 +147,6 @@ private:
int _capabilities_sub; /**< notification of vehicle capabilities updates */
orb_advert_t _triplet_pub; /**< publish position setpoint triplet */
orb_advert_t _fence_pub; /**< publish fence topic */
orb_advert_t _mission_result_pub; /**< publish mission result topic */
orb_advert_t _control_mode_pub; /**< publish vehicle control mode topic */
@@ -157,7 +159,9 @@ private:
perf_counter_t _loop_perf; /**< loop performance counter */
struct fence_s _fence; /**< storage for fence vertices */
Geofence _geofence;
bool _geofence_violation_warning_sent;
bool _fence_valid; /**< flag if fence is valid */
bool _inside_fence; /**< vehicle is inside fence */
@@ -268,12 +272,8 @@ private:
*/
void task_main() __attribute__((noreturn));
void publish_fence(unsigned vertices);
void publish_safepoints(unsigned points);
bool fence_valid(const struct fence_s &fence);
/**
* Functions that are triggered when a new state is entered.
*/
@@ -378,7 +378,6 @@ Navigator::Navigator() :
/* publications */
_triplet_pub(-1),
_fence_pub(-1),
_mission_result_pub(-1),
_control_mode_pub(-1),
@@ -396,10 +395,9 @@ Navigator::Navigator() :
_time_first_inside_orbit(0),
_set_nav_state_timestamp(0),
_need_takeoff(true),
_do_takeoff(false)
_do_takeoff(false),
_geofence_violation_warning_sent(false)
{
memset(&_fence, 0, sizeof(_fence));
_parameter_handles.min_altitude = param_find("NAV_MIN_ALT");
_parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD");
_parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD");
@@ -463,6 +461,8 @@ Navigator::parameters_update()
param_get(_parameter_handles.rtl_alt, &(_parameters.rtl_alt));
_mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled);
_geofence.updateParams();
}
void
@@ -499,7 +499,7 @@ Navigator::offboard_mission_update(bool isrotaryWing)
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count);
missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence);
_mission.set_offboard_dataman_id(offboard_mission.dataman_id);
_mission.set_current_offboard_mission_index(offboard_mission.current_index);
@@ -552,7 +552,20 @@ Navigator::task_main()
mavlink_log_info(_mavlink_fd, "[navigator] started");
_fence_valid = load_fence(GEOFENCE_MAX_VERTICES);
/* Try to load the geofence:
* if /fs/microsd/etc/geofence.txt load from this file
* else clear geofence data in datamanager
*/
struct stat buffer;
if( stat (GEOFENCE_FILENAME, &buffer) == 0 ) {
warnx("Try to load geofence.txt");
_geofence.loadFromFile(GEOFENCE_FILENAME);
} else {
if (_geofence.clearDm() > 0 )
warnx("Geofence cleared");
else
warnx("Could not clear geofence");
}
/*
* do subscriptions
@@ -756,6 +769,21 @@ Navigator::task_main()
on_mission_item_reached();
}
}
/* Check geofence violation */
if(!_geofence.inside(&_global_pos)) {
//xxx: publish geofence violation here (or change local flag depending on which app handles the flight termination)
/* Issue a warning about the geofence violation once */
if (!_geofence_violation_warning_sent)
{
mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation");
_geofence_violation_warning_sent = true;
}
} else {
/* Reset the _geofence_violation_warning_sent field */
_geofence_violation_warning_sent = false;
}
}
/* notify user about state changes */
@@ -814,9 +842,9 @@ Navigator::status()
}
if (_fence_valid) {
warnx("Geofence is valid");
warnx("Vertex longitude latitude");
for (unsigned i = 0; i < _fence.count; i++)
warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat);
// warnx("Vertex longitude latitude");
// for (unsigned i = 0; i < _fence.count; i++)
// warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat);
} else {
warnx("Geofence not set");
}
@@ -840,96 +868,6 @@ Navigator::status()
}
}
void
Navigator::publish_fence(unsigned vertices)
{
if (_fence_pub == -1)
_fence_pub = orb_advertise(ORB_ID(fence), &vertices);
else
orb_publish(ORB_ID(fence), _fence_pub, &vertices);
}
bool
Navigator::fence_valid(const struct fence_s &fence)
{
// NULL fence is valid
if (fence.count == 0) {
return true;
}
// Otherwise
if ((fence.count < 4) || (fence.count > GEOFENCE_MAX_VERTICES)) {
warnx("Fence must have at least 3 sides and not more than %d", GEOFENCE_MAX_VERTICES - 1);
return false;
}
return true;
}
bool
Navigator::load_fence(unsigned vertices)
{
struct fence_s temp_fence;
unsigned i;
for (i = 0; i < vertices; i++) {
if (dm_read(DM_KEY_FENCE_POINTS, i, temp_fence.vertices + i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
break;
}
}
temp_fence.count = i;
if (fence_valid(temp_fence))
memcpy(&_fence, &temp_fence, sizeof(_fence));
else
warnx("Invalid fence file, ignored!");
return _fence.count != 0;
}
void
Navigator::fence_point(int argc, char *argv[])
{
int ix, last;
double lon, lat;
struct fence_vertex_s vertex;
char *end;
if ((argc == 1) && (strcmp("-clear", argv[0]) == 0)) {
dm_clear(DM_KEY_FENCE_POINTS);
publish_fence(0);
return;
}
if (argc < 3)
errx(1, "Specify: -clear | sequence latitude longitude [-publish]");
ix = atoi(argv[0]);
if (ix >= DM_KEY_FENCE_POINTS_MAX)
errx(1, "Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX);
lat = strtod(argv[1], &end);
lon = strtod(argv[2], &end);
last = 0;
if ((argc > 3) && (strcmp(argv[3], "-publish") == 0))
last = 1;
vertex.lat = (float)lat;
vertex.lon = (float)lon;
if (dm_write(DM_KEY_FENCE_POINTS, ix, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) == sizeof(vertex)) {
if (last)
publish_fence((unsigned)ix + 1);
return;
}
errx(1, "can't store fence point");
}
StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
{
/* STATE_NONE */
@@ -1582,10 +1520,20 @@ bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const
}
}
void Navigator::add_fence_point(int argc, char *argv[])
{
_geofence.addPoint(argc, argv);
}
void Navigator::load_fence_from_file(const char *filename)
{
_geofence.loadFromFile(filename);
}
static void usage()
{
errx(1, "usage: navigator {start|stop|status|fence}");
errx(1, "usage: navigator {start|stop|status|fence|fencefile}");
}
int navigator_main(int argc, char *argv[])
@@ -1626,7 +1574,9 @@ int navigator_main(int argc, char *argv[])
navigator::g_navigator->status();
} else if (!strcmp(argv[1], "fence")) {
navigator::g_navigator->fence_point(argc - 2, argv + 2);
navigator::g_navigator->add_fence_point(argc - 2, argv + 2);
} else if (!strcmp(argv[1], "fencefile")) {
navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME);
} else {
usage();
src/modules/px4iofirmware/registers.c
+1 -1
View File
@@ -604,7 +604,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) {
disabled = true;
} else if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
} else if (REG_TO_SIGNED(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
count++;
}
src/modules/px4iofirmware/safety.c
-1
View File
@@ -45,7 +45,6 @@
#include "px4io.h"
static struct hrt_call arming_call;
static struct hrt_call heartbeat_call;
static struct hrt_call failsafe_call;
/*
src/modules/systemlib/mixer/mixer.cpp
-1
View File
@@ -171,7 +171,6 @@ NullMixer *
NullMixer::from_text(const char *buf, unsigned &buflen)
{
NullMixer *nm = nullptr;
const char *end = buf + buflen;
/* enforce that the mixer ends with space or a new line */
for (int i = buflen - 1; i >= 0; i--) {
src/systemcmds/preflight_check/preflight_check.c
+1 -2
View File
@@ -44,6 +44,7 @@
#include <string.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <systemlib/err.h>
#include <systemlib/param/param.h>
@@ -87,9 +88,7 @@ int preflight_check_main(int argc, char *argv[])
/* give the system some time to sample the sensors in the background */
usleep(150000);
/* ---- MAG ---- */
close(fd);
fd = open(MAG_DEVICE_PATH, 0);
if (fd < 0) {
warn("failed to open magnetometer - start with 'hmc5883 start' or 'lsm303d start'");