forked from Archive/PX4-Autopilot
Merge branch 'master' of github.com:PX4/Firmware into eff_plus_plus
This commit is contained in:
commit
1744bf6e12
|
@ -36,7 +36,7 @@ then
|
||||||
param set FW_T_TIME_CONST 5
|
param set FW_T_TIME_CONST 5
|
||||||
fi
|
fi
|
||||||
|
|
||||||
set MIXER FMU_Q
|
set MIXER phantom
|
||||||
|
|
||||||
# Provide ESC a constant 1000 us pulse
|
# Provide ESC a constant 1000 us pulse
|
||||||
set PWM_OUTPUTS 4
|
set PWM_OUTPUTS 4
|
||||||
|
|
|
@ -0,0 +1,67 @@
|
||||||
|
Phantom FX-61 mixer
|
||||||
|
===================
|
||||||
|
|
||||||
|
This file defines mixers suitable for controlling a delta wing aircraft using
|
||||||
|
PX4/Pixhawk. The configuration assumes the elevon servos are connected to
|
||||||
|
servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is
|
||||||
|
assumed to be unused.
|
||||||
|
|
||||||
|
Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
|
||||||
|
(roll), 1 (pitch) and 3 (thrust).
|
||||||
|
|
||||||
|
See the README for more information on the scaler format.
|
||||||
|
|
||||||
|
Elevon mixers
|
||||||
|
-------------
|
||||||
|
Three scalers total (output, roll, pitch).
|
||||||
|
|
||||||
|
On the assumption that the two elevon servos are physically reversed, the pitch
|
||||||
|
input is inverted between the two servos.
|
||||||
|
|
||||||
|
The scaling factor are set so that pitch will have more travel than roll.
|
||||||
|
|
||||||
|
M: 2
|
||||||
|
O: 10000 10000 0 -10000 10000
|
||||||
|
S: 0 0 -6000 -6000 0 -10000 10000
|
||||||
|
S: 0 1 6500 6500 0 -10000 10000
|
||||||
|
|
||||||
|
M: 2
|
||||||
|
O: 10000 10000 0 -10000 10000
|
||||||
|
S: 0 0 -6000 -6000 0 -10000 10000
|
||||||
|
S: 0 1 -6500 -6500 0 -10000 10000
|
||||||
|
|
||||||
|
Output 2
|
||||||
|
--------
|
||||||
|
This mixer is empty.
|
||||||
|
|
||||||
|
Z:
|
||||||
|
|
||||||
|
Motor speed mixer
|
||||||
|
-----------------
|
||||||
|
Two scalers total (output, thrust).
|
||||||
|
|
||||||
|
This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
|
||||||
|
range. Inputs below zero are treated as zero.
|
||||||
|
|
||||||
|
M: 1
|
||||||
|
O: 10000 10000 0 -10000 10000
|
||||||
|
S: 0 3 0 20000 -10000 -10000 10000
|
||||||
|
|
||||||
|
Gimbal / flaps / payload mixer for last four channels
|
||||||
|
-----------------------------------------------------
|
||||||
|
|
||||||
|
M: 1
|
||||||
|
O: 10000 10000 0 -10000 10000
|
||||||
|
S: 0 4 10000 10000 0 -10000 10000
|
||||||
|
|
||||||
|
M: 1
|
||||||
|
O: 10000 10000 0 -10000 10000
|
||||||
|
S: 0 5 10000 10000 0 -10000 10000
|
||||||
|
|
||||||
|
M: 1
|
||||||
|
O: 10000 10000 0 -10000 10000
|
||||||
|
S: 0 6 10000 10000 0 -10000 10000
|
||||||
|
|
||||||
|
M: 1
|
||||||
|
O: 10000 10000 0 -10000 10000
|
||||||
|
S: 0 7 10000 10000 0 -10000 10000
|
|
@ -172,6 +172,9 @@ ETSAirspeed::collect()
|
||||||
diff_pres_pa = diff_pres_pa_raw - _diff_pres_offset;
|
diff_pres_pa = diff_pres_pa_raw - _diff_pres_offset;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// The raw value still should be compensated for the known offset
|
||||||
|
diff_pres_pa_raw -= _diff_pres_offset;
|
||||||
|
|
||||||
// Track maximum differential pressure measured (so we can work out top speed).
|
// Track maximum differential pressure measured (so we can work out top speed).
|
||||||
if (diff_pres_pa > _max_differential_pressure_pa) {
|
if (diff_pres_pa > _max_differential_pressure_pa) {
|
||||||
_max_differential_pressure_pa = diff_pres_pa;
|
_max_differential_pressure_pa = diff_pres_pa;
|
||||||
|
@ -186,7 +189,6 @@ ETSAirspeed::collect()
|
||||||
report.differential_pressure_filtered_pa = (float)diff_pres_pa;
|
report.differential_pressure_filtered_pa = (float)diff_pres_pa;
|
||||||
report.differential_pressure_raw_pa = (float)diff_pres_pa_raw;
|
report.differential_pressure_raw_pa = (float)diff_pres_pa_raw;
|
||||||
report.temperature = -1000.0f;
|
report.temperature = -1000.0f;
|
||||||
report.voltage = 0;
|
|
||||||
report.max_differential_pressure_pa = _max_differential_pressure_pa;
|
report.max_differential_pressure_pa = _max_differential_pressure_pa;
|
||||||
|
|
||||||
if (_airspeed_pub > 0 && !(_pub_blocked)) {
|
if (_airspeed_pub > 0 && !(_pub_blocked)) {
|
||||||
|
|
|
@ -225,7 +225,10 @@ MEASAirspeed::collect()
|
||||||
// correct for 5V rail voltage if possible
|
// correct for 5V rail voltage if possible
|
||||||
voltage_correction(diff_press_pa_raw, temperature);
|
voltage_correction(diff_press_pa_raw, temperature);
|
||||||
|
|
||||||
float diff_press_pa = fabsf(diff_press_pa_raw - _diff_pres_offset);
|
// the raw value still should be compensated for the known offset
|
||||||
|
diff_press_pa_raw -= _diff_pres_offset;
|
||||||
|
|
||||||
|
float diff_press_pa = fabsf(diff_press_pa_raw);
|
||||||
|
|
||||||
/*
|
/*
|
||||||
note that we return both the absolute value with offset
|
note that we return both the absolute value with offset
|
||||||
|
@ -265,7 +268,6 @@ MEASAirspeed::collect()
|
||||||
}
|
}
|
||||||
|
|
||||||
report.differential_pressure_raw_pa = diff_press_pa_raw;
|
report.differential_pressure_raw_pa = diff_press_pa_raw;
|
||||||
report.voltage = 0;
|
|
||||||
report.max_differential_pressure_pa = _max_differential_pressure_pa;
|
report.max_differential_pressure_pa = _max_differential_pressure_pa;
|
||||||
|
|
||||||
if (_airspeed_pub > 0 && !(_pub_blocked)) {
|
if (_airspeed_pub > 0 && !(_pub_blocked)) {
|
||||||
|
|
|
@ -1,7 +1,6 @@
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
*
|
*
|
||||||
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
|
||||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
|
||||||
*
|
*
|
||||||
* Redistribution and use in source and binary forms, with or without
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
|
|
@ -1,7 +1,6 @@
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
*
|
*
|
||||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||||
* Author: Anton Babushkin <anton.babushkin@me.com>
|
|
||||||
*
|
*
|
||||||
* Redistribution and use in source and binary forms, with or without
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
|
|
@ -1,7 +1,6 @@
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
*
|
*
|
||||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||||
* Author: Anton Babushkin <anton.babushkin@me.com>
|
|
||||||
*
|
*
|
||||||
* Redistribution and use in source and binary forms, with or without
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
|
|
@ -51,6 +51,7 @@
|
||||||
#include <mavlink/mavlink_log.h>
|
#include <mavlink/mavlink_log.h>
|
||||||
#include <systemlib/param/param.h>
|
#include <systemlib/param/param.h>
|
||||||
#include <systemlib/err.h>
|
#include <systemlib/err.h>
|
||||||
|
#include <systemlib/airspeed.h>
|
||||||
|
|
||||||
/* oddly, ERROR is not defined for c++ */
|
/* oddly, ERROR is not defined for c++ */
|
||||||
#ifdef ERROR
|
#ifdef ERROR
|
||||||
|
@ -64,19 +65,17 @@ int do_airspeed_calibration(int mavlink_fd)
|
||||||
{
|
{
|
||||||
/* give directions */
|
/* give directions */
|
||||||
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
|
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
|
||||||
mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind");
|
|
||||||
|
|
||||||
const int calibration_count = 2000;
|
const unsigned calibration_count = 2000;
|
||||||
|
|
||||||
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
|
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
|
||||||
struct differential_pressure_s diff_pres;
|
struct differential_pressure_s diff_pres;
|
||||||
|
|
||||||
int calibration_counter = 0;
|
|
||||||
float diff_pres_offset = 0.0f;
|
float diff_pres_offset = 0.0f;
|
||||||
|
|
||||||
/* Reset sensor parameters */
|
/* Reset sensor parameters */
|
||||||
struct airspeed_scale airscale = {
|
struct airspeed_scale airscale = {
|
||||||
0.0f,
|
diff_pres_offset,
|
||||||
1.0f,
|
1.0f,
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -95,12 +94,29 @@ int do_airspeed_calibration(int mavlink_fd)
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!paramreset_successful) {
|
if (!paramreset_successful) {
|
||||||
warn("FAILED to set scale / offsets for airspeed");
|
|
||||||
mavlink_log_critical(mavlink_fd, "dpress reset failed");
|
/* only warn if analog scaling is zero */
|
||||||
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
|
float analog_scaling = 0.0f;
|
||||||
return ERROR;
|
param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
|
||||||
|
if (fabsf(analog_scaling) < 0.1f) {
|
||||||
|
mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]");
|
||||||
|
close(diff_pres_sub);
|
||||||
|
return ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* set scaling offset parameter */
|
||||||
|
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
|
||||||
|
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
|
||||||
|
close(diff_pres_sub);
|
||||||
|
return ERROR;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
unsigned calibration_counter = 0;
|
||||||
|
|
||||||
|
mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind");
|
||||||
|
usleep(500 * 1000);
|
||||||
|
|
||||||
while (calibration_counter < calibration_count) {
|
while (calibration_counter < calibration_count) {
|
||||||
|
|
||||||
/* wait blocking for new data */
|
/* wait blocking for new data */
|
||||||
|
@ -112,11 +128,12 @@ int do_airspeed_calibration(int mavlink_fd)
|
||||||
|
|
||||||
if (poll_ret) {
|
if (poll_ret) {
|
||||||
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
|
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
|
||||||
|
|
||||||
diff_pres_offset += diff_pres.differential_pressure_raw_pa;
|
diff_pres_offset += diff_pres.differential_pressure_raw_pa;
|
||||||
calibration_counter++;
|
calibration_counter++;
|
||||||
|
|
||||||
if (calibration_counter % (calibration_count / 20) == 0) {
|
if (calibration_counter % (calibration_count / 20) == 0) {
|
||||||
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
|
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 80) / calibration_count);
|
||||||
}
|
}
|
||||||
|
|
||||||
} else if (poll_ret == 0) {
|
} else if (poll_ret == 0) {
|
||||||
|
@ -131,6 +148,16 @@ int do_airspeed_calibration(int mavlink_fd)
|
||||||
|
|
||||||
if (isfinite(diff_pres_offset)) {
|
if (isfinite(diff_pres_offset)) {
|
||||||
|
|
||||||
|
int fd_scale = open(AIRSPEED_DEVICE_PATH, 0);
|
||||||
|
airscale.offset_pa = diff_pres_offset;
|
||||||
|
if (fd_scale > 0) {
|
||||||
|
if (OK != ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
|
||||||
|
mavlink_log_critical(mavlink_fd, "airspeed offset update failed");
|
||||||
|
}
|
||||||
|
|
||||||
|
close(fd_scale);
|
||||||
|
}
|
||||||
|
|
||||||
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
|
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
|
||||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
|
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
|
||||||
close(diff_pres_sub);
|
close(diff_pres_sub);
|
||||||
|
@ -147,14 +174,91 @@ int do_airspeed_calibration(int mavlink_fd)
|
||||||
return ERROR;
|
return ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
|
|
||||||
tune_neutral(true);
|
|
||||||
close(diff_pres_sub);
|
|
||||||
return OK;
|
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
|
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
|
||||||
close(diff_pres_sub);
|
close(diff_pres_sub);
|
||||||
return ERROR;
|
return ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset);
|
||||||
|
|
||||||
|
/* wait 500 ms to ensure parameter propagated through the system */
|
||||||
|
usleep(500 * 1000);
|
||||||
|
|
||||||
|
calibration_counter = 0;
|
||||||
|
const int maxcount = 3000;
|
||||||
|
|
||||||
|
/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
|
||||||
|
while (calibration_counter < maxcount) {
|
||||||
|
|
||||||
|
/* 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);
|
||||||
|
|
||||||
|
calibration_counter++;
|
||||||
|
|
||||||
|
if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
|
||||||
|
if (calibration_counter % 100 == 0) {
|
||||||
|
mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)",
|
||||||
|
(int)diff_pres.differential_pressure_raw_pa);
|
||||||
|
}
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* do not allow negative values */
|
||||||
|
if (diff_pres.differential_pressure_raw_pa < 0.0f) {
|
||||||
|
mavlink_log_info(mavlink_fd, "negative pressure: ERROR (%d Pa)",
|
||||||
|
(int)diff_pres.differential_pressure_raw_pa);
|
||||||
|
mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa);
|
||||||
|
close(diff_pres_sub);
|
||||||
|
|
||||||
|
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
|
||||||
|
|
||||||
|
diff_pres_offset = 0.0f;
|
||||||
|
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
|
||||||
|
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
|
||||||
|
close(diff_pres_sub);
|
||||||
|
return ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* save */
|
||||||
|
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 0);
|
||||||
|
(void)param_save_default();
|
||||||
|
|
||||||
|
close(diff_pres_sub);
|
||||||
|
|
||||||
|
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
|
||||||
|
return ERROR;
|
||||||
|
} else {
|
||||||
|
mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)",
|
||||||
|
(int)diff_pres.differential_pressure_raw_pa);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
} else if (poll_ret == 0) {
|
||||||
|
/* any poll failure for 1s is a reason to abort */
|
||||||
|
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
|
||||||
|
close(diff_pres_sub);
|
||||||
|
return ERROR;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (calibration_counter == maxcount) {
|
||||||
|
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
|
||||||
|
close(diff_pres_sub);
|
||||||
|
return ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100);
|
||||||
|
|
||||||
|
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
|
||||||
|
tune_neutral(true);
|
||||||
|
close(diff_pres_sub);
|
||||||
|
return OK;
|
||||||
}
|
}
|
||||||
|
|
|
@ -1,6 +1,6 @@
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
*
|
*
|
||||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
|
||||||
*
|
*
|
||||||
* Redistribution and use in source and binary forms, with or without
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
|
|
@ -1,6 +1,6 @@
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
*
|
*
|
||||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||||
*
|
*
|
||||||
* Redistribution and use in source and binary forms, with or without
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
|
|
@ -1,6 +1,6 @@
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
*
|
*
|
||||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||||
*
|
*
|
||||||
* Redistribution and use in source and binary forms, with or without
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
|
|
@ -1,7 +1,6 @@
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
*
|
*
|
||||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||||
* Author: Anton Babushkin <anton.babushkin@me.com>
|
|
||||||
*
|
*
|
||||||
* Redistribution and use in source and binary forms, with or without
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
|
|
@ -1,7 +1,6 @@
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
*
|
*
|
||||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
* Copyright (c) 2012 PX4 Development Team. All rights reserved.
|
||||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
|
||||||
*
|
*
|
||||||
* Redistribution and use in source and binary forms, with or without
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
|
|
@ -1,7 +1,6 @@
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
*
|
*
|
||||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
* Copyright (c) 2012 PX4 Development Team. All rights reserved.
|
||||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
|
||||||
*
|
*
|
||||||
* Redistribution and use in source and binary forms, with or without
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
|
|
@ -403,10 +403,12 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
|
||||||
return arming_res;
|
return arming_res;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
|
bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local,
|
||||||
|
struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local,
|
||||||
|
struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
|
||||||
{
|
{
|
||||||
/* only handle commands that are meant to be handled by this system and component */
|
/* only handle commands that are meant to be handled by this system and component */
|
||||||
if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components
|
if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -425,7 +427,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
||||||
|
|
||||||
/* set HIL state */
|
/* set HIL state */
|
||||||
hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
|
hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
|
||||||
transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd);
|
transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd);
|
||||||
|
|
||||||
// Transition the arming state
|
// Transition the arming state
|
||||||
arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
|
arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
|
||||||
|
@ -434,43 +436,43 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
||||||
/* use autopilot-specific mode */
|
/* use autopilot-specific mode */
|
||||||
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
|
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
|
||||||
/* MANUAL */
|
/* MANUAL */
|
||||||
main_ret = main_state_transition(status, MAIN_STATE_MANUAL);
|
main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL);
|
||||||
|
|
||||||
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
|
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
|
||||||
/* ALTCTL */
|
/* ALTCTL */
|
||||||
main_ret = main_state_transition(status, MAIN_STATE_ALTCTL);
|
main_ret = main_state_transition(status_local, MAIN_STATE_ALTCTL);
|
||||||
|
|
||||||
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
|
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
|
||||||
/* POSCTL */
|
/* POSCTL */
|
||||||
main_ret = main_state_transition(status, MAIN_STATE_POSCTL);
|
main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL);
|
||||||
|
|
||||||
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
|
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
|
||||||
/* AUTO */
|
/* AUTO */
|
||||||
main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION);
|
main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
|
||||||
|
|
||||||
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
|
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
|
||||||
/* ACRO */
|
/* ACRO */
|
||||||
main_ret = main_state_transition(status, MAIN_STATE_ACRO);
|
main_ret = main_state_transition(status_local, MAIN_STATE_ACRO);
|
||||||
|
|
||||||
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
|
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
|
||||||
/* OFFBOARD */
|
/* OFFBOARD */
|
||||||
main_ret = main_state_transition(status, MAIN_STATE_OFFBOARD);
|
main_ret = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
|
||||||
}
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
/* use base mode */
|
/* use base mode */
|
||||||
if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
|
if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
|
||||||
/* AUTO */
|
/* AUTO */
|
||||||
main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION);
|
main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
|
||||||
|
|
||||||
} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
|
} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
|
||||||
if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
|
if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
|
||||||
/* POSCTL */
|
/* POSCTL */
|
||||||
main_ret = main_state_transition(status, MAIN_STATE_POSCTL);
|
main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL);
|
||||||
|
|
||||||
} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
|
} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
|
||||||
/* MANUAL */
|
/* MANUAL */
|
||||||
main_ret = main_state_transition(status, MAIN_STATE_MANUAL);
|
main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -485,22 +487,25 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
|
case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
|
||||||
// Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
|
// Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints
|
||||||
// We use an float epsilon delta to test float equality.
|
// for logic state parameters
|
||||||
if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) {
|
|
||||||
mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", (double)cmd->param1);
|
if (static_cast<int>(cmd->param1 + 0.5f) != 0 && static_cast<int>(cmd->param1 + 0.5f) != 1) {
|
||||||
|
mavlink_log_critical(mavlink_fd, "Unsupported ARM_DISARM param: %.3f", (double)cmd->param1);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|
||||||
|
bool cmd_arms = (static_cast<int>(cmd->param1 + 0.5f) == 1);
|
||||||
|
|
||||||
// Flick to inair restore first if this comes from an onboard system
|
// Flick to inair restore first if this comes from an onboard system
|
||||||
if (cmd->source_system == status->system_id && cmd->source_component == status->component_id) {
|
if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) {
|
||||||
status->arming_state = ARMING_STATE_IN_AIR_RESTORE;
|
status_local->arming_state = ARMING_STATE_IN_AIR_RESTORE;
|
||||||
}
|
}
|
||||||
|
|
||||||
transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command");
|
transition_result_t arming_res = arm_disarm(cmd_arms, mavlink_fd, "arm/disarm component command");
|
||||||
|
|
||||||
if (arming_res == TRANSITION_DENIED) {
|
if (arming_res == TRANSITION_DENIED) {
|
||||||
mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
|
mavlink_log_critical(mavlink_fd, "REJECTING component arm cmd");
|
||||||
cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
|
cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
@ -512,20 +517,24 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
||||||
|
|
||||||
case VEHICLE_CMD_OVERRIDE_GOTO: {
|
case VEHICLE_CMD_OVERRIDE_GOTO: {
|
||||||
// TODO listen vehicle_command topic directly from navigator (?)
|
// TODO listen vehicle_command topic directly from navigator (?)
|
||||||
unsigned int mav_goto = cmd->param1;
|
|
||||||
|
// Increase by 0.5f and rely on the integer cast
|
||||||
|
// implicit floor(). This is the *safest* way to
|
||||||
|
// convert from floats representing small ints to actual ints.
|
||||||
|
unsigned int mav_goto = (cmd->param1 + 0.5f);
|
||||||
|
|
||||||
if (mav_goto == 0) { // MAV_GOTO_DO_HOLD
|
if (mav_goto == 0) { // MAV_GOTO_DO_HOLD
|
||||||
status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
|
status_local->nav_state = NAVIGATION_STATE_AUTO_LOITER;
|
||||||
mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd");
|
mavlink_log_critical(mavlink_fd, "Pause mission cmd");
|
||||||
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
|
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
|
||||||
|
|
||||||
} else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE
|
} else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE
|
||||||
status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
|
status_local->nav_state = NAVIGATION_STATE_AUTO_MISSION;
|
||||||
mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd");
|
mavlink_log_critical(mavlink_fd, "Continue mission cmd");
|
||||||
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
|
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f",
|
mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f",
|
||||||
(double)cmd->param1,
|
(double)cmd->param1,
|
||||||
(double)cmd->param2,
|
(double)cmd->param2,
|
||||||
(double)cmd->param3,
|
(double)cmd->param3,
|
||||||
|
@ -543,7 +552,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
||||||
|
|
||||||
//XXX: to enable the parachute, a param needs to be set
|
//XXX: to enable the parachute, a param needs to be set
|
||||||
//xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
|
//xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
|
||||||
if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) {
|
if (armed_local->armed && cmd->param3 > 0.5 && parachute_enabled) {
|
||||||
transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
|
transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
|
||||||
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
|
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
|
||||||
|
|
||||||
|
@ -561,7 +570,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
||||||
|
|
||||||
if (use_current) {
|
if (use_current) {
|
||||||
/* use current position */
|
/* use current position */
|
||||||
if (status->condition_global_position_valid) {
|
if (status_local->condition_global_position_valid) {
|
||||||
home->lat = global_pos->lat;
|
home->lat = global_pos->lat;
|
||||||
home->lon = global_pos->lon;
|
home->lon = global_pos->lon;
|
||||||
home->alt = global_pos->alt;
|
home->alt = global_pos->alt;
|
||||||
|
@ -598,7 +607,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
||||||
}
|
}
|
||||||
|
|
||||||
/* mark home position as set */
|
/* mark home position as set */
|
||||||
status->condition_home_position_valid = true;
|
status_local->condition_home_position_valid = true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
@ -688,11 +697,11 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
|
|
||||||
/* initialize */
|
/* initialize */
|
||||||
if (led_init() != 0) {
|
if (led_init() != 0) {
|
||||||
warnx("ERROR: Failed to initialize leds");
|
warnx("ERROR: LED INIT FAIL");
|
||||||
}
|
}
|
||||||
|
|
||||||
if (buzzer_init() != OK) {
|
if (buzzer_init() != OK) {
|
||||||
warnx("ERROR: Failed to initialize buzzer");
|
warnx("ERROR: BUZZER INIT FAIL");
|
||||||
}
|
}
|
||||||
|
|
||||||
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
|
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
|
||||||
|
@ -766,8 +775,9 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d",
|
mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d",
|
||||||
mission.dataman_id, mission.count, mission.current_seq);
|
mission.dataman_id, mission.count, mission.current_seq);
|
||||||
} else {
|
} else {
|
||||||
warnx("reading mission state failed");
|
const char *missionfail = "reading mission state failed";
|
||||||
mavlink_log_info(mavlink_fd, "[cmd] reading mission state failed");
|
warnx("%s", missionfail);
|
||||||
|
mavlink_log_critical(mavlink_fd, missionfail);
|
||||||
|
|
||||||
/* initialize mission state in dataman */
|
/* initialize mission state in dataman */
|
||||||
mission.dataman_id = 0;
|
mission.dataman_id = 0;
|
||||||
|
@ -780,8 +790,6 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
orb_publish(ORB_ID(offboard_mission), mission_pub, &mission);
|
orb_publish(ORB_ID(offboard_mission), mission_pub, &mission);
|
||||||
}
|
}
|
||||||
|
|
||||||
mavlink_log_info(mavlink_fd, "[cmd] started");
|
|
||||||
|
|
||||||
int ret;
|
int ret;
|
||||||
|
|
||||||
pthread_attr_t commander_low_prio_attr;
|
pthread_attr_t commander_low_prio_attr;
|
||||||
|
@ -1074,7 +1082,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
|
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
|
||||||
|
|
||||||
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) {
|
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) {
|
||||||
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch");
|
mavlink_log_info(mavlink_fd, "DISARMED by safety switch");
|
||||||
arming_state_changed = true;
|
arming_state_changed = true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -1178,10 +1186,10 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
status_changed = true;
|
status_changed = true;
|
||||||
|
|
||||||
if (status.condition_landed) {
|
if (status.condition_landed) {
|
||||||
mavlink_log_critical(mavlink_fd, "#audio: LANDED");
|
mavlink_log_critical(mavlink_fd, "LANDED MODE");
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
mavlink_log_critical(mavlink_fd, "#audio: IN AIR");
|
mavlink_log_critical(mavlink_fd, "IN AIR MODE");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -1261,14 +1269,14 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
/* if battery voltage is getting lower, warn using buzzer, etc. */
|
/* if battery voltage is getting lower, warn using buzzer, etc. */
|
||||||
if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) {
|
if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) {
|
||||||
low_battery_voltage_actions_done = true;
|
low_battery_voltage_actions_done = true;
|
||||||
mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
|
mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
|
||||||
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
|
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
|
||||||
status_changed = true;
|
status_changed = true;
|
||||||
|
|
||||||
} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
|
} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
|
||||||
/* critical battery voltage, this is rather an emergency, change state machine */
|
/* critical battery voltage, this is rather an emergency, change state machine */
|
||||||
critical_battery_voltage_actions_done = true;
|
critical_battery_voltage_actions_done = true;
|
||||||
mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY");
|
mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
|
||||||
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
|
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
|
||||||
|
|
||||||
if (armed.armed) {
|
if (armed.armed) {
|
||||||
|
@ -1330,12 +1338,12 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
/* handle the case where RC signal was regained */
|
/* handle the case where RC signal was regained */
|
||||||
if (!status.rc_signal_found_once) {
|
if (!status.rc_signal_found_once) {
|
||||||
status.rc_signal_found_once = true;
|
status.rc_signal_found_once = true;
|
||||||
mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time");
|
mavlink_log_critical(mavlink_fd, "detected RC signal first time");
|
||||||
status_changed = true;
|
status_changed = true;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
if (status.rc_signal_lost) {
|
if (status.rc_signal_lost) {
|
||||||
mavlink_log_critical(mavlink_fd, "#audio: RC signal regained");
|
mavlink_log_critical(mavlink_fd, "RC signal regained");
|
||||||
status_changed = true;
|
status_changed = true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -1376,7 +1384,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
* the system can be armed in auto if armed via the GCS.
|
* the system can be armed in auto if armed via the GCS.
|
||||||
*/
|
*/
|
||||||
if (status.main_state != MAIN_STATE_MANUAL) {
|
if (status.main_state != MAIN_STATE_MANUAL) {
|
||||||
print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first.");
|
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
|
||||||
} else {
|
} else {
|
||||||
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd);
|
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd);
|
||||||
if (arming_ret == TRANSITION_CHANGED) {
|
if (arming_ret == TRANSITION_CHANGED) {
|
||||||
|
@ -1396,16 +1404,16 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
|
|
||||||
if (arming_ret == TRANSITION_CHANGED) {
|
if (arming_ret == TRANSITION_CHANGED) {
|
||||||
if (status.arming_state == ARMING_STATE_ARMED) {
|
if (status.arming_state == ARMING_STATE_ARMED) {
|
||||||
mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
|
mavlink_log_info(mavlink_fd, "ARMED by RC");
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
|
mavlink_log_info(mavlink_fd, "DISARMED by RC");
|
||||||
}
|
}
|
||||||
arming_state_changed = true;
|
arming_state_changed = true;
|
||||||
|
|
||||||
} else if (arming_ret == TRANSITION_DENIED) {
|
} else if (arming_ret == TRANSITION_DENIED) {
|
||||||
/* DENIED here indicates bug in the commander */
|
/* DENIED here indicates bug in the commander */
|
||||||
mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied");
|
mavlink_log_critical(mavlink_fd, "arming state transition denied");
|
||||||
tune_negative(true);
|
tune_negative(true);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1419,12 +1427,12 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
|
|
||||||
} else if (main_res == TRANSITION_DENIED) {
|
} else if (main_res == TRANSITION_DENIED) {
|
||||||
/* DENIED here indicates bug in the commander */
|
/* DENIED here indicates bug in the commander */
|
||||||
mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied");
|
mavlink_log_critical(mavlink_fd, "main state transition denied");
|
||||||
}
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
if (!status.rc_signal_lost) {
|
if (!status.rc_signal_lost) {
|
||||||
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
|
mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST");
|
||||||
status.rc_signal_lost = true;
|
status.rc_signal_lost = true;
|
||||||
status_changed = true;
|
status_changed = true;
|
||||||
}
|
}
|
||||||
|
@ -1436,14 +1444,14 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) {
|
if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) {
|
||||||
/* handle the case where data link was regained */
|
/* handle the case where data link was regained */
|
||||||
if (telemetry_lost[i]) {
|
if (telemetry_lost[i]) {
|
||||||
mavlink_log_critical(mavlink_fd, "#audio: data link %i regained", i);
|
mavlink_log_critical(mavlink_fd, "data link %i regained", i);
|
||||||
telemetry_lost[i] = false;
|
telemetry_lost[i] = false;
|
||||||
}
|
}
|
||||||
have_link = true;
|
have_link = true;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
if (!telemetry_lost[i]) {
|
if (!telemetry_lost[i]) {
|
||||||
mavlink_log_critical(mavlink_fd, "#audio: data link %i lost", i);
|
mavlink_log_critical(mavlink_fd, "data link %i lost", i);
|
||||||
telemetry_lost[i] = true;
|
telemetry_lost[i] = true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -1458,7 +1466,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
if (!status.data_link_lost) {
|
if (!status.data_link_lost) {
|
||||||
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: ALL DATA LINKS LOST");
|
mavlink_log_critical(mavlink_fd, "ALL DATA LINKS LOST");
|
||||||
status.data_link_lost = true;
|
status.data_link_lost = true;
|
||||||
status_changed = true;
|
status_changed = true;
|
||||||
}
|
}
|
||||||
|
@ -1642,22 +1650,22 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed)
|
control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed)
|
||||||
{
|
{
|
||||||
/* driving rgbled */
|
/* driving rgbled */
|
||||||
if (changed) {
|
if (changed) {
|
||||||
bool set_normal_color = false;
|
bool set_normal_color = false;
|
||||||
|
|
||||||
/* set mode */
|
/* set mode */
|
||||||
if (status->arming_state == ARMING_STATE_ARMED) {
|
if (status_local->arming_state == ARMING_STATE_ARMED) {
|
||||||
rgbled_set_mode(RGBLED_MODE_ON);
|
rgbled_set_mode(RGBLED_MODE_ON);
|
||||||
set_normal_color = true;
|
set_normal_color = true;
|
||||||
|
|
||||||
} else if (status->arming_state == ARMING_STATE_ARMED_ERROR) {
|
} else if (status_local->arming_state == ARMING_STATE_ARMED_ERROR) {
|
||||||
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
|
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
|
||||||
rgbled_set_color(RGBLED_COLOR_RED);
|
rgbled_set_color(RGBLED_COLOR_RED);
|
||||||
|
|
||||||
} else if (status->arming_state == ARMING_STATE_STANDBY) {
|
} else if (status_local->arming_state == ARMING_STATE_STANDBY) {
|
||||||
rgbled_set_mode(RGBLED_MODE_BREATHE);
|
rgbled_set_mode(RGBLED_MODE_BREATHE);
|
||||||
set_normal_color = true;
|
set_normal_color = true;
|
||||||
|
|
||||||
|
@ -1668,12 +1676,12 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
|
||||||
|
|
||||||
if (set_normal_color) {
|
if (set_normal_color) {
|
||||||
/* set color */
|
/* set color */
|
||||||
if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe) {
|
if (status_local->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) {
|
||||||
rgbled_set_color(RGBLED_COLOR_AMBER);
|
rgbled_set_color(RGBLED_COLOR_AMBER);
|
||||||
/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
|
/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
if (status->condition_local_position_valid) {
|
if (status_local->condition_local_position_valid) {
|
||||||
rgbled_set_color(RGBLED_COLOR_GREEN);
|
rgbled_set_color(RGBLED_COLOR_GREEN);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
@ -1706,7 +1714,7 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
/* give system warnings on error LED, XXX maybe add memory usage warning too */
|
/* give system warnings on error LED, XXX maybe add memory usage warning too */
|
||||||
if (status->load > 0.95f) {
|
if (status_local->load > 0.95f) {
|
||||||
if (leds_counter % 2 == 0) {
|
if (leds_counter % 2 == 0) {
|
||||||
led_toggle(LED_AMBER);
|
led_toggle(LED_AMBER);
|
||||||
}
|
}
|
||||||
|
@ -1719,16 +1727,16 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
|
||||||
}
|
}
|
||||||
|
|
||||||
transition_result_t
|
transition_result_t
|
||||||
set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man)
|
set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_setpoint_s *sp_man)
|
||||||
{
|
{
|
||||||
/* set main state according to RC switches */
|
/* set main state according to RC switches */
|
||||||
transition_result_t res = TRANSITION_DENIED;
|
transition_result_t res = TRANSITION_DENIED;
|
||||||
|
|
||||||
/* offboard switch overrides main switch */
|
/* offboard switch overrides main switch */
|
||||||
if (sp_man->offboard_switch == SWITCH_POS_ON) {
|
if (sp_man->offboard_switch == SWITCH_POS_ON) {
|
||||||
res = main_state_transition(status, MAIN_STATE_OFFBOARD);
|
res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
|
||||||
if (res == TRANSITION_DENIED) {
|
if (res == TRANSITION_DENIED) {
|
||||||
print_reject_mode(status, "OFFBOARD");
|
print_reject_mode(status_local, "OFFBOARD");
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
return res;
|
return res;
|
||||||
|
@ -1743,78 +1751,78 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
|
||||||
|
|
||||||
case SWITCH_POS_OFF: // MANUAL
|
case SWITCH_POS_OFF: // MANUAL
|
||||||
if (sp_man->acro_switch == SWITCH_POS_ON) {
|
if (sp_man->acro_switch == SWITCH_POS_ON) {
|
||||||
res = main_state_transition(status, MAIN_STATE_ACRO);
|
res = main_state_transition(status_local, MAIN_STATE_ACRO);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
res = main_state_transition(status, MAIN_STATE_MANUAL);
|
res = main_state_transition(status_local, MAIN_STATE_MANUAL);
|
||||||
}
|
}
|
||||||
// TRANSITION_DENIED is not possible here
|
// TRANSITION_DENIED is not possible here
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case SWITCH_POS_MIDDLE: // ASSIST
|
case SWITCH_POS_MIDDLE: // ASSIST
|
||||||
if (sp_man->posctl_switch == SWITCH_POS_ON) {
|
if (sp_man->posctl_switch == SWITCH_POS_ON) {
|
||||||
res = main_state_transition(status, MAIN_STATE_POSCTL);
|
res = main_state_transition(status_local, MAIN_STATE_POSCTL);
|
||||||
|
|
||||||
if (res != TRANSITION_DENIED) {
|
if (res != TRANSITION_DENIED) {
|
||||||
break; // changed successfully or already in this state
|
break; // changed successfully or already in this state
|
||||||
}
|
}
|
||||||
|
|
||||||
print_reject_mode(status, "POSCTL");
|
print_reject_mode(status_local, "POSCTL");
|
||||||
}
|
}
|
||||||
|
|
||||||
// fallback to ALTCTL
|
// fallback to ALTCTL
|
||||||
res = main_state_transition(status, MAIN_STATE_ALTCTL);
|
res = main_state_transition(status_local, MAIN_STATE_ALTCTL);
|
||||||
|
|
||||||
if (res != TRANSITION_DENIED) {
|
if (res != TRANSITION_DENIED) {
|
||||||
break; // changed successfully or already in this mode
|
break; // changed successfully or already in this mode
|
||||||
}
|
}
|
||||||
|
|
||||||
if (sp_man->posctl_switch != SWITCH_POS_ON) {
|
if (sp_man->posctl_switch != SWITCH_POS_ON) {
|
||||||
print_reject_mode(status, "ALTCTL");
|
print_reject_mode(status_local, "ALTCTL");
|
||||||
}
|
}
|
||||||
|
|
||||||
// fallback to MANUAL
|
// fallback to MANUAL
|
||||||
res = main_state_transition(status, MAIN_STATE_MANUAL);
|
res = main_state_transition(status_local, MAIN_STATE_MANUAL);
|
||||||
// TRANSITION_DENIED is not possible here
|
// TRANSITION_DENIED is not possible here
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case SWITCH_POS_ON: // AUTO
|
case SWITCH_POS_ON: // AUTO
|
||||||
if (sp_man->return_switch == SWITCH_POS_ON) {
|
if (sp_man->return_switch == SWITCH_POS_ON) {
|
||||||
res = main_state_transition(status, MAIN_STATE_AUTO_RTL);
|
res = main_state_transition(status_local, MAIN_STATE_AUTO_RTL);
|
||||||
|
|
||||||
if (res != TRANSITION_DENIED) {
|
if (res != TRANSITION_DENIED) {
|
||||||
break; // changed successfully or already in this state
|
break; // changed successfully or already in this state
|
||||||
}
|
}
|
||||||
|
|
||||||
print_reject_mode(status, "AUTO_RTL");
|
print_reject_mode(status_local, "AUTO_RTL");
|
||||||
|
|
||||||
// fallback to LOITER if home position not set
|
// fallback to LOITER if home position not set
|
||||||
res = main_state_transition(status, MAIN_STATE_AUTO_LOITER);
|
res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
|
||||||
|
|
||||||
if (res != TRANSITION_DENIED) {
|
if (res != TRANSITION_DENIED) {
|
||||||
break; // changed successfully or already in this state
|
break; // changed successfully or already in this state
|
||||||
}
|
}
|
||||||
|
|
||||||
} else if (sp_man->loiter_switch == SWITCH_POS_ON) {
|
} else if (sp_man->loiter_switch == SWITCH_POS_ON) {
|
||||||
res = main_state_transition(status, MAIN_STATE_AUTO_LOITER);
|
res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
|
||||||
|
|
||||||
if (res != TRANSITION_DENIED) {
|
if (res != TRANSITION_DENIED) {
|
||||||
break; // changed successfully or already in this state
|
break; // changed successfully or already in this state
|
||||||
}
|
}
|
||||||
|
|
||||||
print_reject_mode(status, "AUTO_LOITER");
|
print_reject_mode(status_local, "AUTO_LOITER");
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
res = main_state_transition(status, MAIN_STATE_AUTO_MISSION);
|
res = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
|
||||||
|
|
||||||
if (res != TRANSITION_DENIED) {
|
if (res != TRANSITION_DENIED) {
|
||||||
break; // changed successfully or already in this state
|
break; // changed successfully or already in this state
|
||||||
}
|
}
|
||||||
|
|
||||||
print_reject_mode(status, "AUTO_MISSION");
|
print_reject_mode(status_local, "AUTO_MISSION");
|
||||||
|
|
||||||
// fallback to LOITER if home position not set
|
// fallback to LOITER if home position not set
|
||||||
res = main_state_transition(status, MAIN_STATE_AUTO_LOITER);
|
res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
|
||||||
|
|
||||||
if (res != TRANSITION_DENIED) {
|
if (res != TRANSITION_DENIED) {
|
||||||
break; // changed successfully or already in this state
|
break; // changed successfully or already in this state
|
||||||
|
@ -1822,21 +1830,21 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
|
||||||
}
|
}
|
||||||
|
|
||||||
// fallback to POSCTL
|
// fallback to POSCTL
|
||||||
res = main_state_transition(status, MAIN_STATE_POSCTL);
|
res = main_state_transition(status_local, MAIN_STATE_POSCTL);
|
||||||
|
|
||||||
if (res != TRANSITION_DENIED) {
|
if (res != TRANSITION_DENIED) {
|
||||||
break; // changed successfully or already in this state
|
break; // changed successfully or already in this state
|
||||||
}
|
}
|
||||||
|
|
||||||
// fallback to ALTCTL
|
// fallback to ALTCTL
|
||||||
res = main_state_transition(status, MAIN_STATE_ALTCTL);
|
res = main_state_transition(status_local, MAIN_STATE_ALTCTL);
|
||||||
|
|
||||||
if (res != TRANSITION_DENIED) {
|
if (res != TRANSITION_DENIED) {
|
||||||
break; // changed successfully or already in this state
|
break; // changed successfully or already in this state
|
||||||
}
|
}
|
||||||
|
|
||||||
// fallback to MANUAL
|
// fallback to MANUAL
|
||||||
res = main_state_transition(status, MAIN_STATE_MANUAL);
|
res = main_state_transition(status_local, MAIN_STATE_MANUAL);
|
||||||
// TRANSITION_DENIED is not possible here
|
// TRANSITION_DENIED is not possible here
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
@ -2001,15 +2009,13 @@ set_control_mode()
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
print_reject_mode(struct vehicle_status_s *status, const char *msg)
|
print_reject_mode(struct vehicle_status_s *status_local, const char *msg)
|
||||||
{
|
{
|
||||||
hrt_abstime t = hrt_absolute_time();
|
hrt_abstime t = hrt_absolute_time();
|
||||||
|
|
||||||
if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
|
if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
|
||||||
last_print_mode_reject_time = t;
|
last_print_mode_reject_time = t;
|
||||||
char s[80];
|
mavlink_log_critical(mavlink_fd, "REJECT %s", msg);
|
||||||
sprintf(s, "#audio: REJECT %s", msg);
|
|
||||||
mavlink_log_critical(mavlink_fd, s);
|
|
||||||
|
|
||||||
/* only buzz if armed, because else we're driving people nuts indoors
|
/* only buzz if armed, because else we're driving people nuts indoors
|
||||||
they really need to look at the leds as well. */
|
they really need to look at the leds as well. */
|
||||||
|
@ -2024,9 +2030,7 @@ print_reject_arm(const char *msg)
|
||||||
|
|
||||||
if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
|
if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
|
||||||
last_print_mode_reject_time = t;
|
last_print_mode_reject_time = t;
|
||||||
char s[80];
|
mavlink_log_critical(mavlink_fd, msg);
|
||||||
sprintf(s, "#audio: %s", msg);
|
|
||||||
mavlink_log_critical(mavlink_fd, s);
|
|
||||||
tune_negative(true);
|
tune_negative(true);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -2039,12 +2043,12 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case VEHICLE_CMD_RESULT_DENIED:
|
case VEHICLE_CMD_RESULT_DENIED:
|
||||||
mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command);
|
mavlink_log_critical(mavlink_fd, "command denied: %u", cmd.command);
|
||||||
tune_negative(true);
|
tune_negative(true);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case VEHICLE_CMD_RESULT_FAILED:
|
case VEHICLE_CMD_RESULT_FAILED:
|
||||||
mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command);
|
mavlink_log_critical(mavlink_fd, "command failed: %u", cmd.command);
|
||||||
tune_negative(true);
|
tune_negative(true);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
@ -2055,7 +2059,7 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case VEHICLE_CMD_RESULT_UNSUPPORTED:
|
case VEHICLE_CMD_RESULT_UNSUPPORTED:
|
||||||
mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command);
|
mavlink_log_critical(mavlink_fd, "command unsupported: %u", cmd.command);
|
||||||
tune_negative(true);
|
tune_negative(true);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
|
|
@ -1,9 +1,6 @@
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
*
|
*
|
||||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
|
||||||
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
|
||||||
* Julian Oes <joes@student.ethz.ch>
|
|
||||||
* Anton Babushkin <anton.babushkin@me.com>
|
|
||||||
*
|
*
|
||||||
* Redistribution and use in source and binary forms, with or without
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
@ -37,6 +34,11 @@
|
||||||
/**
|
/**
|
||||||
* @file commander_helper.cpp
|
* @file commander_helper.cpp
|
||||||
* Commander helper functions implementations
|
* Commander helper functions implementations
|
||||||
|
*
|
||||||
|
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||||
|
* @author Julian Oes <julian@oes.ch>
|
||||||
|
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||||
|
*
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
|
|
|
@ -1,8 +1,6 @@
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
*
|
*
|
||||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
* Copyright (c) 2013, 2014 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
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
@ -36,6 +34,9 @@
|
||||||
/**
|
/**
|
||||||
* @file commander_helper.h
|
* @file commander_helper.h
|
||||||
* Commander helper functions definitions
|
* Commander helper functions definitions
|
||||||
|
*
|
||||||
|
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||||
|
* @author Julian Oes <julian@oes.ch>
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#ifndef COMMANDER_HELPER_H_
|
#ifndef COMMANDER_HELPER_H_
|
||||||
|
@ -77,6 +78,8 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern);
|
||||||
* Use integral of current if battery capacity known (BAT_CAPACITY parameter set),
|
* Use integral of current if battery capacity known (BAT_CAPACITY parameter set),
|
||||||
* else use simple estimate based on voltage.
|
* else use simple estimate based on voltage.
|
||||||
*
|
*
|
||||||
|
* @param voltage the current battery voltage
|
||||||
|
* @param discharged the discharged capacity
|
||||||
* @return the estimated remaining capacity in 0..1
|
* @return the estimated remaining capacity in 0..1
|
||||||
*/
|
*/
|
||||||
float battery_remaining_estimate_voltage(float voltage, float discharged);
|
float battery_remaining_estimate_voltage(float voltage, float discharged);
|
||||||
|
|
|
@ -1,6 +1,6 @@
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
*
|
*
|
||||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
|
||||||
*
|
*
|
||||||
* Redistribution and use in source and binary forms, with or without
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
|
|
@ -52,6 +52,7 @@
|
||||||
#include <uORB/topics/vehicle_status.h>
|
#include <uORB/topics/vehicle_status.h>
|
||||||
#include <uORB/topics/actuator_controls.h>
|
#include <uORB/topics/actuator_controls.h>
|
||||||
#include <uORB/topics/differential_pressure.h>
|
#include <uORB/topics/differential_pressure.h>
|
||||||
|
#include <uORB/topics/airspeed.h>
|
||||||
#include <systemlib/systemlib.h>
|
#include <systemlib/systemlib.h>
|
||||||
#include <systemlib/param/param.h>
|
#include <systemlib/param/param.h>
|
||||||
#include <systemlib/err.h>
|
#include <systemlib/err.h>
|
||||||
|
@ -87,7 +88,7 @@ static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = {
|
||||||
};
|
};
|
||||||
|
|
||||||
// You can index into the array with an arming_state_t in order to get it's textual representation
|
// You can index into the array with an arming_state_t in order to get it's textual representation
|
||||||
static const char *state_names[ARMING_STATE_MAX] = {
|
static const char * const state_names[ARMING_STATE_MAX] = {
|
||||||
"ARMING_STATE_INIT",
|
"ARMING_STATE_INIT",
|
||||||
"ARMING_STATE_STANDBY",
|
"ARMING_STATE_STANDBY",
|
||||||
"ARMING_STATE_ARMED",
|
"ARMING_STATE_ARMED",
|
||||||
|
@ -160,7 +161,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current
|
||||||
// Fail transition if we need safety switch press
|
// Fail transition if we need safety switch press
|
||||||
} else if (safety->safety_switch_available && !safety->safety_off) {
|
} else if (safety->safety_switch_available && !safety->safety_off) {
|
||||||
|
|
||||||
mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch!");
|
mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch!");
|
||||||
|
|
||||||
valid_transition = false;
|
valid_transition = false;
|
||||||
}
|
}
|
||||||
|
@ -171,16 +172,16 @@ arming_state_transition(struct vehicle_status_s *status, /// current
|
||||||
// Fail transition if power is not good
|
// Fail transition if power is not good
|
||||||
if (!status->condition_power_input_valid) {
|
if (!status->condition_power_input_valid) {
|
||||||
|
|
||||||
mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module.");
|
mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
|
||||||
valid_transition = false;
|
valid_transition = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Fail transition if power levels on the avionics rail
|
// Fail transition if power levels on the avionics rail
|
||||||
// are measured but are insufficient
|
// are measured but are insufficient
|
||||||
if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) &&
|
if (status->condition_power_input_valid && ((status->avionics_power_rail_voltage > 0.0f) &&
|
||||||
(status->avionics_power_rail_voltage < 4.9f)) {
|
(status->avionics_power_rail_voltage < 4.9f))) {
|
||||||
|
|
||||||
mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage);
|
mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage);
|
||||||
valid_transition = false;
|
valid_transition = false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -625,7 +626,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
|
||||||
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
|
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
|
||||||
|
|
||||||
if (fd < 0) {
|
if (fd < 0) {
|
||||||
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING");
|
mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL SENSOR MISSING");
|
||||||
failed = true;
|
failed = true;
|
||||||
goto system_eval;
|
goto system_eval;
|
||||||
}
|
}
|
||||||
|
@ -633,7 +634,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
|
||||||
ret = ioctl(fd, ACCELIOCSELFTEST, 0);
|
ret = ioctl(fd, ACCELIOCSELFTEST, 0);
|
||||||
|
|
||||||
if (ret != OK) {
|
if (ret != OK) {
|
||||||
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION");
|
mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL CALIBRATION");
|
||||||
failed = true;
|
failed = true;
|
||||||
goto system_eval;
|
goto system_eval;
|
||||||
}
|
}
|
||||||
|
@ -647,45 +648,36 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
|
||||||
float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
|
float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
|
||||||
|
|
||||||
if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
|
if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
|
||||||
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE");
|
mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE");
|
||||||
mavlink_log_info(mavlink_fd, "#audio: hold still while arming");
|
mavlink_log_critical(mavlink_fd, "hold still while arming");
|
||||||
/* this is frickin' fatal */
|
/* this is frickin' fatal */
|
||||||
failed = true;
|
failed = true;
|
||||||
goto system_eval;
|
goto system_eval;
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ");
|
mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL READ");
|
||||||
/* this is frickin' fatal */
|
/* this is frickin' fatal */
|
||||||
failed = true;
|
failed = true;
|
||||||
goto system_eval;
|
goto system_eval;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!status->is_rotary_wing) {
|
if (!status->is_rotary_wing) {
|
||||||
|
|
||||||
/* accel done, close it */
|
/* accel done, close it */
|
||||||
close(fd);
|
close(fd);
|
||||||
fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
|
fd = orb_subscribe(ORB_ID(airspeed));
|
||||||
|
|
||||||
if (fd <= 0) {
|
struct airspeed_s airspeed;
|
||||||
mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING");
|
|
||||||
|
if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
|
||||||
|
(hrt_elapsed_time(&airspeed.timestamp) > (50 * 1000))) {
|
||||||
|
mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING");
|
||||||
failed = true;
|
failed = true;
|
||||||
goto system_eval;
|
goto system_eval;
|
||||||
}
|
}
|
||||||
|
|
||||||
struct differential_pressure_s diff_pres;
|
if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) {
|
||||||
|
mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION MISSING");
|
||||||
ret = read(fd, &diff_pres, sizeof(diff_pres));
|
// XXX do not make this fatal yet
|
||||||
|
|
||||||
if (ret == sizeof(diff_pres)) {
|
|
||||||
if (fabsf(diff_pres.differential_pressure_filtered_pa > 6.0f)) {
|
|
||||||
mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING");
|
|
||||||
// XXX do not make this fatal yet
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ");
|
|
||||||
/* this is frickin' fatal */
|
|
||||||
failed = true;
|
|
||||||
goto system_eval;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -1,8 +1,6 @@
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
*
|
*
|
||||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
* Copyright (c) 2013, 2014 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
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
@ -36,14 +34,14 @@
|
||||||
/**
|
/**
|
||||||
* @file state_machine_helper.h
|
* @file state_machine_helper.h
|
||||||
* State machine helper functions definitions
|
* State machine helper functions definitions
|
||||||
|
*
|
||||||
|
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||||
|
* @author Julian Oes <julian@oes.ch>
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#ifndef STATE_MACHINE_HELPER_H_
|
#ifndef STATE_MACHINE_HELPER_H_
|
||||||
#define STATE_MACHINE_HELPER_H_
|
#define STATE_MACHINE_HELPER_H_
|
||||||
|
|
||||||
#define GPS_NOFIX_COUNTER_LIMIT 4 //need GPS_NOFIX_COUNTER_LIMIT gps packets with a bad fix to call an error (if outdoor)
|
|
||||||
#define GPS_GOTFIX_COUNTER_REQUIRED 4 //need GPS_GOTFIX_COUNTER_REQUIRED gps packets with a good fix to obtain position lock
|
|
||||||
|
|
||||||
#include <uORB/uORB.h>
|
#include <uORB/uORB.h>
|
||||||
#include <uORB/topics/vehicle_status.h>
|
#include <uORB/topics/vehicle_status.h>
|
||||||
#include <uORB/topics/actuator_armed.h>
|
#include <uORB/topics/actuator_armed.h>
|
||||||
|
|
|
@ -1 +0,0 @@
|
||||||
Subproject commit 8b65d755b8c4834f90a323172c25d91c45068e21
|
|
|
@ -128,6 +128,13 @@ public:
|
||||||
*/
|
*/
|
||||||
int start();
|
int start();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Task status
|
||||||
|
*
|
||||||
|
* @return true if the mainloop is running
|
||||||
|
*/
|
||||||
|
bool task_running() { return _task_running; }
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Print the current status.
|
* Print the current status.
|
||||||
*/
|
*/
|
||||||
|
@ -155,7 +162,8 @@ public:
|
||||||
private:
|
private:
|
||||||
|
|
||||||
bool _task_should_exit; /**< if true, sensor task should exit */
|
bool _task_should_exit; /**< if true, sensor task should exit */
|
||||||
int _estimator_task; /**< task handle for sensor task */
|
bool _task_running; /**< if true, task is running in its mainloop */
|
||||||
|
int _estimator_task; /**< task handle for sensor task */
|
||||||
#ifndef SENSOR_COMBINED_SUB
|
#ifndef SENSOR_COMBINED_SUB
|
||||||
int _gyro_sub; /**< gyro sensor subscription */
|
int _gyro_sub; /**< gyro sensor subscription */
|
||||||
int _accel_sub; /**< accel sensor subscription */
|
int _accel_sub; /**< accel sensor subscription */
|
||||||
|
@ -317,12 +325,13 @@ namespace estimator
|
||||||
#endif
|
#endif
|
||||||
static const int ERROR = -1;
|
static const int ERROR = -1;
|
||||||
|
|
||||||
FixedwingEstimator *g_estimator;
|
FixedwingEstimator *g_estimator = nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
FixedwingEstimator::FixedwingEstimator() :
|
FixedwingEstimator::FixedwingEstimator() :
|
||||||
|
|
||||||
_task_should_exit(false),
|
_task_should_exit(false),
|
||||||
|
_task_running(false),
|
||||||
_estimator_task(-1),
|
_estimator_task(-1),
|
||||||
|
|
||||||
/* subscriptions */
|
/* subscriptions */
|
||||||
|
@ -570,61 +579,26 @@ FixedwingEstimator::check_filter_state()
|
||||||
|
|
||||||
int check = _ekf->CheckAndBound(&ekf_report);
|
int check = _ekf->CheckAndBound(&ekf_report);
|
||||||
|
|
||||||
const char* ekfname = "att pos estimator: ";
|
const char* const feedback[] = { 0,
|
||||||
|
"NaN in states, resetting",
|
||||||
|
"stale IMU data, resetting",
|
||||||
|
"got initial position lock",
|
||||||
|
"excessive gyro offsets",
|
||||||
|
"GPS velocity divergence",
|
||||||
|
"excessive covariances",
|
||||||
|
"unknown condition"};
|
||||||
|
|
||||||
switch (check) {
|
// Print out error condition
|
||||||
case 0:
|
if (check) {
|
||||||
/* all ok */
|
unsigned warn_index = static_cast<unsigned>(check);
|
||||||
break;
|
unsigned max_warn_index = (sizeof(feedback) / sizeof(feedback[0]));
|
||||||
case 1:
|
|
||||||
{
|
if (max_warn_index < warn_index) {
|
||||||
const char* str = "NaN in states, resetting";
|
warn_index = max_warn_index;
|
||||||
warnx("%s", str);
|
|
||||||
mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case 2:
|
|
||||||
{
|
|
||||||
const char* str = "stale IMU data, resetting";
|
|
||||||
warnx("%s", str);
|
|
||||||
mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case 3:
|
|
||||||
{
|
|
||||||
const char* str = "switching to dynamic state";
|
|
||||||
warnx("%s", str);
|
|
||||||
mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case 4:
|
|
||||||
{
|
|
||||||
const char* str = "excessive gyro offsets";
|
|
||||||
warnx("%s", str);
|
|
||||||
mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case 5:
|
|
||||||
{
|
|
||||||
const char* str = "GPS velocity divergence";
|
|
||||||
warnx("%s", str);
|
|
||||||
mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case 6:
|
|
||||||
{
|
|
||||||
const char* str = "excessive covariances";
|
|
||||||
warnx("%s", str);
|
|
||||||
mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
|
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
default:
|
warnx("reset: %s", feedback[warn_index]);
|
||||||
{
|
mavlink_log_critical(_mavlink_fd, "[ekf] re-init: %s", feedback[warn_index]);
|
||||||
const char* str = "unknown reset condition";
|
|
||||||
warnx("%s", str);
|
|
||||||
mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
struct estimator_status_report rep;
|
struct estimator_status_report rep;
|
||||||
|
@ -656,6 +630,10 @@ FixedwingEstimator::check_filter_state()
|
||||||
rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1);
|
rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1);
|
||||||
rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2);
|
rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2);
|
||||||
rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive) << 3);
|
rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive) << 3);
|
||||||
|
// rep.health_flags |= (((uint8_t)ekf_report.onGround) << 4);
|
||||||
|
// rep.health_flags |= (((uint8_t)ekf_report.staticMode) << 5);
|
||||||
|
// rep.health_flags |= (((uint8_t)ekf_report.useCompass) << 6);
|
||||||
|
// rep.health_flags |= (((uint8_t)ekf_report.useAirspeed) << 7);
|
||||||
|
|
||||||
rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0);
|
rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0);
|
||||||
rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1);
|
rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1);
|
||||||
|
@ -783,6 +761,8 @@ FixedwingEstimator::task_main()
|
||||||
_gps.vel_e_m_s = 0.0f;
|
_gps.vel_e_m_s = 0.0f;
|
||||||
_gps.vel_d_m_s = 0.0f;
|
_gps.vel_d_m_s = 0.0f;
|
||||||
|
|
||||||
|
_task_running = true;
|
||||||
|
|
||||||
while (!_task_should_exit) {
|
while (!_task_should_exit) {
|
||||||
|
|
||||||
/* wait for up to 500ms for data */
|
/* wait for up to 500ms for data */
|
||||||
|
@ -1213,10 +1193,10 @@ FixedwingEstimator::task_main()
|
||||||
_baro_gps_offset = 0.0f;
|
_baro_gps_offset = 0.0f;
|
||||||
|
|
||||||
_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
|
_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
|
||||||
|
|
||||||
} else if (_ekf->statesInitialised) {
|
} else if (_ekf->statesInitialised) {
|
||||||
|
|
||||||
// We're apparently initialized in this case now
|
// We're apparently initialized in this case now
|
||||||
|
|
||||||
int check = check_filter_state();
|
int check = check_filter_state();
|
||||||
|
|
||||||
if (check) {
|
if (check) {
|
||||||
|
@ -1224,7 +1204,6 @@ FixedwingEstimator::task_main()
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// Run the strapdown INS equations every IMU update
|
// Run the strapdown INS equations every IMU update
|
||||||
_ekf->UpdateStrapdownEquationsNED();
|
_ekf->UpdateStrapdownEquationsNED();
|
||||||
#if 0
|
#if 0
|
||||||
|
@ -1292,7 +1271,11 @@ FixedwingEstimator::task_main()
|
||||||
// run the fusion step
|
// run the fusion step
|
||||||
_ekf->FuseVelposNED();
|
_ekf->FuseVelposNED();
|
||||||
|
|
||||||
} else if (_ekf->statesInitialised) {
|
} else if (!_gps_initialized) {
|
||||||
|
|
||||||
|
// force static mode
|
||||||
|
_ekf->staticMode = true;
|
||||||
|
|
||||||
// Convert GPS measurements to Pos NE, hgt and Vel NED
|
// Convert GPS measurements to Pos NE, hgt and Vel NED
|
||||||
_ekf->velNED[0] = 0.0f;
|
_ekf->velNED[0] = 0.0f;
|
||||||
_ekf->velNED[1] = 0.0f;
|
_ekf->velNED[1] = 0.0f;
|
||||||
|
@ -1314,7 +1297,7 @@ FixedwingEstimator::task_main()
|
||||||
_ekf->fusePosData = false;
|
_ekf->fusePosData = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (newHgtData && _ekf->statesInitialised) {
|
if (newHgtData) {
|
||||||
// Could use a blend of GPS and baro alt data if desired
|
// Could use a blend of GPS and baro alt data if desired
|
||||||
_ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
|
_ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
|
||||||
_ekf->fuseHgtData = true;
|
_ekf->fuseHgtData = true;
|
||||||
|
@ -1328,7 +1311,7 @@ FixedwingEstimator::task_main()
|
||||||
}
|
}
|
||||||
|
|
||||||
// Fuse Magnetometer Measurements
|
// Fuse Magnetometer Measurements
|
||||||
if (newDataMag && _ekf->statesInitialised) {
|
if (newDataMag) {
|
||||||
_ekf->fuseMagData = true;
|
_ekf->fuseMagData = true;
|
||||||
_ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
|
_ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
|
||||||
|
|
||||||
|
@ -1342,7 +1325,7 @@ FixedwingEstimator::task_main()
|
||||||
}
|
}
|
||||||
|
|
||||||
// Fuse Airspeed Measurements
|
// Fuse Airspeed Measurements
|
||||||
if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) {
|
if (newAdsData && _ekf->VtasMeas > 7.0f) {
|
||||||
_ekf->fuseVtasData = true;
|
_ekf->fuseVtasData = true;
|
||||||
_ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
|
_ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
|
||||||
_ekf->FuseAirspeed();
|
_ekf->FuseAirspeed();
|
||||||
|
@ -1410,7 +1393,7 @@ FixedwingEstimator::task_main()
|
||||||
|
|
||||||
_velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy);
|
_velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy);
|
||||||
_velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz);
|
_velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz);
|
||||||
_airspeed_filtered = 0.95f*_airspeed_filtered + + 0.05f*_airspeed.true_airspeed_m_s;
|
_airspeed_filtered = 0.95f*_airspeed_filtered + 0.05f*_airspeed.true_airspeed_m_s;
|
||||||
|
|
||||||
|
|
||||||
/* crude land detector for fixedwing only,
|
/* crude land detector for fixedwing only,
|
||||||
|
@ -1501,27 +1484,28 @@ FixedwingEstimator::task_main()
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
|
||||||
|
_wind.timestamp = _global_pos.timestamp;
|
||||||
|
_wind.windspeed_north = _ekf->states[14];
|
||||||
|
_wind.windspeed_east = _ekf->states[15];
|
||||||
|
_wind.covariance_north = _ekf->P[14][14];
|
||||||
|
_wind.covariance_east = _ekf->P[15][15];
|
||||||
|
|
||||||
|
/* lazily publish the wind estimate only once available */
|
||||||
|
if (_wind_pub > 0) {
|
||||||
|
/* publish the wind estimate */
|
||||||
|
orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
/* advertise and publish */
|
||||||
|
_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
|
|
||||||
_wind.timestamp = _global_pos.timestamp;
|
|
||||||
_wind.windspeed_north = _ekf->states[14];
|
|
||||||
_wind.windspeed_east = _ekf->states[15];
|
|
||||||
_wind.covariance_north = _ekf->P[14][14];
|
|
||||||
_wind.covariance_east = _ekf->P[15][15];
|
|
||||||
|
|
||||||
/* lazily publish the wind estimate only once available */
|
|
||||||
if (_wind_pub > 0) {
|
|
||||||
/* publish the wind estimate */
|
|
||||||
orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);
|
|
||||||
|
|
||||||
} else {
|
|
||||||
/* advertise and publish */
|
|
||||||
_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
@ -1529,6 +1513,8 @@ FixedwingEstimator::task_main()
|
||||||
perf_end(_loop_perf);
|
perf_end(_loop_perf);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
_task_running = false;
|
||||||
|
|
||||||
warnx("exiting.\n");
|
warnx("exiting.\n");
|
||||||
|
|
||||||
_estimator_task = -1;
|
_estimator_task = -1;
|
||||||
|
@ -1681,6 +1667,14 @@ int ekf_att_pos_estimator_main(int argc, char *argv[])
|
||||||
err(1, "start failed");
|
err(1, "start failed");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* avoid memory fragmentation by not exiting start handler until the task has fully started */
|
||||||
|
while (estimator::g_estimator == nullptr || !estimator::g_estimator->task_running()) {
|
||||||
|
usleep(50000);
|
||||||
|
printf(".");
|
||||||
|
fflush(stdout);
|
||||||
|
}
|
||||||
|
printf("\n");
|
||||||
|
|
||||||
exit(0);
|
exit(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -2279,7 +2279,7 @@ void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, d
|
||||||
|
|
||||||
void AttPosEKF::OnGroundCheck()
|
void AttPosEKF::OnGroundCheck()
|
||||||
{
|
{
|
||||||
onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 6.0f));
|
onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f));
|
||||||
if (staticMode) {
|
if (staticMode) {
|
||||||
staticMode = (!refSet || (GPSstatus < GPS_FIX_3D));
|
staticMode = (!refSet || (GPSstatus < GPS_FIX_3D));
|
||||||
}
|
}
|
||||||
|
@ -2879,12 +2879,18 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
|
||||||
current_ekf_state.statesNaN = false;
|
current_ekf_state.statesNaN = false;
|
||||||
|
|
||||||
current_ekf_state.velHealth = true;
|
current_ekf_state.velHealth = true;
|
||||||
//current_ekf_state.posHealth = ?;
|
current_ekf_state.posHealth = true;
|
||||||
//current_ekf_state.hgtHealth = ?;
|
current_ekf_state.hgtHealth = true;
|
||||||
|
|
||||||
current_ekf_state.velTimeout = false;
|
current_ekf_state.velTimeout = false;
|
||||||
//current_ekf_state.posTimeout = ?;
|
current_ekf_state.posTimeout = false;
|
||||||
//current_ekf_state.hgtTimeout = ?;
|
current_ekf_state.hgtTimeout = false;
|
||||||
|
|
||||||
|
fuseVelData = false;
|
||||||
|
fusePosData = false;
|
||||||
|
fuseHgtData = false;
|
||||||
|
fuseMagData = false;
|
||||||
|
fuseVtasData = false;
|
||||||
|
|
||||||
// Fill variables with valid data
|
// Fill variables with valid data
|
||||||
velNED[0] = initvelNED[0];
|
velNED[0] = initvelNED[0];
|
||||||
|
|
|
@ -1,7 +1,6 @@
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
*
|
*
|
||||||
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
|
||||||
* Author: Lorenz Meier
|
|
||||||
*
|
*
|
||||||
* Redistribution and use in source and binary forms, with or without
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
@ -99,13 +98,21 @@ public:
|
||||||
/**
|
/**
|
||||||
* Start the sensors task.
|
* Start the sensors task.
|
||||||
*
|
*
|
||||||
* @return OK on success.
|
* @return OK on success.
|
||||||
*/
|
*/
|
||||||
int start();
|
int start();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Task status
|
||||||
|
*
|
||||||
|
* @return true if the mainloop is running
|
||||||
|
*/
|
||||||
|
bool task_running() { return _task_running; }
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
bool _task_should_exit; /**< if true, sensor task should exit */
|
bool _task_should_exit; /**< if true, sensor task should exit */
|
||||||
|
bool _task_running; /**< if true, task is running in its mainloop */
|
||||||
int _control_task; /**< task handle for sensor task */
|
int _control_task; /**< task handle for sensor task */
|
||||||
|
|
||||||
int _att_sub; /**< vehicle attitude subscription */
|
int _att_sub; /**< vehicle attitude subscription */
|
||||||
|
@ -276,6 +283,7 @@ private:
|
||||||
* Main sensor collection task.
|
* Main sensor collection task.
|
||||||
*/
|
*/
|
||||||
void task_main();
|
void task_main();
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
namespace att_control
|
namespace att_control
|
||||||
|
@ -287,12 +295,13 @@ namespace att_control
|
||||||
#endif
|
#endif
|
||||||
static const int ERROR = -1;
|
static const int ERROR = -1;
|
||||||
|
|
||||||
FixedwingAttitudeControl *g_control;
|
FixedwingAttitudeControl *g_control = nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
FixedwingAttitudeControl::FixedwingAttitudeControl() :
|
FixedwingAttitudeControl::FixedwingAttitudeControl() :
|
||||||
|
|
||||||
_task_should_exit(false),
|
_task_should_exit(false),
|
||||||
|
_task_running(false),
|
||||||
_control_task(-1),
|
_control_task(-1),
|
||||||
|
|
||||||
/* subscriptions */
|
/* subscriptions */
|
||||||
|
@ -598,6 +607,8 @@ FixedwingAttitudeControl::task_main()
|
||||||
fds[1].fd = _att_sub;
|
fds[1].fd = _att_sub;
|
||||||
fds[1].events = POLLIN;
|
fds[1].events = POLLIN;
|
||||||
|
|
||||||
|
_task_running = true;
|
||||||
|
|
||||||
while (!_task_should_exit) {
|
while (!_task_should_exit) {
|
||||||
|
|
||||||
static int loop_counter = 0;
|
static int loop_counter = 0;
|
||||||
|
@ -921,6 +932,7 @@ FixedwingAttitudeControl::task_main()
|
||||||
warnx("exiting.\n");
|
warnx("exiting.\n");
|
||||||
|
|
||||||
_control_task = -1;
|
_control_task = -1;
|
||||||
|
_task_running = false;
|
||||||
_exit(0);
|
_exit(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -966,6 +978,14 @@ int fw_att_control_main(int argc, char *argv[])
|
||||||
err(1, "start failed");
|
err(1, "start failed");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* avoid memory fragmentation by not exiting start handler until the task has fully started */
|
||||||
|
while (att_control::g_control == nullptr || !att_control::g_control->task_running()) {
|
||||||
|
usleep(50000);
|
||||||
|
printf(".");
|
||||||
|
fflush(stdout);
|
||||||
|
}
|
||||||
|
printf("\n");
|
||||||
|
|
||||||
exit(0);
|
exit(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -1,7 +1,6 @@
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
*
|
*
|
||||||
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
|
||||||
* Author: Lorenz Meier
|
|
||||||
*
|
*
|
||||||
* Redistribution and use in source and binary forms, with or without
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
@ -120,10 +119,18 @@ public:
|
||||||
*/
|
*/
|
||||||
int start();
|
int start();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Task status
|
||||||
|
*
|
||||||
|
* @return true if the mainloop is running
|
||||||
|
*/
|
||||||
|
bool task_running() { return _task_running; }
|
||||||
|
|
||||||
private:
|
private:
|
||||||
int _mavlink_fd;
|
int _mavlink_fd;
|
||||||
|
|
||||||
bool _task_should_exit; /**< if true, sensor task should exit */
|
bool _task_should_exit; /**< if true, sensor task should exit */
|
||||||
|
bool _task_running; /**< if true, task is running in its mainloop */
|
||||||
int _control_task; /**< task handle for sensor task */
|
int _control_task; /**< task handle for sensor task */
|
||||||
|
|
||||||
int _global_pos_sub;
|
int _global_pos_sub;
|
||||||
|
@ -391,13 +398,14 @@ namespace l1_control
|
||||||
#endif
|
#endif
|
||||||
static const int ERROR = -1;
|
static const int ERROR = -1;
|
||||||
|
|
||||||
FixedwingPositionControl *g_control;
|
FixedwingPositionControl *g_control = nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
FixedwingPositionControl::FixedwingPositionControl() :
|
FixedwingPositionControl::FixedwingPositionControl() :
|
||||||
|
|
||||||
_mavlink_fd(-1),
|
_mavlink_fd(-1),
|
||||||
_task_should_exit(false),
|
_task_should_exit(false),
|
||||||
|
_task_running(false),
|
||||||
_control_task(-1),
|
_control_task(-1),
|
||||||
|
|
||||||
/* subscriptions */
|
/* subscriptions */
|
||||||
|
@ -1290,6 +1298,8 @@ FixedwingPositionControl::task_main()
|
||||||
fds[1].fd = _global_pos_sub;
|
fds[1].fd = _global_pos_sub;
|
||||||
fds[1].events = POLLIN;
|
fds[1].events = POLLIN;
|
||||||
|
|
||||||
|
_task_running = true;
|
||||||
|
|
||||||
while (!_task_should_exit) {
|
while (!_task_should_exit) {
|
||||||
|
|
||||||
/* wait for up to 500ms for data */
|
/* wait for up to 500ms for data */
|
||||||
|
@ -1390,6 +1400,8 @@ FixedwingPositionControl::task_main()
|
||||||
perf_end(_loop_perf);
|
perf_end(_loop_perf);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
_task_running = false;
|
||||||
|
|
||||||
warnx("exiting.\n");
|
warnx("exiting.\n");
|
||||||
|
|
||||||
_control_task = -1;
|
_control_task = -1;
|
||||||
|
@ -1478,6 +1490,14 @@ int fw_pos_control_l1_main(int argc, char *argv[])
|
||||||
err(1, "start failed");
|
err(1, "start failed");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* avoid memory fragmentation by not exiting start handler until the task has fully started */
|
||||||
|
while (l1_control::g_control == nullptr || !l1_control::g_control->task_running()) {
|
||||||
|
usleep(50000);
|
||||||
|
printf(".");
|
||||||
|
fflush(stdout);
|
||||||
|
}
|
||||||
|
printf("\n");
|
||||||
|
|
||||||
exit(0);
|
exit(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -194,16 +194,25 @@ PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
|
||||||
/**
|
/**
|
||||||
* Differential pressure sensor offset
|
* Differential pressure sensor offset
|
||||||
*
|
*
|
||||||
|
* The offset (zero-reading) in Pascal
|
||||||
|
*
|
||||||
* @group Sensor Calibration
|
* @group Sensor Calibration
|
||||||
*/
|
*/
|
||||||
PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f);
|
PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Differential pressure sensor analog enabled
|
* Differential pressure sensor analog scaling
|
||||||
|
*
|
||||||
|
* Pick the appropriate scaling from the datasheet.
|
||||||
|
* this number defines the (linear) conversion from voltage
|
||||||
|
* to Pascal (pa). For the MPXV7002DP this is 1000.
|
||||||
|
*
|
||||||
|
* NOTE: If the sensor always registers zero, try switching
|
||||||
|
* the static and dynamic tubes.
|
||||||
*
|
*
|
||||||
* @group Sensor Calibration
|
* @group Sensor Calibration
|
||||||
*/
|
*/
|
||||||
PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0);
|
PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0);
|
||||||
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
|
@ -248,7 +248,7 @@ private:
|
||||||
float accel_offset[3];
|
float accel_offset[3];
|
||||||
float accel_scale[3];
|
float accel_scale[3];
|
||||||
float diff_pres_offset_pa;
|
float diff_pres_offset_pa;
|
||||||
float diff_pres_analog_enabled;
|
float diff_pres_analog_scale;
|
||||||
|
|
||||||
int board_rotation;
|
int board_rotation;
|
||||||
int external_mag_rotation;
|
int external_mag_rotation;
|
||||||
|
@ -311,7 +311,7 @@ private:
|
||||||
param_t mag_offset[3];
|
param_t mag_offset[3];
|
||||||
param_t mag_scale[3];
|
param_t mag_scale[3];
|
||||||
param_t diff_pres_offset_pa;
|
param_t diff_pres_offset_pa;
|
||||||
param_t diff_pres_analog_enabled;
|
param_t diff_pres_analog_scale;
|
||||||
|
|
||||||
param_t rc_map_roll;
|
param_t rc_map_roll;
|
||||||
param_t rc_map_pitch;
|
param_t rc_map_pitch;
|
||||||
|
@ -501,6 +501,7 @@ Sensors::Sensors() :
|
||||||
_battery_current_timestamp(0)
|
_battery_current_timestamp(0)
|
||||||
{
|
{
|
||||||
memset(&_rc, 0, sizeof(_rc));
|
memset(&_rc, 0, sizeof(_rc));
|
||||||
|
memset(&_diff_pres, 0, sizeof(_diff_pres));
|
||||||
|
|
||||||
/* basic r/c parameters */
|
/* basic r/c parameters */
|
||||||
for (unsigned i = 0; i < _rc_max_chan_count; i++) {
|
for (unsigned i = 0; i < _rc_max_chan_count; i++) {
|
||||||
|
@ -590,7 +591,7 @@ Sensors::Sensors() :
|
||||||
|
|
||||||
/* Differential pressure offset */
|
/* Differential pressure offset */
|
||||||
_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
|
_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
|
||||||
_parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA");
|
_parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC");
|
||||||
|
|
||||||
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
|
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
|
||||||
_parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING");
|
_parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING");
|
||||||
|
@ -798,7 +799,7 @@ Sensors::parameters_update()
|
||||||
|
|
||||||
/* Airspeed offset */
|
/* Airspeed offset */
|
||||||
param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa));
|
param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa));
|
||||||
param_get(_parameter_handles.diff_pres_analog_enabled, &(_parameters.diff_pres_analog_enabled));
|
param_get(_parameter_handles.diff_pres_analog_scale, &(_parameters.diff_pres_analog_scale));
|
||||||
|
|
||||||
/* scaling of ADC ticks to battery voltage */
|
/* scaling of ADC ticks to battery voltage */
|
||||||
if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
|
if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
|
||||||
|
@ -1323,22 +1324,23 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
|
||||||
} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
|
} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
|
||||||
|
|
||||||
/* calculate airspeed, raw is the difference from */
|
/* calculate airspeed, raw is the difference from */
|
||||||
float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; //V_ref/4096 * (voltage divider factor)
|
float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; // V_ref/4096 * (voltage divider factor)
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* The voltage divider pulls the signal down, only act on
|
* The voltage divider pulls the signal down, only act on
|
||||||
* a valid voltage from a connected sensor. Also assume a non-
|
* a valid voltage from a connected sensor. Also assume a non-
|
||||||
* zero offset from the sensor if its connected.
|
* zero offset from the sensor if its connected.
|
||||||
*/
|
*/
|
||||||
if (voltage > 0.4f && (_parameters.diff_pres_analog_enabled > 0)) {
|
if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0.0f)) {
|
||||||
|
|
||||||
float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor
|
float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa;
|
||||||
|
float diff_pres_pa = (diff_pres_pa_raw > 0.0f) ? diff_pres_pa_raw : 0.0f;
|
||||||
|
|
||||||
_diff_pres.timestamp = t;
|
_diff_pres.timestamp = t;
|
||||||
_diff_pres.differential_pressure_pa = diff_pres_pa;
|
_diff_pres.differential_pressure_pa = diff_pres_pa;
|
||||||
_diff_pres.differential_pressure_filtered_pa = diff_pres_pa;
|
_diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw;
|
||||||
|
_diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa * 0.1f);
|
||||||
_diff_pres.temperature = -1000.0f;
|
_diff_pres.temperature = -1000.0f;
|
||||||
_diff_pres.voltage = voltage;
|
|
||||||
|
|
||||||
/* announce the airspeed if needed, just publish else */
|
/* announce the airspeed if needed, just publish else */
|
||||||
if (_diff_pres_pub > 0) {
|
if (_diff_pres_pub > 0) {
|
||||||
|
|
|
@ -58,7 +58,6 @@ struct differential_pressure_s {
|
||||||
float differential_pressure_raw_pa; /**< Raw differential pressure reading (may be negative) */
|
float differential_pressure_raw_pa; /**< Raw differential pressure reading (may be negative) */
|
||||||
float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */
|
float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */
|
||||||
float max_differential_pressure_pa; /**< Maximum differential pressure reading */
|
float max_differential_pressure_pa; /**< Maximum differential pressure reading */
|
||||||
float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */
|
|
||||||
float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */
|
float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
Loading…
Reference in New Issue