fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 10:54:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Sync state_machine_helper_posix to state_machine_helper

Browse Source 
state_machine_helper_posix.cpp was out of sync with
state_machine_helper_posix.cpp.

Added debug to detect when sensors is started before uorb.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois 2015-04-21 22:04:01 -07:00
parent 3f7d4de74a
commit da29004a26
3 changed files with 35 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

89
src/modules/commander/state_machine_helper_posix.cpp
View File

@ -53,18 +53,17 @@
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/airspeed.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_airspeed.h>
#include <drivers/drv_device.h>
#include <mavlink/mavlink_log.h>
#include "state_machine_helper.h"
#include "commander_helper.h"
#include "PreflightCheck.h"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
@ -78,12 +77,12 @@ static const int ERROR = -1;
// though the transition is marked as true additional checks must be made. See arming_state_transition
// code for those checks.
static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] = {
// INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
{ /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, false, false, false, false },
// INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
{ /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, false, true, false, false },
{ /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, true, false, false, false },
{ /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, false, true },
{ /* vehicle_status_s::ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false },
{ /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false },
{ /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, true, true, true, false, false },
{ /* vehicle_status_s::ARMING_STATE_REBOOT */ true, true, false, false, true, true, true },
{ /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI
};
@ -208,14 +207,28 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
// HIL can always go to standby
if (status->hil_state == vehicle_status_s::HIL_STATE_ON && new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
warnx("HIL STATE ON -> STANDBY");
valid_transition = true;
}
/* Sensors need to be initialized for STANDBY state */
if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
warnx("NOT ARMING: Sensors not operational.");
mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational.");
feedback_provided = true;
valid_transition = false;
status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
}
/* Check if we are trying to arm, checks look good but we are in STANDBY_ERROR */
if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR &&
new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
if (status->condition_system_sensors_initialized) {
mavlink_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming");
} else {
mavlink_log_critical(mavlink_fd, "Preflight check failed, refusing to arm");
}
feedback_provided = true;
}
// Finish up the state transition
@ -607,69 +620,15 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
{
int ret;
bool failed = false;
int fd = px4_open(ACCEL0_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL SENSOR MISSING");
failed = true;
goto system_eval;
}
ret = px4_ioctl(fd, ACCELIOCSELFTEST, 0);
if (ret != OK) {
mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL CALIBRATION");
failed = true;
goto system_eval;
}
/* check measurement result range */
struct accel_report acc;
ret = px4_read(fd, &acc, sizeof(acc));
if (ret == sizeof(acc)) {
/* evaluate values */
float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE, hold still");
/* this is frickin' fatal */
failed = true;
goto system_eval;
}
} else {
mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL READ");
/* this is frickin' fatal */
failed = true;
goto system_eval;
}
/* at this point this equals the preflight check, but might add additional
* quantities later.
*/
bool checkAirspeed = false;
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) {
/* accel done, close it */
px4_close(fd);
fd = orb_subscribe(ORB_ID(airspeed));
struct airspeed_s airspeed;
if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
(hrt_elapsed_time(&airspeed.timestamp) > (50 * 1000))) {
mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING");
failed = true;
goto system_eval;
}
if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) {
mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
// XXX do not make this fatal yet
}
checkAirspeed = true;
}
system_eval:
px4_close(fd);
return (failed);
return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, true);
}

4
src/modules/sensors/sensors_posix.cpp
View File

@ -2152,6 +2152,10 @@ Sensors::task_main()
/* advertise the sensor_combined topic and make the initial publication */
_sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
if (_sensor_pub < 0) {
warnx("Sensors::task_main ERROR - uORB not started");
}
/* wakeup source(s) */
struct pollfd fds[1];

7
src/modules/uORB/MuORB.cpp
View File

@ -420,6 +420,13 @@ ssize_t
ORBDevNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data)
{
PX4_DEBUG("ORBDevNode::publish meta = %p\n", meta);
if (handle < 0) {
PX4_DEBUG("ORBDevNode::publish called with invalid handle\n", meta);
errno = EINVAL;
return ERROR;
}
ORBDevNode *devnode = (ORBDevNode *)handle;
int ret;