aboutsummaryrefslogtreecommitdiff
path: root/apps
diff options
context:
space:
mode:
Diffstat (limited to 'apps')
-rw-r--r--apps/ardrone_interface/ardrone_interface.c5
-rw-r--r--apps/commander/commander.c10
-rw-r--r--apps/commander/state_machine_helper.c13
-rw-r--r--apps/commander/state_machine_helper.h6
-rw-r--r--apps/drivers/mpu6000/mpu6000.cpp2
-rw-r--r--apps/mavlink/mavlink.c45
-rw-r--r--apps/uORB/objects_common.cpp3
-rw-r--r--apps/uORB/topics/actuator_controls.h3
-rw-r--r--apps/uORB/topics/debug_key_value.h75
9 files changed, 148 insertions, 14 deletions
diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c
index a865992ed..220c3d3b5 100644
--- a/apps/ardrone_interface/ardrone_interface.c
+++ b/apps/ardrone_interface/ardrone_interface.c
@@ -289,7 +289,10 @@ int ardrone_interface_thread_main(int argc, char *argv[])
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
- if (armed.armed) {
+ /* for now only spin if armed and immediately shut down
+ * if in failsafe
+ */
+ if (armed.armed && !armed.failsafe) {
ardrone_mixing_and_output(ardrone_write, &actuator_controls);
} else {
/* Silently lock down motor speeds to zero */
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 80d2c58f8..4135abf60 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -69,6 +69,7 @@
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/subsystem_info.h>
+#include <uORB/topics/actuator_controls.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/param/param.h>
@@ -1174,6 +1175,7 @@ int commander_thread_main(int argc, char *argv[])
/* End battery voltage check */
/* Start RC state check */
+ bool prev_lost = current_status.rc_signal_lost;
if (rc.chan_count > 4 && (hrt_absolute_time() - rc.timestamp) < 100000) {
@@ -1238,10 +1240,18 @@ int commander_thread_main(int argc, char *argv[])
/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
current_status.rc_signal_cutting_off = true;
current_status.rc_signal_lost_interval = hrt_absolute_time() - rc.timestamp;
+
/* if the RC signal is gone for a full second, consider it lost */
if (current_status.rc_signal_lost_interval > 1000000) current_status.rc_signal_lost = true;
}
+ /* Check if this is the first loss or first gain*/
+ if ((!prev_lost && current_status.rc_signal_lost) ||
+ prev_lost && !current_status.rc_signal_lost) {
+ /* publish rc lost */
+ publish_armed_status(&current_status);
+ }
+
/* End mode switch */
/* END RC state check */
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
index 4e2166a3a..51ed95114 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -199,10 +199,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
if (invalid_state == false || old_state != new_state) {
current_status->state_machine = new_state;
state_machine_publish(status_pub, current_status, mavlink_fd);
- struct actuator_armed_s armed;
- armed.armed = current_status->flag_system_armed;
- orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
- orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
+ publish_armed_status(current_status);
ret = OK;
}
if (invalid_state) {
@@ -220,6 +217,14 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
printf("[commander] new state: %s\n", system_state_txt[current_status->state_machine]);
}
+void publish_armed_status(const struct vehicle_status_s *current_status) {
+ struct actuator_armed_s armed;
+ armed.armed = current_status->flag_system_armed;
+ armed.failsafe = current_status->rc_signal_lost;
+ orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
+ orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
+}
+
/*
* Private functions, update the state machine
diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h
index 4908c799a..c4d1b78a5 100644
--- a/apps/commander/state_machine_helper.h
+++ b/apps/commander/state_machine_helper.h
@@ -188,6 +188,12 @@ void state_machine_emergency_always_critical(int status_pub, struct vehicle_stat
*/
void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+/**
+ * Publish the armed state depending on the current system state
+ *
+ * @param current_status the current system status
+ */
+void publish_armed_status(const struct vehicle_status_s *current_status);
diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp
index 04ba4cbbb..ad4976bc2 100644
--- a/apps/drivers/mpu6000/mpu6000.cpp
+++ b/apps/drivers/mpu6000/mpu6000.cpp
@@ -1120,5 +1120,5 @@ mpu6000_main(int argc, char *argv[])
if (!strcmp(argv[1], "info"))
mpu6000::info();
- errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'");
+ errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
}
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index ef0eab130..0d9be58e9 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -75,6 +75,7 @@
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/debug_key_value.h>
#include <systemlib/param/param.h>
#include "waypoints.h"
@@ -166,6 +167,7 @@ static struct mavlink_subscriptions {
int spa_sub;
int spl_sub;
int spg_sub;
+ int debug_key_value;
bool initialized;
} mavlink_subs = {
.sensor_sub = 0,
@@ -183,6 +185,7 @@ static struct mavlink_subscriptions {
.spa_sub = 0,
.spl_sub = 0,
.spg_sub = 0,
+ .debug_key_value = 0,
.initialized = false
};
@@ -543,6 +546,10 @@ static int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int ma
case MAVLINK_MSG_ID_MANUAL_CONTROL:
/* manual_control_setpoint triggers this message */
if (subs->man_control_sp_sub) orb_set_interval(subs->man_control_sp_sub, min_interval);
+ break;
+ case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
+ if (subs->debug_key_value) orb_set_interval(subs->debug_key_value, min_interval);
+ break;
default:
/* not found */
ret = ERROR;
@@ -588,6 +595,7 @@ static void *uorb_receiveloop(void *arg)
struct actuator_outputs_s act_outputs;
struct manual_control_setpoint_s man_control;
struct actuator_controls_s actuators;
+ struct debug_key_value_s debug;
} buf;
/* --- SENSORS RAW VALUE --- */
@@ -709,6 +717,13 @@ static void *uorb_receiveloop(void *arg)
fds[fdsc_count].events = POLLIN;
fdsc_count++;
+ /* --- DEBUG VALUE OUTPUT --- */
+ /* subscribe to ORB for debug value outputs */
+ subs->debug_key_value = orb_subscribe(ORB_ID(debug_key_value));
+ fds[fdsc_count].fd = subs->debug_key_value;
+ fds[fdsc_count].events = POLLIN;
+ fdsc_count++;
+
/* all subscriptions initialized, return success */
subs->initialized = true;
@@ -1060,10 +1075,17 @@ static void *uorb_receiveloop(void *arg)
/* --- ACTUATOR CONTROL --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs->actuators_sub, &buf.actuators);
- mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl0", buf.actuators.control[0]);
- mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl1", buf.actuators.control[1]);
- mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl2", buf.actuators.control[2]);
- mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl3", buf.actuators.control[3]);
+ /* send, add spaces so that string buffer is at least 10 chars long */
+ mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl0 ", buf.actuators.control[0]);
+ mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl1 ", buf.actuators.control[1]);
+ mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl2 ", buf.actuators.control[2]);
+ mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl3 ", buf.actuators.control[3]);
+ }
+
+ /* --- DEBUG KEY/VALUE --- */
+ if (fds[ifds++].revents & POLLIN) {
+ orb_copy(ORB_ID(debug_key_value), subs->debug_key_value, &buf.debug);
+ mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, buf.debug.key, buf.debug.value);
}
}
}
@@ -1501,6 +1523,7 @@ int mavlink_thread_main(int argc, char *argv[])
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 2);
/* 200 Hz / 5 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 5);
+ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 5);
/* 5 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 200);
} else if (baudrate >= 460800) {
@@ -1508,15 +1531,19 @@ int mavlink_thread_main(int argc, char *argv[])
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 5);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 5);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 5);
+ /* 100 Hz / 10 ms */
+ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10);
/* 50 Hz / 20 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 20);
/* 2 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
} else if (baudrate >= 115200) {
/* 50 Hz / 20 ms */
- set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 50);
- set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50);
- set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50);
+ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 20);
+ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 20);
+ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20);
+ /* 20 Hz / 50 ms */
+ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
/* 10 Hz / 100 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 100);
/* 1 Hz */
@@ -1527,6 +1554,8 @@ int mavlink_thread_main(int argc, char *argv[])
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 100);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 100);
/* 5 Hz / 200 ms */
+ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
+ /* 5 Hz / 200 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
/* 0.2 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 5000);
@@ -1535,6 +1564,8 @@ int mavlink_thread_main(int argc, char *argv[])
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 1000);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000);
+ /* 1 Hz / 1000 ms */
+ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000);
/* 0.5 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000);
/* 0.1 Hz */
diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp
index de26dcf01..e40199e97 100644
--- a/apps/uORB/objects_common.cpp
+++ b/apps/uORB/objects_common.cpp
@@ -123,3 +123,6 @@ ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s);
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/debug_key_value.h"
+ORB_DEFINE(debug_key_value, struct debug_key_value_s);
diff --git a/apps/uORB/topics/actuator_controls.h b/apps/uORB/topics/actuator_controls.h
index 2b7d7de5d..e6d7c8670 100644
--- a/apps/uORB/topics/actuator_controls.h
+++ b/apps/uORB/topics/actuator_controls.h
@@ -67,7 +67,8 @@ ORB_DECLARE(actuator_controls_3);
/** global 'actuator output is live' control. */
struct actuator_armed_s {
- bool armed;
+ bool armed; /**< Set to true if system is armed */
+ bool failsafe; /**< Set to true if no valid control input is available */
};
ORB_DECLARE(actuator_armed);
diff --git a/apps/uORB/topics/debug_key_value.h b/apps/uORB/topics/debug_key_value.h
new file mode 100644
index 000000000..a9d1b83fd
--- /dev/null
+++ b/apps/uORB/topics/debug_key_value.h
@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Author: @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 debug_key_value.h
+ * Definition of the debug named value uORB topic. Allows to send a 10-char key
+ * with a float as value.
+ */
+
+#ifndef TOPIC_DEBUG_KEY_VALUE_H_
+#define TOPIC_DEBUG_KEY_VALUE_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ */
+
+/**
+ * Key/value pair for debugging
+ */
+struct debug_key_value_s {
+
+ /*
+ * Actual data, this is specific to the type of data which is stored in this struct
+ * A line containing L0GME will be added by the Python logging code generator to the
+ * logged dataset.
+ */
+ uint32_t timestamp_ms; /**< in milliseconds since system start */
+ char key[10]; /**< max. 10 characters as key / name */
+ float value; /**< the value to send as debug output */
+
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(debug_key_value);
+
+#endif
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-17 13:59:13 +0000