aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorAnton Babushkin <anton.babushkin@me.com>2014-09-28 13:43:58 +0400
committerAnton Babushkin <anton.babushkin@me.com>2014-09-28 13:43:58 +0400
commit1107f5903621f8e41faec6bd31900826b8f99524 (patch)
tree14692267e6c2f3cb510f84514fde68edb2497798
parent2d81c2cf466596b7c4f787a52836fd200af2a097 (diff)
parent8a18cfa3869555389e7e9ff8f104d83f9c54cb43 (diff)
downloadpx4-firmware-1107f5903621f8e41faec6bd31900826b8f99524.tar.gz
px4-firmware-1107f5903621f8e41faec6bd31900826b8f99524.tar.bz2
px4-firmware-1107f5903621f8e41faec6bd31900826b8f99524.zip
Merge branch 'master' into mpc_track
-rw-r--r--src/drivers/drv_pwm_output.h7
-rw-r--r--src/drivers/px4io/px4io.cpp22
-rw-r--r--src/modules/dataman/dataman.c2
-rw-r--r--src/modules/navigator/geofence.cpp6
-rw-r--r--src/modules/navigator/mission_feasibility_checker.cpp20
-rw-r--r--src/modules/px4iofirmware/mixer.cpp62
-rw-r--r--src/modules/px4iofirmware/protocol.h4
-rw-r--r--src/modules/px4iofirmware/registers.c18
-rw-r--r--src/modules/systemlib/mixer/mixer.h6
-rw-r--r--src/modules/systemlib/mixer/mixer_multirotor.cpp22
-rw-r--r--src/modules/uORB/objects_common.cpp3
-rw-r--r--src/modules/uORB/topics/multirotor_motor_limits.h69
-rw-r--r--src/systemcmds/pwm/pwm.c21
13 files changed, 222 insertions, 40 deletions
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 5aff6825b..bc586f395 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -205,10 +205,13 @@ ORB_DECLARE(output_pwm);
#define PWM_SERVO_GET_DISABLE_LOCKDOWN _IOC(_PWM_SERVO_BASE, 22)
/** force safety switch off (to disable use of safety switch) */
-#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23)
+#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23)
/** force failsafe mode (failsafe values are set immediately even if failsafe condition not met) */
-#define PWM_SERVO_SET_FORCE_FAILSAFE _IOC(_PWM_SERVO_BASE, 24)
+#define PWM_SERVO_SET_FORCE_FAILSAFE _IOC(_PWM_SERVO_BASE, 24)
+
+/** make failsafe non-recoverable (termination) if it occurs */
+#define PWM_SERVO_SET_TERMINATION_FAILSAFE _IOC(_PWM_SERVO_BASE, 25)
/*
*
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 97919538f..3fdb1dd1f 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1175,6 +1175,14 @@ PX4IO::io_set_arming_state()
clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
}
+ // XXX this is for future support in the commander
+ // but can be removed if unneeded
+ // if (armed.termination_failsafe) {
+ // set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
+ // } else {
+ // clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
+ // }
+
if (armed.ready_to_arm) {
set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
@@ -2038,7 +2046,8 @@ PX4IO::print_status(bool extended_status)
((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""),
((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : ""),
- ((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : "")
+ ((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : ""),
+ ((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : "")
);
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
@@ -2262,6 +2271,17 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
}
break;
+ case PWM_SERVO_SET_TERMINATION_FAILSAFE:
+ /* if failsafe occurs, do not allow the system to recover */
+ if (arg == 0) {
+ /* clear termination failsafe flag */
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE, 0);
+ } else {
+ /* set termination failsafe flag */
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE);
+ }
+ break;
+
case DSM_BIND_START:
/* only allow DSM2, DSM-X and DSM-X with more than 7 channels */
diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c
index ca1fe9bbb..b2355d4d8 100644
--- a/src/modules/dataman/dataman.c
+++ b/src/modules/dataman/dataman.c
@@ -797,7 +797,7 @@ start(void)
sem_init(&g_init_sema, 1, 0);
/* start the worker thread */
- if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, task_main, NULL)) <= 0) {
+ if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2000, task_main, NULL)) <= 0) {
warn("task start failed");
return -1;
}
diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp
index 266215308..49aec21e0 100644
--- a/src/modules/navigator/geofence.cpp
+++ b/src/modules/navigator/geofence.cpp
@@ -76,11 +76,7 @@ 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);
+ return inside(vehicle->lat, vehicle->lon, vehicle->alt);
}
bool Geofence::inside(double lat, double lon, float altitude)
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
index 606521f20..7ae7f60cb 100644
--- a/src/modules/navigator/mission_feasibility_checker.cpp
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -84,7 +84,12 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_
bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
{
- return (checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt));
+ /* Perform checks and issue feedback to the user for all checks */
+ bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
+ bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt);
+
+ /* Mission is only marked as feasible if all checks return true */
+ return (resGeofence && resHomeAltitude);
}
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
@@ -93,7 +98,13 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre
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, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt));
+ /* Perform checks and issue feedback to the user for all checks */
+ bool resLanding = checkFixedWingLanding(dm_current, nMissionItems);
+ bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
+ bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt);
+
+ /* Mission is only marked as feasible if all checks return true */
+ return (resLanding && resGeofence && resHomeAltitude);
}
bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
@@ -216,9 +227,8 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
}
}
-
-// float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt;
- return false;
+ /* No landing waypoints or no waypoints */
+ return true;
}
void MissionFeasibilityChecker::updateNavigationCapabilities()
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 606c639f9..3e19333d8 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -58,7 +58,7 @@ extern "C" {
/*
* Maximum interval in us before FMU signal is considered lost
*/
-#define FMU_INPUT_DROP_LIMIT_US 200000
+#define FMU_INPUT_DROP_LIMIT_US 500000
/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
#define ROLL 0
@@ -98,7 +98,8 @@ mixer_tick(void)
{
/* check that we are receiving fresh data from the FMU */
- if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
+ if ((system_state.fmu_data_received_time == 0) ||
+ hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
/* too long without FMU input, time to go to failsafe */
if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
@@ -109,6 +110,9 @@ mixer_tick(void)
} else {
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
+
+ /* this flag is never cleared once OK */
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED;
}
/* default to failsafe mixing - it will be forced below if flag is set */
@@ -139,7 +143,9 @@ mixer_tick(void)
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
!(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
- !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
+ !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
+ /* do not enter manual override if we asked for termination failsafe and FMU is lost */
+ !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) {
/* if allowed, mix from RC inputs directly */
source = MIX_OVERRIDE;
@@ -155,29 +161,11 @@ mixer_tick(void)
}
/*
- * Check if we should force failsafe - and do it if we have to
- */
- if (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) {
- source = MIX_FAILSAFE;
- }
-
- /*
- * Set failsafe status flag depending on mixing source
- */
- if (source == MIX_FAILSAFE) {
- r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE;
- } else {
- r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE);
- }
-
- /*
* Decide whether the servos should be armed right now.
*
* We must be armed, and we must have a PWM source; either raw from
* FMU or from the mixer.
*
- * XXX correct behaviour for failsafe may require an additional case
- * here.
*/
should_arm = (
/* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
@@ -195,6 +183,38 @@ mixer_tick(void)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
/*
+ * Check if failsafe termination is set - if yes,
+ * set the force failsafe flag once entering the first
+ * failsafe condition.
+ */
+ if ( /* if we have requested flight termination style failsafe (noreturn) */
+ (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) &&
+ /* and we ended up in a failsafe condition */
+ (source == MIX_FAILSAFE) &&
+ /* and we should be armed, so we intended to provide outputs */
+ should_arm &&
+ /* and FMU is initialized */
+ (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED)) {
+ r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
+ }
+
+ /*
+ * Check if we should force failsafe - and do it if we have to
+ */
+ if (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) {
+ source = MIX_FAILSAFE;
+ }
+
+ /*
+ * Set failsafe status flag depending on mixing source
+ */
+ if (source == MIX_FAILSAFE) {
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE;
+ } else {
+ r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE);
+ }
+
+ /*
* Run the mixers.
*/
if (source == MIX_FAILSAFE) {
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 050783687..4739f6e40 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2012, 2013 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
@@ -111,6 +111,7 @@
#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */
#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */
#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */
+#define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 13) /* FMU was initialized and OK once */
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */
@@ -180,6 +181,7 @@
#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 6) /* Disable the IO-internal evaluation of the RC */
#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 7) /* If set, the system operates normally, but won't actuate any servos */
#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 8) /* If set, the system will always output the failsafe values */
+#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 9) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */
#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 0da778b6f..7a5a5e484 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2012, 2013 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
@@ -190,7 +190,8 @@ volatile uint16_t r_page_setup[] =
PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
PX4IO_P_SETUP_ARMING_LOCKDOWN | \
- PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE)
+ PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \
+ PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)
#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
@@ -518,6 +519,19 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
}
+ /*
+ * If the failsafe termination flag is set, do not allow the autopilot to unset it
+ */
+ value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE);
+
+ /*
+ * If failsafe termination is enabled and force failsafe bit is set, do not allow
+ * the autopilot to clear it.
+ */
+ if (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) {
+ value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE);
+ }
+
r_setup_arming = value;
break;
diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h
index cdf60febc..17989558e 100644
--- a/src/modules/systemlib/mixer/mixer.h
+++ b/src/modules/systemlib/mixer/mixer.h
@@ -130,6 +130,9 @@
#include <nuttx/config.h>
#include "drivers/drv_mixer.h"
+
+#include <uORB/topics/multirotor_motor_limits.h>
+
#include "mixer_load.h"
/**
@@ -531,6 +534,9 @@ private:
float _yaw_scale;
float _idle_speed;
+ orb_advert_t _limits_pub;
+ multirotor_motor_limits_s _limits;
+
unsigned _rotor_count;
const Rotor *_rotors;
diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index 4b22a46d0..57e17b67d 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp
@@ -36,7 +36,8 @@
*
* Multi-rotor mixers.
*/
-
+#include <uORB/uORB.h>
+#include <uORB/topics/multirotor_motor_limits.h>
#include <nuttx/config.h>
#include <sys/types.h>
@@ -302,6 +303,11 @@ MultirotorMixer::mix(float *outputs, unsigned space)
float min_out = 0.0f;
float max_out = 0.0f;
+ _limits.roll_pitch = false;
+ _limits.yaw = false;
+ _limits.throttle_upper = false;
+ _limits.throttle_lower = false;
+
/* perform initial mix pass yielding unbounded outputs, ignore yaw */
for (unsigned i = 0; i < _rotor_count; i++) {
float out = roll * _rotors[i].roll_scale +
@@ -311,6 +317,7 @@ MultirotorMixer::mix(float *outputs, unsigned space)
/* limit yaw if it causes outputs clipping */
if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) {
yaw = -out / _rotors[i].yaw_scale;
+ _limits.yaw = true;
}
/* calculate min and max output values */
@@ -332,6 +339,7 @@ MultirotorMixer::mix(float *outputs, unsigned space)
for (unsigned i = 0; i < _rotor_count; i++) {
outputs[i] = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust;
}
+ _limits.roll_pitch = true;
} else {
/* roll/pitch mixed without limiting, add yaw control */
@@ -344,6 +352,7 @@ MultirotorMixer::mix(float *outputs, unsigned space)
float scale_out;
if (max_out > 1.0f) {
scale_out = 1.0f / max_out;
+ _limits.throttle_upper = true;
} else {
scale_out = 1.0f;
@@ -351,9 +360,20 @@ MultirotorMixer::mix(float *outputs, unsigned space)
/* scale outputs to range _idle_speed..1, and do final limiting */
for (unsigned i = 0; i < _rotor_count; i++) {
+ if (outputs[i] < _idle_speed) {
+ _limits.throttle_lower = true;
+ }
outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out), _idle_speed, 1.0f);
}
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+ /* publish/advertise motor limits if running on FMU */
+ if (_limits_pub > 0) {
+ orb_publish(ORB_ID(multirotor_motor_limits), _limits_pub, &_limits);
+ } else {
+ _limits_pub = orb_advertise(ORB_ID(multirotor_motor_limits), &_limits);
+ }
+#endif
return _rotor_count;
}
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index f7d5f8737..b921865eb 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -192,6 +192,9 @@ ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s);
ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s);
ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s);
+#include "topics/multirotor_motor_limits.h"
+ORB_DEFINE(multirotor_motor_limits, struct multirotor_motor_limits_s);
+
#include "topics/telemetry_status.h"
ORB_DEFINE(telemetry_status_0, struct telemetry_status_s);
ORB_DEFINE(telemetry_status_1, struct telemetry_status_s);
diff --git a/src/modules/uORB/topics/multirotor_motor_limits.h b/src/modules/uORB/topics/multirotor_motor_limits.h
new file mode 100644
index 000000000..44c51e4d9
--- /dev/null
+++ b/src/modules/uORB/topics/multirotor_motor_limits.h
@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ * 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 multirotor_motor_limits.h
+ *
+ * Definition of multirotor_motor_limits topic
+ */
+
+#ifndef MULTIROTOR_MOTOR_LIMITS_H_
+#define MULTIROTOR_MOTOR_LIMITS_H_
+
+#include "../uORB.h"
+#include <stdint.h>
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Motor limits
+ */
+struct multirotor_motor_limits_s {
+ uint8_t roll_pitch : 1; // roll/pitch limit reached
+ uint8_t yaw : 1; // yaw limit reached
+ uint8_t throttle_lower : 1; // lower throttle limit reached
+ uint8_t throttle_upper : 1; // upper throttle limit reached
+ uint8_t reserved : 4;
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(multirotor_motor_limits);
+
+#endif
diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index c8d698b86..478c2a772 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -654,9 +654,28 @@ pwm_main(int argc, char *argv[])
}
}
exit(0);
+ } else if (!strcmp(argv[1], "terminatefail")) {
+
+ if (argc < 3) {
+ errx(1, "arg missing [on|off]");
+ } else {
+
+ if (!strcmp(argv[2], "on")) {
+ /* force failsafe */
+ ret = ioctl(fd, PWM_SERVO_SET_TERMINATION_FAILSAFE, 1);
+ } else {
+ /* force failsafe */
+ ret = ioctl(fd, PWM_SERVO_SET_TERMINATION_FAILSAFE, 0);
+ }
+
+ if (ret != OK) {
+ warnx("FAILED setting termination failsafe %s", argv[2]);
+ }
+ }
+ exit(0);
}
- usage("specify arm|disarm|rate|failsafe|disarmed|min|max|test|info|forcefail");
+ usage("specify arm|disarm|rate|failsafe\n\t\tdisarmed|min|max|test|info|forcefail|terminatefail");
return 0;
}