diff options
Diffstat (limited to 'apps')
-rw-r--r-- | apps/ardrone_interface/ardrone_interface.c | 5 | ||||
-rw-r--r-- | apps/commander/commander.c | 10 | ||||
-rw-r--r-- | apps/commander/state_machine_helper.c | 13 | ||||
-rw-r--r-- | apps/commander/state_machine_helper.h | 6 | ||||
-rw-r--r-- | apps/drivers/mpu6000/mpu6000.cpp | 2 | ||||
-rw-r--r-- | apps/mavlink/mavlink.c | 45 | ||||
-rw-r--r-- | apps/uORB/objects_common.cpp | 3 | ||||
-rw-r--r-- | apps/uORB/topics/actuator_controls.h | 3 | ||||
-rw-r--r-- | apps/uORB/topics/debug_key_value.h | 75 |
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(¤t_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 |