fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-21 23:57:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'px4/new_state_machine_drton' into fmuv2_bringup_new_state_machine_drton

Browse Source 
Conflicts:
	src/drivers/blinkm/blinkm.cpp
	src/drivers/px4io/px4io.cpp
	src/modules/commander/state_machine_helper.c
	src/modules/px4iofirmware/protocol.h
	src/modules/px4iofirmware/registers.c
	src/modules/systemlib/systemlib.h
	src/systemcmds/reboot/reboot.c
This commit is contained in:
Julian Oes
2013-08-15 14:04:46 +02:00
parent 50cf1c01b7 561ec495b7
commit 56575eb068
86 changed files with 5851 additions and 3938 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+7 -7
View File
@@ -57,7 +57,7 @@
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>
@@ -216,8 +216,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
memset(&raw, 0, sizeof(raw));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_status_s state;
memset(&state, 0, sizeof(state));
struct vehicle_control_mode_s control_mode;
memset(&control_mode, 0, sizeof(control_mode));
uint64_t last_data = 0;
uint64_t last_measurement = 0;
@@ -230,8 +230,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* subscribe to param changes */
int sub_params = orb_subscribe(ORB_ID(parameter_update));
/* subscribe to system state*/
int sub_state = orb_subscribe(ORB_ID(vehicle_status));
/* subscribe to control mode*/
int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
/* advertise attitude */
orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
@@ -282,9 +282,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* XXX this is seriously bad - should be an emergency */
} else if (ret == 0) {
/* check if we're in HIL - not getting sensor data is fine then */
orb_copy(ORB_ID(vehicle_status), sub_state, &state);
orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);
if (!state.flag_hil_enabled) {
if (!control_mode.flag_system_hil_enabled) {
fprintf(stderr,
"[att ekf] WARNING: Not getting sensors - sensor app running?\n");
}
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
+7 -7
View File
@@ -35,7 +35,7 @@
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>
@@ -547,8 +547,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_status_s state;
memset(&state, 0, sizeof(state));
struct vehicle_control_mode_s control_mode;
memset(&control_mode, 0, sizeof(control_mode));
uint64_t last_data = 0;
uint64_t last_measurement = 0;
@@ -561,8 +561,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* subscribe to param changes */
int sub_params = orb_subscribe(ORB_ID(parameter_update));
/* subscribe to system state*/
int sub_state = orb_subscribe(ORB_ID(vehicle_status));
/* subscribe to control mode */
int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
/* advertise attitude */
orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
@@ -612,9 +612,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* XXX this is seriously bad - should be an emergency */
} else if (ret == 0) {
/* check if we're in HIL - not getting sensor data is fine then */
orb_copy(ORB_ID(vehicle_status), sub_state, &state);
orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);
if (!state.flag_hil_enabled) {
if (!control_mode.flag_system_hil_enabled) {
fprintf(stderr,
"[att so3_comp] WARNING: Not getting sensors - sensor app running?\n");
}
src/modules/commander/accelerometer_calibration.c → src/modules/commander/accelerometer_calibration.cpp
+52 -41
View File
@@ -33,7 +33,7 @@
****************************************************************************/
/**
* @file accelerometer_calibration.c
* @file accelerometer_calibration.cpp
*
* Implementation of accelerometer calibration.
*
@@ -104,32 +104,43 @@
*/
#include "accelerometer_calibration.h"
#include "commander_helper.h"
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <fcntl.h>
#include <sys/prctl.h>
#include <math.h>
#include <string.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_combined.h>
#include <drivers/drv_accel.h>
#include <systemlib/conversions.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <mavlink/mavlink_log.h>
void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd);
int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]);
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]);
int detect_orientation(int mavlink_fd, int sub_sensor_combined);
int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num);
int mat_invert3(float src[3][3], float dst[3][3]);
int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g);
void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd) {
void do_accel_calibration(int mavlink_fd) {
/* announce change */
mavlink_log_info(mavlink_fd, "accel calibration started");
/* set to accel calibration mode */
status->flag_preflight_accel_calibration = true;
state_machine_publish(status_pub, status, mavlink_fd);
/* measure and calculate offsets & scales */
float accel_offs[3];
float accel_scale[3];
int res = do_accel_calibration_mesurements(mavlink_fd, accel_offs, accel_scale);
int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale);
if (res == OK) {
/* measurements complete successfully, set parameters */
@@ -165,24 +176,17 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m
}
mavlink_log_info(mavlink_fd, "accel calibration done");
tune_confirm();
sleep(2);
tune_confirm();
sleep(2);
/* third beep by cal end routine */
tune_positive();
} else {
/* measurements error */
mavlink_log_info(mavlink_fd, "accel calibration aborted");
tune_error();
sleep(2);
tune_negative();
}
/* exit accel calibration mode */
status->flag_preflight_accel_calibration = false;
state_machine_publish(status_pub, status, mavlink_fd);
}
int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) {
int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) {
const int samples_num = 2500;
float accel_ref[6][3];
bool data_collected[6] = { false, false, false, false, false, false };
@@ -229,10 +233,13 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float
sprintf(str, "meas started: %s", orientation_strs[orient]);
mavlink_log_info(mavlink_fd, str);
read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num);
str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], accel_ref[orient][0], accel_ref[orient][1], accel_ref[orient][2]);
str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient],
(double)accel_ref[orient][0],
(double)accel_ref[orient][1],
(double)accel_ref[orient][2]);
mavlink_log_info(mavlink_fd, str);
data_collected[orient] = true;
tune_confirm();
tune_neutral();
}
close(sensor_combined_sub);
@@ -274,7 +281,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
float accel_err_thr = 5.0f;
/* still time required in us */
int64_t still_time = 2000000;
struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
struct pollfd fds[1];
fds[0].fd = sub_sensor_combined;
fds[0].events = POLLIN;
hrt_abstime t_start = hrt_absolute_time();
/* set timeout to 30s */
@@ -342,29 +351,29 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
}
}
if ( fabs(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
fabs(accel_ema[1]) < accel_err_thr &&
fabs(accel_ema[2]) < accel_err_thr )
if ( fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr )
return 0; // [ g, 0, 0 ]
if ( fabs(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
fabs(accel_ema[1]) < accel_err_thr &&
fabs(accel_ema[2]) < accel_err_thr )
if ( fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr )
return 1; // [ -g, 0, 0 ]
if ( fabs(accel_ema[0]) < accel_err_thr &&
fabs(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
fabs(accel_ema[2]) < accel_err_thr )
if ( fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr )
return 2; // [ 0, g, 0 ]
if ( fabs(accel_ema[0]) < accel_err_thr &&
fabs(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
fabs(accel_ema[2]) < accel_err_thr )
if ( fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr )
return 3; // [ 0, -g, 0 ]
if ( fabs(accel_ema[0]) < accel_err_thr &&
fabs(accel_ema[1]) < accel_err_thr &&
fabs(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr )
if ( fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr )
return 4; // [ 0, 0, g ]
if ( fabs(accel_ema[0]) < accel_err_thr &&
fabs(accel_ema[1]) < accel_err_thr &&
fabs(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr )
if ( fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr )
return 5; // [ 0, 0, -g ]
mavlink_log_info(mavlink_fd, "ERROR: invalid orientation");
@@ -376,7 +385,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
* Read specified number of accelerometer samples, calculate average and dispersion.
*/
int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) {
struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } };
struct pollfd fds[1];
fds[0].fd = sensor_combined_sub;
fds[0].events = POLLIN;
int count = 0;
float accel_sum[3] = { 0.0f, 0.0f, 0.0f };
@@ -404,7 +415,7 @@ int mat_invert3(float src[3][3], float dst[3][3]) {
float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) -
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.0)
if (det == 0.0f)
return ERROR; // Singular matrix
dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det;
src/modules/commander/accelerometer_calibration.h
+1 -2
View File
@@ -44,8 +44,7 @@
#define ACCELEROMETER_CALIBRATION_H_
#include <stdint.h>
#include <uORB/topics/vehicle_status.h>
void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd);
void do_accel_calibration(int mavlink_fd);
#endif /* ACCELEROMETER_CALIBRATION_H_ */
src/modules/commander/airspeed_calibration.cpp
+113
View File
@@ -0,0 +1,113 @@
/****************************************************************************
*
* Copyright (C) 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.
*
****************************************************************************/
/**
* @file airspeed_calibration.cpp
* Airspeed sensor calibration routine
*/
#include "airspeed_calibration.h"
#include "commander_helper.h"
#include <stdio.h>
#include <poll.h>
#include <math.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/differential_pressure.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
void do_airspeed_calibration(int mavlink_fd)
{
/* give directions */
mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still");
const int calibration_count = 2500;
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s diff_pres;
int calibration_counter = 0;
float diff_pres_offset = 0.0f;
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
struct pollfd fds[1];
fds[0].fd = diff_pres_sub;
fds[0].events = POLLIN;
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
diff_pres_offset += diff_pres.differential_pressure_pa;
calibration_counter++;
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "airspeed calibration aborted");
return;
}
}
diff_pres_offset = diff_pres_offset / calibration_count;
if (isfinite(diff_pres_offset)) {
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, "Setting offs failed!");
}
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
}
//char buf[50];
//sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]);
//mavlink_log_info(mavlink_fd, buf);
mavlink_log_info(mavlink_fd, "airspeed calibration done");
tune_positive();
} else {
mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
}
close(diff_pres_sub);
}
src/modules/commander/airspeed_calibration.h
+46
View File
@@ -0,0 +1,46 @@
/****************************************************************************
*
* Copyright (C) 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.
*
****************************************************************************/
/**
* @file gyro_calibration.h
* Airspeed sensor calibration routine
*/
#ifndef AIRSPEED_CALIBRATION_H_
#define AIRSPEED_CALIBRATION_H_
#include <stdint.h>
void do_airspeed_calibration(int mavlink_fd);
#endif /* AIRSPEED_CALIBRATION_H_ */
src/modules/commander/baro_calibration.cpp
+54
View File
@@ -0,0 +1,54 @@
/****************************************************************************
*
* Copyright (C) 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.
*
****************************************************************************/
/**
* @file baro_calibration.cpp
* Barometer calibration routine
*/
#include "baro_calibration.h"
#include <poll.h>
#include <math.h>
#include <fcntl.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_combined.h>
#include <drivers/drv_baro.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/param/param.h>
void do_baro_calibration(int mavlink_fd)
{
// TODO implement this
return;
}
src/modules/commander/baro_calibration.h
+46
View File
@@ -0,0 +1,46 @@
/****************************************************************************
*
* Copyright (C) 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.
*
****************************************************************************/
/**
* @file mag_calibration.h
* Barometer calibration routine
*/
#ifndef BARO_CALIBRATION_H_
#define BARO_CALIBRATION_H_
#include <stdint.h>
void do_baro_calibration(int mavlink_fd);
#endif /* BARO_CALIBRATION_H_ */
src/modules/commander/calibration_routines.c → src/modules/commander/calibration_routines.cpp
+2 -1
View File
@@ -33,7 +33,7 @@
****************************************************************************/
/**
* @file calibration_routines.c
* @file calibration_routines.cpp
* Calibration routines implementations.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
@@ -217,3 +217,4 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
return 0;
}
src/modules/commander/commander.c
-2097
View File
File diff suppressed because it is too large Load Diff
src/modules/commander/commander.cpp
+1640
View File
File diff suppressed because it is too large Load Diff
src/modules/commander/commander_helper.cpp
+219
View File
@@ -0,0 +1,219 @@
/****************************************************************************
*
* Copyright (C) 2013 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 commander_helper.cpp
* Commander helper functions implementations
*/
#include <stdio.h>
#include <unistd.h>
#include <stdint.h>
#include <stdbool.h>
#include <fcntl.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <systemlib/err.h>
#include <systemlib/param/param.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_tone_alarm.h>
#include <drivers/drv_led.h>
#include "commander_helper.h"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
bool is_multirotor(const struct vehicle_status_s *current_status)
{
return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
(current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
(current_status->system_type == VEHICLE_TYPE_OCTOROTOR) ||
(current_status->system_type == VEHICLE_TYPE_TRICOPTER));
}
bool is_rotary_wing(const struct vehicle_status_s *current_status)
{
return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER)
|| (current_status->system_type == VEHICLE_TYPE_COAXIAL);
}
static int buzzer;
int buzzer_init()
{
buzzer = open("/dev/tone_alarm", O_WRONLY);
if (buzzer < 0) {
warnx("Buzzer: open fail\n");
return ERROR;
}
return OK;
}
void buzzer_deinit()
{
close(buzzer);
}
void tune_error()
{
ioctl(buzzer, TONE_SET_ALARM, 2);
}
void tune_positive()
{
ioctl(buzzer, TONE_SET_ALARM, 3);
}
void tune_neutral()
{
ioctl(buzzer, TONE_SET_ALARM, 4);
}
void tune_negative()
{
ioctl(buzzer, TONE_SET_ALARM, 5);
}
int tune_arm()
{
return ioctl(buzzer, TONE_SET_ALARM, 12);
}
int tune_critical_bat()
{
return ioctl(buzzer, TONE_SET_ALARM, 14);
}
int tune_low_bat()
{
return ioctl(buzzer, TONE_SET_ALARM, 13);
}
void tune_stop()
{
ioctl(buzzer, TONE_SET_ALARM, 0);
}
static int leds;
int led_init()
{
leds = open(LED_DEVICE_PATH, 0);
if (leds < 0) {
warnx("LED: open fail\n");
return ERROR;
}
if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) {
warnx("LED: ioctl fail\n");
return ERROR;
}
return 0;
}
void led_deinit()
{
close(leds);
}
int led_toggle(int led)
{
static int last_blue = LED_ON;
static int last_amber = LED_ON;
if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON;
if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON;
return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led);
}
int led_on(int led)
{
return ioctl(leds, LED_ON, led);
}
int led_off(int led)
{
return ioctl(leds, LED_OFF, led);
}
float battery_remaining_estimate_voltage(float voltage)
{
float ret = 0;
static param_t bat_volt_empty;
static param_t bat_volt_full;
static param_t bat_n_cells;
static bool initialized = false;
static unsigned int counter = 0;
static float ncells = 3;
// XXX change cells to int (and param to INT32)
if (!initialized) {
bat_volt_empty = param_find("BAT_V_EMPTY");
bat_volt_full = param_find("BAT_V_FULL");
bat_n_cells = param_find("BAT_N_CELLS");
initialized = true;
}
static float chemistry_voltage_empty = 3.2f;
static float chemistry_voltage_full = 4.05f;
if (counter % 100 == 0) {
param_get(bat_volt_empty, &chemistry_voltage_empty);
param_get(bat_volt_full, &chemistry_voltage_full);
param_get(bat_n_cells, &ncells);
}
counter++;
ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty));
/* limit to sane values */
ret = (ret < 0.0f) ? 0.0f : ret;
ret = (ret > 1.0f) ? 1.0f : ret;
return ret;
}
src/modules/commander/commander_helper.h
+80
View File
@@ -0,0 +1,80 @@
/****************************************************************************
*
* Copyright (C) 2013 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 commander_helper.h
* Commander helper functions definitions
*/
#ifndef COMMANDER_HELPER_H_
#define COMMANDER_HELPER_H_
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_control_mode.h>
bool is_multirotor(const struct vehicle_status_s *current_status);
bool is_rotary_wing(const struct vehicle_status_s *current_status);
int buzzer_init(void);
void buzzer_deinit(void);
void tune_error(void);
void tune_positive(void);
void tune_neutral(void);
void tune_negative(void);
int tune_arm(void);
int tune_critical_bat(void);
int tune_low_bat(void);
void tune_stop(void);
int led_init(void);
void led_deinit(void);
int led_toggle(int led);
int led_on(int led);
int led_off(int led);
/**
* Provides a coarse estimate of remaining battery power.
*
* The estimate is very basic and based on decharging voltage curves.
*
* @return the estimated remaining capacity in 0..1
*/
float battery_remaining_estimate_voltage(float voltage);
#endif /* COMMANDER_HELPER_H_ */
src/modules/commander/commander.h → src/modules/commander/commander_params.c
+14 -18
View File
@@ -1,10 +1,7 @@
/****************************************************************************
*
* 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>
* Copyright (c) 2013 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
@@ -36,23 +33,22 @@
****************************************************************************/
/**
* @file commander.h
* Main system state machine definition.
* @file commander_params.c
*
* Parameters defined by the sensors task.
*
* @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_
#include <nuttx/config.h>
#include <systemlib/param/param.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_ */
PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */
PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f);
PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f);
PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3);
src/modules/commander/gyro_calibration.cpp
+283
View File
@@ -0,0 +1,283 @@
/****************************************************************************
*
* Copyright (C) 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.
*
****************************************************************************/
/**
* @file gyro_calibration.cpp
* Gyroscope calibration routine
*/
#include "gyro_calibration.h"
#include "commander_helper.h"
#include <stdio.h>
#include <fcntl.h>
#include <poll.h>
#include <math.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_combined.h>
#include <drivers/drv_gyro.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
void do_gyro_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still");
const int calibration_count = 5000;
int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
struct sensor_combined_s raw;
int calibration_counter = 0;
float gyro_offset[3] = {0.0f, 0.0f, 0.0f};
/* set offsets to zero */
int fd = open(GYRO_DEVICE_PATH, 0);
struct gyro_scale gscale_null = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null))
warn("WARNING: failed to set scale / offsets for gyro");
close(fd);
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
struct pollfd fds[1];
fds[0].fd = sub_sensor_combined;
fds[0].events = POLLIN;
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
gyro_offset[0] += raw.gyro_rad_s[0];
gyro_offset[1] += raw.gyro_rad_s[1];
gyro_offset[2] += raw.gyro_rad_s[2];
calibration_counter++;
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
return;
}
}
gyro_offset[0] = gyro_offset[0] / calibration_count;
gyro_offset[1] = gyro_offset[1] / calibration_count;
gyro_offset[2] = gyro_offset[2] / calibration_count;
if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) {
if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))
|| param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))
|| param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) {
mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!");
}
/* set offsets to actual value */
fd = open(GYRO_DEVICE_PATH, 0);
struct gyro_scale gscale = {
gyro_offset[0],
1.0f,
gyro_offset[1],
1.0f,
gyro_offset[2],
1.0f,
};
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
warn("WARNING: failed to set scale / offsets for gyro");
close(fd);
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warnx("WARNING: auto-save of params to storage failed");
mavlink_log_critical(mavlink_fd, "gyro store failed");
// XXX negative tune
return;
}
mavlink_log_info(mavlink_fd, "gyro calibration done");
tune_positive();
/* third beep by cal end routine */
} else {
mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)");
return;
}
/*** --- SCALING --- ***/
mavlink_log_info(mavlink_fd, "offset calibration finished. Rotate for scale 30x");
mavlink_log_info(mavlink_fd, "or do not rotate and wait for 5 seconds to skip.");
warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip.");
unsigned rotations_count = 30;
float gyro_integral = 0.0f;
float baseline_integral = 0.0f;
// XXX change to mag topic
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
float mag_last = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]);
if (mag_last > M_PI_F) mag_last -= 2*M_PI_F;
if (mag_last < -M_PI_F) mag_last += 2*M_PI_F;
uint64_t last_time = hrt_absolute_time();
uint64_t start_time = hrt_absolute_time();
while ((int)fabsf(baseline_integral / (2.0f * M_PI_F)) < rotations_count) {
/* abort this loop if not rotated more than 180 degrees within 5 seconds */
if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f)
&& (hrt_absolute_time() - start_time > 5 * 1e6)) {
mavlink_log_info(mavlink_fd, "gyro scale calibration skipped");
mavlink_log_info(mavlink_fd, "gyro calibration done");
tune_positive();
return;
}
/* wait blocking for new data */
struct pollfd fds[1];
fds[0].fd = sub_sensor_combined;
fds[0].events = POLLIN;
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
float dt_ms = (hrt_absolute_time() - last_time) / 1e3f;
last_time = hrt_absolute_time();
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
// XXX this is just a proof of concept and needs world / body
// transformation and more
//math::Vector2f magNav(raw.magnetometer_ga);
// calculate error between estimate and measurement
// apply declination correction for true heading as well.
//float mag = -atan2f(magNav(1),magNav(0));
float mag = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]);
if (mag > M_PI_F) mag -= 2*M_PI_F;
if (mag < -M_PI_F) mag += 2*M_PI_F;
float diff = mag - mag_last;
if (diff > M_PI_F) diff -= 2*M_PI_F;
if (diff < -M_PI_F) diff += 2*M_PI_F;
baseline_integral += diff;
mag_last = mag;
// Jump through some timing scale hoops to avoid
// operating near the 1e6/1e8 max sane resolution of float.
gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f;
warnx("dbg: b: %6.4f, g: %6.4f", baseline_integral, gyro_integral);
// } else if (poll_ret == 0) {
// /* any poll failure for 1s is a reason to abort */
// mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
// return;
}
}
float gyro_scale = baseline_integral / gyro_integral;
float gyro_scales[] = { gyro_scale, gyro_scale, gyro_scale };
warnx("gyro scale: yaw (z): %6.4f", gyro_scale);
mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", gyro_scale);
if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) {
if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scales[0]))
|| param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scales[1]))
|| param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scales[2]))) {
mavlink_log_critical(mavlink_fd, "Setting gyro scale failed!");
}
/* set offsets to actual value */
fd = open(GYRO_DEVICE_PATH, 0);
struct gyro_scale gscale = {
gyro_offset[0],
gyro_scales[0],
gyro_offset[1],
gyro_scales[1],
gyro_offset[2],
gyro_scales[2],
};
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
warn("WARNING: failed to set scale / offsets for gyro");
close(fd);
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
}
// char buf[50];
// sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
// mavlink_log_info(mavlink_fd, buf);
mavlink_log_info(mavlink_fd, "gyro calibration done");
tune_positive();
/* third beep by cal end routine */
} else {
mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)");
}
close(sub_sensor_combined);
}
src/modules/commander/gyro_calibration.h
+46
View File
@@ -0,0 +1,46 @@
/****************************************************************************
*
* Copyright (C) 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.
*
****************************************************************************/
/**
* @file gyro_calibration.h
* Gyroscope calibration routine
*/
#ifndef GYRO_CALIBRATION_H_
#define GYRO_CALIBRATION_H_
#include <stdint.h>
void do_gyro_calibration(int mavlink_fd);
#endif /* GYRO_CALIBRATION_H_ */
src/modules/commander/mag_calibration.cpp
+280
View File
@@ -0,0 +1,280 @@
/****************************************************************************
*
* Copyright (C) 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.
*
****************************************************************************/
/**
* @file mag_calibration.cpp
* Magnetometer calibration routine
*/
#include "mag_calibration.h"
#include "commander_helper.h"
#include "calibration_routines.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <poll.h>
#include <math.h>
#include <fcntl.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_combined.h>
#include <drivers/drv_mag.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
void do_mag_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, "mag calibration starting, hold still");
int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
struct mag_report mag;
/* 45 seconds */
uint64_t calibration_interval = 45 * 1000 * 1000;
/* maximum 2000 values */
const unsigned int calibration_maxcount = 500;
unsigned int calibration_counter = 0;
/* limit update rate to get equally spaced measurements over time (in ms) */
orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
/* erase old calibration */
struct mag_scale mscale_null = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
warn("WARNING: failed to set scale / offsets for mag");
mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag");
}
/* calibrate range */
if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) {
warnx("failed to calibrate scale");
}
close(fd);
/* calibrate offsets */
// uint64_t calibration_start = hrt_absolute_time();
uint64_t axis_deadline = hrt_absolute_time();
uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
const char axislabels[3] = { 'X', 'Y', 'Z'};
int axis_index = -1;
float *x = (float *)malloc(sizeof(float) * calibration_maxcount);
float *y = (float *)malloc(sizeof(float) * calibration_maxcount);
float *z = (float *)malloc(sizeof(float) * calibration_maxcount);
if (x == NULL || y == NULL || z == NULL) {
warnx("mag cal failed: out of memory");
mavlink_log_info(mavlink_fd, "mag cal failed: out of memory");
warnx("x:%p y:%p z:%p\n", x, y, z);
return;
}
while (hrt_absolute_time() < calibration_deadline &&
calibration_counter < calibration_maxcount) {
/* wait blocking for new data */
struct pollfd fds[1];
fds[0].fd = sub_mag;
fds[0].events = POLLIN;
/* user guidance */
if (hrt_absolute_time() >= axis_deadline &&
axis_index < 3) {
axis_index++;
char buf[50];
sprintf(buf, "please rotate around %c", axislabels[axis_index]);
mavlink_log_info(mavlink_fd, buf);
tune_neutral();
axis_deadline += calibration_interval / 3;
}
if (!(axis_index < 3)) {
break;
}
// int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time();
// if ((axis_left / 1000) == 0 && axis_left > 0) {
// char buf[50];
// sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]);
// mavlink_log_info(mavlink_fd, buf);
// }
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
x[calibration_counter] = mag.x;
y[calibration_counter] = mag.y;
z[calibration_counter] = mag.z;
/* get min/max values */
// if (mag.x < mag_min[0]) {
// mag_min[0] = mag.x;
// }
// else if (mag.x > mag_max[0]) {
// mag_max[0] = mag.x;
// }
// if (raw.magnetometer_ga[1] < mag_min[1]) {
// mag_min[1] = raw.magnetometer_ga[1];
// }
// else if (raw.magnetometer_ga[1] > mag_max[1]) {
// mag_max[1] = raw.magnetometer_ga[1];
// }
// if (raw.magnetometer_ga[2] < mag_min[2]) {
// mag_min[2] = raw.magnetometer_ga[2];
// }
// else if (raw.magnetometer_ga[2] > mag_max[2]) {
// mag_max[2] = raw.magnetometer_ga[2];
// }
calibration_counter++;
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)");
break;
}
}
float sphere_x;
float sphere_y;
float sphere_z;
float sphere_radius;
sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
free(x);
free(y);
free(z);
if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) {
fd = open(MAG_DEVICE_PATH, 0);
struct mag_scale mscale;
if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale))
warn("WARNING: failed to get scale / offsets for mag");
mscale.x_offset = sphere_x;
mscale.y_offset = sphere_y;
mscale.z_offset = sphere_z;
if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
warn("WARNING: failed to set scale / offsets for mag");
close(fd);
/* announce and set new offset */
if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
warnx("Setting X mag offset failed!\n");
}
if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
warnx("Setting Y mag offset failed!\n");
}
if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
warnx("Setting Z mag offset failed!\n");
}
if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
warnx("Setting X mag scale failed!\n");
}
if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
warnx("Setting Y mag scale failed!\n");
}
if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
warnx("Setting Z mag scale failed!\n");
}
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
mavlink_log_info(mavlink_fd, "FAILED storing calibration");
}
warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n",
(double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale,
(double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius);
char buf[52];
sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
(double)mscale.y_offset, (double)mscale.z_offset);
mavlink_log_info(mavlink_fd, buf);
sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
(double)mscale.y_scale, (double)mscale.z_scale);
mavlink_log_info(mavlink_fd, buf);
mavlink_log_info(mavlink_fd, "mag calibration done");
tune_positive();
/* third beep by cal end routine */
} else {
mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
}
close(sub_mag);
}
src/modules/commander/mag_calibration.h
+46
View File
@@ -0,0 +1,46 @@
/****************************************************************************
*
* Copyright (C) 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.
*
****************************************************************************/
/**
* @file mag_calibration.h
* Magnetometer calibration routine
*/
#ifndef MAG_CALIBRATION_H_
#define MAG_CALIBRATION_H_
#include <stdint.h>
void do_mag_calibration(int mavlink_fd);
#endif /* MAG_CALIBRATION_H_ */
src/modules/commander/module.mk
+11 -4
View File
@@ -36,8 +36,15 @@
#
MODULE_COMMAND = commander
SRCS = commander.c \
state_machine_helper.c \
calibration_routines.c \
accelerometer_calibration.c
SRCS = commander.cpp \
commander_params.c \
state_machine_helper.cpp \
commander_helper.cpp \
calibration_routines.cpp \
accelerometer_calibration.cpp \
gyro_calibration.cpp \
mag_calibration.cpp \
baro_calibration.cpp \
rc_calibration.cpp \
airspeed_calibration.cpp
src/modules/commander/px4_custom_mode.h
+18
View File
@@ -0,0 +1,18 @@
/*
* px4_custom_mode.h
*
* Created on: 09.08.2013
* Author: ton
*/
#ifndef PX4_CUSTOM_MODE_H_
#define PX4_CUSTOM_MODE_H_
enum PX4_CUSTOM_MODE {
PX4_CUSTOM_MODE_MANUAL = 1,
PX4_CUSTOM_MODE_SEATBELT,
PX4_CUSTOM_MODE_EASY,
PX4_CUSTOM_MODE_AUTO,
};
#endif /* PX4_CUSTOM_MODE_H_ */
src/modules/commander/rc_calibration.cpp
+83
View File
@@ -0,0 +1,83 @@
/****************************************************************************
*
* Copyright (C) 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.
*
****************************************************************************/
/**
* @file rc_calibration.cpp
* Remote Control calibration routine
*/
#include "rc_calibration.h"
#include "commander_helper.h"
#include <poll.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
void do_rc_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, "trim calibration starting");
/* XXX fix this */
// if (current_status.rc_signal) {
// mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal.");
// return;
// }
int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
struct manual_control_setpoint_s sp;
orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
/* set parameters */
float p = sp.roll;
param_set(param_find("TRIM_ROLL"), &p);
p = sp.pitch;
param_set(param_find("TRIM_PITCH"), &p);
p = sp.yaw;
param_set(param_find("TRIM_YAW"), &p);
/* store to permanent storage */
/* auto-save */
int save_ret = param_save_default();
if (save_ret != 0) {
mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed");
}
tune_positive();
mavlink_log_info(mavlink_fd, "trim calibration done");
}
src/modules/commander/rc_calibration.h
+46
View File
@@ -0,0 +1,46 @@
/****************************************************************************
*
* Copyright (C) 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.
*
****************************************************************************/
/**
* @file rc_calibration.h
* Remote Control calibration routine
*/
#ifndef RC_CALIBRATION_H_
#define RC_CALIBRATION_H_
#include <stdint.h>
void do_rc_calibration(int mavlink_fd);
#endif /* RC_CALIBRATION_H_ */
src/modules/commander/state_machine_helper.c
-757
View File
@@ -1,757 +0,0 @@
/****************************************************************************
*
* 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);
systemreset(false);
/* 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;
/* XXX allow arming by external components on multicopters only if not yet armed by RC */
/* XXX allow arming only if core sensors are ok */
armed.ready_to_arm = true;
/* 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;
}
src/modules/commander/state_machine_helper.cpp
+695
View File
@@ -0,0 +1,695 @@
/****************************************************************************
*
* Copyright (C) 2013 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.cpp
* State machine helper functions implementations
*/
#include <stdio.h>
#include <unistd.h>
#include <stdint.h>
#include <stdbool.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <mavlink/mavlink_log.h>
#include "state_machine_helper.h"
#include "commander_helper.h"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
static bool arming_state_changed = true;
static bool main_state_changed = true;
static bool navigation_state_changed = true;
transition_result_t
arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, arming_state_t new_arming_state, struct actuator_armed_s *armed)
{
transition_result_t ret = TRANSITION_DENIED;
/* only check transition if the new state is actually different from the current one */
if (new_arming_state == status->arming_state) {
ret = TRANSITION_NOT_CHANGED;
} else {
switch (new_arming_state) {
case ARMING_STATE_INIT:
/* allow going back from INIT for calibration */
if (status->arming_state == ARMING_STATE_STANDBY) {
ret = TRANSITION_CHANGED;
armed->armed = false;
armed->ready_to_arm = false;
}
break;
case ARMING_STATE_STANDBY:
/* allow coming from INIT and disarming from ARMED */
if (status->arming_state == ARMING_STATE_INIT
|| status->arming_state == ARMING_STATE_ARMED) {
/* sensors need to be initialized for STANDBY state */
if (status->condition_system_sensors_initialized) {
ret = TRANSITION_CHANGED;
armed->armed = false;
armed->ready_to_arm = true;
}
}
break;
case ARMING_STATE_ARMED:
/* allow arming from STANDBY and IN-AIR-RESTORE */
if ((status->arming_state == ARMING_STATE_STANDBY
|| status->arming_state == ARMING_STATE_IN_AIR_RESTORE)
&& (!safety->safety_switch_available || safety->safety_off)) /* only allow arming if safety is off */
{
ret = TRANSITION_CHANGED;
armed->armed = true;
armed->ready_to_arm = false;
}
break;
case ARMING_STATE_ARMED_ERROR:
/* an armed error happens when ARMED obviously */
if (status->arming_state == ARMING_STATE_ARMED) {
ret = TRANSITION_CHANGED;
armed->armed = true;
armed->ready_to_arm = false;
}
break;
case ARMING_STATE_STANDBY_ERROR:
/* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */
if (status->arming_state == ARMING_STATE_STANDBY
|| status->arming_state == ARMING_STATE_INIT
|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
ret = TRANSITION_CHANGED;
armed->armed = false;
armed->ready_to_arm = false;
}
break;
case ARMING_STATE_REBOOT:
/* an armed error happens when ARMED obviously */
if (status->arming_state == ARMING_STATE_INIT
|| status->arming_state == ARMING_STATE_STANDBY
|| status->arming_state == ARMING_STATE_STANDBY_ERROR) {
ret = TRANSITION_CHANGED;
armed->armed = false;
armed->ready_to_arm = false;
}
break;
case ARMING_STATE_IN_AIR_RESTORE:
/* XXX implement */
break;
default:
break;
}
if (ret == TRANSITION_CHANGED) {
status->arming_state = new_arming_state;
arming_state_changed = true;
}
}
return ret;
}
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed)
{
// System is safe if:
// 1) Not armed
// 2) Armed, but in software lockdown (HIL)
// 3) Safety switch is present AND engaged -> actuators locked
if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) {
return true;
} else {
return false;
}
}
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 *current_state, main_state_t new_main_state)
{
transition_result_t ret = TRANSITION_DENIED;
/* only check transition if the new state is actually different from the current one */
if (new_main_state == current_state->main_state) {
ret = TRANSITION_NOT_CHANGED;
} else {
switch (new_main_state) {
case MAIN_STATE_MANUAL:
ret = TRANSITION_CHANGED;
break;
case MAIN_STATE_SEATBELT:
/* need position estimate */
// TODO only need altitude estimate really
if (current_state->condition_local_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_EASY:
/* need position estimate */
if (current_state->condition_local_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_AUTO:
/* need position estimate */
if (current_state->condition_local_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
}
if (ret == TRANSITION_CHANGED) {
current_state->main_state = new_main_state;
main_state_changed = true;
}
}
return ret;
}
bool
check_main_state_changed()
{
if (main_state_changed) {
main_state_changed = false;
return true;
} else {
return false;
}
}
transition_result_t
navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode)
{
transition_result_t ret = TRANSITION_DENIED;
/* only check transition if the new state is actually different from the current one */
if (new_navigation_state == current_status->navigation_state) {
ret = TRANSITION_NOT_CHANGED;
} else {
switch (new_navigation_state) {
case NAVIGATION_STATE_STANDBY:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = false;
control_mode->flag_control_attitude_enabled = false;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_manual_enabled = false;
break;
case NAVIGATION_STATE_DIRECT:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = false;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_manual_enabled = true;
break;
case NAVIGATION_STATE_STABILIZE:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_manual_enabled = true;
break;
case NAVIGATION_STATE_ALTHOLD:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = true;
break;
case NAVIGATION_STATE_VECTOR:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = true;
break;
case NAVIGATION_STATE_AUTO_READY:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
break;
case NAVIGATION_STATE_AUTO_TAKEOFF:
/* only transitions from AUTO_READY */
if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
break;
case NAVIGATION_STATE_AUTO_LOITER:
/* deny transitions from landed states */
if (current_status->navigation_state != NAVIGATION_STATE_STANDBY &&
current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
break;
case NAVIGATION_STATE_AUTO_MISSION:
/* deny transitions from landed states */
if (current_status->navigation_state != NAVIGATION_STATE_STANDBY &&
current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
break;
case NAVIGATION_STATE_AUTO_RTL:
/* deny transitions from landed states */
if (current_status->navigation_state != NAVIGATION_STATE_STANDBY &&
current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
break;
case NAVIGATION_STATE_AUTO_LAND:
/* deny transitions from landed states */
if (current_status->navigation_state != NAVIGATION_STATE_STANDBY &&
current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
break;
default:
break;
}
if (ret == TRANSITION_CHANGED) {
current_status->navigation_state = new_navigation_state;
navigation_state_changed = true;
}
}
return ret;
}
bool
check_navigation_state_changed()
{
if (navigation_state_changed) {
navigation_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, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd)
{
bool valid_transition = false;
int ret = ERROR;
warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state);
if (current_status->hil_state == new_state) {
warnx("Hil state not changed");
valid_transition = true;
} else {
switch (new_state) {
case HIL_STATE_OFF:
if (current_status->arming_state == ARMING_STATE_INIT
|| current_status->arming_state == ARMING_STATE_STANDBY) {
current_control_mode->flag_system_hil_enabled = false;
mavlink_log_critical(mavlink_fd, "Switched to OFF hil state");
valid_transition = true;
}
break;
case HIL_STATE_ON:
if (current_status->arming_state == ARMING_STATE_INIT
|| current_status->arming_state == ARMING_STATE_STANDBY) {
current_control_mode->flag_system_hil_enabled = true;
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
valid_transition = true;
}
break;
default:
warnx("Unknown hil state");
break;
}
}
if (valid_transition) {
current_status->hil_state = new_state;
current_status->counter++;
current_status->timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
current_control_mode->timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode);
ret = OK;
} else {
mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition");
}
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
+18 -150
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
*
@@ -46,164 +46,32 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/vehicle_control_mode.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);
typedef enum {
TRANSITION_DENIED = -1,
TRANSITION_NOT_CHANGED = 0,
TRANSITION_CHANGED
/* 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);
} transition_result_t;
// 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);
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);
// 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);
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();
/**
* 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);
transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);
/**
* 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);
bool check_main_state_changed();
/**
* 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);
transition_result_t navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode);
bool check_navigation_state_changed();
int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd);
#endif /* STATE_MACHINE_HELPER_H_ */
src/modules/fixedwing_att_control/fixedwing_att_control_att.c
+2 -2
View File
@@ -142,7 +142,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
}
/* Roll (P) */
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0);
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0, NULL, NULL, NULL);
/* Pitch (P) */
@@ -152,7 +152,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
/* set pitch plus feedforward roll compensation */
rates_sp->pitch = pid_calculate(&pitch_controller,
att_sp->pitch_body + pitch_sp_rollcompensation,
att->pitch, 0, 0);
att->pitch, 0, 0, NULL, NULL, NULL);
/* Yaw (from coordinated turn constraint or lateral force) */
rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll))
src/modules/fixedwing_att_control/fixedwing_att_control_main.c
+82 -81
View File
@@ -186,87 +186,88 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
/* control */
if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
/* attitude control */
fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
/* angular rate control */
fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
/* pass through throttle */
actuators.control[3] = att_sp.thrust;
/* set flaps to zero */
actuators.control[4] = 0.0f;
} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
if (vstatus.rc_signal_lost) {
/* put plane into loiter */
att_sp.roll_body = 0.3f;
att_sp.pitch_body = 0.0f;
/* limit throttle to 60 % of last value if sane */
if (isfinite(manual_sp.throttle) &&
(manual_sp.throttle >= 0.0f) &&
(manual_sp.throttle <= 1.0f)) {
att_sp.thrust = 0.6f * manual_sp.throttle;
} else {
att_sp.thrust = 0.0f;
}
att_sp.yaw_body = 0;
// XXX disable yaw control, loiter
} else {
att_sp.roll_body = manual_sp.roll;
att_sp.pitch_body = manual_sp.pitch;
att_sp.yaw_body = 0;
att_sp.thrust = manual_sp.throttle;
}
att_sp.timestamp = hrt_absolute_time();
/* attitude control */
fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
/* angular rate control */
fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
/* pass through throttle */
actuators.control[3] = att_sp.thrust;
/* pass through flaps */
if (isfinite(manual_sp.flaps)) {
actuators.control[4] = manual_sp.flaps;
} else {
actuators.control[4] = 0.0f;
}
} else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
/* directly pass through values */
actuators.control[0] = manual_sp.roll;
/* positive pitch means negative actuator -> pull up */
actuators.control[1] = manual_sp.pitch;
actuators.control[2] = manual_sp.yaw;
actuators.control[3] = manual_sp.throttle;
if (isfinite(manual_sp.flaps)) {
actuators.control[4] = manual_sp.flaps;
} else {
actuators.control[4] = 0.0f;
}
}
}
#warning fix this
// if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
// vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
// /* attitude control */
// fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
//
// /* angular rate control */
// fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
//
// /* pass through throttle */
// actuators.control[3] = att_sp.thrust;
//
// /* set flaps to zero */
// actuators.control[4] = 0.0f;
//
// } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
// if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
//
// /* if the RC signal is lost, try to stay level and go slowly back down to ground */
// if (vstatus.rc_signal_lost) {
//
// /* put plane into loiter */
// att_sp.roll_body = 0.3f;
// att_sp.pitch_body = 0.0f;
//
// /* limit throttle to 60 % of last value if sane */
// if (isfinite(manual_sp.throttle) &&
// (manual_sp.throttle >= 0.0f) &&
// (manual_sp.throttle <= 1.0f)) {
// att_sp.thrust = 0.6f * manual_sp.throttle;
//
// } else {
// att_sp.thrust = 0.0f;
// }
//
// att_sp.yaw_body = 0;
//
// // XXX disable yaw control, loiter
//
// } else {
//
// att_sp.roll_body = manual_sp.roll;
// att_sp.pitch_body = manual_sp.pitch;
// att_sp.yaw_body = 0;
// att_sp.thrust = manual_sp.throttle;
// }
//
// att_sp.timestamp = hrt_absolute_time();
//
// /* attitude control */
// fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
//
// /* angular rate control */
// fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
//
// /* pass through throttle */
// actuators.control[3] = att_sp.thrust;
//
// /* pass through flaps */
// if (isfinite(manual_sp.flaps)) {
// actuators.control[4] = manual_sp.flaps;
//
// } else {
// actuators.control[4] = 0.0f;
// }
//
// } else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
// /* directly pass through values */
// actuators.control[0] = manual_sp.roll;
// /* positive pitch means negative actuator -> pull up */
// actuators.control[1] = manual_sp.pitch;
// actuators.control[2] = manual_sp.yaw;
// actuators.control[3] = manual_sp.throttle;
//
// if (isfinite(manual_sp.flaps)) {
// actuators.control[4] = manual_sp.flaps;
//
// } else {
// actuators.control[4] = 0.0f;
// }
// }
// }
/* publish rates */
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
+3 -3
View File
@@ -196,11 +196,11 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
/* roll rate (PI) */
actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT, NULL, NULL, NULL);
/* pitch rate (PI) */
actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT, NULL, NULL, NULL);
/* yaw rate (PI) */
actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT, NULL, NULL, NULL);
counter++;
src/modules/fixedwing_backside/fixedwing.cpp
+31 -22
View File
@@ -39,6 +39,8 @@
#include "fixedwing.hpp"
#if 0
namespace control
{
@@ -155,8 +157,9 @@ void BlockMultiModeBacksideAutopilot::update()
for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++)
_actuators.control[i] = 0.0f;
#warning this if is incomplete, should be based on flags
// only update guidance in auto mode
if (_status.state_machine == SYSTEM_STATE_AUTO) {
if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) {
// update guidance
_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
}
@@ -165,9 +168,10 @@ void BlockMultiModeBacksideAutopilot::update()
// once the system switches from manual or auto to stabilized
// the setpoint should update to loitering around this position
#warning should be base on flags
// handle autopilot modes
if (_status.state_machine == SYSTEM_STATE_AUTO ||
_status.state_machine == SYSTEM_STATE_STABILIZED) {
if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
_status.navigation_state == NAVIGATION_STATE_SEATBELT) {
// update guidance
_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
@@ -219,21 +223,23 @@ void BlockMultiModeBacksideAutopilot::update()
// This is not a hack, but a design choice.
/* do not limit in HIL */
if (!_status.flag_hil_enabled) {
/* limit to value of manual throttle */
_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
_actuators.control[CH_THR] : _manual.throttle;
}
#warning fix this
// if (!_status.flag_hil_enabled) {
// /* limit to value of manual throttle */
// _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
// _actuators.control[CH_THR] : _manual.throttle;
// }
} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
} else if (_status.navigation_state == NAVIGATION_STATE_MANUAL) {
if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
_actuators.control[CH_AIL] = _manual.roll;
_actuators.control[CH_ELV] = _manual.pitch;
_actuators.control[CH_RDR] = _manual.yaw;
_actuators.control[CH_THR] = _manual.throttle;
} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
#warning fix here too
// if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
// _actuators.control[CH_AIL] = _manual.roll;
// _actuators.control[CH_ELV] = _manual.pitch;
// _actuators.control[CH_RDR] = _manual.yaw;
// _actuators.control[CH_THR] = _manual.throttle;
//
// } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
// calculate velocity, XXX should be airspeed, but using ground speed for now
// for the purpose of control we will limit the velocity feedback between
@@ -284,12 +290,14 @@ void BlockMultiModeBacksideAutopilot::update()
// This is not a hack, but a design choice.
/* do not limit in HIL */
if (!_status.flag_hil_enabled) {
/* limit to value of manual throttle */
_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
_actuators.control[CH_THR] : _manual.throttle;
}
}
#warning fix this
// if (!_status.flag_hil_enabled) {
// /* limit to value of manual throttle */
// _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
// _actuators.control[CH_THR] : _manual.throttle;
// }
#warning fix this
// }
// body rates controller, disabled for now
else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) {
@@ -322,3 +330,4 @@ BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot()
} // namespace control
#endif
src/modules/fixedwing_backside/fixedwing_backside_main.cpp
+4 -2
View File
@@ -151,12 +151,14 @@ int control_demo_thread_main(int argc, char *argv[])
using namespace control;
fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB");
#warning fix this
// fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB");
thread_running = true;
while (!thread_should_exit) {
autopilot.update();
#warning fix this
// autopilot.update();
}
warnx("exiting.");
src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
+5 -5
View File
@@ -271,7 +271,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f); //arbitrary high limit
pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim);
pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim);
pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD); //TODO: remove hardcoded value
pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD_F); //TODO: remove hardcoded value
}
/* only run controller if attitude changed */
@@ -319,7 +319,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
// XXX what is xtrack_err.past_end?
if (distance_res == OK /*&& !xtrack_err.past_end*/) {
float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance
float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f, NULL, NULL, NULL); //p.xtrack_p * xtrack_err.distance
float psi_c = psi_track + delta_psi_c;
float psi_e = psi_c - att.yaw;
@@ -336,7 +336,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
}
/* calculate roll setpoint, do this artificially around zero */
float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f);
float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f, NULL, NULL, NULL);
float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following
float psi_rate_c = delta_psi_rate_c + psi_rate_track;
@@ -359,7 +359,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g
attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT);
attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT, NULL, NULL, NULL);
if (verbose) {
printf("psi_rate_c %.4f ", (double)psi_rate_c);
@@ -379,7 +379,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
if (pos_updated) {
//TODO: take care of relative vs. ab. altitude
attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f, NULL, NULL, NULL);
}
src/modules/gpio_led/gpio_led.c
+11 -4
View File
@@ -51,6 +51,7 @@
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <poll.h>
#include <drivers/drv_gpio.h>
#include <modules/px4iofirmware/protocol.h>
@@ -62,6 +63,8 @@ struct gpio_led_s {
int pin;
struct vehicle_status_s status;
int vehicle_status_sub;
struct actuator_armed_s armed;
int actuator_armed_sub;
bool led_state;
int counter;
};
@@ -227,10 +230,15 @@ void gpio_led_cycle(FAR void *arg)
if (status_updated)
orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status);
orb_check(priv->vehicle_status_sub, &status_updated);
if (status_updated)
orb_copy(ORB_ID(actuator_armed), priv->actuator_armed_sub, &priv->armed);
/* select pattern for current status */
int pattern = 0;
if (priv->status.flag_system_armed) {
if (priv->armed.armed) {
if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
pattern = 0x3f; // ****** solid (armed)
@@ -239,11 +247,10 @@ void gpio_led_cycle(FAR void *arg)
}
} else {
if (priv->status.state_machine == SYSTEM_STATE_PREFLIGHT) {
if (priv->armed.ready_to_arm) {
pattern = 0x00; // ______ off (disarmed, preflight check)
} else if (priv->status.state_machine == SYSTEM_STATE_STANDBY &&
priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
} else if (priv->armed.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
pattern = 0x38; // ***___ slow blink (disarmed, ready)
} else {
src/modules/mavlink/mavlink.c
+59 -87
View File
@@ -64,6 +64,7 @@
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <mavlink/mavlink_log.h>
#include <commander/px4_custom_mode.h>
#include "waypoints.h"
#include "orb_topics.h"
@@ -181,102 +182,68 @@ set_hil_on_off(bool hil_enabled)
}
void
get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
{
/* reset MAVLink mode bitfield */
*mavlink_mode = 0;
*mavlink_base_mode = 0;
*mavlink_custom_mode = 0;
/* set mode flags independent of system state */
/**
* Set mode flags
**/
/* HIL */
if (v_status.flag_hil_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
if (v_status.hil_state == HIL_STATE_ON) {
*mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
}
/* manual input */
if (v_status.flag_control_manual_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
/* arming state */
if (v_status.arming_state == ARMING_STATE_ARMED
|| v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
*mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
}
/* attitude or rate control */
if (v_status.flag_control_attitude_enabled ||
v_status.flag_control_rates_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
/* main state */
*mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
if (v_status.main_state == MAIN_STATE_MANUAL) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
*mavlink_custom_mode = PX4_CUSTOM_MODE_MANUAL;
} else if (v_status.main_state == MAIN_STATE_SEATBELT) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
*mavlink_custom_mode = PX4_CUSTOM_MODE_SEATBELT;
} else if (v_status.main_state == MAIN_STATE_EASY) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
*mavlink_custom_mode = PX4_CUSTOM_MODE_EASY;
} else if (v_status.main_state == MAIN_STATE_AUTO) {
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
*mavlink_custom_mode = PX4_CUSTOM_MODE_AUTO;
}
/* vector control */
if (v_status.flag_control_velocity_enabled ||
v_status.flag_control_position_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
}
/**
* Set mavlink state
**/
/* autonomous mode */
if (v_status.state_machine == SYSTEM_STATE_AUTO) {
*mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
}
/* set arming state */
if (armed.armed) {
*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
} else {
*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
}
switch (v_status.state_machine) {
case SYSTEM_STATE_PREFLIGHT:
if (v_status.flag_preflight_gyro_calibration ||
v_status.flag_preflight_mag_calibration ||
v_status.flag_preflight_accel_calibration) {
*mavlink_state = MAV_STATE_CALIBRATING;
} else {
*mavlink_state = MAV_STATE_UNINIT;
}
break;
case SYSTEM_STATE_STANDBY:
/* set calibration state */
if (v_status.preflight_calibration) {
*mavlink_state = MAV_STATE_CALIBRATING;
} else if (v_status.system_emergency) {
*mavlink_state = MAV_STATE_EMERGENCY;
} else if (v_status.arming_state == ARMING_STATE_INIT
|| v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE
|| v_status.arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review
*mavlink_state = MAV_STATE_UNINIT;
} else if (v_status.arming_state == ARMING_STATE_ARMED) {
*mavlink_state = MAV_STATE_ACTIVE;
} else if (v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
*mavlink_state = MAV_STATE_CRITICAL;
} else if (v_status.arming_state == ARMING_STATE_STANDBY) {
*mavlink_state = MAV_STATE_STANDBY;
break;
case SYSTEM_STATE_GROUND_READY:
*mavlink_state = MAV_STATE_ACTIVE;
break;
case SYSTEM_STATE_MANUAL:
*mavlink_state = MAV_STATE_ACTIVE;
break;
case SYSTEM_STATE_STABILIZED:
*mavlink_state = MAV_STATE_ACTIVE;
break;
case SYSTEM_STATE_AUTO:
*mavlink_state = MAV_STATE_ACTIVE;
break;
case SYSTEM_STATE_MISSION_ABORT:
*mavlink_state = MAV_STATE_EMERGENCY;
break;
case SYSTEM_STATE_EMCY_LANDING:
*mavlink_state = MAV_STATE_EMERGENCY;
break;
case SYSTEM_STATE_EMCY_CUTOFF:
*mavlink_state = MAV_STATE_EMERGENCY;
break;
case SYSTEM_STATE_GROUND_ERROR:
*mavlink_state = MAV_STATE_EMERGENCY;
break;
case SYSTEM_STATE_REBOOT:
} else if (v_status.arming_state == ARMING_STATE_REBOOT) {
*mavlink_state = MAV_STATE_POWEROFF;
break;
} else {
warnx("Unknown mavlink state");
*mavlink_state = MAV_STATE_CRITICAL;
}
}
@@ -568,6 +535,7 @@ int mavlink_thread_main(int argc, char *argv[])
default:
usage();
break;
}
}
@@ -674,14 +642,18 @@ int mavlink_thread_main(int argc, char *argv[])
/* translate the current system state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_mode = 0;
get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
uint8_t mavlink_base_mode = 0;
uint32_t mavlink_custom_mode = 0;
get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
/* send heartbeat */
mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state);
/* switch HIL mode if required */
set_hil_on_off(v_status.flag_hil_enabled);
if (v_status.hil_state == HIL_STATE_ON)
set_hil_on_off(true);
else if (v_status.hil_state == HIL_STATE_OFF)
set_hil_on_off(false);
/* send status (values already copied in the section above) */
mavlink_msg_sys_status_send(chan,
@@ -689,8 +661,8 @@ int mavlink_thread_main(int argc, char *argv[])
v_status.onboard_control_sensors_enabled,
v_status.onboard_control_sensors_health,
v_status.load,
v_status.voltage_battery * 1000.0f,
v_status.current_battery * 1000.0f,
v_status.battery_voltage * 1000.0f,
v_status.battery_current * 1000.0f,
v_status.battery_remaining,
v_status.drop_rate_comm,
v_status.errors_comm,
src/modules/mavlink/orb_listener.c
+17 -12
View File
@@ -272,19 +272,23 @@ l_vehicle_status(const struct listener *l)
orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
/* enable or disable HIL */
set_hil_on_off(v_status.flag_hil_enabled);
if (v_status.hil_state == HIL_STATE_ON)
set_hil_on_off(true);
else if (v_status.hil_state == HIL_STATE_OFF)
set_hil_on_off(false);
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_mode = 0;
get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
uint8_t mavlink_base_mode = 0;
uint32_t mavlink_custom_mode = 0;
get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
/* send heartbeat */
mavlink_msg_heartbeat_send(chan,
mavlink_system.type,
MAV_AUTOPILOT_PX4,
mavlink_mode,
v_status.state_machine,
mavlink_base_mode,
mavlink_custom_mode,
mavlink_state);
}
@@ -470,8 +474,9 @@ l_actuator_outputs(const struct listener *l)
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_mode = 0;
get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
uint8_t mavlink_base_mode = 0;
uint32_t mavlink_custom_mode = 0;
get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
/* HIL message as per MAVLink spec */
@@ -488,7 +493,7 @@ l_actuator_outputs(const struct listener *l)
-1,
-1,
-1,
mavlink_mode,
mavlink_base_mode,
0);
} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
@@ -502,7 +507,7 @@ l_actuator_outputs(const struct listener *l)
((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
-1,
-1,
mavlink_mode,
mavlink_base_mode,
0);
} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
@@ -516,7 +521,7 @@ l_actuator_outputs(const struct listener *l)
((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f,
mavlink_mode,
mavlink_base_mode,
0);
} else {
@@ -530,7 +535,7 @@ l_actuator_outputs(const struct listener *l)
(act_outputs.output[5] - 1500.0f) / 500.0f,
(act_outputs.output[6] - 1500.0f) / 500.0f,
(act_outputs.output[7] - 1500.0f) / 500.0f,
mavlink_mode,
mavlink_base_mode,
0);
}
}
@@ -673,7 +678,7 @@ uorb_receive_thread(void *arg)
/* handle the poll result */
if (poll_ret == 0) {
mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s");
/* silent */
} else if (poll_ret < 0) {
mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data");
src/modules/mavlink/orb_topics.h
+4 -1
View File
@@ -55,10 +55,12 @@
#include <uORB/topics/vehicle_global_position_set_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/debug_key_value.h>
@@ -75,8 +77,9 @@ struct mavlink_subscriptions {
int act_3_sub;
int gps_sub;
int man_control_sp_sub;
int armed_sub;
int safety_sub;
int actuators_sub;
int armed_sub;
int local_pos_sub;
int spa_sub;
int spl_sub;
src/modules/mavlink/util.h
+1 -1
View File
@@ -51,4 +51,4 @@ extern mavlink_wpm_storage *wpm;
/**
* Translate the custom state into standard mavlink modes and state.
*/
extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode);
extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
src/modules/mavlink_onboard/mavlink.c
+67 -59
View File
@@ -273,18 +273,18 @@ void mavlink_update_system(void)
}
void
get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed,
get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed,
uint8_t *mavlink_state, uint8_t *mavlink_mode)
{
/* reset MAVLink mode bitfield */
*mavlink_mode = 0;
/* set mode flags independent of system state */
if (v_status->flag_control_manual_enabled) {
if (control_mode->flag_control_manual_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
}
if (v_status->flag_hil_enabled) {
if (control_mode->flag_system_hil_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
}
@@ -295,61 +295,67 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct
*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
}
switch (v_status->state_machine) {
case SYSTEM_STATE_PREFLIGHT:
if (v_status->flag_preflight_gyro_calibration ||
v_status->flag_preflight_mag_calibration ||
v_status->flag_preflight_accel_calibration) {
*mavlink_state = MAV_STATE_CALIBRATING;
} else {
*mavlink_state = MAV_STATE_UNINIT;
}
break;
case SYSTEM_STATE_STANDBY:
*mavlink_state = MAV_STATE_STANDBY;
break;
case SYSTEM_STATE_GROUND_READY:
*mavlink_state = MAV_STATE_ACTIVE;
break;
case SYSTEM_STATE_MANUAL:
*mavlink_state = MAV_STATE_ACTIVE;
*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case SYSTEM_STATE_STABILIZED:
*mavlink_state = MAV_STATE_ACTIVE;
*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
break;
case SYSTEM_STATE_AUTO:
*mavlink_state = MAV_STATE_ACTIVE;
if (control_mode->flag_control_velocity_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
break;
case SYSTEM_STATE_MISSION_ABORT:
*mavlink_state = MAV_STATE_EMERGENCY;
break;
case SYSTEM_STATE_EMCY_LANDING:
*mavlink_state = MAV_STATE_EMERGENCY;
break;
case SYSTEM_STATE_EMCY_CUTOFF:
*mavlink_state = MAV_STATE_EMERGENCY;
break;
case SYSTEM_STATE_GROUND_ERROR:
*mavlink_state = MAV_STATE_EMERGENCY;
break;
case SYSTEM_STATE_REBOOT:
*mavlink_state = MAV_STATE_POWEROFF;
break;
} else {
*mavlink_mode &= ~MAV_MODE_FLAG_GUIDED_ENABLED;
}
// switch (v_status->state_machine) {
// case SYSTEM_STATE_PREFLIGHT:
// if (v_status->flag_preflight_gyro_calibration ||
// v_status->flag_preflight_mag_calibration ||
// v_status->flag_preflight_accel_calibration) {
// *mavlink_state = MAV_STATE_CALIBRATING;
// } else {
// *mavlink_state = MAV_STATE_UNINIT;
// }
// break;
//
// case SYSTEM_STATE_STANDBY:
// *mavlink_state = MAV_STATE_STANDBY;
// break;
//
// case SYSTEM_STATE_GROUND_READY:
// *mavlink_state = MAV_STATE_ACTIVE;
// break;
//
// case SYSTEM_STATE_MANUAL:
// *mavlink_state = MAV_STATE_ACTIVE;
// *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
// break;
//
// case SYSTEM_STATE_STABILIZED:
// *mavlink_state = MAV_STATE_ACTIVE;
// *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
// break;
//
// case SYSTEM_STATE_AUTO:
// *mavlink_state = MAV_STATE_ACTIVE;
// *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
// break;
//
// case SYSTEM_STATE_MISSION_ABORT:
// *mavlink_state = MAV_STATE_EMERGENCY;
// break;
//
// case SYSTEM_STATE_EMCY_LANDING:
// *mavlink_state = MAV_STATE_EMERGENCY;
// break;
//
// case SYSTEM_STATE_EMCY_CUTOFF:
// *mavlink_state = MAV_STATE_EMERGENCY;
// break;
//
// case SYSTEM_STATE_GROUND_ERROR:
// *mavlink_state = MAV_STATE_EMERGENCY;
// break;
//
// case SYSTEM_STATE_REBOOT:
// *mavlink_state = MAV_STATE_POWEROFF;
// break;
// }
}
/**
@@ -361,7 +367,9 @@ int mavlink_thread_main(int argc, char *argv[])
char *device_name = "/dev/ttyS1";
baudrate = 57600;
/* XXX this is never written? */
struct vehicle_status_s v_status;
struct vehicle_control_mode_s control_mode;
struct actuator_armed_s armed;
/* work around some stupidity in task_create's argv handling */
@@ -430,10 +438,10 @@ int mavlink_thread_main(int argc, char *argv[])
/* translate the current system state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_mode = 0;
get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode);
get_mavlink_mode_and_state(&control_mode, &armed, &mavlink_state, &mavlink_mode);
/* send heartbeat */
mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state);
/* send status (values already copied in the section above) */
mavlink_msg_sys_status_send(chan,
@@ -441,8 +449,8 @@ int mavlink_thread_main(int argc, char *argv[])
v_status.onboard_control_sensors_enabled,
v_status.onboard_control_sensors_health,
v_status.load,
v_status.voltage_battery * 1000.0f,
v_status.current_battery * 1000.0f,
v_status.battery_voltage * 1000.0f,
v_status.battery_current * 1000.0f,
v_status.battery_remaining,
v_status.drop_rate_comm,
v_status.errors_comm,
src/modules/mavlink_onboard/mavlink_receiver.c
+1 -1
View File
@@ -327,4 +327,4 @@ receive_start(int uart)
pthread_t thread;
pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
return thread;
}
}
src/modules/mavlink_onboard/orb_topics.h
+3 -1
View File
@@ -52,9 +52,11 @@
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/debug_key_value.h>
#include <drivers/drv_rc_input.h>
@@ -69,7 +71,7 @@ struct mavlink_subscriptions {
int act_3_sub;
int gps_sub;
int man_control_sp_sub;
int armed_sub;
int safety_sub;
int actuators_sub;
int local_pos_sub;
int spa_sub;
src/modules/mavlink_onboard/util.h
+2 -1
View File
@@ -50,5 +50,6 @@ extern volatile bool thread_should_exit;
/**
* Translate the custom state into standard mavlink modes and state.
*/
extern void get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed,
extern void
get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed,
uint8_t *mavlink_state, uint8_t *mavlink_mode);
src/modules/multirotor_att_control/multirotor_att_control_main.c
+80 -107
View File
@@ -57,12 +57,14 @@
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <drivers/drv_gyro.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/offboard_control_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_control_debug.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
@@ -83,14 +85,17 @@ static int mc_task;
static bool motor_test_mode = false;
static orb_advert_t actuator_pub;
static orb_advert_t control_debug_pub;
static struct vehicle_status_s state;
static int
mc_thread_main(int argc, char *argv[])
{
/* declare and safely initialize all structs */
memset(&state, 0, sizeof(state));
struct vehicle_control_mode_s control_mode;
memset(&control_mode, 0, sizeof(control_mode));
struct actuator_armed_s armed;
memset(&armed, 0, sizeof(armed));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_attitude_setpoint_s att_sp;
@@ -107,12 +112,16 @@ mc_thread_main(int argc, char *argv[])
struct actuator_controls_s actuators;
memset(&actuators, 0, sizeof(actuators));
struct vehicle_control_debug_s control_debug;
memset(&control_debug, 0, sizeof(control_debug));
/* subscribe to attitude, motor setpoints and system state */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int param_sub = orb_subscribe(ORB_ID(parameter_update));
int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
@@ -133,6 +142,8 @@ mc_thread_main(int argc, char *argv[])
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
actuators.control[i] = 0.0f;
}
control_debug_pub = orb_advertise(ORB_ID(vehicle_control_debug), &control_debug);
actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
@@ -150,7 +161,9 @@ mc_thread_main(int argc, char *argv[])
/* store last control mode to detect mode switches */
bool flag_control_manual_enabled = false;
bool flag_control_attitude_enabled = false;
bool flag_system_armed = false;
bool flag_armed = false;
bool flag_control_position_enabled = false;
bool flag_control_velocity_enabled = false;
/* store if yaw position or yaw speed has been changed */
bool control_yaw_position = true;
@@ -162,7 +175,6 @@ mc_thread_main(int argc, char *argv[])
param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR");
float failsafe_throttle = 0.0f;
while (!thread_should_exit) {
/* wait for a sensor update, check for exit condition every 500 ms */
@@ -175,7 +187,6 @@ mc_thread_main(int argc, char *argv[])
} else if (ret == 0) {
/* no return value, ignore */
} else {
/* only update parameters if they changed */
if (fds[1].revents & POLLIN) {
/* read from param to clear updated flag */
@@ -193,10 +204,16 @@ mc_thread_main(int argc, char *argv[])
/* get a local copy of system state */
bool updated;
orb_check(state_sub, &updated);
orb_check(control_mode_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
}
orb_check(armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
}
/* get a local copy of manual setpoint */
@@ -215,9 +232,8 @@ mc_thread_main(int argc, char *argv[])
/* get a local copy of the current sensor values */
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
/** STEP 1: Define which input is the dominating control input */
if (state.flag_control_offboard_enabled) {
if (control_mode.flag_control_offboard_enabled) {
/* offboard inputs */
if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
rates_sp.roll = offboard_sp.p1;
@@ -240,102 +256,51 @@ mc_thread_main(int argc, char *argv[])
}
} else if (state.flag_control_manual_enabled) {
if (state.flag_control_attitude_enabled) {
} else if (control_mode.flag_control_manual_enabled) {
if (control_mode.flag_control_attitude_enabled) {
/* initialize to current yaw if switching to manual or att control */
if (state.flag_control_attitude_enabled != flag_control_attitude_enabled ||
state.flag_control_manual_enabled != flag_control_manual_enabled ||
state.flag_system_armed != flag_system_armed) {
if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled ||
control_mode.flag_control_manual_enabled != flag_control_manual_enabled ||
armed.armed != flag_armed) {
att_sp.yaw_body = att.yaw;
}
static bool rc_loss_first_time = true;
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
if (state.rc_signal_lost) {
/* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */
param_get(failsafe_throttle_handle, &failsafe_throttle);
att_sp.roll_body = 0.0f;
att_sp.pitch_body = 0.0f;
/*
* Only go to failsafe throttle if last known throttle was
* high enough to create some lift to make hovering state likely.
*
* This is to prevent that someone landing, but not disarming his
* multicopter (throttle = 0) does not make it jump up in the air
* if shutting down his remote.
*/
if (isfinite(manual.throttle) && manual.throttle > 0.2f) {
att_sp.thrust = failsafe_throttle;
rc_loss_first_time = true;
} else {
att_sp.thrust = 0.0f;
}
att_sp.roll_body = manual.roll;
att_sp.pitch_body = manual.pitch;
/* keep current yaw, do not attempt to go to north orientation,
* since if the pilot regains RC control, he will be lost regarding
* the current orientation.
*/
if (rc_loss_first_time)
att_sp.yaw_body = att.yaw;
/* set attitude if arming */
if (!flag_control_attitude_enabled && armed.armed) {
att_sp.yaw_body = att.yaw;
}
rc_loss_first_time = false;
/* only move setpoint if manual input is != 0 */
if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.1f) {
rates_sp.yaw = manual.yaw;
control_yaw_position = false;
first_time_after_yaw_speed_control = true;
} else {
rc_loss_first_time = true;
att_sp.roll_body = manual.roll;
att_sp.pitch_body = manual.pitch;
/* set attitude if arming */
if (!flag_control_attitude_enabled && state.flag_system_armed) {
if (first_time_after_yaw_speed_control) {
att_sp.yaw_body = att.yaw;
first_time_after_yaw_speed_control = false;
}
/* act if stabilization is active or if the (nonsense) direct pass through mode is set */
if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS ||
state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) {
rates_sp.yaw = manual.yaw;
control_yaw_position = false;
} else {
/*
* This mode SHOULD be the default mode, which is:
* VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS
*
* However, we fall back to this setting for all other (nonsense)
* settings as well.
*/
/* only move setpoint if manual input is != 0 */
if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
rates_sp.yaw = manual.yaw;
control_yaw_position = false;
first_time_after_yaw_speed_control = true;
} else {
if (first_time_after_yaw_speed_control) {
att_sp.yaw_body = att.yaw;
first_time_after_yaw_speed_control = false;
}
control_yaw_position = true;
}
}
}
att_sp.thrust = manual.throttle;
att_sp.timestamp = hrt_absolute_time();
control_yaw_position = true;
}
att_sp.thrust = manual.throttle;
att_sp.timestamp = hrt_absolute_time();
/* STEP 2: publish the controller output */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
if (motor_test_mode) {
printf("testmode");
att_sp.roll_body = 0.0f;
@@ -348,23 +313,26 @@ mc_thread_main(int argc, char *argv[])
}
} else {
/* manual rate inputs, from RC control or joystick */
if (state.flag_control_rates_enabled &&
state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) {
rates_sp.roll = manual.roll;
rates_sp.pitch = manual.pitch;
rates_sp.yaw = manual.yaw;
rates_sp.thrust = manual.throttle;
rates_sp.timestamp = hrt_absolute_time();
}
//XXX TODO add acro mode here
/* manual rate inputs, from RC control or joystick */
// if (state.flag_control_rates_enabled &&
// state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) {
// rates_sp.roll = manual.roll;
//
// rates_sp.pitch = manual.pitch;
// rates_sp.yaw = manual.yaw;
// rates_sp.thrust = manual.throttle;
// rates_sp.timestamp = hrt_absolute_time();
// }
}
}
/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
if (state.flag_control_attitude_enabled) {
multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position);
if (control_mode.flag_control_attitude_enabled) {
multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, &control_debug_pub, &control_debug);
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
}
@@ -372,7 +340,8 @@ mc_thread_main(int argc, char *argv[])
/* measure in what intervals the controller runs */
perf_count(mc_interval_perf);
float gyro[3];
float rates[3];
float rates_acc[3];
/* get current rate setpoint */
bool rates_sp_valid = false;
@@ -383,17 +352,21 @@ mc_thread_main(int argc, char *argv[])
}
/* apply controller */
gyro[0] = att.rollspeed;
gyro[1] = att.pitchspeed;
gyro[2] = att.yawspeed;
rates[0] = att.rollspeed;
rates[1] = att.pitchspeed;
rates[2] = att.yawspeed;
multirotor_control_rates(&rates_sp, gyro, &actuators);
multirotor_control_rates(&rates_sp, rates, &actuators, &control_debug_pub, &control_debug);
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
/* update state */
flag_control_attitude_enabled = state.flag_control_attitude_enabled;
flag_control_manual_enabled = state.flag_control_manual_enabled;
flag_system_armed = state.flag_system_armed;
orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug);
/* update control_mode */
flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled;
flag_control_manual_enabled = control_mode.flag_control_manual_enabled;
flag_control_position_enabled = control_mode.flag_control_position_enabled;
flag_control_velocity_enabled = control_mode.flag_control_velocity_enabled;
flag_armed = armed.armed;
perf_end(mc_loop_perf);
} /* end of poll call for attitude updates */
@@ -410,7 +383,7 @@ mc_thread_main(int argc, char *argv[])
close(att_sub);
close(state_sub);
close(control_mode_sub);
close(manual_sub);
close(actuator_pub);
close(att_sp_pub);
src/modules/multirotor_att_control/multirotor_attitude_control.c
+7 -39
View File
@@ -65,50 +65,29 @@
PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f);
PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f);
PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f);
//PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f);
//PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f);
PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f);
PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f);
//PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f);
//PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f);
//PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f);
//PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f);
struct mc_att_control_params {
float yaw_p;
float yaw_i;
float yaw_d;
//float yaw_awu;
//float yaw_lim;
float att_p;
float att_i;
float att_d;
//float att_awu;
//float att_lim;
//float att_xoff;
//float att_yoff;
};
struct mc_att_control_param_handles {
param_t yaw_p;
param_t yaw_i;
param_t yaw_d;
//param_t yaw_awu;
//param_t yaw_lim;
param_t att_p;
param_t att_i;
param_t att_d;
//param_t att_awu;
//param_t att_lim;
//param_t att_xoff;
//param_t att_yoff;
};
/**
@@ -130,17 +109,10 @@ static int parameters_init(struct mc_att_control_param_handles *h)
h->yaw_p = param_find("MC_YAWPOS_P");
h->yaw_i = param_find("MC_YAWPOS_I");
h->yaw_d = param_find("MC_YAWPOS_D");
//h->yaw_awu = param_find("MC_YAWPOS_AWU");
//h->yaw_lim = param_find("MC_YAWPOS_LIM");
h->att_p = param_find("MC_ATT_P");
h->att_i = param_find("MC_ATT_I");
h->att_d = param_find("MC_ATT_D");
//h->att_awu = param_find("MC_ATT_AWU");
//h->att_lim = param_find("MC_ATT_LIM");
//h->att_xoff = param_find("MC_ATT_XOFF");
//h->att_yoff = param_find("MC_ATT_YOFF");
return OK;
}
@@ -150,23 +122,17 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
param_get(h->yaw_p, &(p->yaw_p));
param_get(h->yaw_i, &(p->yaw_i));
param_get(h->yaw_d, &(p->yaw_d));
//param_get(h->yaw_awu, &(p->yaw_awu));
//param_get(h->yaw_lim, &(p->yaw_lim));
param_get(h->att_p, &(p->att_p));
param_get(h->att_i, &(p->att_i));
param_get(h->att_d, &(p->att_d));
//param_get(h->att_awu, &(p->att_awu));
//param_get(h->att_lim, &(p->att_lim));
//param_get(h->att_xoff, &(p->att_xoff));
//param_get(h->att_yoff, &(p->att_yoff));
return OK;
}
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position)
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position,
const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug)
{
static uint64_t last_run = 0;
static uint64_t last_input = 0;
@@ -207,7 +173,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
/* apply parameters */
pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
}
/* reset integral if on ground */
@@ -221,11 +187,13 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
/* control pitch (forward) output */
rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body ,
att->pitch, att->pitchspeed, deltaT);
att->pitch, att->pitchspeed, deltaT, &control_debug->pitch_p, &control_debug->pitch_i, &control_debug->pitch_d);
/* control roll (left/right) output */
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body ,
att->roll, att->rollspeed, deltaT);
att->roll, att->rollspeed, deltaT, NULL, NULL, NULL);
// printf("rates_sp: %4.4f, att setpoint: %4.4f\n, pitch: %4.4f, pitchspeed: %4.4f, dT: %4.4f", rates_sp->pitch, att_sp->pitch_body, att->pitch, att->pitchspeed, deltaT);
if (control_yaw_position) {
/* control yaw rate */
src/modules/multirotor_att_control/multirotor_attitude_control.h
+4 -1
View File
@@ -58,8 +58,11 @@
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_control_debug.h>
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position);
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position,
const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug);
#endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */
src/modules/multirotor_att_control/multirotor_rate_control.c
+14 -3
View File
@@ -58,6 +58,9 @@
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); /* same on Flamewheel */
PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
@@ -152,7 +155,8 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
}
void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[], struct actuator_controls_s *actuators)
const float rates[], struct actuator_controls_s *actuators,
const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug)
{
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
@@ -172,8 +176,13 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
static struct mc_rate_control_params p;
static struct mc_rate_control_param_handles h;
float pitch_control_last = 0.0f;
float roll_control_last = 0.0f;
static bool initialized = false;
float diff_filter_factor = 1.0f;
/* initialize the pid controllers when the function is called for the first time */
if (initialized == false) {
parameters_init(&h);
@@ -201,11 +210,13 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
/* control pitch (forward) output */
float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch ,
rates[1], 0.0f, deltaT);
rates[1], 0.0f, deltaT,
&control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d);
/* control roll (left/right) output */
float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll ,
rates[0], 0.0f, deltaT);
rates[0], 0.0f, deltaT,
&control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d);
/* control yaw rate */ //XXX use library here
float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]);
src/modules/multirotor_att_control/multirotor_rate_control.h
+3 -1
View File
@@ -57,8 +57,10 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_control_debug.h>
void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[], struct actuator_controls_s *actuators);
const float rates[], struct actuator_controls_s *actuators,
const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug);
#endif /* MULTIROTOR_RATE_CONTROL_H_ */
src/modules/px4iofirmware/controls.c
+2
View File
@@ -300,6 +300,8 @@ controls_tick() {
} else {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
}
} else {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
}
}
src/modules/px4iofirmware/dsm.c
+2 -2
View File
@@ -129,9 +129,9 @@ dsm_bind(uint16_t cmd, int pulses)
case dsm_bind_send_pulses:
for (int i = 0; i < pulses; i++) {
stm32_gpiowrite(usart1RxAsOutp, false);
up_udelay(50);
up_udelay(25);
stm32_gpiowrite(usart1RxAsOutp, true);
up_udelay(50);
up_udelay(25);
}
break;
case dsm_bind_reinit_uart:
src/modules/px4iofirmware/mixer.cpp
+112 -20
View File
@@ -59,6 +59,12 @@ extern "C" {
*/
#define FMU_INPUT_DROP_LIMIT_US 200000
/*
* Time that the ESCs need to initialize
*/
#define ESC_INIT_TIME_US 1000000
#define ESC_RAMP_TIME_US 2000000
/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
#define ROLL 0
#define PITCH 1
@@ -68,6 +74,17 @@ extern "C" {
/* current servo arm/disarm state */
static bool mixer_servos_armed = false;
static bool should_arm = false;
static bool should_always_enable_pwm = false;
static uint64_t esc_init_time;
enum esc_state_e {
ESC_OFF,
ESC_INIT,
ESC_RAMP,
ESC_ON
};
static esc_state_e esc_state;
/* selected control values and count for mixing */
enum mixer_source {
@@ -98,7 +115,7 @@ mixer_tick(void)
if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
isr_debug(1, "AP RX timeout");
}
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM);
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK);
r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
} else {
@@ -112,12 +129,11 @@ mixer_tick(void)
* Decide which set of controls we're using.
*/
/* do not mix if mixer is invalid or if RAW_PWM mode is on and FMU is good */
/* do not mix if RAW_PWM mode is on and FMU is good */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
/* don't actually mix anything - we already have raw PWM values or
not a valid mixer. */
/* don't actually mix anything - we already have raw PWM values */
source = MIX_NONE;
} else {
@@ -167,6 +183,48 @@ mixer_tick(void)
float outputs[PX4IO_SERVO_COUNT];
unsigned mixed;
uint16_t ramp_promille;
/* update esc init state, but only if we are truely armed and not just PWM enabled */
if (mixer_servos_armed && should_arm) {
switch (esc_state) {
/* after arming, some ESCs need an initalization period, count the time from here */
case ESC_OFF:
esc_init_time = hrt_absolute_time();
esc_state = ESC_INIT;
break;
/* after waiting long enough for the ESC initialization, we can start with the ramp to start the ESCs */
case ESC_INIT:
if (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US) {
esc_state = ESC_RAMP;
}
break;
/* then ramp until the min speed is reached */
case ESC_RAMP:
if (hrt_elapsed_time(&esc_init_time) > (ESC_INIT_TIME_US + ESC_RAMP_TIME_US)) {
esc_state = ESC_ON;
}
break;
case ESC_ON:
default:
break;
}
} else {
esc_state = ESC_OFF;
}
/* do the calculations during the ramp for all at once */
if(esc_state == ESC_RAMP) {
ramp_promille = (1000*(hrt_elapsed_time(&esc_init_time)-ESC_INIT_TIME_US))/ESC_RAMP_TIME_US;
}
/* mix */
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
@@ -176,9 +234,27 @@ mixer_tick(void)
/* save actuator values for FMU readback */
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
/* scale to servo output */
r_page_servos[i] = (outputs[i] * 600.0f) + 1500;
switch (esc_state) {
case ESC_INIT:
r_page_servos[i] = (outputs[i] * 600 + 1500);
break;
case ESC_RAMP:
r_page_servos[i] = (outputs[i]
* (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 - ramp_promille*r_page_servo_control_min[i] - (1000-ramp_promille)*900)/2/1000
+ (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 + ramp_promille*r_page_servo_control_min[i] + (1000-ramp_promille)*900)/2/1000);
break;
case ESC_ON:
r_page_servos[i] = (outputs[i]
* (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2
+ (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2);
break;
case ESC_OFF:
default:
break;
}
}
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
r_page_servos[i] = 0;
@@ -193,30 +269,46 @@ mixer_tick(void)
* XXX correct behaviour for failsafe may require an additional case
* here.
*/
bool should_arm = (
/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
/* there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
/* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) &&
/* FMU is available or FMU is not available but override is an option */
((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ))
should_arm = (
/* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
/* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
/* and FMU is armed */ && (
((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
/* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) )
/* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)
/* or failsafe was set manually */ || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)
)
);
if (should_arm && !mixer_servos_armed) {
should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
/* need to arm, but not armed */
up_pwm_servo_arm(true);
mixer_servos_armed = true;
r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
isr_debug(5, "> PWM enabled");
} else if (!should_arm && mixer_servos_armed) {
} else if ((!should_arm && !should_always_enable_pwm) && mixer_servos_armed) {
/* armed but need to disarm */
up_pwm_servo_arm(false);
mixer_servos_armed = false;
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
isr_debug(5, "> PWM disabled");
}
if (mixer_servos_armed) {
if (mixer_servos_armed && should_arm) {
/* update the servo outputs. */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
up_pwm_servo_set(i, r_page_servos[i]);
} else if (mixer_servos_armed && should_always_enable_pwm) {
/* set the idle servo outputs. */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
up_pwm_servo_set(i, r_page_servo_idle[i]);
}
}
@@ -265,9 +357,8 @@ static unsigned mixer_text_length = 0;
void
mixer_handle_text(const void *buffer, size_t length)
{
/* do not allow a mixer change while fully armed */
if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
/* do not allow a mixer change while outputs armed */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
return;
}
@@ -344,6 +435,7 @@ mixer_set_failsafe()
* Check if a custom failsafe value has been written,
* or if the mixer is not ok and bail out.
*/
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ||
!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
return;
src/modules/px4iofirmware/protocol.h
+13 -2
View File
@@ -94,7 +94,7 @@
#define PX4IO_P_STATUS_CPULOAD 1
#define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */
#define PX4IO_P_STATUS_FLAGS_ARMED (1 << 0) /* arm-ok and locally armed */
#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED (1 << 0) /* arm-ok and locally armed */
#define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */
#define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */
@@ -106,7 +106,8 @@
#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */
#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */
#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */
#define PX4IO_P_STATUS_FLAGS_RC_DSM11 (1 << 12) /* DSM input is 11 bit data */
#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */
#define PX4IO_P_STATUS_FLAGS_RC_DSM11 (1 << 13) /* DSM input is 11 bit data */
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */
@@ -157,6 +158,7 @@
#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */
#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 3) /* use custom failsafe values, not 0 values of mixer */
#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */
#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */
#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */
@@ -209,6 +211,15 @@ enum { /* DSM bind states */
#define PX4IO_PAGE_TEST 127
#define PX4IO_P_TEST_LED 0 /* set the amber LED on/off */
/* PWM minimum values for certain ESCs */
#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM maximum values for certain ESCs */
#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM idle values that are active, even when SAFETY_SAFE */
#define PX4IO_PAGE_IDLE_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/**
* As-needed mixer data upload.
*
src/modules/px4iofirmware/px4io.h
+3
View File
@@ -78,6 +78,9 @@ extern volatile uint16_t r_page_setup[]; /* PX4IO_PAGE_SETUP */
extern volatile uint16_t r_page_controls[]; /* PX4IO_PAGE_CONTROLS */
extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */
extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
extern uint16_t r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */
extern uint16_t r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */
extern uint16_t r_page_servo_idle[]; /* PX4IO_PAGE_IDLE_PWM */
/*
* Register aliases.
src/modules/px4iofirmware/registers.c
+112 -7
View File
@@ -158,7 +158,9 @@ volatile uint16_t r_page_setup[] =
#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
PX4IO_P_SETUP_ARMING_IO_ARM_OK)
PX4IO_P_SETUP_ARMING_IO_ARM_OK) | \
PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE
#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
@@ -196,6 +198,30 @@ uint16_t r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRI
*/
uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 };
/**
* PAGE 106
*
* minimum PWM values when armed
*
*/
uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
/**
* PAGE 107
*
* maximum PWM values when armed
*
*/
uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 };
/**
* PAGE 108
*
* idle PWM values for difficult ESCs
*
*/
uint16_t r_page_servo_idle[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
int
registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
{
@@ -259,6 +285,75 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
}
break;
case PX4IO_PAGE_CONTROL_MIN_PWM:
/* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
if (*values == 0)
/* set to default */
r_page_servo_control_min[offset] = 900;
else if (*values > 1200)
r_page_servo_control_min[offset] = 1200;
else if (*values < 900)
r_page_servo_control_min[offset] = 900;
else
r_page_servo_control_min[offset] = *values;
offset++;
num_values--;
values++;
}
break;
case PX4IO_PAGE_CONTROL_MAX_PWM:
/* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
if (*values == 0)
/* set to default */
r_page_servo_control_max[offset] = 2100;
else if (*values > 2100)
r_page_servo_control_max[offset] = 2100;
else if (*values < 1800)
r_page_servo_control_max[offset] = 1800;
else
r_page_servo_control_max[offset] = *values;
offset++;
num_values--;
values++;
}
break;
case PX4IO_PAGE_IDLE_PWM:
/* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
if (*values == 0)
/* set to default */
r_page_servo_idle[offset] = 0;
else if (*values < 900)
r_page_servo_idle[offset] = 900;
else if (*values > 2100)
r_page_servo_idle[offset] = 2100;
else
r_page_servo_idle[offset] = *values;
/* flag the failsafe values as custom */
r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE;
offset++;
num_values--;
values++;
}
break;
/* handle text going to the mixer parser */
case PX4IO_PAGE_MIXERLOAD:
mixer_handle_text(values, num_values * sizeof(*values));
@@ -331,9 +426,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
* so that an in-air reset of FMU can not lead to a
* lockup of the IO arming state.
*/
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
}
// XXX do not reset IO's safety state by FMU for now
// if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
// r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
// }
r_setup_arming = value;
@@ -397,9 +494,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
case PX4IO_PAGE_RC_CONFIG: {
/* do not allow a RC config change while fully armed */
if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
/* do not allow a RC config change while outputs armed */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
break;
}
@@ -659,6 +755,15 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
case PX4IO_PAGE_FAILSAFE_PWM:
SELECT_PAGE(r_page_servo_failsafe);
break;
case PX4IO_PAGE_CONTROL_MIN_PWM:
SELECT_PAGE(r_page_servo_control_min);
break;
case PX4IO_PAGE_CONTROL_MAX_PWM:
SELECT_PAGE(r_page_servo_control_max);
break;
case PX4IO_PAGE_IDLE_PWM:
SELECT_PAGE(r_page_servo_idle);
break;
default:
return -1;
src/modules/px4iofirmware/safety.c
+5 -5
View File
@@ -110,7 +110,7 @@ safety_check_button(void *arg)
* state machine, keep ARM_COUNTER_THRESHOLD the same
* length in all cases of the if/else struct below.
*/
if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
(r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) {
if (counter < ARM_COUNTER_THRESHOLD) {
@@ -118,18 +118,18 @@ safety_check_button(void *arg)
} else if (counter == ARM_COUNTER_THRESHOLD) {
/* switch to armed state */
r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED;
r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
counter++;
}
} else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
} else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
if (counter < ARM_COUNTER_THRESHOLD) {
counter++;
} else if (counter == ARM_COUNTER_THRESHOLD) {
/* change to disarmed state and notify the FMU */
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
counter++;
}
@@ -140,7 +140,7 @@ safety_check_button(void *arg)
/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM;
if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) {
if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) {
pattern = LED_PATTERN_IO_FMU_ARMED;
src/modules/sdlog2/sdlog2.c
+81 -19
View File
@@ -60,6 +60,7 @@
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
@@ -75,6 +76,7 @@
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_control_debug.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/differential_pressure.h>
@@ -192,7 +194,7 @@ static int file_copy(const char *file_old, const char *file_new);
static void handle_command(struct vehicle_command_s *cmd);
static void handle_status(struct vehicle_status_s *cmd);
static void handle_status(struct actuator_armed_s *armed);
/**
* Create folder for current logging session. Store folder name in 'log_folder'.
@@ -623,9 +625,10 @@ int sdlog2_thread_main(int argc, char *argv[])
errx(1, "can't allocate log buffer, exiting.");
}
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* number of messages */
const ssize_t fdsc = 19;
/* --- IMPORTANT: DEFINE MAX NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* max number of messages */
const ssize_t fdsc = 21;
/* Sanity check variable and index */
ssize_t fdsc_count = 0;
/* file descriptors to wait for */
@@ -634,6 +637,9 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vehicle_status_s buf_status;
memset(&buf_status, 0, sizeof(buf_status));
struct actuator_armed_s buf_armed;
memset(&buf_armed, 0, sizeof(buf_armed));
/* warning! using union here to save memory, elements should be used separately! */
union {
struct vehicle_command_s cmd;
@@ -650,6 +656,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vehicle_global_position_setpoint_s global_pos_sp;
struct vehicle_gps_position_s gps_pos;
struct vehicle_vicon_position_s vicon_pos;
struct vehicle_control_debug_s control_debug;
struct optical_flow_s flow;
struct rc_channels_s rc;
struct differential_pressure_s diff_pres;
@@ -661,6 +668,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct {
int cmd_sub;
int status_sub;
int armed_sub;
int sensor_sub;
int att_sub;
int att_sp_sub;
@@ -674,6 +682,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int global_pos_sp_sub;
int gps_pos_sub;
int vicon_pos_sub;
int control_debug_sub;
int flow_sub;
int rc_sub;
int airspeed_sub;
@@ -695,6 +704,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_GPS_s log_GPS;
struct log_ATTC_s log_ATTC;
struct log_STAT_s log_STAT;
struct log_CTRL_s log_CTRL;
struct log_RC_s log_RC;
struct log_OUT0_s log_OUT0;
struct log_AIRS_s log_AIRS;
@@ -716,6 +726,12 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- ACTUATOR ARMED --- */
subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed));
fds[fdsc_count].fd = subs.armed_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- VEHICLE STATUS --- */
subs.status_sub = orb_subscribe(ORB_ID(vehicle_status));
fds[fdsc_count].fd = subs.status_sub;
@@ -800,6 +816,12 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- CONTROL DEBUG --- */
subs.control_debug_sub = orb_subscribe(ORB_ID(vehicle_control_debug));
fds[fdsc_count].fd = subs.control_debug_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- OPTICAL FLOW --- */
subs.flow_sub = orb_subscribe(ORB_ID(optical_flow));
fds[fdsc_count].fd = subs.flow_sub;
@@ -883,22 +905,33 @@ int sdlog2_thread_main(int argc, char *argv[])
handled_topics++;
}
/* --- ARMED- LOG MANAGEMENT --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(actuator_armed), subs.armed_sub, &buf_armed);
if (log_when_armed) {
handle_status(&buf_armed);
}
handled_topics++;
}
/* --- VEHICLE STATUS - LOG MANAGEMENT --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status);
if (log_when_armed) {
handle_status(&buf_status);
}
//if (log_when_armed) {
// handle_status(&buf_armed);
//}
handled_topics++;
//handled_topics++;
}
if (!logging_enabled || !check_data || handled_topics >= poll_ret) {
continue;
}
ifds = 1; // Begin from fds[1] again
ifds = 2; // Begin from fds[2] again
pthread_mutex_lock(&logbuffer_mutex);
@@ -911,13 +944,18 @@ int sdlog2_thread_main(int argc, char *argv[])
if (fds[ifds++].revents & POLLIN) {
// Don't orb_copy, it's already done few lines above
log_msg.msg_type = LOG_STAT_MSG;
log_msg.body.log_STAT.state = (unsigned char) buf_status.state_machine;
log_msg.body.log_STAT.flight_mode = (unsigned char) buf_status.flight_mode;
log_msg.body.log_STAT.manual_control_mode = (unsigned char) buf_status.manual_control_mode;
log_msg.body.log_STAT.manual_sas_mode = (unsigned char) buf_status.manual_sas_mode;
log_msg.body.log_STAT.armed = (unsigned char) buf_status.flag_system_armed;
log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery;
log_msg.body.log_STAT.battery_current = buf_status.current_battery;
// XXX fix this
// log_msg.body.log_STAT.state = (unsigned char) buf_status.state_machine;
// log_msg.body.log_STAT.flight_mode = (unsigned char) buf_status.flight_mode;
// log_msg.body.log_STAT.manual_control_mode = (unsigned char) buf_status.manual_control_mode;
// log_msg.body.log_STAT.manual_sas_mode = (unsigned char) buf_status.manual_sas_mode;
log_msg.body.log_STAT.state = 0;
log_msg.body.log_STAT.flight_mode = 0;
log_msg.body.log_STAT.manual_control_mode = 0;
log_msg.body.log_STAT.manual_sas_mode = 0;
log_msg.body.log_STAT.armed = (unsigned char) buf_armed.armed; /* XXX fmu armed correct? */
log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage;
log_msg.body.log_STAT.battery_current = buf_status.battery_current;
log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
log_msg.body.log_STAT.battery_warning = (unsigned char) buf_status.battery_warning;
LOGBUFFER_WRITE_AND_COUNT(STAT);
@@ -1006,6 +1044,9 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_ATT.roll_rate = buf.att.rollspeed;
log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed;
log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed;
log_msg.body.log_ATT.roll_acc = buf.att.rollacc;
log_msg.body.log_ATT.pitch_acc = buf.att.pitchacc;
log_msg.body.log_ATT.yaw_acc = buf.att.yawacc;
LOGBUFFER_WRITE_AND_COUNT(ATT);
}
@@ -1121,6 +1162,27 @@ int sdlog2_thread_main(int argc, char *argv[])
// TODO not implemented yet
}
/* --- CONTROL DEBUG --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_control_debug), subs.control_debug_sub, &buf.control_debug);
log_msg.msg_type = LOG_CTRL_MSG;
log_msg.body.log_CTRL.roll_p = buf.control_debug.roll_p;
log_msg.body.log_CTRL.roll_i = buf.control_debug.roll_i;
log_msg.body.log_CTRL.roll_d = buf.control_debug.roll_d;
log_msg.body.log_CTRL.roll_rate_p = buf.control_debug.roll_rate_p;
log_msg.body.log_CTRL.roll_rate_i = buf.control_debug.roll_rate_i;
log_msg.body.log_CTRL.roll_rate_d = buf.control_debug.roll_rate_d;
log_msg.body.log_CTRL.pitch_p = buf.control_debug.pitch_p;
log_msg.body.log_CTRL.pitch_i = buf.control_debug.pitch_i;
log_msg.body.log_CTRL.pitch_d = buf.control_debug.pitch_d;
log_msg.body.log_CTRL.pitch_rate_p = buf.control_debug.pitch_rate_p;
log_msg.body.log_CTRL.pitch_rate_i = buf.control_debug.pitch_rate_i;
log_msg.body.log_CTRL.pitch_rate_d = buf.control_debug.pitch_rate_d;
LOGBUFFER_WRITE_AND_COUNT(CTRL);
}
/* --- FLOW --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow);
@@ -1295,10 +1357,10 @@ void handle_command(struct vehicle_command_s *cmd)
}
}
void handle_status(struct vehicle_status_s *status)
void handle_status(struct actuator_armed_s *armed)
{
if (status->flag_system_armed != flag_system_armed) {
flag_system_armed = status->flag_system_armed;
if (armed->armed != flag_system_armed) {
flag_system_armed = armed->armed;
if (flag_system_armed) {
sdlog2_start_log();
src/modules/sdlog2/sdlog2_messages.h
+27 -6
View File
@@ -63,6 +63,9 @@ struct log_ATT_s {
float roll_rate;
float pitch_rate;
float yaw_rate;
float roll_acc;
float pitch_acc;
float yaw_acc;
};
/* --- ATSP - ATTITUDE SET POINT --- */
@@ -160,27 +163,44 @@ struct log_STAT_s {
uint8_t battery_warning;
};
/* --- CTRL - CONTROL DEBUG --- */
#define LOG_CTRL_MSG 11
struct log_CTRL_s {
float roll_p;
float roll_i;
float roll_d;
float roll_rate_p;
float roll_rate_i;
float roll_rate_d;
float pitch_p;
float pitch_i;
float pitch_d;
float pitch_rate_p;
float pitch_rate_i;
float pitch_rate_d;
};
/* --- RC - RC INPUT CHANNELS --- */
#define LOG_RC_MSG 11
#define LOG_RC_MSG 12
struct log_RC_s {
float channel[8];
};
/* --- OUT0 - ACTUATOR_0 OUTPUT --- */
#define LOG_OUT0_MSG 12
#define LOG_OUT0_MSG 13
struct log_OUT0_s {
float output[8];
};
/* --- AIRS - AIRSPEED --- */
#define LOG_AIRS_MSG 13
#define LOG_AIRS_MSG 14
struct log_AIRS_s {
float indicated_airspeed;
float true_airspeed;
};
/* --- ARSP - ATTITUDE RATE SET POINT --- */
#define LOG_ARSP_MSG 14
#define LOG_ARSP_MSG 15
struct log_ARSP_s {
float roll_rate_sp;
float pitch_rate_sp;
@@ -188,7 +208,7 @@ struct log_ARSP_s {
};
/* --- FLOW - OPTICAL FLOW --- */
#define LOG_FLOW_MSG 15
#define LOG_FLOW_MSG 16
struct log_FLOW_s {
int16_t flow_raw_x;
int16_t flow_raw_y;
@@ -250,7 +270,7 @@ struct log_ESC_s {
static const struct log_format_s log_formats[] = {
LOG_FORMAT(TIME, "Q", "StartTime"),
LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"),
LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollR,PitchR,YawR,RollA,PitchA,YawA"),
LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"),
@@ -259,6 +279,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"),
LOG_FORMAT(CTRL, "ffffffffffff", "RP,RI,RD,RRP,RRI,RRD,PP,PI,PD,PRP,PRI,PRD"),
LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
src/modules/sensors/sensor_params.c
+6 -6
View File
@@ -156,6 +156,7 @@ PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f);
PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */
PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
@@ -169,13 +170,12 @@ PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
PARAM_DEFINE_INT32(RC_MAP_OVER_SW, 5);
PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 6);
PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5);
PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 6);
PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_SAS_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_RTL_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0);
src/modules/sensors/sensors.cpp
+74 -90
View File
@@ -74,7 +74,7 @@
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/differential_pressure.h>
@@ -191,7 +191,7 @@ private:
int _baro_sub; /**< raw baro data subscription */
int _airspeed_sub; /**< airspeed subscription */
int _diff_pres_sub; /**< raw differential pressure subscription */
int _vstatus_sub; /**< vehicle status subscription */
int _vcontrol_mode_sub; /**< vehicle control mode subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */
@@ -234,13 +234,12 @@ private:
int rc_map_yaw;
int rc_map_throttle;
int rc_map_manual_override_sw;
int rc_map_auto_mode_sw;
int rc_map_mode_sw;
int rc_map_return_sw;
int rc_map_assisted_sw;
int rc_map_mission_sw;
int rc_map_manual_mode_sw;
int rc_map_sas_mode_sw;
int rc_map_rtl_sw;
int rc_map_offboard_ctrl_mode_sw;
// int rc_map_offboard_ctrl_mode_sw;
int rc_map_flaps;
@@ -257,8 +256,6 @@ private:
float battery_voltage_scaling;
int rc_rl1_DSM_VCC_control;
} _parameters; /**< local copies of interesting parameters */
struct {
@@ -285,13 +282,12 @@ private:
param_t rc_map_yaw;
param_t rc_map_throttle;
param_t rc_map_manual_override_sw;
param_t rc_map_auto_mode_sw;
param_t rc_map_mode_sw;
param_t rc_map_return_sw;
param_t rc_map_assisted_sw;
param_t rc_map_mission_sw;
param_t rc_map_manual_mode_sw;
param_t rc_map_sas_mode_sw;
param_t rc_map_rtl_sw;
param_t rc_map_offboard_ctrl_mode_sw;
// param_t rc_map_offboard_ctrl_mode_sw;
param_t rc_map_flaps;
@@ -308,8 +304,6 @@ private:
param_t battery_voltage_scaling;
param_t rc_rl1_DSM_VCC_control;
} _parameter_handles; /**< handles for interesting parameters */
@@ -384,9 +378,9 @@ private:
void diff_pres_poll(struct sensor_combined_s &raw);
/**
* Check for changes in vehicle status.
* Check for changes in vehicle control mode.
*/
void vehicle_status_poll();
void vehicle_control_mode_poll();
/**
* Check for changes in parameters.
@@ -441,7 +435,7 @@ Sensors::Sensors() :
_mag_sub(-1),
_rc_sub(-1),
_baro_sub(-1),
_vstatus_sub(-1),
_vcontrol_mode_sub(-1),
_params_sub(-1),
_manual_control_sub(-1),
@@ -492,16 +486,16 @@ Sensors::Sensors() :
_parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE");
/* mandatory mode switches, mapped to channel 5 and 6 per default */
_parameter_handles.rc_map_manual_override_sw = param_find("RC_MAP_OVER_SW");
_parameter_handles.rc_map_auto_mode_sw = param_find("RC_MAP_MODE_SW");
_parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW");
_parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_SW");
_parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS");
/* optional mode switches, not mapped per default */
_parameter_handles.rc_map_manual_mode_sw = param_find("RC_MAP_MAN_SW");
_parameter_handles.rc_map_sas_mode_sw = param_find("RC_MAP_SAS_SW");
_parameter_handles.rc_map_rtl_sw = param_find("RC_MAP_RTL_SW");
_parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW");
_parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW");
_parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW");
// _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW");
_parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1");
_parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2");
@@ -544,9 +538,6 @@ Sensors::Sensors() :
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
/* DSM VCC relay control */
_parameter_handles.rc_rl1_DSM_VCC_control = param_find("RC_RL1_DSM_VCC");
/* fetch initial parameter values */
parameters_update();
}
@@ -630,33 +621,29 @@ Sensors::parameters_update()
warnx("Failed getting throttle chan index");
}
if (param_get(_parameter_handles.rc_map_manual_override_sw, &(_parameters.rc_map_manual_override_sw)) != OK) {
warnx("Failed getting override sw chan index");
if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) {
warnx("Failed getting mode sw chan index");
}
if (param_get(_parameter_handles.rc_map_auto_mode_sw, &(_parameters.rc_map_auto_mode_sw)) != OK) {
warnx("Failed getting auto mode sw chan index");
if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) {
warnx("Failed getting return sw chan index");
}
if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) {
warnx("Failed getting assisted sw chan index");
}
if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) {
warnx("Failed getting mission sw chan index");
}
if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
warnx("Failed getting flaps chan index");
}
if (param_get(_parameter_handles.rc_map_manual_mode_sw, &(_parameters.rc_map_manual_mode_sw)) != OK) {
warnx("Failed getting manual mode sw chan index");
}
if (param_get(_parameter_handles.rc_map_rtl_sw, &(_parameters.rc_map_rtl_sw)) != OK) {
warnx("Failed getting rtl sw chan index");
}
if (param_get(_parameter_handles.rc_map_sas_mode_sw, &(_parameters.rc_map_sas_mode_sw)) != OK) {
warnx("Failed getting sas mode sw chan index");
}
if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) {
warnx("Failed getting offboard control mode sw chan index");
}
// if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) {
// warnx("Failed getting offboard control mode sw chan index");
// }
if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) {
warnx("Failed getting mode aux 1 index");
@@ -689,15 +676,14 @@ Sensors::parameters_update()
_rc.function[PITCH] = _parameters.rc_map_pitch - 1;
_rc.function[YAW] = _parameters.rc_map_yaw - 1;
_rc.function[OVERRIDE] = _parameters.rc_map_manual_override_sw - 1;
_rc.function[AUTO_MODE] = _parameters.rc_map_auto_mode_sw - 1;
_rc.function[MODE] = _parameters.rc_map_mode_sw - 1;
_rc.function[RETURN] = _parameters.rc_map_return_sw - 1;
_rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1;
_rc.function[MISSION] = _parameters.rc_map_mission_sw - 1;
_rc.function[FLAPS] = _parameters.rc_map_flaps - 1;
_rc.function[MANUAL_MODE] = _parameters.rc_map_manual_mode_sw - 1;
_rc.function[RTL] = _parameters.rc_map_rtl_sw - 1;
_rc.function[SAS_MODE] = _parameters.rc_map_sas_mode_sw - 1;
_rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1;
// _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1;
_rc.function[AUX_1] = _parameters.rc_map_aux1 - 1;
_rc.function[AUX_2] = _parameters.rc_map_aux2 - 1;
@@ -738,11 +724,6 @@ Sensors::parameters_update()
warnx("Failed updating voltage scaling param");
}
/* relay 1 DSM VCC control */
if (param_get(_parameter_handles.rc_rl1_DSM_VCC_control, &(_parameters.rc_rl1_DSM_VCC_control)) != OK) {
warnx("Failed updating relay 1 DSM VCC control");
}
return OK;
}
@@ -989,21 +970,21 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
}
void
Sensors::vehicle_status_poll()
Sensors::vehicle_control_mode_poll()
{
struct vehicle_status_s vstatus;
bool vstatus_updated;
struct vehicle_control_mode_s vcontrol_mode;
bool vcontrol_mode_updated;
/* Check HIL state if vehicle status has changed */
orb_check(_vstatus_sub, &vstatus_updated);
/* Check HIL state if vehicle control mode has changed */
orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated);
if (vstatus_updated) {
if (vcontrol_mode_updated) {
orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &vstatus);
orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode);
/* switching from non-HIL to HIL mode */
//printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled);
if (vstatus.flag_hil_enabled && !_hil_enabled) {
if (vcontrol_mode.flag_system_hil_enabled && !_hil_enabled) {
_hil_enabled = true;
_publishing = false;
@@ -1206,10 +1187,11 @@ Sensors::ppm_poll()
manual_control.yaw = NAN;
manual_control.throttle = NAN;
manual_control.manual_mode_switch = NAN;
manual_control.manual_sas_switch = NAN;
manual_control.return_to_launch_switch = NAN;
manual_control.auto_offboard_input_switch = NAN;
manual_control.mode_switch = NAN;
manual_control.return_switch = NAN;
manual_control.assisted_switch = NAN;
manual_control.mission_switch = NAN;
// manual_control.auto_offboard_input_switch = NAN;
manual_control.flaps = NAN;
manual_control.aux1 = NAN;
@@ -1309,11 +1291,17 @@ Sensors::ppm_poll()
manual_control.yaw *= _parameters.rc_scale_yaw;
}
/* override switch input */
manual_control.manual_override_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OVERRIDE]].scaled);
/* mode switch input */
manual_control.auto_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[AUTO_MODE]].scaled);
manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
/* land switch input */
manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
/* assisted switch input */
manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled);
/* mission switch input */
manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
/* flaps */
if (_rc.function[FLAPS] >= 0) {
@@ -1325,21 +1313,17 @@ Sensors::ppm_poll()
}
}
if (_rc.function[MANUAL_MODE] >= 0) {
manual_control.manual_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MANUAL_MODE]].scaled);
if (_rc.function[MODE] >= 0) {
manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
}
if (_rc.function[SAS_MODE] >= 0) {
manual_control.manual_sas_switch = limit_minus_one_to_one(_rc.chan[_rc.function[SAS_MODE]].scaled);
if (_rc.function[MISSION] >= 0) {
manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
}
if (_rc.function[RTL] >= 0) {
manual_control.return_to_launch_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RTL]].scaled);
}
if (_rc.function[OFFBOARD_MODE] >= 0) {
manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled);
}
// if (_rc.function[OFFBOARD_MODE] >= 0) {
// manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled);
// }
/* aux functions, only assign if valid mapping is present */
if (_rc.function[AUX_1] >= 0) {
@@ -1412,12 +1396,12 @@ Sensors::task_main()
_rc_sub = orb_subscribe(ORB_ID(input_rc));
_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vstatus_sub, 200);
orb_set_interval(_vcontrol_mode_sub, 200);
/* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */
orb_set_interval(_gyro_sub, 4);
@@ -1473,7 +1457,7 @@ Sensors::task_main()
perf_begin(_loop_perf);
/* check vehicle status for changes to publication state */
vehicle_status_poll();
vehicle_control_mode_poll();
/* check parameters for updates */
parameter_update_poll();
src/modules/systemlib/pid/pid.c
+5 -1
View File
@@ -124,7 +124,7 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float
* @param dt
* @return
*/
__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt, float *ctrl_p, float *ctrl_i, float *ctrl_d)
{
/* error = setpoint - actual_position
integral = integral + (error*dt)
@@ -196,6 +196,10 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
pid->last_output = output;
}
*ctrl_p = (error * pid->kp);
*ctrl_i = (i * pid->ki);
*ctrl_d = (d * pid->kd);
return pid->last_output;
}
src/modules/systemlib/pid/pid.h
+1 -1
View File
@@ -85,7 +85,7 @@ typedef struct {
__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode, float dt_min);
__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit);
//void pid_set(PID_t *pid, float sp);
__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt, float *ctrl_p, float *ctrl_i, float *ctrl_d);
__EXPORT void pid_reset_integral(PID_t *pid);
src/modules/uORB/objects_common.cpp
+12 -1
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* 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
@@ -77,6 +77,9 @@ ORB_DEFINE(home_position, struct home_position_s);
#include "topics/vehicle_status.h"
ORB_DEFINE(vehicle_status, struct vehicle_status_s);
#include "topics/safety.h"
ORB_DEFINE(safety, struct safety_s);
#include "topics/battery_status.h"
ORB_DEFINE(battery_status, struct battery_status_s);
@@ -98,6 +101,9 @@ ORB_DEFINE(rc_channels, struct rc_channels_s);
#include "topics/vehicle_command.h"
ORB_DEFINE(vehicle_command, struct vehicle_command_s);
#include "topics/vehicle_control_mode.h"
ORB_DEFINE(vehicle_control_mode, struct vehicle_control_mode_s);
#include "topics/vehicle_local_position_setpoint.h"
ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoint_s);
@@ -119,6 +125,9 @@ ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s);
#include "topics/manual_control_setpoint.h"
ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s);
#include "topics/vehicle_control_debug.h"
ORB_DEFINE(vehicle_control_debug, struct vehicle_control_debug_s);
#include "topics/offboard_control_setpoint.h"
ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s);
@@ -146,6 +155,8 @@ ORB_DEFINE(actuator_controls_0, struct actuator_controls_s);
ORB_DEFINE(actuator_controls_1, struct actuator_controls_s);
ORB_DEFINE(actuator_controls_2, struct actuator_controls_s);
ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
#include "topics/actuator_armed.h"
ORB_DEFINE(actuator_armed, struct actuator_armed_s);
/* actuator controls, as set by actuators / mixers after limiting */
src/modules/uORB/topics/actuator_armed.h
+58
View File
@@ -0,0 +1,58 @@
/****************************************************************************
*
* Copyright (C) 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.
*
****************************************************************************/
/**
* @file actuator_armed.h
*
* Actuator armed topic
*
*/
#ifndef TOPIC_ACTUATOR_ARMED_H
#define TOPIC_ACTUATOR_ARMED_H
#include <stdint.h>
#include "../uORB.h"
/** global 'actuator output is live' control. */
struct actuator_armed_s {
uint64_t timestamp;
bool armed; /**< Set to true if system is armed */
bool ready_to_arm; /**< Set to true if system is ready to be armed */
bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
};
ORB_DECLARE(actuator_armed);
#endif
src/modules/uORB/topics/actuator_controls.h
+3 -12
View File
@@ -52,6 +52,9 @@
#define NUM_ACTUATOR_CONTROLS 8
#define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */
/* control sets with pre-defined applications */
#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0)
struct actuator_controls_s {
uint64_t timestamp;
float control[NUM_ACTUATOR_CONTROLS];
@@ -63,16 +66,4 @@ ORB_DECLARE(actuator_controls_1);
ORB_DECLARE(actuator_controls_2);
ORB_DECLARE(actuator_controls_3);
/* control sets with pre-defined applications */
#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0)
/** global 'actuator output is live' control. */
struct actuator_armed_s {
bool armed; /**< Set to true if system is armed */
bool ready_to_arm; /**< Set to true if system is ready to be armed */
bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
};
ORB_DECLARE(actuator_armed);
#endif
src/modules/uORB/topics/manual_control_setpoint.h
+7 -6
View File
@@ -56,17 +56,18 @@ struct manual_control_setpoint_s {
float yaw; /**< rudder / yaw rate / yaw */
float throttle; /**< throttle / collective thrust / altitude */
float manual_override_switch; /**< manual override mode (mandatory) */
float auto_mode_switch; /**< auto mode switch (mandatory) */
float mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */
float return_switch; /**< land 2 position switch (mandatory): land, no effect */
float assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */
float mission_switch; /**< mission 2 position switch (optional): mission, loiter */
/**
* Any of the channels below may not be available and be set to NaN
* to indicate that it does not contain valid data.
*/
float manual_mode_switch; /**< manual mode (man, sas, alt) switch (optional) */
float manual_sas_switch; /**< sas mode (rates / attitude) switch (optional) */
float return_to_launch_switch; /**< return to launch switch (0 = disabled, 1 = enabled) */
float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */
// XXX needed or parameter?
//float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */
float flaps; /**< flap position */
src/modules/uORB/topics/rc_channels.h
+16 -14
View File
@@ -53,9 +53,12 @@
/**
* The number of RC channel inputs supported.
* Current (Q1/2013) radios support up to 18 channels,
* leaving at a sane value of 14.
* leaving at a sane value of 15.
* This number can be greater then number of RC channels,
* because single RC channel can be mapped to multiple
* functions, e.g. for various mode switches.
*/
#define RC_CHANNELS_MAX 14
#define RC_CHANNELS_MAX 15
/**
* This defines the mapping of the RC functions.
@@ -68,18 +71,17 @@ enum RC_CHANNELS_FUNCTION
ROLL = 1,
PITCH = 2,
YAW = 3,
OVERRIDE = 4,
AUTO_MODE = 5,
MANUAL_MODE = 6,
SAS_MODE = 7,
RTL = 8,
OFFBOARD_MODE = 9,
FLAPS = 10,
AUX_1 = 11,
AUX_2 = 12,
AUX_3 = 13,
AUX_4 = 14,
AUX_5 = 15,
MODE = 4,
RETURN = 5,
ASSISTED = 6,
MISSION = 7,
OFFBOARD_MODE = 8,
FLAPS = 9,
AUX_1 = 10,
AUX_2 = 11,
AUX_3 = 12,
AUX_4 = 13,
AUX_5 = 14,
RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */
};
src/modules/uORB/topics/safety.h
+57
View File
@@ -0,0 +1,57 @@
/****************************************************************************
*
* Copyright (C) 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.
*
****************************************************************************/
/**
* @file safety.h
*
* Safety topic to pass safety state from px4io driver to commander
* This concerns only the safety button of the px4io but has nothing to do
* with arming/disarming.
*/
#ifndef TOPIC_SAFETY_H
#define TOPIC_SAFETY_H
#include <stdint.h>
#include "../uORB.h"
struct safety_s {
uint64_t timestamp;
bool safety_switch_available; /**< Set to true if a safety switch is connected */
bool safety_off; /**< Set to true if safety is off */
};
ORB_DECLARE(safety);
#endif
src/modules/uORB/topics/vehicle_control_debug.h
+87
View File
@@ -0,0 +1,87 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file vehicle_control_debug.h
* For debugging purposes to log PID parts of controller
*/
#ifndef TOPIC_VEHICLE_CONTROL_DEBUG_H_
#define TOPIC_VEHICLE_CONTROL_DEBUG_H_
#include <stdint.h>
#include "../uORB.h"
/**
* @addtogroup topics
* @{
*/
struct vehicle_control_debug_s
{
uint64_t timestamp; /**< in microseconds since system start */
float roll_p; /**< roll P control part */
float roll_i; /**< roll I control part */
float roll_d; /**< roll D control part */
float roll_rate_p; /**< roll rate P control part */
float roll_rate_i; /**< roll rate I control part */
float roll_rate_d; /**< roll rate D control part */
float pitch_p; /**< pitch P control part */
float pitch_i; /**< pitch I control part */
float pitch_d; /**< pitch D control part */
float pitch_rate_p; /**< pitch rate P control part */
float pitch_rate_i; /**< pitch rate I control part */
float pitch_rate_d; /**< pitch rate D control part */
float yaw_p; /**< yaw P control part */
float yaw_i; /**< yaw I control part */
float yaw_d; /**< yaw D control part */
float yaw_rate_p; /**< yaw rate P control part */
float yaw_rate_i; /**< yaw rate I control part */
float yaw_rate_d; /**< yaw rate D control part */
}; /**< vehicle_control_debug */
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(vehicle_control_debug);
#endif
src/modules/uORB/topics/vehicle_control_mode.h
+93
View File
@@ -0,0 +1,93 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author 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 vehicle_control_mode.h
* Definition of the vehicle_control_mode uORB topic.
*
* All control apps should depend their actions based on the flags set here.
*/
#ifndef VEHICLE_CONTROL_MODE
#define VEHICLE_CONTROL_MODE
#include <stdint.h>
#include <stdbool.h>
#include "../uORB.h"
/**
* @addtogroup topics @{
*/
/**
* state machine / state of vehicle.
*
* Encodes the complete system state and is set by the commander app.
*/
struct vehicle_control_mode_s
{
uint16_t counter; /**< incremented by the writing thread everytime new data is stored */
uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */
// XXX needs yet to be set by state machine helper
bool flag_system_hil_enabled;
bool flag_control_manual_enabled; /**< true if manual input is mixed in */
bool flag_control_offboard_enabled; /**< true if offboard control input is on */
// XXX shouldn't be necessairy
//bool flag_auto_enabled;
bool flag_control_rates_enabled; /**< true if rates are stabilized */
bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */
bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */
bool flag_control_position_enabled; /**< true if position is controlled */
bool flag_control_altitude_enabled; /**< true if altitude is controlled */
bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */
bool failsave_highlevel; /**< Set to true if high-level failsafe mode is enabled */
};
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(vehicle_control_mode);
#endif
src/modules/uORB/topics/vehicle_status.h
+89 -67
View File
@@ -58,22 +58,64 @@
* @addtogroup topics @{
*/
/* State Machine */
typedef enum
{
SYSTEM_STATE_PREFLIGHT = 0,
SYSTEM_STATE_STANDBY = 1,
SYSTEM_STATE_GROUND_READY = 2,
SYSTEM_STATE_MANUAL = 3,
SYSTEM_STATE_STABILIZED = 4,
SYSTEM_STATE_AUTO = 5,
SYSTEM_STATE_MISSION_ABORT = 6,
SYSTEM_STATE_EMCY_LANDING = 7,
SYSTEM_STATE_EMCY_CUTOFF = 8,
SYSTEM_STATE_GROUND_ERROR = 9,
SYSTEM_STATE_REBOOT= 10,
/* main state machine */
typedef enum {
MAIN_STATE_MANUAL = 0,
MAIN_STATE_SEATBELT,
MAIN_STATE_EASY,
MAIN_STATE_AUTO,
} main_state_t;
} commander_state_machine_t;
/* navigation state machine */
typedef enum {
NAVIGATION_STATE_STANDBY = 0, // standby state, disarmed
NAVIGATION_STATE_DIRECT, // true manual control, no any stabilization
NAVIGATION_STATE_STABILIZE, // attitude stabilization
NAVIGATION_STATE_ALTHOLD, // attitude + altitude stabilization
NAVIGATION_STATE_VECTOR, // attitude + altitude + position stabilization
NAVIGATION_STATE_AUTO_READY, // AUTO, landed, reeady for takeoff
NAVIGATION_STATE_AUTO_TAKEOFF, // detect takeoff using land detector and switch to desired AUTO mode
NAVIGATION_STATE_AUTO_LOITER, // pause mission
NAVIGATION_STATE_AUTO_MISSION, // fly mission
NAVIGATION_STATE_AUTO_RTL, // Return To Launch, when home position switch to LAND
NAVIGATION_STATE_AUTO_LAND // land and switch to AUTO_READY when landed (detect using land detector)
} navigation_state_t;
typedef enum {
ARMING_STATE_INIT = 0,
ARMING_STATE_STANDBY,
ARMING_STATE_ARMED,
ARMING_STATE_ARMED_ERROR,
ARMING_STATE_STANDBY_ERROR,
ARMING_STATE_REBOOT,
ARMING_STATE_IN_AIR_RESTORE
} arming_state_t;
typedef enum {
HIL_STATE_OFF = 0,
HIL_STATE_ON
} hil_state_t;
typedef enum {
MODE_SWITCH_MANUAL = 0,
MODE_SWITCH_ASSISTED,
MODE_SWITCH_AUTO
} mode_switch_pos_t;
typedef enum {
ASSISTED_SWITCH_SEATBELT = 0,
ASSISTED_SWITCH_EASY
} assisted_switch_pos_t;
typedef enum {
RETURN_SWITCH_NONE = 0,
RETURN_SWITCH_RETURN
} return_switch_pos_t;
typedef enum {
MISSION_SWITCH_NONE = 0,
MISSION_SWITCH_MISSION
} mission_switch_pos_t;
enum VEHICLE_MODE_FLAG {
VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
@@ -86,26 +128,6 @@ enum VEHICLE_MODE_FLAG {
VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1
}; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */
enum VEHICLE_FLIGHT_MODE {
VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */
VEHICLE_FLIGHT_MODE_STAB, /**< attitude or rate stabilization plus velocity or position stabilization */
VEHICLE_FLIGHT_MODE_HOLD, /**< hold current position (hover or loiter around position when switched) */
VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */
};
enum VEHICLE_MANUAL_CONTROL_MODE {
VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0, /**< no attitude control, direct stick input mixing (only fixed wing) */
VEHICLE_MANUAL_CONTROL_MODE_RATES, /**< body rates control mode */
VEHICLE_MANUAL_CONTROL_MODE_SAS /**< stability augmented system (SAS) mode */
};
enum VEHICLE_MANUAL_SAS_MODE {
VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0, /**< roll, pitch and yaw absolute */
VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE, /**< roll and pitch absolute, yaw rate */
VEHICLE_MANUAL_SAS_MODE_SIMPLE, /**< simple mode (includes altitude hold) */
VEHICLE_MANUAL_SAS_MODE_ALTITUDE /**< altitude hold */
};
/**
* Should match 1:1 MAVLink's MAV_TYPE ENUM
*/
@@ -134,7 +156,7 @@ enum VEHICLE_TYPE {
enum VEHICLE_BATTERY_WARNING {
VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */
VEHICLE_BATTERY_WARNING_WARNING, /**< warning of low voltage 1. stage */
VEHICLE_BATTERY_WARNING_ALERT /**< aleting of low voltage 2. stage */
VEHICLE_BATTERY_WARNING_ALERT /**< alerting of low voltage 2. stage */
};
@@ -150,32 +172,35 @@ struct vehicle_status_s
uint16_t counter; /**< incremented by the writing thread everytime new data is stored */
uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */
//uint64_t failsave_highlevel_begin; TO BE COMPLETED
// uint64_t failsave_highlevel_begin; TO BE COMPLETED
main_state_t main_state; /**< main state machine */
navigation_state_t navigation_state; /**< navigation state machine */
arming_state_t arming_state; /**< current arming state */
hil_state_t hil_state; /**< current hil state */
commander_state_machine_t state_machine; /**< current flight state, main state machine */
enum VEHICLE_FLIGHT_MODE flight_mode; /**< current flight mode, as defined by mode switch */
enum VEHICLE_MANUAL_CONTROL_MODE manual_control_mode; /**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */
enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; /**< current stabilization mode */
int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */
int32_t system_id; /**< system id, inspired by MAVLink's system ID field */
int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */
/* system flags - these represent the state predicates */
bool is_rotary_wing;
bool flag_system_armed; /**< true is motors / actuators are armed */
bool flag_control_manual_enabled; /**< true if manual input is mixed in */
bool flag_control_offboard_enabled; /**< true if offboard control input is on */
bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */
mode_switch_pos_t mode_switch;
return_switch_pos_t return_switch;
assisted_switch_pos_t assisted_switch;
mission_switch_pos_t mission_switch;
bool flag_control_rates_enabled; /**< true if rates are stabilized */
bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */
bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */
bool flag_control_position_enabled; /**< true if position is controlled */
bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */
bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */
bool flag_preflight_accel_calibration;
bool flag_preflight_airspeed_calibration;
bool condition_battery_voltage_valid;
bool condition_system_in_air_restore; /**< true if we can restore in mid air */
bool condition_system_sensors_initialized;
bool condition_system_returned_to_home;
bool condition_auto_mission_available;
bool condition_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */
bool condition_launch_position_valid; /**< indicates a valid launch position */
bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */
bool condition_local_position_valid;
bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */
bool condition_landed; /**< true if vehicle is landed, always true if disarmed */
bool rc_signal_found_once;
bool rc_signal_lost; /**< true if RC reception is terminally lost */
@@ -188,16 +213,22 @@ struct vehicle_status_s
uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */
bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */
//bool failsave_highlevel;
bool failsave_highlevel;
bool preflight_calibration;
bool system_emergency;
/* see SYS_STATUS mavlink message for the following */
uint32_t onboard_control_sensors_present;
uint32_t onboard_control_sensors_enabled;
uint32_t onboard_control_sensors_health;
float load;
float voltage_battery;
float current_battery;
float battery_voltage;
float battery_current;
float battery_remaining;
enum VEHICLE_BATTERY_WARNING battery_warning; /**< current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum */
uint16_t drop_rate_comm;
uint16_t errors_comm;
@@ -205,15 +236,6 @@ struct vehicle_status_s
uint16_t errors_count2;
uint16_t errors_count3;
uint16_t errors_count4;
bool flag_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */
bool flag_local_position_valid;
bool flag_vector_flight_mode_ok; /**< position estimation, battery voltage and other critical subsystems are good for autonomous flight */
bool flag_auto_flight_mode_ok; /**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */
bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */
bool flag_valid_launch_position; /**< indicates a valid launch position */
bool flag_valid_home_position; /**< indicates a valid home position (a valid home position is not always a valid launch) */
bool flag_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */
};
/**