diff --git a/src/modules/commander/state_machine_helper_posix.cpp b/src/modules/commander/state_machine_helper_posix.cpp index 97c2f11043..2d3b78ddaf 100644 --- a/src/modules/commander/state_machine_helper_posix.cpp +++ b/src/modules/commander/state_machine_helper_posix.cpp @@ -53,18 +53,17 @@ #include #include #include -#include #include #include #include #include #include -#include #include #include #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); } diff --git a/src/modules/sensors/sensors_posix.cpp b/src/modules/sensors/sensors_posix.cpp index 01a0c0dee1..e082ed4756 100644 --- a/src/modules/sensors/sensors_posix.cpp +++ b/src/modules/sensors/sensors_posix.cpp @@ -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]; diff --git a/src/modules/uORB/MuORB.cpp b/src/modules/uORB/MuORB.cpp index 791a639e59..0f2a79822b 100644 --- a/src/modules/uORB/MuORB.cpp +++ b/src/modules/uORB/MuORB.cpp @@ -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;