mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-20 20:07:35 +08:00
Cut over commander app to new build system
This commit is contained in:
Lorenz Meier
@@ -0,0 +1,219 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* 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 calibration_routines.c
|
||||
* Calibration routines implementations.
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#include "calibration_routines.h"
|
||||
|
||||
|
||||
int sphere_fit_least_squares(const float x[], const float y[], const float z[],
|
||||
unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius)
|
||||
{
|
||||
|
||||
float x_sumplain = 0.0f;
|
||||
float x_sumsq = 0.0f;
|
||||
float x_sumcube = 0.0f;
|
||||
|
||||
float y_sumplain = 0.0f;
|
||||
float y_sumsq = 0.0f;
|
||||
float y_sumcube = 0.0f;
|
||||
|
||||
float z_sumplain = 0.0f;
|
||||
float z_sumsq = 0.0f;
|
||||
float z_sumcube = 0.0f;
|
||||
|
||||
float xy_sum = 0.0f;
|
||||
float xz_sum = 0.0f;
|
||||
float yz_sum = 0.0f;
|
||||
|
||||
float x2y_sum = 0.0f;
|
||||
float x2z_sum = 0.0f;
|
||||
float y2x_sum = 0.0f;
|
||||
float y2z_sum = 0.0f;
|
||||
float z2x_sum = 0.0f;
|
||||
float z2y_sum = 0.0f;
|
||||
|
||||
for (unsigned int i = 0; i < size; i++) {
|
||||
|
||||
float x2 = x[i] * x[i];
|
||||
float y2 = y[i] * y[i];
|
||||
float z2 = z[i] * z[i];
|
||||
|
||||
x_sumplain += x[i];
|
||||
x_sumsq += x2;
|
||||
x_sumcube += x2 * x[i];
|
||||
|
||||
y_sumplain += y[i];
|
||||
y_sumsq += y2;
|
||||
y_sumcube += y2 * y[i];
|
||||
|
||||
z_sumplain += z[i];
|
||||
z_sumsq += z2;
|
||||
z_sumcube += z2 * z[i];
|
||||
|
||||
xy_sum += x[i] * y[i];
|
||||
xz_sum += x[i] * z[i];
|
||||
yz_sum += y[i] * z[i];
|
||||
|
||||
x2y_sum += x2 * y[i];
|
||||
x2z_sum += x2 * z[i];
|
||||
|
||||
y2x_sum += y2 * x[i];
|
||||
y2z_sum += y2 * z[i];
|
||||
|
||||
z2x_sum += z2 * x[i];
|
||||
z2y_sum += z2 * y[i];
|
||||
}
|
||||
|
||||
//
|
||||
//Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data
|
||||
//
|
||||
// P is a structure that has been computed with the data earlier.
|
||||
// P.npoints is the number of elements; the length of X,Y,Z are identical.
|
||||
// P's members are logically named.
|
||||
//
|
||||
// X[n] is the x component of point n
|
||||
// Y[n] is the y component of point n
|
||||
// Z[n] is the z component of point n
|
||||
//
|
||||
// A is the x coordiante of the sphere
|
||||
// B is the y coordiante of the sphere
|
||||
// C is the z coordiante of the sphere
|
||||
// Rsq is the radius squared of the sphere.
|
||||
//
|
||||
//This method should converge; maybe 5-100 iterations or more.
|
||||
//
|
||||
float x_sum = x_sumplain / size; //sum( X[n] )
|
||||
float x_sum2 = x_sumsq / size; //sum( X[n]^2 )
|
||||
float x_sum3 = x_sumcube / size; //sum( X[n]^3 )
|
||||
float y_sum = y_sumplain / size; //sum( Y[n] )
|
||||
float y_sum2 = y_sumsq / size; //sum( Y[n]^2 )
|
||||
float y_sum3 = y_sumcube / size; //sum( Y[n]^3 )
|
||||
float z_sum = z_sumplain / size; //sum( Z[n] )
|
||||
float z_sum2 = z_sumsq / size; //sum( Z[n]^2 )
|
||||
float z_sum3 = z_sumcube / size; //sum( Z[n]^3 )
|
||||
|
||||
float XY = xy_sum / size; //sum( X[n] * Y[n] )
|
||||
float XZ = xz_sum / size; //sum( X[n] * Z[n] )
|
||||
float YZ = yz_sum / size; //sum( Y[n] * Z[n] )
|
||||
float X2Y = x2y_sum / size; //sum( X[n]^2 * Y[n] )
|
||||
float X2Z = x2z_sum / size; //sum( X[n]^2 * Z[n] )
|
||||
float Y2X = y2x_sum / size; //sum( Y[n]^2 * X[n] )
|
||||
float Y2Z = y2z_sum / size; //sum( Y[n]^2 * Z[n] )
|
||||
float Z2X = z2x_sum / size; //sum( Z[n]^2 * X[n] )
|
||||
float Z2Y = z2y_sum / size; //sum( Z[n]^2 * Y[n] )
|
||||
|
||||
//Reduction of multiplications
|
||||
float F0 = x_sum2 + y_sum2 + z_sum2;
|
||||
float F1 = 0.5f * F0;
|
||||
float F2 = -8.0f * (x_sum3 + Y2X + Z2X);
|
||||
float F3 = -8.0f * (X2Y + y_sum3 + Z2Y);
|
||||
float F4 = -8.0f * (X2Z + Y2Z + z_sum3);
|
||||
|
||||
//Set initial conditions:
|
||||
float A = x_sum;
|
||||
float B = y_sum;
|
||||
float C = z_sum;
|
||||
|
||||
//First iteration computation:
|
||||
float A2 = A * A;
|
||||
float B2 = B * B;
|
||||
float C2 = C * C;
|
||||
float QS = A2 + B2 + C2;
|
||||
float QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum);
|
||||
|
||||
//Set initial conditions:
|
||||
float Rsq = F0 + QB + QS;
|
||||
|
||||
//First iteration computation:
|
||||
float Q0 = 0.5f * (QS - Rsq);
|
||||
float Q1 = F1 + Q0;
|
||||
float Q2 = 8.0f * (QS - Rsq + QB + F0);
|
||||
float aA, aB, aC, nA, nB, nC, dA, dB, dC;
|
||||
|
||||
//Iterate N times, ignore stop condition.
|
||||
int n = 0;
|
||||
|
||||
while (n < max_iterations) {
|
||||
n++;
|
||||
|
||||
//Compute denominator:
|
||||
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;
|
||||
|
||||
//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);
|
||||
nB = B - ((F3 + 16.0f * (A * XY + C * YZ + y_sum * (-B2 - Q0) + B * (y_sum2 + Q1 - A * x_sum - C * z_sum))) / aB);
|
||||
nC = C - ((F4 + 16.0f * (A * XZ + B * YZ + z_sum * (-C2 - Q0) + C * (z_sum2 + Q1 - A * x_sum - B * y_sum))) / aC);
|
||||
|
||||
//Check for stop condition
|
||||
dA = (nA - A);
|
||||
dB = (nB - B);
|
||||
dC = (nC - C);
|
||||
|
||||
if ((dA * dA + dB * dB + dC * dC) <= delta) { break; }
|
||||
|
||||
//Compute next iteration's values
|
||||
A = nA;
|
||||
B = nB;
|
||||
C = nC;
|
||||
A2 = A * A;
|
||||
B2 = B * B;
|
||||
C2 = C * C;
|
||||
QS = A2 + B2 + C2;
|
||||
QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum);
|
||||
Rsq = F0 + QB + QS;
|
||||
Q0 = 0.5f * (QS - Rsq);
|
||||
Q1 = F1 + Q0;
|
||||
Q2 = 8.0f * (QS - Rsq + QB + F0);
|
||||
}
|
||||
|
||||
*sphere_x = A;
|
||||
*sphere_y = B;
|
||||
*sphere_z = C;
|
||||
*sphere_radius = sqrtf(Rsq);
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,61 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* 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 calibration_routines.h
|
||||
* Calibration routines definitions.
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
/**
|
||||
* Least-squares fit of a sphere to a set of points.
|
||||
*
|
||||
* Fits a sphere to a set of points on the sphere surface.
|
||||
*
|
||||
* @param x point coordinates on the X axis
|
||||
* @param y point coordinates on the Y axis
|
||||
* @param z point coordinates on the Z axis
|
||||
* @param size number of points
|
||||
* @param max_iterations abort if maximum number of iterations have been reached. If unsure, set to 100.
|
||||
* @param delta abort if error is below delta. If unsure, set to 0 to run max_iterations times.
|
||||
* @param sphere_x coordinate of the sphere center on the X axis
|
||||
* @param sphere_y coordinate of the sphere center on the Y axis
|
||||
* @param sphere_z coordinate of the sphere center on the Z axis
|
||||
* @param sphere_radius sphere radius
|
||||
*
|
||||
* @return 0 on success, 1 on failure
|
||||
*/
|
||||
int sphere_fit_least_squares(const float x[], const float y[], const float z[],
|
||||
unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius);
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,58 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
|
||||
* Lorenz Meier <lm@inf.ethz.ch>
|
||||
* Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* Julian Oes <joes@student.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 commander.h
|
||||
* Main system state machine definition.
|
||||
*
|
||||
* @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef COMMANDER_H_
|
||||
#define COMMANDER_H_
|
||||
|
||||
#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
|
||||
#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
|
||||
|
||||
void tune_confirm(void);
|
||||
void tune_error(void);
|
||||
|
||||
#endif /* COMMANDER_H_ */
|
||||
@@ -0,0 +1,41 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (C) 2012-2013 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
|
||||
# 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# Main system state machine
|
||||
#
|
||||
|
||||
MODULE_COMMAND = commander
|
||||
SRCS = commander.c \
|
||||
state_machine_helper.c \
|
||||
calibration_routines.c
|
||||
@@ -0,0 +1,752 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* Julian Oes <joes@student.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 state_machine_helper.c
|
||||
* State machine helper functions implementations
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include "state_machine_helper.h"
|
||||
|
||||
static const char *system_state_txt[] = {
|
||||
"SYSTEM_STATE_PREFLIGHT",
|
||||
"SYSTEM_STATE_STANDBY",
|
||||
"SYSTEM_STATE_GROUND_READY",
|
||||
"SYSTEM_STATE_MANUAL",
|
||||
"SYSTEM_STATE_STABILIZED",
|
||||
"SYSTEM_STATE_AUTO",
|
||||
"SYSTEM_STATE_MISSION_ABORT",
|
||||
"SYSTEM_STATE_EMCY_LANDING",
|
||||
"SYSTEM_STATE_EMCY_CUTOFF",
|
||||
"SYSTEM_STATE_GROUND_ERROR",
|
||||
"SYSTEM_STATE_REBOOT",
|
||||
|
||||
};
|
||||
|
||||
/**
|
||||
* Transition from one state to another
|
||||
*/
|
||||
int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state)
|
||||
{
|
||||
int invalid_state = false;
|
||||
int ret = ERROR;
|
||||
|
||||
commander_state_machine_t old_state = current_status->state_machine;
|
||||
|
||||
switch (new_state) {
|
||||
case SYSTEM_STATE_MISSION_ABORT: {
|
||||
/* Indoor or outdoor */
|
||||
// if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
|
||||
ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING);
|
||||
|
||||
// } else {
|
||||
// ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF);
|
||||
// }
|
||||
}
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_EMCY_LANDING:
|
||||
/* Tell the controller to land */
|
||||
|
||||
/* set system flags according to state */
|
||||
current_status->flag_system_armed = true;
|
||||
|
||||
warnx("EMERGENCY LANDING!\n");
|
||||
mavlink_log_critical(mavlink_fd, "EMERGENCY LANDING!");
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_EMCY_CUTOFF:
|
||||
/* Tell the controller to cutoff the motors (thrust = 0) */
|
||||
|
||||
/* set system flags according to state */
|
||||
current_status->flag_system_armed = false;
|
||||
|
||||
warnx("EMERGENCY MOTOR CUTOFF!\n");
|
||||
mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!");
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_GROUND_ERROR:
|
||||
|
||||
/* set system flags according to state */
|
||||
|
||||
/* prevent actuators from arming */
|
||||
current_status->flag_system_armed = false;
|
||||
|
||||
warnx("GROUND ERROR, locking down propulsion system\n");
|
||||
mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system");
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_PREFLIGHT:
|
||||
if (current_status->state_machine == SYSTEM_STATE_STANDBY
|
||||
|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
|
||||
/* set system flags according to state */
|
||||
current_status->flag_system_armed = false;
|
||||
mavlink_log_critical(mavlink_fd, "Switched to PREFLIGHT state");
|
||||
|
||||
} else {
|
||||
invalid_state = true;
|
||||
mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state");
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_REBOOT:
|
||||
if (current_status->state_machine == SYSTEM_STATE_STANDBY
|
||||
|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT
|
||||
|| current_status->flag_hil_enabled) {
|
||||
invalid_state = false;
|
||||
/* set system flags according to state */
|
||||
current_status->flag_system_armed = false;
|
||||
mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM");
|
||||
usleep(500000);
|
||||
up_systemreset();
|
||||
/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
|
||||
|
||||
} else {
|
||||
invalid_state = true;
|
||||
mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT");
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_STANDBY:
|
||||
/* set system flags according to state */
|
||||
|
||||
/* standby enforces disarmed */
|
||||
current_status->flag_system_armed = false;
|
||||
|
||||
mavlink_log_critical(mavlink_fd, "Switched to STANDBY state");
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_GROUND_READY:
|
||||
|
||||
/* set system flags according to state */
|
||||
|
||||
/* ground ready has motors / actuators armed */
|
||||
current_status->flag_system_armed = true;
|
||||
|
||||
mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state");
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_AUTO:
|
||||
|
||||
/* set system flags according to state */
|
||||
|
||||
/* auto is airborne and in auto mode, motors armed */
|
||||
current_status->flag_system_armed = true;
|
||||
|
||||
mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode");
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_STABILIZED:
|
||||
|
||||
/* set system flags according to state */
|
||||
current_status->flag_system_armed = true;
|
||||
|
||||
mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode");
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_MANUAL:
|
||||
|
||||
/* set system flags according to state */
|
||||
current_status->flag_system_armed = true;
|
||||
|
||||
mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode");
|
||||
break;
|
||||
|
||||
default:
|
||||
invalid_state = true;
|
||||
break;
|
||||
}
|
||||
|
||||
if (invalid_state == false || old_state != new_state) {
|
||||
current_status->state_machine = new_state;
|
||||
state_machine_publish(status_pub, current_status, mavlink_fd);
|
||||
publish_armed_status(current_status);
|
||||
ret = OK;
|
||||
}
|
||||
|
||||
if (invalid_state) {
|
||||
mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition");
|
||||
ret = ERROR;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
|
||||
{
|
||||
/* publish the new state */
|
||||
current_status->counter++;
|
||||
current_status->timestamp = hrt_absolute_time();
|
||||
|
||||
/* assemble state vector based on flag values */
|
||||
if (current_status->flag_control_rates_enabled) {
|
||||
current_status->onboard_control_sensors_present |= 0x400;
|
||||
|
||||
} else {
|
||||
current_status->onboard_control_sensors_present &= ~0x400;
|
||||
}
|
||||
|
||||
current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
|
||||
current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
|
||||
current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
|
||||
current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
|
||||
|
||||
current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0;
|
||||
current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
|
||||
current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
|
||||
current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
|
||||
current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
|
||||
|
||||
orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
|
||||
printf("[cmd] 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;
|
||||
/* lock down actuators if required, only in HIL */
|
||||
armed.lockdown = (current_status->flag_hil_enabled) ? true : false;
|
||||
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
|
||||
*/
|
||||
void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
|
||||
{
|
||||
warnx("EMERGENCY HANDLER\n");
|
||||
/* Depending on the current state go to one of the error states */
|
||||
|
||||
if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
|
||||
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR);
|
||||
|
||||
} else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) {
|
||||
|
||||
// DO NOT abort mission
|
||||
//do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT);
|
||||
|
||||
} else {
|
||||
warnx("Unknown system state: #%d\n", current_status->state_machine);
|
||||
}
|
||||
}
|
||||
|
||||
void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors
|
||||
{
|
||||
if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself
|
||||
state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd);
|
||||
|
||||
} else {
|
||||
//global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
// /*
|
||||
// * 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;
|
||||
// }
|
||||
|
||||
// }
|
||||
|
||||
|
||||
// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
|
||||
// {
|
||||
// current_status->onboard_control_sensors_health |= 1 << *subsystem_type;
|
||||
// current_status->counter++;
|
||||
// current_status->timestamp = hrt_absolute_time();
|
||||
// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
|
||||
|
||||
// switch (*subsystem_type) {
|
||||
// case SUBSYSTEM_TYPE_GYRO:
|
||||
// //TODO state machine change (recovering)
|
||||
// break;
|
||||
|
||||
// case SUBSYSTEM_TYPE_ACC:
|
||||
// //TODO state machine change
|
||||
// break;
|
||||
|
||||
// case SUBSYSTEM_TYPE_MAG:
|
||||
// //TODO state machine change
|
||||
// break;
|
||||
|
||||
// case SUBSYSTEM_TYPE_GPS:
|
||||
// //TODO state machine change
|
||||
// break;
|
||||
|
||||
// default:
|
||||
// break;
|
||||
// }
|
||||
|
||||
|
||||
// }
|
||||
|
||||
|
||||
// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
|
||||
// {
|
||||
// bool previosly_healthy = (bool)(current_status->onboard_control_sensors_health & 1 << *subsystem_type);
|
||||
// current_status->onboard_control_sensors_health &= ~(1 << *subsystem_type);
|
||||
// current_status->counter++;
|
||||
// current_status->timestamp = hrt_absolute_time();
|
||||
// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
|
||||
|
||||
// /* if we received unhealthy message more than *_HEALTH_COUNTER_LIMIT, switch to error state */
|
||||
|
||||
// switch (*subsystem_type) {
|
||||
// case SUBSYSTEM_TYPE_GYRO:
|
||||
// //global_data_send_mavlink_statustext_message_out("Commander: gyro unhealthy", MAV_SEVERITY_CRITICAL);
|
||||
|
||||
// if (previosly_healthy) //only throw emergency if previously healthy
|
||||
// state_machine_emergency_always_critical(status_pub, current_status);
|
||||
|
||||
// break;
|
||||
|
||||
// case SUBSYSTEM_TYPE_ACC:
|
||||
// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer unhealthy", MAV_SEVERITY_CRITICAL);
|
||||
|
||||
// if (previosly_healthy) //only throw emergency if previously healthy
|
||||
// state_machine_emergency_always_critical(status_pub, current_status);
|
||||
|
||||
// break;
|
||||
|
||||
// case SUBSYSTEM_TYPE_MAG:
|
||||
// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer unhealthy", MAV_SEVERITY_CRITICAL);
|
||||
|
||||
// if (previosly_healthy) //only throw emergency if previously healthy
|
||||
// state_machine_emergency_always_critical(status_pub, current_status);
|
||||
|
||||
// break;
|
||||
|
||||
// case SUBSYSTEM_TYPE_GPS:
|
||||
// // //TODO: remove this block
|
||||
// // break;
|
||||
// // ///////////////////
|
||||
// //global_data_send_mavlink_statustext_message_out("Commander: GPS unhealthy", MAV_SEVERITY_CRITICAL);
|
||||
|
||||
// // printf("previosly_healthy = %u\n", previosly_healthy);
|
||||
// if (previosly_healthy) //only throw emergency if previously healthy
|
||||
// state_machine_emergency(status_pub, current_status);
|
||||
|
||||
// break;
|
||||
|
||||
// default:
|
||||
// break;
|
||||
// }
|
||||
|
||||
// }
|
||||
|
||||
|
||||
/* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
|
||||
|
||||
|
||||
void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
|
||||
{
|
||||
/* Depending on the current state switch state */
|
||||
if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
|
||||
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
|
||||
}
|
||||
}
|
||||
|
||||
void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
|
||||
{
|
||||
/* Depending on the current state switch state */
|
||||
if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_AUTO) {
|
||||
state_machine_emergency(status_pub, current_status, mavlink_fd);
|
||||
}
|
||||
}
|
||||
|
||||
void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
|
||||
{
|
||||
if (current_status->state_machine == SYSTEM_STATE_STANDBY) {
|
||||
printf("[cmd] arming\n");
|
||||
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
|
||||
}
|
||||
}
|
||||
|
||||
void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
|
||||
{
|
||||
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
|
||||
printf("[cmd] going standby\n");
|
||||
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
|
||||
|
||||
} else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
|
||||
printf("[cmd] MISSION ABORT!\n");
|
||||
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
|
||||
}
|
||||
}
|
||||
|
||||
void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
|
||||
{
|
||||
int old_mode = current_status->flight_mode;
|
||||
current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
|
||||
|
||||
current_status->flag_control_manual_enabled = true;
|
||||
|
||||
/* set behaviour based on airframe */
|
||||
if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
|
||||
(current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
|
||||
(current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) {
|
||||
|
||||
/* assuming a rotary wing, set to SAS */
|
||||
current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
|
||||
current_status->flag_control_attitude_enabled = true;
|
||||
current_status->flag_control_rates_enabled = true;
|
||||
|
||||
} else {
|
||||
|
||||
/* assuming a fixed wing, set to direct pass-through */
|
||||
current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
|
||||
current_status->flag_control_attitude_enabled = false;
|
||||
current_status->flag_control_rates_enabled = false;
|
||||
}
|
||||
|
||||
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
|
||||
|
||||
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
|
||||
printf("[cmd] manual mode\n");
|
||||
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
|
||||
}
|
||||
}
|
||||
|
||||
void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
|
||||
{
|
||||
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
|
||||
int old_mode = current_status->flight_mode;
|
||||
int old_manual_control_mode = current_status->manual_control_mode;
|
||||
current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
|
||||
current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
|
||||
current_status->flag_control_attitude_enabled = true;
|
||||
current_status->flag_control_rates_enabled = true;
|
||||
current_status->flag_control_manual_enabled = true;
|
||||
|
||||
if (old_mode != current_status->flight_mode ||
|
||||
old_manual_control_mode != current_status->manual_control_mode) {
|
||||
printf("[cmd] att stabilized mode\n");
|
||||
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
|
||||
state_machine_publish(status_pub, current_status, mavlink_fd);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
|
||||
{
|
||||
if (!current_status->flag_vector_flight_mode_ok) {
|
||||
mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE");
|
||||
tune_error();
|
||||
return;
|
||||
}
|
||||
|
||||
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
|
||||
printf("[cmd] position guided mode\n");
|
||||
int old_mode = current_status->flight_mode;
|
||||
current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB;
|
||||
current_status->flag_control_manual_enabled = false;
|
||||
current_status->flag_control_attitude_enabled = true;
|
||||
current_status->flag_control_rates_enabled = true;
|
||||
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED);
|
||||
|
||||
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
|
||||
{
|
||||
if (!current_status->flag_vector_flight_mode_ok) {
|
||||
mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE");
|
||||
return;
|
||||
}
|
||||
|
||||
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) {
|
||||
printf("[cmd] auto mode\n");
|
||||
int old_mode = current_status->flight_mode;
|
||||
current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
|
||||
current_status->flag_control_manual_enabled = false;
|
||||
current_status->flag_control_attitude_enabled = true;
|
||||
current_status->flag_control_rates_enabled = true;
|
||||
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
|
||||
|
||||
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
|
||||
{
|
||||
uint8_t 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_system_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_system_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_system_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;
|
||||
}
|
||||
|
||||
uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations
|
||||
{
|
||||
commander_state_machine_t current_system_state = current_status->state_machine;
|
||||
|
||||
uint8_t ret = 1;
|
||||
|
||||
switch (custom_mode) {
|
||||
case SYSTEM_STATE_GROUND_READY:
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_STANDBY:
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_REBOOT:
|
||||
printf("try to reboot\n");
|
||||
|
||||
if (current_system_state == SYSTEM_STATE_STANDBY
|
||||
|| current_system_state == SYSTEM_STATE_PREFLIGHT
|
||||
|| current_status->flag_hil_enabled) {
|
||||
printf("system will reboot\n");
|
||||
mavlink_log_critical(mavlink_fd, "Rebooting..");
|
||||
usleep(200000);
|
||||
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT);
|
||||
ret = 0;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_AUTO:
|
||||
printf("try to switch to auto/takeoff\n");
|
||||
|
||||
if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_MANUAL) {
|
||||
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
|
||||
printf("state: auto\n");
|
||||
ret = 0;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_MANUAL:
|
||||
printf("try to switch to manual\n");
|
||||
|
||||
if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_AUTO) {
|
||||
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
|
||||
printf("state: manual\n");
|
||||
ret = 0;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,209 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* Julian Oes <joes@student.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 state_machine_helper.h
|
||||
* State machine helper functions definitions
|
||||
*/
|
||||
|
||||
#ifndef STATE_MACHINE_HELPER_H_
|
||||
#define STATE_MACHINE_HELPER_H_
|
||||
|
||||
#define GPS_NOFIX_COUNTER_LIMIT 4 //need GPS_NOFIX_COUNTER_LIMIT gps packets with a bad fix to call an error (if outdoor)
|
||||
#define GPS_GOTFIX_COUNTER_REQUIRED 4 //need GPS_GOTFIX_COUNTER_REQUIRED gps packets with a good fix to obtain position lock
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
||||
/**
|
||||
* Switch to new state with no checking.
|
||||
*
|
||||
* do_state_update: this is the functions that all other functions have to call in order to update the state.
|
||||
* the function does not question the state change, this must be done before
|
||||
* The function performs actions that are connected with the new state (buzzer, reboot, ...)
|
||||
*
|
||||
* @param status_pub file descriptor for state update topic publication
|
||||
* @param current_status pointer to the current state machine to operate on
|
||||
* @param mavlink_fd file descriptor for MAVLink statustext messages
|
||||
*
|
||||
* @return 0 (macro OK) or 1 on error (macro ERROR)
|
||||
*/
|
||||
int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state);
|
||||
|
||||
/* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR */
|
||||
// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
|
||||
// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
|
||||
|
||||
// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
|
||||
// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
|
||||
|
||||
// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
|
||||
// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
|
||||
|
||||
|
||||
/**
|
||||
* Handle state machine if got position fix
|
||||
*
|
||||
* @param status_pub file descriptor for state update topic publication
|
||||
* @param current_status pointer to the current state machine to operate on
|
||||
* @param mavlink_fd file descriptor for MAVLink statustext messages
|
||||
*/
|
||||
void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
|
||||
|
||||
/**
|
||||
* Handle state machine if position fix lost
|
||||
*
|
||||
* @param status_pub file descriptor for state update topic publication
|
||||
* @param current_status pointer to the current state machine to operate on
|
||||
* @param mavlink_fd file descriptor for MAVLink statustext messages
|
||||
*/
|
||||
void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
|
||||
|
||||
/**
|
||||
* Handle state machine if user wants to arm
|
||||
*
|
||||
* @param status_pub file descriptor for state update topic publication
|
||||
* @param current_status pointer to the current state machine to operate on
|
||||
* @param mavlink_fd file descriptor for MAVLink statustext messages
|
||||
*/
|
||||
void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
|
||||
|
||||
/**
|
||||
* Handle state machine if user wants to disarm
|
||||
*
|
||||
* @param status_pub file descriptor for state update topic publication
|
||||
* @param current_status pointer to the current state machine to operate on
|
||||
* @param mavlink_fd file descriptor for MAVLink statustext messages
|
||||
*/
|
||||
void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
|
||||
|
||||
/**
|
||||
* Handle state machine if mode switch is manual
|
||||
*
|
||||
* @param status_pub file descriptor for state update topic publication
|
||||
* @param current_status pointer to the current state machine to operate on
|
||||
* @param mavlink_fd file descriptor for MAVLink statustext messages
|
||||
*/
|
||||
void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
|
||||
|
||||
/**
|
||||
* Handle state machine if mode switch is stabilized
|
||||
*
|
||||
* @param status_pub file descriptor for state update topic publication
|
||||
* @param current_status pointer to the current state machine to operate on
|
||||
* @param mavlink_fd file descriptor for MAVLink statustext messages
|
||||
*/
|
||||
void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
|
||||
|
||||
/**
|
||||
* Handle state machine if mode switch is guided
|
||||
*
|
||||
* @param status_pub file descriptor for state update topic publication
|
||||
* @param current_status pointer to the current state machine to operate on
|
||||
* @param mavlink_fd file descriptor for MAVLink statustext messages
|
||||
*/
|
||||
void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
|
||||
|
||||
/**
|
||||
* Handle state machine if mode switch is auto
|
||||
*
|
||||
* @param status_pub file descriptor for state update topic publication
|
||||
* @param current_status pointer to the current state machine to operate on
|
||||
* @param mavlink_fd file descriptor for MAVLink statustext messages
|
||||
*/
|
||||
void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
|
||||
|
||||
/**
|
||||
* Publish current state information
|
||||
*
|
||||
* @param status_pub file descriptor for state update topic publication
|
||||
* @param current_status pointer to the current state machine to operate on
|
||||
* @param mavlink_fd file descriptor for MAVLink statustext messages
|
||||
*/
|
||||
void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
|
||||
|
||||
|
||||
/*
|
||||
* Functions that handle incoming requests to change the state machine or a parameter (probably from the mavlink app).
|
||||
* If the request is obeyed the functions return 0
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* Handles *incoming request* to switch to a specific state, if state change is successful returns 0
|
||||
*
|
||||
* @param status_pub file descriptor for state update topic publication
|
||||
* @param current_status pointer to the current state machine to operate on
|
||||
* @param mavlink_fd file descriptor for MAVLink statustext messages
|
||||
*/
|
||||
uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode);
|
||||
|
||||
/**
|
||||
* Handles *incoming request* to switch to a specific custom state, if state change is successful returns 0
|
||||
*
|
||||
* @param status_pub file descriptor for state update topic publication
|
||||
* @param current_status pointer to the current state machine to operate on
|
||||
* @param mavlink_fd file descriptor for MAVLink statustext messages
|
||||
*/
|
||||
uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode);
|
||||
|
||||
/**
|
||||
* Always switches to critical mode under any circumstances.
|
||||
*
|
||||
* @param status_pub file descriptor for state update topic publication
|
||||
* @param current_status pointer to the current state machine to operate on
|
||||
* @param mavlink_fd file descriptor for MAVLink statustext messages
|
||||
*/
|
||||
void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
|
||||
|
||||
/**
|
||||
* Switches to emergency if required.
|
||||
*
|
||||
* @param status_pub file descriptor for state update topic publication
|
||||
* @param current_status pointer to the current state machine to operate on
|
||||
* @param mavlink_fd file descriptor for MAVLink statustext messages
|
||||
*/
|
||||
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);
|
||||
|
||||
|
||||
|
||||
#endif /* STATE_MACHINE_HELPER_H_ */
|
||||
Reference in New Issue
Block a user