dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

lots' of header juggling and small changes to make mc att control compile for NuttX and ROS

This commit is contained in:
Thomas Gubler 2014-12-16 08:24:51 +01:00
parent 9980e44821
commit 9520983e08
20 changed files with 376 additions and 88 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
CMakeLists.txt
View File

@ -10,7 +10,9 @@ find_package(catkin REQUIRED COMPONENTS
rospy
std_msgs
message_generation
cmake_modules
)
find_package(Eigen REQUIRED)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@ -48,9 +50,17 @@ find_package(catkin REQUIRED COMPONENTS
## Generate messages in the 'msg' folder
add_message_files(
FILES
px4_msgs/rc_channels.msg
px4_msgs/vehicle_attitude.msg
px4_msgs/rc_channels.msg
rc_channels.msg
vehicle_attitude.msg
vehicle_attitude_setpoint.msg
manual_control_setpoint.msg
actuator_controls.msg
actuator_controls_0.msg
vehicle_rates_setpoint.msg
vehicle_attitude.msg
vehicle_control_mode.msg
actuator_armed.msg
parameter_update.msg
)
## Generate services in the 'srv' folder
@ -100,11 +110,19 @@ include_directories(
${catkin_INCLUDE_DIRS}
src/platforms
src/include
src/modules
src/
src/lib
${EIGEN_INCLUDE_DIRS}
)
## Declare a cpp library
add_library(px4
src/platforms/ros/px4_ros_impl.cpp
src/platforms/ros/perf_counter.cpp
src/platforms/ros/geo.cpp
src/lib/mathlib/math/Limits.cpp
src/platforms/ros/circuit_breaker.cpp
)
target_link_libraries(px4
@ -141,14 +159,16 @@ target_link_libraries(subscriber
px4
)
# add_executable(mc_att_control
# src/modules/mc_att_control/mc_att_control_main.cpp
# src/moudles/mc_att_control/mc_att_control_base.cpp)
# add_dependencies(mc_att_control px4_generate_messages_cpp)
# target_link_libraries(mc_att_control
# ${catkin_LIBRARIES}
# px4
# )
## MC Attitude Control
add_executable(mc_att_control
src/modules/mc_att_control/mc_att_control_main.cpp
src/modules/mc_att_control/mc_att_control.cpp
src/modules/mc_att_control/mc_att_control_base.cpp)
add_dependencies(mc_att_control px4_generate_messages_cpp)
target_link_libraries(mc_att_control
${catkin_LIBRARIES}
px4
)
#############

2
Makefile
View File

@ -224,7 +224,7 @@ updatesubmodules:
$(Q) (git submodule init)
$(Q) (git submodule update)
MSG_DIR = $(PX4_BASE)msg/px4_msgs
MSG_DIR = $(PX4_BASE)msg
MSG_TEMPLATE_DIR = $(PX4_BASE)msg/templates
TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics
TOPICS_TEMPORARY_DIR = $(BUILD_DIR)topics_temporary

4
makefiles/config_px4fmu-v2_default.mk
View File

@ -82,8 +82,8 @@ MODULES += modules/position_estimator_inav
# Vehicle Control
#
#MODULES += modules/segway # XXX Needs GCC 4.7 fix
MODULES += modules/fw_pos_control_l1
MODULES += modules/fw_att_control
#MODULES += modules/fw_pos_control_l1
#MODULES += modules/fw_att_control
MODULES += modules/mc_att_control
MODULES += modules/mc_pos_control

2
package.xml
View File

@ -43,9 +43,11 @@
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>eigen</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>eigen</run_depend>
<!-- The export tag contains other, unspecified, tags -->

2
src/include/px4.h
View File

@ -41,6 +41,8 @@
#include "../platforms/px4_includes.h"
#include "../platforms/px4_defines.h"
#ifdef __cplusplus
#include "../platforms/px4_middleware.h"
#include "../platforms/px4_nodehandle.h"
#include "../platforms/px4_subscriber.h"
#endif

5
src/lib/geo/geo.h
View File

@ -44,10 +44,7 @@
*/
#pragma once
#include "uORB/topics/fence.h"
#include "uORB/topics/vehicle_global_position.h"
#include <px4_defines.h>
__BEGIN_DECLS
#include "geo_lookup/geo_mag_declination.h"

12
src/lib/mathlib/math/Matrix.hpp
View File

@ -49,9 +49,8 @@
#ifdef CONFIG_ARCH_ARM
#include "../CMSIS/Include/arm_math.h"
#else
#include <math/eigen_math.h>
#include <platforms/ros/eigen_math.h>
#include <Eigen/Eigen>
#define M_PI_2_F 1.5707963267948966192f
#endif
namespace math
@ -122,6 +121,15 @@ public:
memcpy(data, d, sizeof(data));
}
#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
/**
* set data from boost::array
*/
void set(const boost::array<float, 9ul> d) {
set(static_cast<const float*>(d.data()));
}
#endif
/**
* access by index
*/

2
src/lib/mathlib/math/Vector.hpp
View File

@ -49,7 +49,7 @@
#ifdef CONFIG_ARCH_ARM
#include "../CMSIS/Include/arm_math.h"
#else
#include <math/eigen_math.h>
#include <platforms/ros/eigen_math.h>
#endif
namespace math

1
src/modules/bottle_drop/bottle_drop.cpp
View File

@ -59,6 +59,7 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_global_position.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>

75
src/modules/mc_att_control/mc_att_control.cpp
View File

@ -44,6 +44,8 @@
*/
#include "mc_att_control.h"
#include "mc_att_control_params.h"
#include "math.h"
#define YAW_DEADZONE 0.05f
#define MIN_TAKEOFF_THRUST 0.2f
@ -63,7 +65,6 @@ static const int ERROR = -1;
MulticopterAttitudeControl::MulticopterAttitudeControl() :
MulticopterAttitudeControlBase(),
_task_should_exit(false),
_control_task(-1),
_actuators_0_circuit_breaker_enabled(false),
/* publications */
@ -76,26 +77,26 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
{
_params_handles.roll_p = PX4_PARAM_INIT("MC_ROLL_P");
_params_handles.roll_rate_p = PX4_PARAM_INIT("MC_ROLLRATE_P");
_params_handles.roll_rate_i = PX4_PARAM_INIT("MC_ROLLRATE_I");
_params_handles.roll_rate_d = PX4_PARAM_INIT("MC_ROLLRATE_D");
_params_handles.pitch_p = PX4_PARAM_INIT("MC_PITCH_P");
_params_handles.pitch_rate_p = PX4_PARAM_INIT("MC_PITCHRATE_P");
_params_handles.pitch_rate_i = PX4_PARAM_INIT("MC_PITCHRATE_I");
_params_handles.pitch_rate_d = PX4_PARAM_INIT("MC_PITCHRATE_D");
_params_handles.yaw_p = PX4_PARAM_INIT("MC_YAW_P");
_params_handles.yaw_rate_p = PX4_PARAM_INIT("MC_YAWRATE_P");
_params_handles.yaw_rate_i = PX4_PARAM_INIT("MC_YAWRATE_I");
_params_handles.yaw_rate_d = PX4_PARAM_INIT("MC_YAWRATE_D");
_params_handles.yaw_ff = PX4_PARAM_INIT("MC_YAW_FF");
_params_handles.yaw_rate_max = PX4_PARAM_INIT("MC_YAWRATE_MAX");
_params_handles.man_roll_max = PX4_PARAM_INIT("MC_MAN_R_MAX");
_params_handles.man_pitch_max = PX4_PARAM_INIT("MC_MAN_P_MAX");
_params_handles.man_yaw_max = PX4_PARAM_INIT("MC_MAN_Y_MAX");
_params_handles.acro_roll_max = PX4_PARAM_INIT("MC_ACRO_R_MAX");
_params_handles.acro_pitch_max = PX4_PARAM_INIT("MC_ACRO_P_MAX");
_params_handles.acro_yaw_max = PX4_PARAM_INIT("MC_ACRO_Y_MAX");
_params_handles.roll_p = PX4_PARAM_INIT(MC_ROLL_P);
_params_handles.roll_rate_p = PX4_PARAM_INIT(MC_ROLLRATE_P);
_params_handles.roll_rate_i = PX4_PARAM_INIT(MC_ROLLRATE_I);
_params_handles.roll_rate_d = PX4_PARAM_INIT(MC_ROLLRATE_D);
_params_handles.pitch_p = PX4_PARAM_INIT(MC_PITCH_P);
_params_handles.pitch_rate_p = PX4_PARAM_INIT(MC_PITCHRATE_P);
_params_handles.pitch_rate_i = PX4_PARAM_INIT(MC_PITCHRATE_I);
_params_handles.pitch_rate_d = PX4_PARAM_INIT(MC_PITCHRATE_D);
_params_handles.yaw_p = PX4_PARAM_INIT(MC_YAW_P);
_params_handles.yaw_rate_p = PX4_PARAM_INIT(MC_YAWRATE_P);
_params_handles.yaw_rate_i = PX4_PARAM_INIT(MC_YAWRATE_I);
_params_handles.yaw_rate_d = PX4_PARAM_INIT(MC_YAWRATE_D);
_params_handles.yaw_ff = PX4_PARAM_INIT(MC_YAW_FF);
_params_handles.yaw_rate_max = PX4_PARAM_INIT(MC_YAWRATE_MAX);
_params_handles.man_roll_max = PX4_PARAM_INIT(MC_MAN_R_MAX);
_params_handles.man_pitch_max = PX4_PARAM_INIT(MC_MAN_P_MAX);
_params_handles.man_yaw_max = PX4_PARAM_INIT(MC_MAN_Y_MAX);
_params_handles.acro_roll_max = PX4_PARAM_INIT(MC_ACRO_R_MAX);
_params_handles.acro_pitch_max = PX4_PARAM_INIT(MC_ACRO_P_MAX);
_params_handles.acro_yaw_max = PX4_PARAM_INIT(MC_ACRO_Y_MAX);
/* fetch initial parameter values */
parameters_update();
@ -115,26 +116,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
MulticopterAttitudeControl::~MulticopterAttitudeControl()
{
if (_control_task != -1) {
/* task wakes up every 100ms or so at the longest */
_task_should_exit = true;
/* wait for a second for the task to quit at our request */
unsigned i = 0;
do {
/* wait 20ms */
usleep(20000);
/* if we have given up, kill it */
if (++i > 50) {
task_delete(_control_task);
break;
}
} while (_control_task != -1);
}
// mc_att_control::g_control = nullptr;
}
int
@ -203,8 +184,8 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
/* run controller on attitude changes */
static uint64_t last_run = 0;
float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
float dt = (px4::get_time_micros() - last_run) / 1000000.0f;
last_run = px4::get_time_micros();
/* guard against too small (< 2ms) and too large (> 20ms) dt's */
if (dt < 0.002f) {
@ -219,7 +200,7 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
/* publish the attitude setpoint if needed */
if (_publish_att_sp) {
_v_att_sp_mod.timestamp = hrt_absolute_time();
_v_att_sp_mod.timestamp = px4::get_time_micros();
if (_att_sp_pub != nullptr) {
_att_sp_pub->publish(_v_att_sp_mod);
@ -234,7 +215,7 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
_v_rates_sp_mod.pitch = _rates_sp(1);
_v_rates_sp_mod.yaw = _rates_sp(2);
_v_rates_sp_mod.thrust = _thrust_sp;
_v_rates_sp_mod.timestamp = hrt_absolute_time();
_v_rates_sp_mod.timestamp = px4::get_time_micros();
if (_v_rates_sp_pub != nullptr) {
_v_rates_sp_pub->publish(_v_rates_sp_mod);
@ -258,7 +239,7 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
_v_rates_sp_mod.pitch = _rates_sp(1);
_v_rates_sp_mod.yaw = _rates_sp(2);
_v_rates_sp_mod.thrust = _thrust_sp;
_v_rates_sp_mod.timestamp = hrt_absolute_time();
_v_rates_sp_mod.timestamp = px4::get_time_micros();
if (_v_rates_sp_pub != nullptr) {
_v_rates_sp_pub->publish(_v_rates_sp_mod);
@ -285,7 +266,7 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
_actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp = px4::get_time_micros();
if (!_actuators_0_circuit_breaker_enabled) {
if (_actuators_0_pub != nullptr) {

9
src/modules/mc_att_control/mc_att_control.h
View File

@ -52,22 +52,16 @@
* If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers.
*/
#include <px4.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
// #include <systemlib/systemlib.h>
#include <systemlib/circuit_breaker.h>
#include <lib/mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include "mc_att_control_base.h"
@ -91,7 +85,6 @@ public:
private:
bool _task_should_exit; /**< if true, sensor task should exit */
int _control_task; /**< task handle for sensor task */
bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */
px4::Publisher * _att_sp_pub; /**< attitude setpoint publication */

4
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -129,12 +129,12 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[])
PX4_MAIN_FUNCTION(mc_att_control)
{
warnx("starting");
PX4_INFO("starting");
MulticopterAttitudeControl attctl;
thread_running = true;
attctl.spin();
warnx("exiting.");
PX4_INFO("exiting.");
thread_running = false;
return 0;
}

17
src/modules/systemlib/circuit_breaker.c
View File

@ -42,7 +42,8 @@
* parameter needs to set to the key (magic).
*/
#include <systemlib/param/param.h>
#include <px4.h>
#include <systemlib/circuit_breaker_params.h>
#include <systemlib/circuit_breaker.h>
/**
@ -56,7 +57,7 @@
* @max 894281
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0);
PX4_PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK);
/**
* Circuit breaker for rate controller output
@ -69,7 +70,7 @@ PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0);
* @max 140253
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0);
PX4_PARAM_DEFINE_INT32(CBRK_RATE_CTRL);
/**
* Circuit breaker for IO safety
@ -81,7 +82,7 @@ PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0);
* @max 22027
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0);
PX4_PARAM_DEFINE_INT32(CBRK_IO_SAFETY);
/**
* Circuit breaker for airspeed sensor
@ -93,7 +94,7 @@ PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0);
* @max 162128
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0);
PX4_PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK);
/**
* Circuit breaker for flight termination
@ -106,7 +107,7 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0);
* @max 121212
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212);
PX4_PARAM_DEFINE_INT32(CBRK_FLIGHTTERM);
/**
* Circuit breaker for engine failure detection
@ -120,7 +121,7 @@ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212);
* @max 284953
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953);
PX4_PARAM_DEFINE_INT32(CBRK_ENGINEFAIL);
/**
* Circuit breaker for gps failure detection
@ -134,7 +135,7 @@ PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953);
* @max 240024
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024);
PX4_PARAM_DEFINE_INT32(CBRK_GPSFAIL);
bool circuit_breaker_enabled(const char* breaker, int32_t magic)
{

8
src/modules/systemlib/circuit_breaker.h
View File

@ -61,8 +61,14 @@
__BEGIN_DECLS
#ifdef __cplusplus
extern "C" {
#endif
__EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic);
#ifdef __cplusplus
}
#endif
__EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic);
__END_DECLS
#endif /* CIRCUIT_BREAKER_H_ */

7
src/modules/systemlib/circuit_breaker_params.h Normal file
View File

@ -0,0 +1,7 @@
#define PARAM_CBRK_SUPPLY_CHK_DEFAULT 0
#define PARAM_CBRK_RATE_CTRL_DEFAULT 0
#define PARAM_CBRK_IO_SAFETY_DEFAULT 0
#define PARAM_CBRK_AIRSPD_CHK_DEFAULT 0
#define PARAM_CBRK_FLIGHTTERM_DEFAULT 121212
#define PARAM_CBRK_ENGINEFAIL_DEFAULT 284953
#define PARAM_CBRK_GPSFAIL_DEFAULT 240024

1
src/modules/systemlib/perf_counter.h
View File

@ -40,6 +40,7 @@
#define _SYSTEMLIB_PERF_COUNTER_H value
#include <stdint.h>
#include <platforms/px4_defines.h>
/**
* Counter types.

37
src/platforms/px4_defines.h
View File

@ -51,7 +51,10 @@
* Building for running within the ROS environment
*/
#define __EXPORT
#define noreturn_function
#ifdef __cplusplus
#include "ros/ros.h"
#endif
/* Main entry point */
#define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv)
@ -63,7 +66,7 @@
#define PX4_TOPIC(_name) #_name
/* Topic type */
#define PX4_TOPIC_T(_name) _name
#define PX4_TOPIC_T(_name) px4::_name
/* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */
#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _objptr, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, _objptr);
@ -93,6 +96,38 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value)
/* Get value of parameter */
#define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt)
#define OK 0
#define ERROR -1
//XXX hack to be able to use isfinte from math.h, -D_GLIBCXX_USE_C99_MATH seems not to work
#define isfinite(_value) std::isfinite(_value)
/* Useful constants. */
#define M_E_F 2.7182818284590452354f
#define M_LOG2E_F 1.4426950408889634074f
#define M_LOG10E_F 0.43429448190325182765f
#define M_LN2_F _M_LN2_F
#define M_LN10_F 2.30258509299404568402f
#define M_PI_F 3.14159265358979323846f
#define M_TWOPI_F (M_PI_F * 2.0f)
#define M_PI_2_F 1.57079632679489661923f
#define M_PI_4_F 0.78539816339744830962f
#define M_3PI_4_F 2.3561944901923448370E0f
#define M_SQRTPI_F 1.77245385090551602792981f
#define M_1_PI_F 0.31830988618379067154f
#define M_2_PI_F 0.63661977236758134308f
#define M_2_SQRTPI_F 1.12837916709551257390f
#define M_DEG_TO_RAD_F 0.01745329251994f
#define M_RAD_TO_DEG_F 57.2957795130823f
#define M_SQRT2_F 1.41421356237309504880f
#define M_SQRT1_2_F 0.70710678118654752440f
#define M_LN2LO_F 1.9082149292705877000E-10f
#define M_LN2HI_F 6.9314718036912381649E-1f
#define M_SQRT3_F 1.73205080756887719000f
#define M_IVLN10_F 0.43429448190325182765f /* 1 / log(10) */
#define M_LOG2_E_F _M_LN2_F
#define M_INVLN2_F 1.4426950408889633870E0f /* 1 / log(2) */
#else
/*
* Building for NuttX

13
src/platforms/px4_includes.h
View File

@ -45,8 +45,21 @@
/*
* Building for running within the ROS environment
*/
#ifdef __cplusplus
#include "ros/ros.h"
#include "px4/rc_channels.h"
#include "px4/vehicle_attitude.h"
#include <px4/vehicle_attitude_setpoint.h>
#include <px4/manual_control_setpoint.h>
#include <px4/actuator_controls.h>
#include <px4/actuator_controls_0.h>
#include <px4/vehicle_rates_setpoint.h>
#include <px4/vehicle_attitude.h>
#include <px4/vehicle_control_mode.h>
#include <px4/actuator_armed.h>
#include <px4/parameter_update.h>
#endif
#else
/*

56
src/platforms/ros/circuit_breaker.cpp Normal file
View File

@ -0,0 +1,56 @@
/****************************************************************************
*
* 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 circuit_breaker.c
*
* Circuit breaker parameters.
* Analog to real aviation circuit breakers these parameters
* allow to disable subsystems. They are not supported as standard
* operation procedure and are only provided for development purposes.
* To ensure they are not activated accidentally, the associated
* parameter needs to set to the key (magic).
*/
#include <px4.h>
#include <systemlib/circuit_breaker_params.h>
#include <systemlib/circuit_breaker.h>
bool circuit_breaker_enabled(const char* breaker, int32_t magic)
{
int32_t val;
(void)PX4_PARAM_GET(breaker, &val);
return (val == magic);
}

165
src/platforms/ros/geo.cpp Normal file
View File

@ -0,0 +1,165 @@
/****************************************************************************
*
* 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
* 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 geo.c
*
* Geo / math functions to perform geodesic calculations
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <geo/geo.h>
#include <px4.h>
#include <unistd.h>
#include <pthread.h>
#include <stdio.h>
#include <math.h>
#include <stdbool.h>
#include <string.h>
#include <float.h>
__EXPORT float _wrap_pi(float bearing)
{
/* value is inf or NaN */
if (!isfinite(bearing)) {
return bearing;
}
int c = 0;
while (bearing >= M_PI_F) {
bearing -= M_TWOPI_F;
if (c++ > 3) {
return NAN;
}
}
c = 0;
while (bearing < -M_PI_F) {
bearing += M_TWOPI_F;
if (c++ > 3) {
return NAN;
}
}
return bearing;
}
__EXPORT float _wrap_2pi(float bearing)
{
/* value is inf or NaN */
if (!isfinite(bearing)) {
return bearing;
}
int c = 0;
while (bearing >= M_TWOPI_F) {
bearing -= M_TWOPI_F;
if (c++ > 3) {
return NAN;
}
}
c = 0;
while (bearing < 0.0f) {
bearing += M_TWOPI_F;
if (c++ > 3) {
return NAN;
}
}
return bearing;
}
__EXPORT float _wrap_180(float bearing)
{
/* value is inf or NaN */
if (!isfinite(bearing)) {
return bearing;
}
int c = 0;
while (bearing >= 180.0f) {
bearing -= 360.0f;
if (c++ > 3) {
return NAN;
}
}
c = 0;
while (bearing < -180.0f) {
bearing += 360.0f;
if (c++ > 3) {
return NAN;
}
}
return bearing;
}
__EXPORT float _wrap_360(float bearing)
{
/* value is inf or NaN */
if (!isfinite(bearing)) {
return bearing;
}
int c = 0;
while (bearing >= 360.0f) {
bearing -= 360.0f;
if (c++ > 3) {
return NAN;
}
}
c = 0;
while (bearing < 0.0f) {
bearing += 360.0f;
if (c++ > 3) {
return NAN;
}
}
return bearing;
}