fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-13 16:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged master into geo

Browse Source
This commit is contained in:
Lorenz Meier
2014-07-01 09:34:26 +02:00
parent a43e963bdb 48f4a1e5cd
commit efc992b4d9
171 changed files with 11352 additions and 4720 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/accelerometer_calibration.cpp
+6 -3
View File
@@ -131,6 +131,7 @@
#include <fcntl.h>
#include <sys/prctl.h>
#include <math.h>
#include <float.h>
#include <mathlib/mathlib.h>
#include <string.h>
#include <drivers/drv_hrt.h>
@@ -158,6 +159,8 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
int do_accel_calibration(int mavlink_fd)
{
int fd;
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
struct accel_scale accel_scale = {
@@ -172,7 +175,7 @@ int do_accel_calibration(int mavlink_fd)
int res = OK;
/* reset all offsets to zero and all scales to one */
int fd = open(ACCEL_DEVICE_PATH, 0);
fd = open(ACCEL_DEVICE_PATH, 0);
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
close(fd);
@@ -223,7 +226,7 @@ int do_accel_calibration(int mavlink_fd)
if (res == OK) {
/* apply new scaling and offsets */
int fd = open(ACCEL_DEVICE_PATH, 0);
fd = open(ACCEL_DEVICE_PATH, 0);
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
close(fd);
@@ -524,7 +527,7 @@ int mat_invert3(float src[3][3], float dst[3][3])
src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
if (det == 0.0f) {
if (fabsf(det) < FLT_EPSILON) {
return ERROR; // Singular matrix
}
src/modules/commander/calibration_routines.cpp
+5 -4
View File
@@ -40,6 +40,7 @@
*/
#include <math.h>
#include <float.h>
#include "calibration_routines.h"
@@ -170,7 +171,7 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
float aA, aB, aC, nA, nB, nC, dA, dB, dC;
//Iterate N times, ignore stop condition.
int n = 0;
unsigned int n = 0;
while (n < max_iterations) {
n++;
@@ -179,9 +180,9 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2);
aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2);
aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2);
aA = (aA == 0.0f) ? 1.0f : aA;
aB = (aB == 0.0f) ? 1.0f : aB;
aC = (aC == 0.0f) ? 1.0f : aC;
aA = (fabsf(aA) < FLT_EPSILON) ? 1.0f : aA;
aB = (fabsf(aB) < FLT_EPSILON) ? 1.0f : aB;
aC = (fabsf(aC) < FLT_EPSILON) ? 1.0f : aC;
//Compute next iteration
nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA);
src/modules/commander/commander.cpp
+382 -332
View File
File diff suppressed because it is too large Load Diff
src/modules/commander/commander_helper.cpp
+6
View File
@@ -209,12 +209,18 @@ int led_init()
/* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */
(void)ioctl(leds, LED_ON, LED_BLUE);
/* switch blue off */
led_off(LED_BLUE);
/* we consider the amber led mandatory */
if (ioctl(leds, LED_ON, LED_AMBER)) {
warnx("Amber LED: ioctl fail\n");
return ERROR;
}
/* switch amber off */
led_off(LED_AMBER);
/* then try RGB LEDs, this can fail on FMUv1*/
rgbleds = open(RGBLED_DEVICE_PATH, 0);
src/modules/commander/commander_params.c
+12 -1
View File
@@ -39,7 +39,7 @@
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
*/
#include <nuttx/config.h>
@@ -84,3 +84,14 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
* @group Battery Calibration
*/
PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
/**
* Datalink loss mode enabled.
*
* Set to 1 to enable actions triggered when the datalink is lost.
*
* @group commander
* @min 0
* @max 1
*/
PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0);
src/modules/commander/px4_custom_mode.h
+1
View File
@@ -25,6 +25,7 @@ enum PX4_CUSTOM_SUB_MODE_AUTO {
PX4_CUSTOM_SUB_MODE_AUTO_MISSION,
PX4_CUSTOM_SUB_MODE_AUTO_RTL,
PX4_CUSTOM_SUB_MODE_AUTO_LAND,
PX4_CUSTOM_SUB_MODE_AUTO_RTGS
};
union px4_custom_mode {
src/modules/commander/state_machine_helper.cpp
+338 -345
View File
@@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -36,6 +34,9 @@
/**
* @file state_machine_helper.cpp
* State machine helper functions implementations
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
*/
#include <stdio.h>
@@ -45,14 +46,18 @@
#include <dirent.h>
#include <fcntl.h>
#include <string.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/differential_pressure.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>
@@ -65,9 +70,7 @@
#endif
static const int ERROR = -1;
static bool arming_state_changed = true;
static bool main_state_changed = true;
static bool failsafe_state_changed = true;
static int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);
// This array defines the arming state transitions. The rows are the new state, and the columns
// are the current state. Using new state and current state you can index into the array which
@@ -75,14 +78,14 @@ static bool failsafe_state_changed = true;
// 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[ARMING_STATE_MAX][ARMING_STATE_MAX] = {
// INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
{ /* ARMING_STATE_INIT */ true, true, false, false, false, false, false },
{ /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false },
{ /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true },
{ /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false },
{ /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false },
{ /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true },
{ /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI
// INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
{ /* ARMING_STATE_INIT */ true, true, false, false, false, false, false },
{ /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false },
{ /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true },
{ /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false },
{ /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false },
{ /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true },
{ /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI
};
// You can index into the array with an arming_state_t in order to get it's textual representation
@@ -107,18 +110,31 @@ arming_state_transition(struct vehicle_status_s *status, /// current
ASSERT(ARMING_STATE_INIT == 0);
ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
/*
* Perform an atomic state update
*/
irqstate_t flags = irqsave();
transition_result_t ret = TRANSITION_DENIED;
arming_state_t current_arming_state = status->arming_state;
/* only check transition if the new state is actually different from the current one */
if (new_arming_state == status->arming_state) {
if (new_arming_state == current_arming_state) {
ret = TRANSITION_NOT_CHANGED;
} else {
/*
* Get sensing state if necessary
*/
int prearm_ret = OK;
/* only perform the check if we have to */
if (new_arming_state == ARMING_STATE_ARMED) {
prearm_ret = prearm_check(status, mavlink_fd);
}
/*
* Perform an atomic state update
*/
irqstate_t flags = irqsave();
/* enforce lockdown in HIL */
if (status->hil_state == HIL_STATE_ON) {
armed->lockdown = true;
@@ -133,15 +149,44 @@ arming_state_transition(struct vehicle_status_s *status, /// current
if (valid_transition) {
// We have a good transition. Now perform any secondary validation.
if (new_arming_state == ARMING_STATE_ARMED) {
// Fail transition if we need safety switch press
// Allow if coming from in air restore
// Do not perform pre-arm checks if coming from in air restore
// Allow if HIL_STATE_ON
if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) {
if (mavlink_fd) {
mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch first.");
if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE &&
status->hil_state == HIL_STATE_OFF) {
// Fail transition if pre-arm check fails
if (prearm_ret) {
valid_transition = false;
// Fail transition if we need safety switch press
} else if (safety->safety_switch_available && !safety->safety_off) {
mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch!");
valid_transition = false;
}
// Perform power checks only if circuit breaker is not
// engaged for these checks
if (!status->circuit_breaker_engaged_power_check) {
// Fail transition if power is not good
if (!status->condition_power_input_valid) {
mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module.");
valid_transition = false;
}
// Fail transition if power levels on the avionics rail
// are measured but are insufficient
if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) &&
(status->avionics_power_rail_voltage < 4.9f)) {
mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage);
valid_transition = false;
}
}
valid_transition = false;
}
} else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) {
@@ -165,19 +210,16 @@ arming_state_transition(struct vehicle_status_s *status, /// current
armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY;
ret = TRANSITION_CHANGED;
status->arming_state = new_arming_state;
arming_state_changed = true;
}
/* end of atomic state update */
irqrestore(flags);
}
/* end of atomic state update */
irqrestore(flags);
if (ret == TRANSITION_DENIED) {
static const char *errMsg = "Invalid arming transition from %s to %s";
static const char *errMsg = "INVAL: %s - %s";
if (mavlink_fd) {
mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
}
mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]);
}
@@ -199,69 +241,58 @@ bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safet
}
}
bool
check_arming_state_changed()
{
if (arming_state_changed) {
arming_state_changed = false;
return true;
} else {
return false;
}
}
transition_result_t
main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state)
{
transition_result_t ret = TRANSITION_DENIED;
/* transition may be denied even if requested the same state because conditions may be changed */
/* transition may be denied even if the same state is requested because conditions may have changed */
switch (new_main_state) {
case MAIN_STATE_MANUAL:
ret = TRANSITION_CHANGED;
break;
case MAIN_STATE_ACRO:
ret = TRANSITION_CHANGED;
break;
case MAIN_STATE_ALTCTL:
/* need at minimum altitude estimate */
/* TODO: add this for fixedwing as well */
if (!status->is_rotary_wing ||
(status->condition_local_altitude_valid ||
status->condition_global_position_valid)) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_POSCTL:
/* need at minimum local position estimate */
if (status->condition_local_position_valid ||
status->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_AUTO:
case MAIN_STATE_AUTO_MISSION:
case MAIN_STATE_AUTO_LOITER:
/* need global position estimate */
if (status->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_AUTO_RTL:
/* need global position and home position */
if (status->condition_global_position_valid && status->condition_home_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_MAX:
default:
break;
}
if (ret == TRANSITION_CHANGED) {
if (status->main_state != new_main_state) {
status->main_state = new_main_state;
main_state_changed = true;
} else {
ret = TRANSITION_NOT_CHANGED;
}
@@ -270,70 +301,35 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
return ret;
}
bool
check_main_state_changed()
{
if (main_state_changed) {
main_state_changed = false;
return true;
} else {
return false;
}
}
bool
check_failsafe_state_changed()
{
if (failsafe_state_changed) {
failsafe_state_changed = false;
return true;
} else {
return false;
}
}
/**
* Transition from one hil state to another
*/
int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
* Transition from one hil state to another
*/
transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
bool valid_transition = false;
int ret = ERROR;
transition_result_t ret = TRANSITION_DENIED;
if (current_status->hil_state == new_state) {
valid_transition = true;
ret = TRANSITION_NOT_CHANGED;
} else {
switch (new_state) {
case HIL_STATE_OFF:
/* we're in HIL and unexpected things can happen if we disable HIL now */
mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)");
valid_transition = false;
ret = TRANSITION_DENIED;
break;
case HIL_STATE_ON:
if (current_status->arming_state == ARMING_STATE_INIT
|| current_status->arming_state == ARMING_STATE_STANDBY
|| current_status->arming_state == ARMING_STATE_STANDBY_ERROR) {
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
valid_transition = true;
// Disable publication of all attached sensors
/* Disable publication of all attached sensors */
/* list directory */
DIR *d;
d = opendir("/dev");
if (d) {
struct dirent *direntry;
char devname[24];
@@ -388,290 +384,287 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
}
closedir(d);
ret = TRANSITION_CHANGED;
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
} else {
/* failed opening dir */
warnx("FAILED LISTING DEVICE ROOT DIRECTORY");
return 1;
mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY");
ret = TRANSITION_DENIED;
}
} else {
mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed");
ret = TRANSITION_DENIED;
}
break;
default:
warnx("Unknown hil state");
warnx("Unknown HIL state");
break;
}
}
if (valid_transition) {
if (ret == TRANSITION_CHANGED) {
current_status->hil_state = new_state;
current_status->timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
// XXX also set lockdown here
ret = OK;
} else {
mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition");
orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
}
return ret;
}
/**
* Transition from one failsafe state to another
*/
transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state)
* Check failsafe and main status and set navigation status for navigator accordingly
*/
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished)
{
transition_result_t ret = TRANSITION_DENIED;
navigation_state_t nav_state_old = status->nav_state;
/* transition may be denied even if requested the same state because conditions may be changed */
if (status->failsafe_state == FAILSAFE_STATE_TERMINATION) {
/* transitions from TERMINATION to other states not allowed */
if (new_failsafe_state == FAILSAFE_STATE_TERMINATION) {
ret = TRANSITION_NOT_CHANGED;
}
bool armed = (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR);
status->failsafe = false;
} else {
switch (new_failsafe_state) {
case FAILSAFE_STATE_NORMAL:
/* always allowed (except from TERMINATION state) */
ret = TRANSITION_CHANGED;
break;
/* evaluate main state to decide in normal (non-failsafe) mode */
switch (status->main_state) {
case MAIN_STATE_ACRO:
case MAIN_STATE_MANUAL:
case MAIN_STATE_ALTCTL:
case MAIN_STATE_POSCTL:
/* require RC for all manual modes */
if (status->rc_signal_lost && armed) {
status->failsafe = true;
case FAILSAFE_STATE_RTL:
/* global position and home position required for RTL */
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->set_nav_state = NAV_STATE_RTL;
status->set_nav_state_timestamp = hrt_absolute_time();
ret = TRANSITION_CHANGED;
status->nav_state = NAVIGATION_STATE_AUTO_RTL;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
break;
} else {
switch (status->main_state) {
case MAIN_STATE_ACRO:
status->nav_state = NAVIGATION_STATE_ACRO;
break;
case FAILSAFE_STATE_LAND:
case MAIN_STATE_MANUAL:
status->nav_state = NAVIGATION_STATE_MANUAL;
break;
/* at least relative altitude estimate required for landing */
if (status->condition_local_altitude_valid || status->condition_global_position_valid) {
status->set_nav_state = NAV_STATE_LAND;
status->set_nav_state_timestamp = hrt_absolute_time();
ret = TRANSITION_CHANGED;
case MAIN_STATE_ALTCTL:
status->nav_state = NAVIGATION_STATE_ALTCTL;
break;
case MAIN_STATE_POSCTL:
status->nav_state = NAVIGATION_STATE_POSCTL;
break;
default:
status->nav_state = NAVIGATION_STATE_MANUAL;
break;
}
}
break;
case MAIN_STATE_AUTO_MISSION:
/* go into failsafe
* - if either the datalink is enabled and lost as well as RC is lost
* - if there is no datalink and the mission is finished */
if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) ||
(!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTL;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
break;
/* also go into failsafe if just datalink is lost */
} else if (status->data_link_lost && data_link_loss_enabled) {
status->failsafe = true;
case FAILSAFE_STATE_TERMINATION:
/* always allowed */
ret = TRANSITION_CHANGED;
break;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
default:
break;
/* don't bother if RC is lost and mission is not yet finished */
} else if (status->rc_signal_lost) {
/* this mode is ok, we don't need RC for missions */
status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
} else {
/* everything is perfect */
status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
}
break;
case MAIN_STATE_AUTO_LOITER:
/* go into failsafe if datalink and RC is lost */
if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTL;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
/* also go into failsafe if just datalink is lost */
} else if (status->data_link_lost && data_link_loss_enabled) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
/* go into failsafe if RC is lost and datalink loss is not set up */
} else if (status->rc_signal_lost && !data_link_loss_enabled) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
/* don't bother if RC is lost if datalink is connected */
} else if (status->rc_signal_lost) {
/* this mode is ok, we don't need RC for loitering */
status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
} else {
/* everything is perfect */
status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
}
break;
case MAIN_STATE_AUTO_RTL:
/* require global position and home */
if ((!status->condition_global_position_valid || !status->condition_home_position_valid)) {
status->failsafe = true;
if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
} else {
status->nav_state = NAVIGATION_STATE_AUTO_RTL;
}
break;
default:
break;
}
return status->nav_state != nav_state_old;
}
int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
{
int ret;
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING");
ret = fd;
goto system_eval;
}
ret = ioctl(fd, ACCELIOCSELFTEST, 0);
if (ret != OK) {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION");
goto system_eval;
}
/* check measurement result range */
struct accel_report acc;
ret = read(fd, &acc, sizeof(acc));
if (ret == sizeof(acc)) {
/* evaluate values */
float accel_scale = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
if (accel_scale < 9.78f || accel_scale > 9.83f) {
mavlink_log_info(mavlink_fd, "#audio: Accelerometer calibration recommended.");
}
if (ret == TRANSITION_CHANGED) {
if (status->failsafe_state != new_failsafe_state) {
status->failsafe_state = new_failsafe_state;
failsafe_state_changed = true;
if (accel_scale > 30.0f /* m/s^2 */) {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE");
/* this is frickin' fatal */
ret = ERROR;
goto system_eval;
} else {
ret = OK;
}
} else {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ");
/* this is frickin' fatal */
ret = ERROR;
goto system_eval;
}
} else {
ret = TRANSITION_NOT_CHANGED;
if (!status->is_rotary_wing) {
int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING");
ret = fd;
goto system_eval;
}
struct differential_pressure_s diff_pres;
ret = read(fd, &diff_pres, sizeof(diff_pres));
if (ret == sizeof(diff_pres)) {
if (fabsf(diff_pres.differential_pressure_filtered_pa > 5.0f)) {
mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING");
// XXX do not make this fatal yet
ret = OK;
}
} else {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ");
/* this is frickin' fatal */
ret = ERROR;
goto system_eval;
}
}
system_eval:
close(fd);
return ret;
}
// /*
// * Wrapper functions (to be used in the commander), all functions assume lock on current_status
// */
// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR
// *
// * START SUBSYSTEM/EMERGENCY FUNCTIONS
// * */
// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
// {
// current_status->onboard_control_sensors_present |= 1 << *subsystem_type;
// current_status->counter++;
// current_status->timestamp = hrt_absolute_time();
// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
// }
// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
// {
// current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type);
// current_status->counter++;
// current_status->timestamp = hrt_absolute_time();
// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
// /* if a subsystem was removed something went completely wrong */
// switch (*subsystem_type) {
// case SUBSYSTEM_TYPE_GYRO:
// //global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_ACC:
// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_MAG:
// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_GPS:
// {
// uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV];
// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
// //global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency(status_pub, current_status);
// }
// }
// break;
// default:
// break;
// }
// }
// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
// {
// current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type;
// current_status->counter++;
// current_status->timestamp = hrt_absolute_time();
// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
// }
// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
// {
// current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type);
// current_status->counter++;
// current_status->timestamp = hrt_absolute_time();
// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
// /* if a subsystem was disabled something went completely wrong */
// switch (*subsystem_type) {
// case SUBSYSTEM_TYPE_GYRO:
// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_ACC:
// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_MAG:
// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_GPS:
// {
// uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency(status_pub, current_status);
// }
// }
// break;
// default:
// break;
// }
// }
///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
//
//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
//{
// int ret = 1;
//
//// /* Switch on HIL if in standby and not already in HIL mode */
//// if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
//// && !current_status->flag_hil_enabled) {
//// if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) {
//// /* Enable HIL on request */
//// current_status->flag_hil_enabled = true;
//// ret = OK;
//// state_machine_publish(status_pub, current_status, mavlink_fd);
//// publish_armed_status(current_status);
//// printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
////
//// } else if (current_status->state_machine != SYSTEM_STATE_STANDBY &&
//// current_status->flag_fmu_armed) {
////
//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!")
////
//// } else {
////
//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.")
//// }
//// }
//
// /* switch manual / auto */
// if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
// update_state_machine_mode_auto(status_pub, current_status, mavlink_fd);
//
// } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) {
// update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd);
//
// } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
// update_state_machine_mode_guided(status_pub, current_status, mavlink_fd);
//
// } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
// update_state_machine_mode_manual(status_pub, current_status, mavlink_fd);
// }
//
// /* vehicle is disarmed, mode requests arming */
// if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
// /* only arm in standby state */
// // XXX REMOVE
// if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
// ret = OK;
// printf("[cmd] arming due to command request\n");
// }
// }
//
// /* vehicle is armed, mode requests disarming */
// if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
// /* only disarm in ground ready */
// if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
// ret = OK;
// printf("[cmd] disarming due to command request\n");
// }
// }
//
// /* NEVER actually switch off HIL without reboot */
// if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
// warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
// mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL");
// ret = ERROR;
// }
//
// return ret;
//}
src/modules/commander/state_machine_helper.h
+4 -14
View File
@@ -56,25 +56,15 @@ typedef enum {
} transition_result_t;
transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd = 0);
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
bool check_arming_state_changed();
transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd);
transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);
bool check_main_state_changed();
transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state);
bool check_navigation_state_changed();
bool check_failsafe_state_changed();
void set_navigation_state_changed();
int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished);
#endif /* STATE_MACHINE_HELPER_H_ */