fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 21:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed correct RC loss detection, AR.Drone is now shutting down motors after 1 s of RC loss. Added debug topic.

Browse Source
This commit is contained in:
Lorenz Meier
2012-09-05 11:37:17 +02:00
parent 86a2a4fb9f
commit 84e11a0cac
23 changed files with 213 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/ardrone_interface/ardrone_interface.c
+4 -1
View File
@@ -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 */
apps/commander/commander.c
+10
View File
@@ -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 */
apps/commander/state_machine_helper.c
+9 -4
View File
@@ -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
apps/commander/state_machine_helper.h
+6
View File
@@ -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);
apps/drivers/mpu6000/mpu6000.cpp
+1 -1
View File
@@ -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'");
}
apps/mavlink/mavlink.c
+38 -7
View File
@@ -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 */
apps/uORB/objects_common.cpp
+3
View File
@@ -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);
apps/uORB/topics/actuator_controls.h
+2 -1
View File
@@ -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);
apps/uORB/topics/debug_key_value.h
+75
View File
@@ -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