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

Merged release into master

Browse Source
This commit is contained in:
Lorenz Meier 2015-06-13 11:06:01 +02:00
commit c9fefe236b
47 changed files with 1036 additions and 344 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
View File

@ -6,5 +6,30 @@
sh /etc/init.d/rc.fw_defaults
if [ $AUTOCNF == yes ]
then
param set BAT_N_CELLS 3
param set FW_AIRSPD_MAX 20
param set FW_AIRSPD_MIN 12
param set FW_AIRSPD_TRIM 14
param set FW_ATT_TC 0.3
param set FW_L1_DAMPING 0.74
param set FW_L1_PERIOD 16
param set FW_LND_ANG 15
param set FW_LND_FLALT 5
param set FW_LND_HHDIST 15
param set FW_LND_HVIRT 13
param set FW_LND_TLALT 5
param set FW_THR_LND_MAX 0
param set FW_PR_FF 0.35
param set FW_PR_I 0.1
param set FW_PR_IMAX 0.4
param set FW_PR_P 0.2
param set FW_RR_FF 0.6
param set FW_RR_I 0.1
param set FW_RR_IMAX 0.2
param set FW_RR_P 0.3
fi
set HIL yes
set MIXER AERT

8
ROMFS/px4fmu_common/init.d/3034_fx79
View File

@ -7,4 +7,12 @@
sh /etc/init.d/rc.fw_defaults
if [ $AUTOCNF == yes ]
then
param set NAV_LOITER_RAD 150
param set FW_AIRSPD_MAX 30
param set FW_AIRSPD_MIN 13
param set FW_AIRSPD_TRIM 15
fi
set MIXER FX79

44
ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha
View File

@ -2,33 +2,39 @@
#
# TBS Caipirinha
#
# Thomas Gubler <thomas@px4.io>
# Lorenz Meier <lorenz@px4.io>
#
sh /etc/init.d/rc.fw_defaults
if [ $AUTOCNF == yes ]
then
# TODO: these are the X5 default parameters, update them to the caipi
param set FW_AIRSPD_MIN 15
param set FW_AIRSPD_TRIM 20
param set FW_AIRSPD_MAX 40
param set FW_AIRSPD_MAX 20
param set FW_AIRSPD_MIN 12
param set FW_AIRSPD_TRIM 16
param set FW_ATT_TC 0.3
param set FW_L1_DAMPING 0.74
param set FW_L1_PERIOD 15
param set FW_PR_FF 0.3
param set FW_PR_I 0
param set FW_PR_IMAX 0.2
param set FW_PR_P 0.03
param set FW_P_ROLLFF 0
param set FW_RR_FF 0.3
param set FW_RR_I 0
param set FW_L1_PERIOD 16
param set FW_LND_ANG 15
param set FW_LND_FLALT 5
param set FW_LND_HHDIST 15
param set FW_LND_HVIRT 13
param set FW_LND_TLALT 5
param set FW_THR_LND_MAX 0
param set FW_PR_FF 0.35
param set FW_PR_I 0.01
param set FW_PR_IMAX 0.4
param set FW_PR_P 0.08
param set FW_RR_FF 0.6
param set FW_RR_I 0.01
param set FW_RR_IMAX 0.2
param set FW_RR_P 0.03
param set FW_R_LIM 60
param set FW_R_RMAX 0
param set FW_RR_P 0.04
param set SYS_COMPANION 157600
param set PWM_MAIN_REV0 1
param set PWM_MAIN_REV1 1
fi
set MIXER Q
set MIXER caipi
# Provide ESC a constant 1000 us pulse
set PWM_OUT 4
set PWM_DISARMED 1000

2
ROMFS/px4fmu_common/init.d/rc.logging
View File

@ -11,7 +11,7 @@ then
then
fi
else
if sdlog2 start -r 100 -a -b 22 -t
if sdlog2 start -r 100 -a -b 12 -t
then
fi
fi

4
ROMFS/px4fmu_common/init.d/rcS
View File

@ -485,6 +485,10 @@ then
then
mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 1000
fi
if param compare SYS_COMPANION 157600
then
mavlink start -d /dev/ttyS2 -b 57600 -m osd -r 1000
fi
fi
# UAVCAN

24
ROMFS/px4fmu_common/mixers/FX79.main.mix
View File

@ -27,13 +27,13 @@ for the elevons.
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 7500 7500 0 -10000 10000
S: 0 1 8000 8000 0 -10000 10000
S: 0 0 8500 8500 0 -10000 10000
S: 0 1 9500 9500 0 -10000 10000
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 7500 7500 0 -10000 10000
S: 0 1 -8000 -8000 0 -10000 10000
S: 0 0 8500 8500 0 -10000 10000
S: 0 1 -9500 -9500 0 -10000 10000
Output 2
--------
@ -51,19 +51,3 @@ range. Inputs below zero are treated as zero.
M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000
Inputs to the mixer come from channel group 2 (payload), channels 0
(bay servo 1), 1 (bay servo 2) and 3 (drop release).
-----------------------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
S: 2 0 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 2 1 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 2 2 -10000 -10000 0 -10000 10000

51
ROMFS/px4fmu_common/mixers/caipi.main.mix Normal file
View File

@ -0,0 +1,51 @@
Delta-wing mixer
===========================
Designed for TBS Caipirinha
This file defines mixers suitable for controlling a delta wing aircraft using
PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU
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 for roll inputs is adjusted to implement differential travel
for the elevons.
M: 2
O: 10000 10000 3000 -10000 10000
S: 0 0 8000 8000 0 -10000 10000
S: 0 1 9000 9000 0 -10000 10000
M: 2
O: 10000 10000 -3000 -10000 10000
S: 0 0 8000 8000 0 -10000 10000
S: 0 1 -9000 -9000 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

6
makefiles/nuttx/toolchain_gnu-arm-eabi.mk
View File

@ -132,6 +132,12 @@ LIBC := $(shell ${CC} ${ARCHCPUFLAGS} -print-file-name=libc.a)
ARCHCFLAGS = -std=gnu99
ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics -D__CUSTOM_FILE_IO__
#
# Provide defaults, but allow for module override
WFRAME_LARGER_THAN ?= 1024
# Generic warnings
#
ARCHWARNINGS = -Wall \

5
msg/navigation_capabilities.msg Normal file
View File

@ -0,0 +1,5 @@
uint64 timestamp # timestamp this topic was emitted
float32 turn_distance # the optimal distance to a waypoint to switch to the next
float32 landing_horizontal_slope_displacement
float32 landing_slope_angle_rad
float32 landing_flare_length

6
msg/vehicle_status.msg
View File

@ -7,7 +7,8 @@ uint8 MAIN_STATE_AUTO_LOITER = 4
uint8 MAIN_STATE_AUTO_RTL = 5
uint8 MAIN_STATE_ACRO = 6
uint8 MAIN_STATE_OFFBOARD = 7
uint8 MAIN_STATE_MAX = 8
uint8 MAIN_STATE_STAB = 8
uint8 MAIN_STATE_MAX = 9
# If you change the order, add or remove arming_state_t states make sure to update the arrays
# in state_machine_helper.cpp as well.
@ -39,7 +40,8 @@ uint8 NAVIGATION_STATE_LAND = 11 # Land mode
uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control)
uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode
uint8 NAVIGATION_STATE_OFFBOARD = 14
uint8 NAVIGATION_STATE_MAX = 15
uint8 NAVIGATION_STATE_STAB = 15 # Stabilized mode
uint8 NAVIGATION_STATE_MAX = 16
# VEHICLE_MODE_FLAG, same as MAV_MODE_FLAG of MAVLink 1.0 protocol
uint8 VEHICLE_MODE_FLAG_SAFETY_ARMED = 128

32
src/drivers/px4io/px4io.cpp
View File

@ -1114,6 +1114,27 @@ PX4IO::task_main()
}
(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_REVERSE, pwm_invert_mask);
float trim_val;
param_t trim_parm;
trim_parm = param_find("TRIM_ROLL");
if (trim_parm != PARAM_INVALID) {
param_get(trim_parm, &trim_val);
(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_ROLL, FLOAT_TO_REG(trim_val));
}
trim_parm = param_find("TRIM_PITCH");
if (trim_parm != PARAM_INVALID) {
param_get(trim_parm, &trim_val);
(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_PITCH, FLOAT_TO_REG(trim_val));
}
trim_parm = param_find("TRIM_YAW");
if (trim_parm != PARAM_INVALID) {
param_get(trim_parm, &trim_val);
(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_YAW, FLOAT_TO_REG(trim_val));
}
}
}
@ -1240,7 +1261,7 @@ PX4IO::io_set_arming_state()
uint16_t set = 0;
uint16_t clear = 0;
if (have_armed == OK) {
if (have_armed == OK) {
_in_esc_calibration_mode = armed.in_esc_calibration_mode;
if (armed.armed || _in_esc_calibration_mode) {
set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
@ -2127,7 +2148,14 @@ PX4IO::print_status(bool extended_status)
for (unsigned i = 0; i < _max_actuators; i++) {
printf("%s", (pwm_invert_mask & (1 << i)) ? "x" : "_");
}
printf("]\n");
printf("]");
float trim_roll = REG_TO_FLOAT(io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_ROLL));
float trim_pitch = REG_TO_FLOAT(io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_PITCH));
float trim_yaw = REG_TO_FLOAT(io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_YAW));
printf(" trims: r: %8.4f p: %8.4f y: %8.4f\n",
(double)trim_roll, (double)trim_pitch, (double)trim_yaw);
uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT);
printf("%d raw R/C inputs", raw_inputs);

173
src/modules/commander/commander.cpp
View File

@ -37,9 +37,9 @@
*
* @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
* @author Lorenz Meier <lorenz@px4.io>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Thomas Gubler <thomas@px4.io>
* @author Julian Oes <julian@px4.io>
* @author Anton Babushkin <anton@px4.io>
*/
#include <px4_config.h>
@ -93,7 +93,7 @@
#include <uORB/topics/geofence_result.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <drivers/drv_led.h>
#include <drivers/drv_hrt.h>
@ -148,7 +148,12 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage
#define DIFFPRESS_TIMEOUT 2000000
#define PRINT_INTERVAL 5000000
#define PRINT_MODE_REJECT_INTERVAL 2000000
#define PRINT_MODE_REJECT_INTERVAL 10000000
#define INAIR_RESTART_HOLDOFF_INTERVAL 2000000
#define HIL_ID_MIN 1000
#define HIL_ID_MAX 1999
enum MAV_MODE_FLAG {
MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */
@ -174,6 +179,7 @@ static volatile bool thread_should_exit = false; /**< daemon exit flag */
static volatile bool thread_running = false; /**< daemon status flag */
static int daemon_task; /**< Handle of daemon task / thread */
static bool need_param_autosave = false; /**< Flag set to true if parameters should be autosaved in next iteration (happens on param update and if functionality is enabled) */
static hrt_abstime commander_boot_timestamp = 0;
static unsigned int leds_counter;
/* To remember when last notification was sent */
@ -189,6 +195,7 @@ static struct actuator_armed_s armed;
static struct safety_s safety;
static struct vehicle_control_mode_s control_mode;
static struct offboard_control_mode_s offboard_control_mode;
static struct home_position_s _home;
/**
* The daemon app only briefly exists to start
@ -212,7 +219,7 @@ void usage(const char *reason);
*/
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);
struct vehicle_local_position_s *local_pos, orb_advert_t *home_pub);
/**
* Mainloop of commander.
@ -254,6 +261,15 @@ void *commander_low_prio_loop(void *arg);
void answer_command(struct vehicle_command_s &cmd, unsigned result);
/**
* check whether autostart ID is in the reserved range for HIL setups
*/
bool is_hil_setup(int id);
bool is_hil_setup(int id) {
return (id >= HIL_ID_MIN) && (id <= HIL_ID_MAX);
}
int commander_main(int argc, char *argv[])
{
@ -354,6 +370,7 @@ int commander_main(int argc, char *argv[])
if (!strcmp(argv[1], "arm")) {
int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0);
arm_disarm(true, mavlink_fd_local, "command line");
warnx("note: not updating home position on commandline arming!");
close(mavlink_fd_local);
return 0;
}
@ -383,6 +400,8 @@ void print_status()
warnx("type: %s", (status.is_rotary_wing) ? "symmetric motion" : "forward motion");
warnx("usb powered: %s", (status.usb_connected) ? "yes" : "no");
warnx("avionics rail: %6.2f V", (double)status.avionics_power_rail_voltage);
warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", _home.lat, _home.lon, (double)_home.alt);
warnx("home: x = %.7f, y = %.7f, z = %.2f ", (double)_home.x, (double)_home.y, (double)_home.z);
/* read all relevant states */
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
@ -437,6 +456,12 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
{
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
// For HIL platforms, require that simulated sensors are connected
if (is_hil_setup(autostart_id) && status.hil_state != vehicle_status_s::HIL_STATE_ON) {
mavlink_and_console_log_critical(mavlink_fd_local, "HIL platform: Connect to simulator before arming");
return TRANSITION_DENIED;
}
// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
// output appropriate error messages if the state cannot transition.
arming_res = arming_state_transition(&status, &safety, arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, &armed,
@ -454,7 +479,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
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)
struct home_position_s *home, struct vehicle_global_position_s *global_pos,
struct vehicle_local_position_s *local_pos, orb_advert_t *home_pub)
{
/* only handle commands that are meant to be handled by this system and component */
if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id)
@ -480,7 +506,16 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd);
// Transition the arming state
arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
bool cmd_arm = base_mode & MAV_MODE_FLAG_SAFETY_ARMED;
arming_ret = arm_disarm(cmd_arm, mavlink_fd, "set mode command");
/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
if (cmd_arm && (arming_ret == TRANSITION_CHANGED) &&
(hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL))) {
commander_set_home_position(*home_pub, *home, *local_pos, *global_pos);
}
if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
/* use autopilot-specific mode */
@ -504,6 +539,10 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
/* ACRO */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ACRO);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_STABILIZED) {
/* STABILIZED */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_STAB);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
/* OFFBOARD */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD);
@ -521,6 +560,9 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL);
} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
/* STABILIZED */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_STAB);
} else {
/* MANUAL */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL);
}
@ -845,6 +887,7 @@ int commander_thread_main(int argc, char *argv[])
main_states_str[vehicle_status_s::MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER";
main_states_str[vehicle_status_s::MAIN_STATE_AUTO_RTL] = "AUTO_RTL";
main_states_str[vehicle_status_s::MAIN_STATE_ACRO] = "ACRO";
main_states_str[vehicle_status_s::MAIN_STATE_STAB] = "STAB";
main_states_str[vehicle_status_s::MAIN_STATE_OFFBOARD] = "OFFBOARD";
const char *arming_states_str[vehicle_status_s::ARMING_STATE_MAX];
@ -858,6 +901,7 @@ int commander_thread_main(int argc, char *argv[])
const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX];
nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL] = "MANUAL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_STAB] = "STAB";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_ALTCTL] = "ALTCTL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL] = "POSCTL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION";
@ -955,8 +999,7 @@ int commander_thread_main(int argc, char *argv[])
/* home position */
orb_advert_t home_pub = nullptr;
struct home_position_s home;
memset(&home, 0, sizeof(home));
memset(&_home, 0, sizeof(_home));
/* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */
orb_advert_t mission_pub = nullptr;
@ -1141,15 +1184,22 @@ int commander_thread_main(int argc, char *argv[])
}
// Run preflight check
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !status.rc_input_mode, !status.circuit_breaker_engaged_gpsfailure_check);
if (!status.condition_system_sensors_initialized) {
set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune
}
else {
param_get(_param_autostart_id, &autostart_id);
if (is_hil_setup(autostart_id)) {
// HIL configuration selected: real sensors will be disabled
status.condition_system_sensors_initialized = false;
set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
} else {
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !status.rc_input_mode, !status.circuit_breaker_engaged_gpsfailure_check);
if (!status.condition_system_sensors_initialized) {
set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune
}
else {
set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
}
}
const hrt_abstime commander_boot_timestamp = hrt_absolute_time();
commander_boot_timestamp = hrt_absolute_time();
transition_result_t arming_ret;
@ -1317,8 +1367,15 @@ int commander_thread_main(int argc, char *argv[])
}
/* provide RC and sensor status feedback to the user */
(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed,
!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check);
if (is_hil_setup(autostart_id)) {
/* HIL configuration: check only RC input */
(void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false,
!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false);
} else {
/* check sensors also */
(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed,
!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check);
}
}
telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
@ -1589,6 +1646,8 @@ int commander_thread_main(int argc, char *argv[])
low_battery_voltage_actions_done = true;
if (armed.armed) {
mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
} else {
mavlink_log_critical(mavlink_fd, "LOW BATTERY, TAKEOFF DISCOURAGED");
}
status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
@ -1676,7 +1735,7 @@ int commander_thread_main(int argc, char *argv[])
if (status.gps_failure && !gpsIsNoisy) {
status.gps_failure = false;
status_changed = true;
mavlink_log_critical(mavlink_fd, "gps regained");
mavlink_log_critical(mavlink_fd, "gps fix regained");
}
} else if (!status.gps_failure) {
@ -1710,12 +1769,12 @@ int commander_thread_main(int argc, char *argv[])
if (!flight_termination_printed) {
warnx("Flight termination because of navigator request or geofence");
mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
mavlink_log_critical(mavlink_fd, "Geofence violation: flight termination");
flight_termination_printed = true;
}
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
mavlink_log_critical(mavlink_fd, "Flight termination active");
}
} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
@ -1725,7 +1784,7 @@ int commander_thread_main(int argc, char *argv[])
/* handle the case where RC signal was regained */
if (!status.rc_signal_found_once) {
status.rc_signal_found_once = true;
mavlink_log_info(mavlink_fd, "Detected RC signal first time");
mavlink_log_info(mavlink_fd, "Detected radio control");
status_changed = true;
} else {
@ -1742,7 +1801,10 @@ int commander_thread_main(int argc, char *argv[])
* do it only for rotary wings */
if (status.is_rotary_wing &&
(status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) &&
(status.main_state == vehicle_status_s::MAIN_STATE_MANUAL || status.main_state == vehicle_status_s::MAIN_STATE_ACRO || status.condition_landed) &&
(status.main_state == vehicle_status_s::MAIN_STATE_MANUAL ||
status.main_state == vehicle_status_s::MAIN_STATE_ACRO ||
status.main_state == vehicle_status_s::MAIN_STATE_STAB ||
status.condition_landed) &&
sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
@ -1775,7 +1837,8 @@ int commander_thread_main(int argc, char *argv[])
* for being in manual mode only applies to manual arming actions.
* the system can be armed in auto if armed via the GCS.
*/
if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL) {
if ((status.main_state != vehicle_status_s::MAIN_STATE_MANUAL) &&
(status.main_state != vehicle_status_s::MAIN_STATE_STAB)) {
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else {
@ -1932,7 +1995,7 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
/* handle it */
if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub)) {
if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, &home_pub)) {
status_changed = true;
}
}
@ -1944,6 +2007,7 @@ int commander_thread_main(int argc, char *argv[])
* and both failed we want to terminate the flight */
if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL &&
status.main_state !=vehicle_status_s::MAIN_STATE_ACRO &&
status.main_state !=vehicle_status_s::MAIN_STATE_STAB &&
status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL &&
status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL &&
((status.data_link_lost && status.gps_failure) ||
@ -1968,6 +2032,7 @@ int commander_thread_main(int argc, char *argv[])
* if both failed we want to terminate the flight */
if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO ||
status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL ||
status.main_state !=vehicle_status_s::MAIN_STATE_STAB ||
status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL ||
status.main_state ==vehicle_status_s::MAIN_STATE_POSCTL) &&
((status.rc_signal_lost && status.gps_failure) ||
@ -1987,17 +2052,17 @@ int commander_thread_main(int argc, char *argv[])
}
}
//Get current timestamp
/* Get current timestamp */
const hrt_abstime now = hrt_absolute_time();
//First time home position update
if (!status.condition_home_position_valid) {
commander_set_home_position(home_pub, home, local_position, global_position);
/* First time home position update - but only if disarmed */
if (!status.condition_home_position_valid && !armed.armed) {
commander_set_home_position(home_pub, _home, local_position, global_position);
}
/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + 2000000) {
commander_set_home_position(home_pub, home, local_position, global_position);
else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) {
commander_set_home_position(home_pub, _home, local_position, global_position);
}
/* print new state */
@ -2014,14 +2079,6 @@ int commander_thread_main(int argc, char *argv[])
mission_result.finished,
mission_result.stay_in_failsafe);
// TODO handle mode changes by commands
if (main_state_changed) {
status_changed = true;
warnx("main state: %s", main_states_str[status.main_state]);
mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]);
main_state_changed = false;
}
if (status.failsafe != failsafe_old) {
status_changed = true;
@ -2035,10 +2092,12 @@ int commander_thread_main(int argc, char *argv[])
failsafe_old = status.failsafe;
}
if (nav_state_changed) {
// TODO handle mode changes by commands
if (main_state_changed || nav_state_changed) {
status_changed = true;
warnx("nav state: %s", nav_states_str[status.nav_state]);
mavlink_log_info(mavlink_fd, "[cmd] nav state: %s", nav_states_str[status.nav_state]);
warnx("main state: %s nav state: %s", main_states_str[status.main_state], nav_states_str[status.nav_state]);
mavlink_log_info(mavlink_fd, "Flight mode: %s", nav_states_str[status.nav_state]);
main_state_changed = false;
}
/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
@ -2061,11 +2120,13 @@ int commander_thread_main(int argc, char *argv[])
set_tune(TONE_ARMING_WARNING_TUNE);
arm_tune_played = true;
} else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
} else if ((status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
/* play tune on battery critical */
set_tune(TONE_BATTERY_WARNING_FAST_TUNE);
} else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status.failsafe) {
} else if ((status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
(status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status.failsafe)) {
/* play tune on battery warning or failsafe */
set_tune(TONE_BATTERY_WARNING_SLOW_TUNE);
@ -2292,7 +2353,15 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
case manual_control_setpoint_s::SWITCH_POS_OFF: // MANUAL
if (sp_man->acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO);
/* manual mode is stabilized already for multirotors, so switch to acro
* for any non-manual mode
*/
if (status.is_rotary_wing) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO);
} else {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB);
}
} else {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL);
@ -2403,6 +2472,20 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case vehicle_status_s::NAVIGATION_STATE_STAB:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_termination_enabled = false;
/* override is not ok in stabilized mode */
control_mode.flag_external_manual_override_ok = false;
break;
case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;

42
src/modules/commander/px4_custom_mode.h
View File

@ -1,8 +1,41 @@
/*
* px4_custom_mode.h
/****************************************************************************
*
* Created on: 09.08.2013
* Author: ton
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file px4_custom_mode.h
* PX4 custom flight modes
*
* @author Anton Babushkin <anton@px4.io>
*/
#ifndef PX4_CUSTOM_MODE_H_
@ -17,6 +50,7 @@ enum PX4_CUSTOM_MAIN_MODE {
PX4_CUSTOM_MAIN_MODE_AUTO,
PX4_CUSTOM_MAIN_MODE_ACRO,
PX4_CUSTOM_MAIN_MODE_OFFBOARD,
PX4_CUSTOM_MAIN_MODE_STABILIZED
};
enum PX4_CUSTOM_SUB_MODE_AUTO {

34
src/modules/commander/rc_calibration.cpp
View File

@ -59,7 +59,7 @@ static const int ERROR = -1;
int do_trim_calibration(int mavlink_fd)
{
int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
usleep(200000);
usleep(400000);
struct manual_control_setpoint_s sp;
bool changed;
orb_check(sub_man, &changed);
@ -71,20 +71,34 @@ int do_trim_calibration(int mavlink_fd)
orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
/* set parameters */
float p = sp.y;
param_set(param_find("TRIM_ROLL"), &p);
p = sp.x;
param_set(param_find("TRIM_PITCH"), &p);
p = sp.r;
param_set(param_find("TRIM_YAW"), &p);
/* load trim values which are active */
float roll_trim_active;
param_get(param_find("TRIM_ROLL"), &roll_trim_active);
float pitch_trim_active;
param_get(param_find("TRIM_PITCH"), &pitch_trim_active);
float yaw_trim_active;
param_get(param_find("TRIM_YAW"), &yaw_trim_active);
/* set parameters: the new trim values are the combination of active trim values
and the values coming from the remote control of the user
*/
float p = sp.y + roll_trim_active;
int p1r = param_set(param_find("TRIM_ROLL"), &p);
/*
we explicitly swap sign here because the trim is added to the actuator controls
which are moving in an inverse sense to manual pitch inputs
*/
p = -sp.x + pitch_trim_active;
int p2r = param_set(param_find("TRIM_PITCH"), &p);
p = sp.r + yaw_trim_active;
int p3r = param_set(param_find("TRIM_YAW"), &p);
/* store to permanent storage */
/* auto-save */
int save_ret = param_save_default();
if (save_ret != 0) {
mavlink_log_critical(mavlink_fd, "TRIM: SAVE FAIL");
if (save_ret != 0 || p1r != 0 || p2r != 0 || p3r != 0) {
mavlink_log_critical(mavlink_fd, "TRIM: PARAM SET FAIL");
px4_close(sub_man);
return ERROR;
}

11
src/modules/commander/state_machine_helper.cpp
View File

@ -126,7 +126,8 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
int prearm_ret = OK;
/* only perform the check if we have to */
if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
prearm_ret = prearm_check(status, mavlink_fd);
}
@ -304,6 +305,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
switch (new_main_state) {
case vehicle_status_s::MAIN_STATE_MANUAL:
case vehicle_status_s::MAIN_STATE_ACRO:
case vehicle_status_s::MAIN_STATE_STAB:
ret = TRANSITION_CHANGED;
break;
@ -538,10 +540,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
switch (status->main_state) {
case vehicle_status_s::MAIN_STATE_ACRO:
case vehicle_status_s::MAIN_STATE_MANUAL:
case vehicle_status_s::MAIN_STATE_STAB:
case vehicle_status_s::MAIN_STATE_ALTCTL:
case vehicle_status_s::MAIN_STATE_POSCTL:
/* require RC for all manual modes */
if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) {
if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed && !status->condition_landed) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
@ -564,6 +567,10 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
break;
case vehicle_status_s::MAIN_STATE_STAB:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
break;
case vehicle_status_s::MAIN_STATE_ALTCTL:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
break;

9
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
View File

@ -174,7 +174,7 @@ private:
struct map_projection_reference_s _pos_ref;
float _baro_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */
float _filter_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */
float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */
hrt_abstime _last_debug_print = 0;
@ -193,7 +193,6 @@ private:
bool _gpsIsGood; ///< True if the current GPS fix is good enough for us to use
uint64_t _previousGPSTimestamp; ///< Timestamp of last good GPS fix we have received
bool _baro_init;
float _baroAltRef;
bool _gps_initialized;
hrt_abstime _filter_start_time;
hrt_abstime _last_sensor_timestamp;
@ -333,6 +332,12 @@ private:
**/
void initializeGPS();
/**
* Initialize the reference position for the local coordinate frame
*/
void initReferencePosition(hrt_abstime timestamp,
double lat, double lon, float gps_alt, float baro_alt);
/**
* @brief
* Polls all uORB subscriptions if any new sensor data has been publish and sets the appropriate

140
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -75,6 +75,9 @@ static uint64_t IMUusec = 0;
static constexpr float rc = 10.0f; // RC time constant of 1st order LPF in seconds
static constexpr uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; ///< units: microseconds
static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we signal a total GPS failure
static constexpr unsigned MAG_SWITCH_HYSTERESIS = 10; ///< Ignore the first few mag failures (which amounts to a few milliseconds)
static constexpr unsigned GYRO_SWITCH_HYSTERESIS = 5; ///< Ignore the first few gyro failures (which amounts to a few milliseconds)
static constexpr unsigned ACCEL_SWITCH_HYSTERESIS = 5; ///< Ignore the first few accel failures (which amounts to a few milliseconds)
/**
* estimator app start / stop handling function
@ -157,7 +160,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_sensor_combined {},
_pos_ref{},
_baro_ref_offset(0.0f),
_filter_ref_offset(0.0f),
_baro_gps_offset(0.0f),
/* performance counters */
@ -177,7 +180,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_gpsIsGood(false),
_previousGPSTimestamp(0),
_baro_init(false),
_baroAltRef(0.0f),
_gps_initialized(false),
_filter_start_time(0),
_last_sensor_timestamp(0),
@ -408,6 +410,15 @@ int AttitudePositionEstimatorEKF::check_filter_state()
// Count the reset condition
perf_count(_perf_reset);
// GPS is in scaled integers, convert
double lat = _gps.lat / 1.0e7;
double lon = _gps.lon / 1.0e7;
float gps_alt = _gps.alt / 1e3f;
// Set up height correctly
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
initReferencePosition(_gps.timestamp_position, lat, lon, gps_alt, _baro.altitude);
} else if (_ekf_logging) {
_ekf->GetFilterState(&ekf_report);
@ -590,6 +601,7 @@ void AttitudePositionEstimatorEKF::task_main()
_baro_init = false;
_gps_initialized = false;
_last_sensor_timestamp = hrt_absolute_time();
_last_run = _last_sensor_timestamp;
@ -626,7 +638,10 @@ void AttitudePositionEstimatorEKF::task_main()
// gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds
// maintain heavily filtered values for both baro and gps altitude
// Assume the filtered output should be identical for both sensors
_baro_gps_offset = _baro_alt_filt - _gps_alt_filt;
if (_gpsIsGood) {
_baro_gps_offset = _baro_alt_filt - _gps_alt_filt;
}
// if (hrt_elapsed_time(&_last_debug_print) >= 5e6) {
// _last_debug_print = hrt_absolute_time();
// perf_print_counter(_perf_baro);
@ -648,12 +663,15 @@ void AttitudePositionEstimatorEKF::task_main()
_ekf->posNE[1] = 0.0f;
_local_pos.ref_alt = 0.0f;
_baro_ref_offset = 0.0f;
_baro_gps_offset = 0.0f;
_baro_alt_filt = _baro.altitude;
_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
_filter_ref_offset = -_baro.altitude;
warnx("filter ref off: baro_alt: %8.4f", (double)_filter_ref_offset);
} else {
if (!_gps_initialized && _gpsIsGood) {
@ -705,6 +723,32 @@ void AttitudePositionEstimatorEKF::task_main()
return;
}
void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp,
double lat, double lon, float gps_alt, float baro_alt)
{
// Reference altitude
if (isfinite(_ekf->states[9])) {
_filter_ref_offset = _ekf->states[9];
} else if (isfinite(-_ekf->hgtMea)) {
_filter_ref_offset = -_ekf->hgtMea;
} else {
_filter_ref_offset = -_baro.altitude;
}
// init filtered gps and baro altitudes
_gps_alt_filt = gps_alt;
_baro_alt_filt = baro_alt;
// Initialize projection
_local_pos.ref_lat = lat;
_local_pos.ref_lon = lon;
_local_pos.ref_alt = gps_alt;
_local_pos.ref_timestamp = timestamp;
map_projection_init(&_pos_ref, lat, lon);
mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
}
void AttitudePositionEstimatorEKF::initializeGPS()
{
// GPS is in scaled integers, convert
@ -714,11 +758,6 @@ void AttitudePositionEstimatorEKF::initializeGPS()
// Set up height correctly
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
_baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame
// init filtered gps and baro altitudes
_gps_alt_filt = gps_alt;
_baro_alt_filt = _baro.altitude;
_ekf->baroHgt = _baro.altitude;
_ekf->hgtMea = _ekf->baroHgt;
@ -740,20 +779,13 @@ void AttitudePositionEstimatorEKF::initializeGPS()
_ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination);
// Initialize projection
_local_pos.ref_lat = lat;
_local_pos.ref_lon = lon;
_local_pos.ref_alt = gps_alt;
_local_pos.ref_timestamp = _gps.timestamp_position;
map_projection_init(&_pos_ref, lat, lon);
mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
initReferencePosition(_gps.timestamp_position, lat, lon, gps_alt, _baro.altitude);
#if 0
warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt,
(double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref,
(double)_baro_ref_offset);
(double)_filter_ref_offset);
warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv,
(double)math::degrees(declination));
#endif
@ -814,7 +846,8 @@ void AttitudePositionEstimatorEKF::publishLocalPosition()
_local_pos.y = _ekf->states[8];
// XXX need to announce change of Z reference somehow elegantly
_local_pos.z = _ekf->states[9] - _baro_ref_offset - _baroAltRef;
_local_pos.z = _ekf->states[9] - _filter_ref_offset;
//_local_pos.z_stable = _ekf->states[9];
_local_pos.vx = _ekf->states[4];
_local_pos.vy = _ekf->states[5];
@ -829,6 +862,17 @@ void AttitudePositionEstimatorEKF::publishLocalPosition()
_local_pos.z_global = false;
_local_pos.yaw = _att.yaw;
if (!isfinite(_local_pos.x) ||
!isfinite(_local_pos.y) ||
!isfinite(_local_pos.z) ||
!isfinite(_local_pos.vx) ||
!isfinite(_local_pos.vy) ||
!isfinite(_local_pos.vz))
{
// bad data, abort publication
return;
}
/* lazily publish the local position only once available */
if (_local_pos_pub != nullptr) {
/* publish the attitude setpoint */
@ -862,7 +906,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
}
/* local pos alt is negative, change sign and add alt offsets */
_global_pos.alt = (-_local_pos.z) - _baro_gps_offset;
_global_pos.alt = (-_local_pos.z) - _filter_ref_offset - _baro_gps_offset;
if (_local_pos.v_z_valid) {
_global_pos.vel_d = _local_pos.vz;
@ -877,6 +921,17 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
_global_pos.eph = _gps.eph;
_global_pos.epv = _gps.epv;
if (!isfinite(_global_pos.lat) ||
!isfinite(_global_pos.lon) ||
!isfinite(_global_pos.alt) ||
!isfinite(_global_pos.vel_n) ||
!isfinite(_global_pos.vel_e) ||
!isfinite(_global_pos.vel_d))
{
// bad data, abort publication
return;
}
/* lazily publish the global position only once available */
if (_global_pos_pub != nullptr) {
/* publish the global position */
@ -1073,8 +1128,9 @@ void AttitudePositionEstimatorEKF::print_status()
// 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
printf("dtIMU: %8.6f filt: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)IMUmsec);
printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f));
printf("baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref_offset,
printf("alt RAW: baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)_ekf->gpsHgt);
printf("alt EST: local alt: %8.4f (NED), AMSL alt: %8.4f (ENU)\n", (double)(_local_pos.z), (double)_global_pos.alt);
printf("filter ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_filter_ref_offset,
(double)_baro_gps_offset);
printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y,
(double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
@ -1173,7 +1229,7 @@ void AttitudePositionEstimatorEKF::pollData()
if (PX4_ISFINITE(_sensor_combined.gyro_rad_s[0]) &&
PX4_ISFINITE(_sensor_combined.gyro_rad_s[1]) &&
PX4_ISFINITE(_sensor_combined.gyro_rad_s[2]) &&
(_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) {
(_sensor_combined.gyro_errcount <= (_sensor_combined.gyro1_errcount + GYRO_SWITCH_HYSTERESIS))) {
_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
@ -1212,7 +1268,7 @@ void AttitudePositionEstimatorEKF::pollData()
int last_accel_main = _accel_main;
/* fail over to the 2nd accel if we know the first is down */
if (_sensor_combined.accelerometer_errcount <= _sensor_combined.accelerometer1_errcount) {
if (_sensor_combined.accelerometer_errcount <= (_sensor_combined.accelerometer1_errcount + ACCEL_SWITCH_HYSTERESIS)) {
_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
@ -1320,7 +1376,11 @@ void AttitudePositionEstimatorEKF::pollData()
_ekf->updateDtGpsFilt(math::constrain(dtLastGoodGPS, 0.01f, POS_RESET_THRESHOLD));
// update LPF
_gps_alt_filt += (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt);
float filter_step = (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt);
if (isfinite(filter_step)) {
_gps_alt_filt += filter_step;
}
}
//check if we had a GPS outage for a long time
@ -1403,15 +1463,19 @@ void AttitudePositionEstimatorEKF::pollData()
}
baro_last = _baro.timestamp;
if(!_baro_init) {
if (!_baro_init) {
_baro_init = true;
_baroAltRef = _baro.altitude;
_baro_alt_filt = _baro.altitude;
}
_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f));
_ekf->baroHgt = _baro.altitude;
_baro_alt_filt += (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt);
float filter_step = (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt);
if (isfinite(filter_step)) {
_baro_alt_filt += filter_step;
}
perf_count(_perf_baro);
}
@ -1435,7 +1499,7 @@ void AttitudePositionEstimatorEKF::pollData()
/* fail over to the 2nd mag if we know the first is down */
if (hrt_elapsed_time(&_sensor_combined.magnetometer_timestamp) < mag_timeout_us &&
_sensor_combined.magnetometer_errcount <= _sensor_combined.magnetometer1_errcount &&
_sensor_combined.magnetometer_errcount <= (_sensor_combined.magnetometer1_errcount + MAG_SWITCH_HYSTERESIS) &&
mag0.length() > 0.1f) {
_ekf->magData.x = mag0.x;
_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
@ -1463,7 +1527,7 @@ void AttitudePositionEstimatorEKF::pollData()
}
if (last_mag_main != _mag_main) {
mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Switched from #%d to %d", last_mag_main, _mag_main);
mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Failover from unit %d to unit %d", last_mag_main, _mag_main);
}
}
@ -1497,30 +1561,34 @@ int AttitudePositionEstimatorEKF::trip_nan()
float nan_val = 0.0f / 0.0f;
warnx("system not armed, tripping state vector with NaN values");
warnx("system not armed, tripping state vector with NaN");
_ekf->states[5] = nan_val;
usleep(100000);
warnx("tripping covariance #1 with NaN values");
warnx("tripping covariance #1 with NaN");
_ekf->KH[2][2] = nan_val; // intermediate result used for covariance updates
usleep(100000);
warnx("tripping covariance #2 with NaN values");
warnx("tripping covariance #2 with NaN");
_ekf->KHP[5][5] = nan_val; // intermediate result used for covariance updates
usleep(100000);
warnx("tripping covariance #3 with NaN values");
warnx("tripping covariance #3 with NaN");
_ekf->P[3][3] = nan_val; // covariance matrix
usleep(100000);
warnx("tripping Kalman gains with NaN values");
warnx("tripping Kalman gains with NaN");
_ekf->Kfusion[0] = nan_val; // Kalman gains
usleep(100000);
warnx("tripping stored states[0] with NaN values");
warnx("tripping stored states[0] with NaN");
_ekf->storedStates[0][0] = nan_val;
usleep(100000);
warnx("tripping states[9] with NaN");
_ekf->states[9] = nan_val;
usleep(100000);
warnx("\nDONE - FILTER STATE:");
print_status();
}

45
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@ -859,11 +859,36 @@ FixedwingAttitudeControl::task_main()
_yaw_ctrl.reset_integrator();
}
} else if (_vcontrol_mode.flag_control_velocity_enabled) {
/* the pilot does not want to change direction,
* take straight attitude setpoint from position controller
*/
if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) {
roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
} else {
roll_sp = (_manual.y * _parameters.man_roll_max)
+ _parameters.rollsp_offset_rad;
}
pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
throttle_sp = _att_sp.thrust;
/* reset integrals where needed */
if (_att_sp.roll_reset_integral) {
_roll_ctrl.reset_integrator();
}
if (_att_sp.pitch_reset_integral) {
_pitch_ctrl.reset_integrator();
}
if (_att_sp.yaw_reset_integral) {
_yaw_ctrl.reset_integrator();
}
} else if (_vcontrol_mode.flag_control_altitude_enabled) {
/*
* Velocity should be controlled and manual is enabled.
*/
roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
+ _parameters.rollsp_offset_rad;
roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad;
pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
throttle_sp = _att_sp.thrust;
@ -890,10 +915,8 @@ FixedwingAttitudeControl::task_main()
* the intended attitude setpoint. Later, after the rate control step the
* trim is added again to the control signal.
*/
roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
+ _parameters.rollsp_offset_rad;
pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch)
+ _parameters.pitchsp_offset_rad;
roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad;
pitch_sp = -(_manual.x * _parameters.man_pitch_max) + _parameters.pitchsp_offset_rad;
/* allow manual control of rudder deflection */
yaw_manual = _manual.r;
throttle_sp = _manual.z;
@ -1060,9 +1083,9 @@ FixedwingAttitudeControl::task_main()
} else {
/* manual/direct control */
_actuators.control[0] = _manual.y;
_actuators.control[1] = -_manual.x;
_actuators.control[2] = _manual.r;
_actuators.control[0] = _manual.y + _parameters.trim_roll;
_actuators.control[1] = -_manual.x + _parameters.trim_pitch;
_actuators.control[2] = _manual.r + _parameters.trim_yaw;
_actuators.control[3] = _manual.z;
_actuators.control[4] = _manual.flaps;
}
@ -1079,8 +1102,8 @@ FixedwingAttitudeControl::task_main()
/* Only publish if any of the proper modes are enabled */
if(_vcontrol_mode.flag_control_rates_enabled ||
_vcontrol_mode.flag_control_attitude_enabled ||
_vcontrol_mode.flag_control_manual_enabled)
_vcontrol_mode.flag_control_attitude_enabled ||
_vcontrol_mode.flag_control_manual_enabled)
{
/* publish the actuator controls */
if (_actuators_0_pub != nullptr) {

257
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -94,6 +94,13 @@
#include <platforms/px4_defines.h>
static int _control_task = -1; /**< task handle for sensor task */
#define HDG_HOLD_DIST_NEXT 3000.0f // initial distance of waypoint in front of plane in heading hold mode
#define HDG_HOLD_REACHED_DIST 1000.0f // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode
#define HDG_HOLD_SET_BACK_DIST 100.0f // distance by which previous waypoint is set behind the plane
#define HDG_HOLD_YAWRATE_THRESH 0.1f // max yawrate at which plane locks yaw for heading hold mode
#define HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll input from user which does not change the locked heading
#define THROTTLE_THRESH 0.05f // max throttle from user which will not lead to motors spinning up in altitude controlled modes
/**
@ -165,7 +172,13 @@ private:
perf_counter_t _loop_perf; /**< loop performance counter */
float _hold_alt; /**< hold altitude for velocity mode */
float _hold_alt; /**< hold altitude for altitude mode */
float _ground_alt; /**< ground altitude at which plane was launched */
float _hdg_hold_yaw; /**< hold heading for velocity mode */
bool _hdg_hold_enabled; /**< heading hold enabled */
bool _yaw_lock_engaged; /**< yaw is locked for heading hold */
struct position_setpoint_s _hdg_hold_prev_wp; /**< position where heading hold started */
struct position_setpoint_s _hdg_hold_curr_wp; /**< position to which heading hold flies */
hrt_abstime _control_position_last_called; /**<last call of control_position */
/* land states */
@ -176,6 +189,9 @@ private:
bool land_onslope;
bool land_useterrain;
bool _was_in_air; /**< indicated wether the plane was in the air in the previous interation*/
hrt_abstime _time_went_in_air; /**< time at which the plane went in the air */
/* takeoff/launch states */
LaunchDetectionResult launch_detection_state;
@ -205,6 +221,7 @@ private:
enum FW_POSCTRL_MODE {
FW_POSCTRL_MODE_AUTO,
FW_POSCTRL_MODE_POSITION,
FW_POSCTRL_MODE_ALTITUDE,
FW_POSCTRL_MODE_OTHER
} _control_mode_current; ///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode.
@ -355,6 +372,17 @@ private:
*/
void navigation_capabilities_publish();
/**
* Get a new waypoint based on heading and distance from current position
*
* @param heading the heading to fly to
* @param distance the distance of the generated waypoint
* @param waypoint_prev the waypoint at the current position
* @param waypoint_next the waypoint in the heading direction
*/
void get_waypoint_heading_distance(float heading, float distance,
struct position_setpoint_s &waypoint_prev, struct position_setpoint_s &waypoint_next, bool flag_init);
/**
* Return the terrain estimate during landing: uses the wp altitude value or the terrain estimate if available
*/
@ -363,6 +391,17 @@ private:
/**
* Control position.
*/
/**
* Do takeoff help when in altitude controlled modes
*/
void do_takeoff_help();
/**
* Update desired altitude base on user pitch stick input
*/
void update_desired_altitude(float dt);
bool control_position(const math::Vector<2> &global_pos, const math::Vector<3> &ground_speed,
const struct position_setpoint_triplet_s &_pos_sp_triplet);
@ -454,6 +493,12 @@ FixedwingPositionControl::FixedwingPositionControl() :
_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
_hold_alt(0.0f),
_ground_alt(0.0f),
_hdg_hold_yaw(0.0f),
_hdg_hold_enabled(false),
_yaw_lock_engaged(false),
_hdg_hold_prev_wp{},
_hdg_hold_curr_wp{},
_control_position_last_called(0),
land_noreturn_horizontal(false),
@ -462,6 +507,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
land_motor_lim(false),
land_onslope(false),
land_useterrain(false),
_was_in_air(false),
_time_went_in_air(0),
launch_detection_state(LAUNCHDETECTION_RES_NONE),
last_manual(false),
landingslope(),
@ -855,6 +902,8 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &c
void FixedwingPositionControl::navigation_capabilities_publish()
{
_nav_capabilities.timestamp = hrt_absolute_time();
if (_nav_capabilities_pub != nullptr) {
orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities);
} else {
@ -862,6 +911,31 @@ void FixedwingPositionControl::navigation_capabilities_publish()
}
}
void FixedwingPositionControl::get_waypoint_heading_distance(float heading, float distance,
struct position_setpoint_s &waypoint_prev, struct position_setpoint_s &waypoint_next, bool flag_init)
{
waypoint_prev.valid = true;
waypoint_prev.alt = _hold_alt;
if (flag_init) {
// on init set first waypoint to momentary position
waypoint_prev.lat = _global_pos.lat - cos(heading) * (double)(HDG_HOLD_SET_BACK_DIST) / 1e6;
waypoint_prev.lon = _global_pos.lon - sin(heading) * (double)(HDG_HOLD_SET_BACK_DIST) / 1e6;
} else {
/*
for previous waypoint use the one still in front of us but shift it such that it is
located on the desired flight path but HDG_HOLD_SET_BACK_DIST behind us
*/
waypoint_prev.lat = waypoint_next.lat - cos(heading) * (double)(HDG_HOLD_REACHED_DIST + HDG_HOLD_SET_BACK_DIST) / 1e6;
waypoint_prev.lon = waypoint_next.lon - sin(heading) * (double)(HDG_HOLD_REACHED_DIST + HDG_HOLD_SET_BACK_DIST) / 1e6;
}
waypoint_next.valid = true;
waypoint_next.lat = waypoint_prev.lat + cos(heading) * (double)(distance + HDG_HOLD_SET_BACK_DIST) / 1e6;
waypoint_next.lon = waypoint_prev.lon + sin(heading) * (double)(distance + HDG_HOLD_SET_BACK_DIST) / 1e6;
waypoint_next.alt = _hold_alt;
}
float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos)
{
if (!isfinite(global_pos.terrain_alt)) {
@ -881,6 +955,41 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint
}
}
void FixedwingPositionControl::update_desired_altitude(float dt)
{
const float deadBand = (60.0f/1000.0f);
const float factor = 1.0f - deadBand;
static bool was_in_deadband = false;
if (_manual.x > deadBand) {
float pitch = (_manual.x - deadBand) / factor;
_hold_alt -= (_parameters.max_climb_rate * dt) * pitch;
was_in_deadband = false;
} else if (_manual.x < - deadBand) {
float pitch = (_manual.x + deadBand) / factor;
_hold_alt -= (_parameters.max_sink_rate * dt) * pitch;
was_in_deadband = false;
} else if (!was_in_deadband) {
/* store altitude at which manual.x was inside deadBand
* The aircraft should immediately try to fly at this altitude
* as this is what the pilot expects when he moves the stick to the center */
_hold_alt = _global_pos.alt;
was_in_deadband = true;
}
}
void FixedwingPositionControl::do_takeoff_help()
{
const hrt_abstime delta_takeoff = 10000000;
const float throttle_threshold = 0.3f;
const float delta_alt_takeoff = 30.0f;
/* demand 30 m above ground if user switched into this mode during takeoff */
if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _ground_alt + delta_alt_takeoff) {
_hold_alt = _ground_alt + delta_alt_takeoff;
}
}
bool
FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<3> &ground_speed,
const struct position_setpoint_triplet_s &pos_sp_triplet)
@ -912,6 +1021,17 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* no throttle limit as default */
float throttle_max = 1.0f;
/* save time when airplane is in air */
if (!_was_in_air && !_vehicle_status.condition_landed) {
_was_in_air = true;
_time_went_in_air = hrt_absolute_time();
_ground_alt = _global_pos.alt;
}
/* reset flag when airplane landed */
if (_vehicle_status.condition_landed) {
_was_in_air = false;
}
if (_control_mode.flag_control_auto_enabled &&
pos_sp_triplet.current.valid) {
/* AUTONOMOUS FLIGHT */
@ -928,6 +1048,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* reset hold altitude */
_hold_alt = _global_pos.alt;
/* reset hold yaw */
_hdg_hold_yaw = _att.yaw;
/* get circle mode */
bool was_circle_mode = _l1_control.circle_mode();
@ -1251,13 +1373,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else if (_control_mode.flag_control_velocity_enabled &&
_control_mode.flag_control_altitude_enabled) {
/* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */
/* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed,
heading is set to a distant waypoint */
const float deadBand = (60.0f/1000.0f);
const float factor = 1.0f - deadBand;
if (_control_mode_current != FW_POSCTRL_MODE_POSITION) {
/* Need to init because last loop iteration was in a different mode */
_hold_alt = _global_pos.alt;
_hdg_hold_yaw = _att.yaw;
_hdg_hold_enabled = false; // this makes sure the waypoints are reset below
_yaw_lock_engaged = false;
}
/* Reset integrators if switching to this mode from a other mode in which posctl was not active */
if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
@ -1274,30 +1398,118 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.z;
/* Get demanded vertical velocity from pitch control */
static bool was_in_deadband = false;
if (_manual.x > deadBand) {
float pitch = (_manual.x - deadBand) / factor;
_hold_alt -= (_parameters.max_climb_rate * dt) * pitch;
was_in_deadband = false;
} else if (_manual.x < - deadBand) {
float pitch = (_manual.x + deadBand) / factor;
_hold_alt -= (_parameters.max_sink_rate * dt) * pitch;
was_in_deadband = false;
} else if (!was_in_deadband) {
/* store altitude at which manual.x was inside deadBand
* The aircraft should immediately try to fly at this altitude
* as this is what the pilot expects when he moves the stick to the center */
_hold_alt = _global_pos.alt;
was_in_deadband = true;
/* update desired altitude based on user pitch stick input */
update_desired_altitude(dt);
/* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/
do_takeoff_help();
/* throttle limiting */
throttle_max = _parameters.throttle_max;
if (fabsf(_manual.z) < THROTTLE_THRESH) {
throttle_max = 0.0f;
}
tecs_update_pitch_throttle(_hold_alt,
altctrl_airspeed,
eas2tas,
math::radians(_parameters.pitch_limit_min),
math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min,
_parameters.throttle_max,
throttle_max,
_parameters.throttle_cruise,
false,
math::radians(_parameters.pitch_limit_min),
_global_pos.alt,
ground_speed,
tecs_status_s::TECS_MODE_NORMAL);
/* heading control */
if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH) {
/* heading / roll is zero, lock onto current heading */
if (fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
// little yaw movement, lock to current heading
_yaw_lock_engaged = true;
}
if (_yaw_lock_engaged) {
/* just switched back from non heading-hold to heading hold */
if (!_hdg_hold_enabled) {
_hdg_hold_enabled = true;
_hdg_hold_yaw = _att.yaw;
get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true);
}
/* we have a valid heading hold position, are we too close? */
if (get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon,
_hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon) < HDG_HOLD_REACHED_DIST) {
get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false);
}
math::Vector<2> prev_wp;
prev_wp(0) = (float)_hdg_hold_prev_wp.lat;
prev_wp(1) = (float)_hdg_hold_prev_wp.lon;
math::Vector<2> curr_wp;
curr_wp(0) = (float)_hdg_hold_curr_wp.lat;
curr_wp(1) = (float)_hdg_hold_curr_wp.lon;
/* populate l1 control setpoint */
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
}
} else {
_hdg_hold_enabled = false;
_yaw_lock_engaged = false;
}
} else if (_control_mode.flag_control_altitude_enabled) {
/* ALTITUDE CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */
if (_control_mode_current != FW_POSCTRL_MODE_POSITION && _control_mode_current != FW_POSCTRL_MODE_ALTITUDE) {
/* Need to init because last loop iteration was in a different mode */
_hold_alt = _global_pos.alt;
}
/* Reset integrators if switching to this mode from a other mode in which posctl was not active */
if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
/* reset integrators */
if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators();
_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
}
}
_control_mode_current = FW_POSCTRL_MODE_ALTITUDE;
/* Get demanded airspeed */
float altctrl_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.z;
/* update desired altitude based on user pitch stick input */
update_desired_altitude(dt);
/* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/
do_takeoff_help();
/* throttle limiting */
throttle_max = _parameters.throttle_max;
if (fabsf(_manual.z) < THROTTLE_THRESH) {
throttle_max = 0.0f;
}
tecs_update_pitch_throttle(_hold_alt,
altctrl_airspeed,
eas2tas,
math::radians(_parameters.pitch_limit_min),
math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min,
throttle_max,
_parameters.throttle_cruise,
false,
math::radians(_parameters.pitch_limit_min),
@ -1476,7 +1688,8 @@ FixedwingPositionControl::task_main()
float turn_distance = _l1_control.switch_distance(100.0f);
/* lazily publish navigation capabilities */
if (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0) {
if ((hrt_elapsed_time(&_nav_capabilities.timestamp) > 1000000) || (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON
&& turn_distance > 0)) {
/* set new turn distance */
_nav_capabilities.turn_distance = turn_distance;

21
src/modules/land_detector/FixedwingLandDetector.cpp
View File

@ -84,11 +84,22 @@ bool FixedwingLandDetector::update()
const uint64_t now = hrt_absolute_time();
bool landDetected = false;
// TODO: reset filtered values on arming?
_velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx *
_vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy);
_velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz);
_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s;
if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) {
float val = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx *
_vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy);
if (isfinite(val)) {
_velocity_xy_filtered = val;
}
val = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz);
if (isfinite(val)) {
_velocity_z_filtered = val;
}
}
if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) {
_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s;
}
// crude land detector for fixedwing
if (_velocity_xy_filtered < _params.maxVelocity

2
src/modules/land_detector/LandDetector.h
View File

@ -87,7 +87,7 @@ protected:
virtual void initialize() = 0;
/**
* @brief Convinience function for polling uORB subscriptions
* @brief Convenience function for polling uORB subscriptions
* @return true if there was new data and it was successfully copied
**/
bool orb_update(const struct orb_metadata *meta, int handle, void *buffer);

27
src/modules/land_detector/land_detector_params.c
View File

@ -45,6 +45,8 @@
*
* Maximum vertical velocity allowed to trigger a land (m/s up and down)
*
* @unit m/s
*
* @group Land Detector
*/
PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.30f);
@ -54,6 +56,8 @@ PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.30f);
*
* Maximum horizontal velocity allowed to trigger a land (m/s)
*
* @unit m/s
*
* @group Land Detector
*/
PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.00f);
@ -63,6 +67,8 @@ PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.00f);
*
* Maximum allowed around each axis to trigger a land (degrees per second)
*
* @unit deg/s
*
* @group Land Detector
*/
PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 20.0f);
@ -72,6 +78,9 @@ PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 20.0f);
*
* Maximum actuator output on throttle before triggering a land
*
* @min 0.1
* @max 0.5
*
* @group Land Detector
*/
PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.20f);
@ -81,24 +90,36 @@ PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.20f);
*
* Maximum horizontal velocity allowed to trigger a land (m/s)
*
* @min 0.5
* @max 10
* @unit m/s
*
* @group Land Detector
*/
PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 0.40f);
PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 4.0f);
/**
* Fixedwing max climb rate
*
* Maximum vertical velocity allowed to trigger a land (m/s up and down)
*
* @min 5
* @max 20
* @unit m/s
*
* @group Land Detector
*/
PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.00f);
PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.0f);
/**
* Airspeed max
*
* Maximum airspeed allowed to trigger a land (m/s)
*
* @min 4
* @max 20
* @unit m/s
*
* @group Land Detector
*/
PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 10.00f);
PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 8.00f);

5
src/modules/mavlink/mavlink_ftp.cpp
View File

@ -299,7 +299,7 @@ MavlinkFTP::_reply(mavlink_file_transfer_protocol_t* ftp_req)
/// @brief Responds to a List command
MavlinkFTP::ErrorCode
MavlinkFTP::_workList(PayloadHeader* payload)
MavlinkFTP::_workList(PayloadHeader* payload, bool list_hidden)
{
char dirPath[kMaxDataLength];
strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength);
@ -383,7 +383,8 @@ MavlinkFTP::_workList(PayloadHeader* payload)
#else
case DT_DIR:
#endif
if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) {
if ((!list_hidden && (strncmp(entry.d_name, ".", 1) == 0)) ||
strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) {
// Don't bother sending these back
direntType = kDirentSkip;
} else {

2
src/modules/mavlink/mavlink_ftp.h
View File

@ -130,7 +130,7 @@ private:
void _reply(mavlink_file_transfer_protocol_t* ftp_req);
int _copy_file(const char *src_path, const char *dst_path, size_t length);
ErrorCode _workList(PayloadHeader *payload);
ErrorCode _workList(PayloadHeader *payload, bool list_hidden = false);
ErrorCode _workOpen(PayloadHeader *payload, int oflag);
ErrorCode _workRead(PayloadHeader *payload);
ErrorCode _workBurst(PayloadHeader* payload, uint8_t target_system_id);

15
src/modules/mavlink/mavlink_main.cpp
View File

@ -1421,6 +1421,8 @@ Mavlink::task_main(int argc, char *argv[])
} else if (strcmp(myoptarg, "onboard") == 0) {
_mode = MAVLINK_MODE_ONBOARD;
} else if (strcmp(optarg, "osd") == 0) {
_mode = MAVLINK_MODE_OSD;
}
break;
@ -1593,7 +1595,6 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("SYS_STATUS", 1.0f);
configure_stream("ATTITUDE", 50.0f);
configure_stream("HIGHRES_IMU", 50.0f);
configure_stream("VFR_HUD", 5.0f);
configure_stream("GPS_RAW_INT", 5.0f);
configure_stream("GLOBAL_POSITION_INT", 50.0f);
configure_stream("LOCAL_POSITION_NED", 30.0f);
@ -1610,6 +1611,18 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f);
break;
case MAVLINK_MODE_OSD:
configure_stream("SYS_STATUS", 5.0f);
configure_stream("ATTITUDE", 25.0f);
configure_stream("VFR_HUD", 5.0f);
configure_stream("GPS_RAW_INT", 1.0f);
configure_stream("GLOBAL_POSITION_INT", 10.0f);
configure_stream("ATTITUDE_TARGET", 10.0f);
configure_stream("BATTERY_STATUS", 1.0f);
configure_stream("SYSTEM_TIME", 1.0f);
configure_stream("RC_CHANNELS_RAW", 5.0f);
break;
default:
break;
}

3
src/modules/mavlink/mavlink_main.h
View File

@ -148,7 +148,8 @@ public:
enum MAVLINK_MODE {
MAVLINK_MODE_NORMAL = 0,
MAVLINK_MODE_CUSTOM,
MAVLINK_MODE_ONBOARD
MAVLINK_MODE_ONBOARD,
MAVLINK_MODE_OSD
};
void set_mode(enum MAVLINK_MODE);

6
src/modules/mavlink/mavlink_messages.cpp
View File

@ -131,6 +131,12 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
break;
case vehicle_status_s::NAVIGATION_STATE_STAB:
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_STABILIZED;
break;
case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED;

10
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -1007,7 +1007,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
* which makes the corner positions unreachable.
* scale yaw up and clip it to overcome this.
*/
rc.values[2] = man.r / 1.2f + 1500;
rc.values[2] = man.r / 1.1f + 1500;
if (rc.values[2] > 2000) {
rc.values[2] = 2000;
} else if (rc.values[2] < 1000) {
@ -1015,7 +1015,12 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
}
/* throttle */
rc.values[3] = man.z + 1000;
rc.values[3] = man.z / 0.9f + 1000;
if (rc.values[3] > 2000) {
rc.values[3] = 2000;
} else if (rc.values[3] < 1000) {
rc.values[3] = 1000;
}
rc.values[4] = decode_switch_pos_n(man.buttons, 0) * 1000 + 1000;
rc.values[5] = decode_switch_pos_n(man.buttons, 1) * 1000 + 1000;
@ -1346,6 +1351,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
hil_sensors.baro_timestamp = timestamp;
hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa
hil_sensors.differential_pressure_filtered_pa = hil_sensors.differential_pressure_pa;
hil_sensors.differential_pressure_timestamp = timestamp;
/* publish combined sensor topic */

3
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -100,6 +100,7 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]);
#define YAW_DEADZONE 0.05f
#define MIN_TAKEOFF_THRUST 0.2f
#define RATES_I_LIMIT 0.3f
#define MANUAL_THROTTLE_MAX_MULTICOPTER 0.9f
class MulticopterAttitudeControl
{
@ -834,7 +835,7 @@ MulticopterAttitudeControl::task_main()
if (_v_control_mode.flag_control_manual_enabled) {
/* manual rates control - ACRO mode */
_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max);
_thrust_sp = _manual_control_sp.z;
_thrust_sp = math::min(_manual_control_sp.z, MANUAL_THROTTLE_MAX_MULTICOPTER);
/* publish attitude rates setpoint */
_v_rates_sp.roll = _rates_sp(0);

3
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -86,6 +86,7 @@
#define TILT_COS_MAX 0.7f
#define SIGMA 0.000001f
#define MIN_DIST 0.01f
#define MANUAL_THROTTLE_MAX_MULTICOPTER 0.9f
/**
* Multicopter position control app start / stop handling function
@ -1390,7 +1391,7 @@ MulticopterPositionControl::task_main()
//Control climb rate directly if no aiding altitude controller is active
if(!_control_mode.flag_control_climb_rate_enabled) {
_att_sp.thrust = _manual.z;
_att_sp.thrust = math::min(_manual.z, MANUAL_THROTTLE_MAX_MULTICOPTER);
}
//Construct attitude setpoint rotation matrix

7
src/modules/mc_pos_control/mc_pos_control_params.c
View File

@ -55,13 +55,16 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.1f);
/**
* Maximum thrust
*
* Limit max allowed thrust.
* Limit max allowed thrust. Setting a value of one can put
* the system into actuator saturation as no spread between
* the motors is possible any more. A value of 0.8 - 0.9
* is recommended.
*
* @min 0.0
* @max 1.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f);
PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.9f);
/**
* Proportional gain for vertical position error

43
src/modules/navigator/mission.cpp
View File

@ -230,9 +230,9 @@ Mission::update_offboard_mission()
* however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing,
failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing,
dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
_navigator->get_home_position()->alt);
_navigator->get_home_position()->alt, _navigator->home_position_valid());
} else {
warnx("offboard mission update failed");
@ -271,6 +271,16 @@ Mission::advance_mission()
}
}
int
Mission::get_absolute_altitude_for_item(struct mission_item_s &mission_item)
{
if (_mission_item.altitude_is_relative) {
return _mission_item.altitude + _navigator->get_home_position()->alt;
} else {
return _mission_item.altitude;
}
}
bool
Mission::check_dist_1wp()
{
@ -297,10 +307,10 @@ Mission::check_dist_1wp()
mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
mission_item.nav_cmd == NAV_CMD_PATHPLANNING) {
/* check distance from home to item */
/* check distance from current position to item */
float dist_to_1wp = get_distance_to_next_waypoint(
mission_item.lat, mission_item.lon,
_navigator->get_home_position()->lat, _navigator->get_home_position()->lon);
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
if (dist_to_1wp < _param_dist_1wp.get()) {
_dist_1wp_ok = true;
@ -354,7 +364,7 @@ Mission::set_mission_items()
/* Copy previous mission item altitude (can be extended to a copy of the full mission item if needed) */
if (pos_sp_triplet->previous.valid) {
_mission_item_previous_alt = _mission_item.altitude;
_mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item);
}
/* get home distance state */
@ -374,13 +384,14 @@ Mission::set_mission_items()
} else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_OFFBOARD) {
mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running");
mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running");
}
_mission_type = MISSION_TYPE_OFFBOARD;
} else {
/* no mission available or mission finished, switch to loiter */
if (_mission_type != MISSION_TYPE_NONE) {
mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished");
/* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */
mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
/* use last setpoint for loiter */
_navigator->set_can_loiter_at_sp(true);
@ -388,8 +399,9 @@ Mission::set_mission_items()
} else if (!user_feedback_done) {
/* only tell users that we got no mission if there has not been any
* better, more specific feedback yet
* https://en.wikipedia.org/wiki/Loiter_(aeronautics)
*/
mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available");
mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, loitering");
}
_mission_type = MISSION_TYPE_NONE;
@ -440,10 +452,7 @@ Mission::set_mission_items()
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->next);
/* calculate takeoff altitude */
float takeoff_alt = _mission_item.altitude;
if (_mission_item.altitude_is_relative) {
takeoff_alt += _navigator->get_home_position()->alt;
}
float takeoff_alt = get_absolute_altitude_for_item(_mission_item);
/* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
if (_navigator->get_vstatus()->condition_landed) {
@ -586,7 +595,7 @@ Mission::altitude_sp_foh_update()
}
/* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */
if (_distance_current_previous - _mission_item.acceptance_radius < 0.0f) {
if (_distance_current_previous - _navigator->get_acceptance_radius(_mission_item.acceptance_radius) < 0.0f) {
return;
}
@ -608,16 +617,16 @@ Mission::altitude_sp_foh_update()
/* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt
* navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */
if (_min_current_sp_distance_xy < _mission_item.acceptance_radius) {
pos_sp_triplet->current.alt = _mission_item.altitude;
if (_min_current_sp_distance_xy < _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) {
pos_sp_triplet->current.alt = get_absolute_altitude_for_item(_mission_item);
} else {
/* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp
* of the mission item as it is used to check if the mission item is reached
* The setpoint is set linearly and such that the system reaches the current altitude at the acceptance
* radius around the current waypoint
**/
float delta_alt = (_mission_item.altitude - _mission_item_previous_alt);
float grad = -delta_alt/(_distance_current_previous - _mission_item.acceptance_radius);
float delta_alt = (get_absolute_altitude_for_item(_mission_item) - _mission_item_previous_alt);
float grad = -delta_alt/(_distance_current_previous - _navigator->get_acceptance_radius(_mission_item.acceptance_radius));
float a = _mission_item_previous_alt - grad * _distance_current_previous;
pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy;

2
src/modules/navigator/mission.h
View File

@ -132,6 +132,8 @@ private:
*/
void altitude_sp_foh_reset();
int get_absolute_altitude_for_item(struct mission_item_s &mission_item);
/**
* Read current or next mission item from the dataman and watch out for DO_JUMPS
* @return true if successful

4
src/modules/navigator/mission_block.cpp
View File

@ -123,12 +123,12 @@ MissionBlock::is_mission_item_reached()
* Therefore the item is marked as reached once the system reaches the loiter
* radius (+ some margin). Time inside and turn count is handled elsewhere.
*/
if (dist >= 0.0f && dist <= _mission_item.loiter_radius * 1.2f) {
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)) {
_waypoint_position_reached = true;
}
} else {
/* for normal mission items used their acceptance radius */
if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) {
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) {
_waypoint_position_reached = true;
}
}

38
src/modules/navigator/mission_feasibility_checker.cpp
View File

@ -63,42 +63,43 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab
}
bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
{
bool failed = false;
/* Init if not done yet */
init();
/* Open mavlink fd */
if (_mavlink_fd < 0) {
/* try to open the mavlink log device every once in a while */
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
}
_mavlink_fd = mavlink_fd;
// check if all mission item commands are supported
failed |= !checkMissionItemValidity(dm_current, nMissionItems);
if (isRotarywing)
failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt);
else
failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt);
if (isRotarywing) {
failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid);
} else {
failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid);
}
if (!failed) {
mavlink_log_info(_mavlink_fd, "Mission checked and ready.");
}
return !failed;
}
bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
{
/* Perform checks and issue feedback to the user for all checks */
bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt);
bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid);
/* Mission is only marked as feasible if all checks return true */
return (resGeofence && resHomeAltitude);
}
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
{
/* Update fixed wing navigation capabilites */
updateNavigationCapabilities();
@ -107,7 +108,7 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre
/* Perform checks and issue feedback to the user for all checks */
bool resLanding = checkFixedWingLanding(dm_current, nMissionItems);
bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt);
bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid);
/* Mission is only marked as feasible if all checks return true */
return (resLanding && resGeofence && resHomeAltitude);
@ -136,7 +137,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
return true;
}
bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error)
bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool throw_error)
{
/* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */
for (size_t i = 0; i < nMissionItems; i++) {
@ -152,6 +153,12 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
}
}
/* always reject relative alt without home set */
if (missionitem.altitude_is_relative && !home_valid) {
mavlink_log_critical(_mavlink_fd, "Rejecting Mission: No home pos, WP %d uses rel alt", i);
return false;
}
/* calculate the global waypoint altitude */
float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude;
@ -197,7 +204,6 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
return false;
}
}
mavlink_log_info(_mavlink_fd, "Mission ready.");
return true;
}

10
src/modules/navigator/mission_feasibility_checker.h
View File

@ -61,16 +61,16 @@ private:
/* Checks for all airframes */
bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error = false);
bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool throw_error = false);
bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems);
/* Checks specific to fixedwing airframes */
bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt);
bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid);
bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems);
void updateNavigationCapabilities();
/* Checks specific to rotarywing airframes */
bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt);
bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid);
public:
MissionFeasibilityChecker();
@ -79,8 +79,8 @@ public:
/*
* Returns true if mission is feasible and false otherwise
*/
bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
float home_alt);
bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
float home_alt, bool home_valid);
};

4
src/modules/navigator/mission_params.c
View File

@ -81,7 +81,7 @@ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1);
* @max 1000
* @group Mission
*/
PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500);
PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 900);
/**
* Altitude setpoint mode
@ -94,7 +94,7 @@ PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500);
* @max 1
* @group Mission
*/
PARAM_DEFINE_INT32(MIS_ALTMODE, 0);
PARAM_DEFINE_INT32(MIS_ALTMODE, 1);
/**
* Multirotor only. Yaw setpoint mode.

18
src/modules/navigator/navigator.h
View File

@ -134,6 +134,7 @@ public:
struct vehicle_gps_position_s* get_gps_position() { return &_gps_pos; }
struct sensor_combined_s* get_sensor_combined() { return &_sensor_combined; }
struct home_position_s* get_home_position() { return &_home_pos; }
bool home_position_valid() { return _home_position_set; }
struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
struct mission_result_s* get_mission_result() { return &_mission_result; }
struct geofence_result_s* get_geofence_result() { return &_geofence_result; }
@ -144,7 +145,22 @@ public:
Geofence& get_geofence() { return _geofence; }
bool get_can_loiter_at_sp() { return _can_loiter_at_sp; }
float get_loiter_radius() { return _param_loiter_radius.get(); }
float get_acceptance_radius() { return _param_acceptance_radius.get(); }
/**
* Get the acceptance radius
*
* @return the distance at which the next waypoint should be used
*/
float get_acceptance_radius();
/**
* Get the acceptance radius given the mission item preset radius
*
* @param mission_item_radius the radius to use in case the controller-derived radius is smaller
*
* @return the distance at which the next waypoint should be used
*/
float get_acceptance_radius(float mission_item_radius);
int get_mavlink_fd() { return _mavlink_fd; }
private:

25
src/modules/navigator/navigator_main.cpp
View File

@ -212,7 +212,10 @@ Navigator::home_position_update()
if (updated) {
orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
_home_position_set = true;
if (_home_pos.timestamp > 0) {
_home_position_set = true;
}
}
}
@ -574,6 +577,26 @@ Navigator::publish_position_setpoint_triplet()
}
}
float
Navigator::get_acceptance_radius()
{
return get_acceptance_radius(_param_acceptance_radius.get());
}
float
Navigator::get_acceptance_radius(float mission_item_radius)
{
float radius = mission_item_radius;
if (hrt_elapsed_time(&_nav_caps.timestamp) < 5000000) {
if (_nav_caps.turn_distance > radius) {
radius = _nav_caps.turn_distance;
}
}
return radius;
}
void Navigator::add_fence_point(int argc, char *argv[])
{
_geofence.addPoint(argc, argv);

99
src/modules/px4iofirmware/controls.c
View File

@ -64,6 +64,10 @@ static int _dsm_fd;
static uint16_t rc_value_override = 0;
#ifdef ADC_RSSI
static unsigned _rssi_adc_counts = 0;
#endif
bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated)
{
perf_begin(c_gather_dsm);
@ -71,17 +75,22 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
uint8_t n_bytes = 0;
uint8_t *bytes;
*dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes);
if (*dsm_updated) {
r_raw_rc_count = temp_count & 0x7fff;
if (temp_count & 0x8000)
if (temp_count & 0x8000) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
else
} else {
r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
}
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
perf_end(c_gather_dsm);
/* get data from FD and attempt to parse with DSM and ST24 libs */
@ -94,7 +103,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
/* set updated flag if one complete packet was parsed */
st24_rssi = RC_INPUT_RSSI_MAX;
*st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count,
&st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
&st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
}
if (*st24_updated) {
@ -121,7 +130,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
/* set updated flag if one complete packet was parsed */
sumd_rssi = RC_INPUT_RSSI_MAX;
*sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count,
&sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
&sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
}
if (*sumd_updated) {
@ -170,7 +179,8 @@ controls_init(void)
}
void
controls_tick() {
controls_tick()
{
/*
* Gather R/C control inputs from supported sources.
@ -184,19 +194,24 @@ controls_tick() {
uint16_t rssi = 0;
#ifdef ADC_RSSI
if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) {
unsigned counts = adc_measure(ADC_RSSI);
if (counts != 0xffff) {
/* use 1:1 scaling on 3.3V ADC input */
unsigned mV = counts * 3300 / 4096;
/* scale to 0..253 and lowpass */
rssi = (rssi * 0.99f) + ((mV / (3300 / RC_INPUT_RSSI_MAX)) * 0.01f);
if (counts != 0xffff) {
/* low pass*/
_rssi_adc_counts = (_rssi_adc_counts * 0.998f) + (counts * 0.002f);
/* use 1:1 scaling on 3.3V, 12-Bit ADC input */
unsigned mV = _rssi_adc_counts * 3300 / 4095;
/* scale to 0..100 (RC_INPUT_RSSI_MAX == 100) */
rssi = (mV * RC_INPUT_RSSI_MAX / 3300);
if (rssi > RC_INPUT_RSSI_MAX) {
rssi = RC_INPUT_RSSI_MAX;
}
}
}
#endif
/* zero RSSI if signal is lost */
@ -207,21 +222,26 @@ controls_tick() {
perf_begin(c_gather_dsm);
bool dsm_updated, st24_updated, sumd_updated;
(void)dsm_port_input(&rssi, &dsm_updated, &st24_updated, &sumd_updated);
if (dsm_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
}
if (st24_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24;
}
if (sumd_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD;
}
perf_end(c_gather_dsm);
perf_begin(c_gather_sbus);
bool sbus_failsafe, sbus_frame_drop;
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS);
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop,
PX4IO_RC_INPUT_CHANNELS);
if (sbus_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
@ -231,17 +251,19 @@ controls_tick() {
if (sbus_frame_drop) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
sbus_rssi = RC_INPUT_RSSI_MAX / 2;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
}
if (sbus_failsafe) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
/* set RSSI to an emulated value if ADC RSSI is off */
/* set RSSI to an emulated value if ADC RSSI is off */
if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
rssi = sbus_rssi;
}
@ -257,17 +279,20 @@ controls_tick() {
*/
perf_begin(c_gather_ppm);
bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);
if (ppm_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
perf_end(c_gather_ppm);
/* limit number of channels to allowable data size */
if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) {
r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
}
/* store RSSI */
r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi;
@ -308,10 +333,13 @@ controls_tick() {
/*
* 1) Constrain to min/max values, as later processing depends on bounds.
*/
if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) {
raw = conf[PX4IO_P_RC_CONFIG_MIN];
if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
}
if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) {
raw = conf[PX4IO_P_RC_CONFIG_MAX];
}
/*
* 2) Scale around the mid point differently for lower and upper range.
@ -330,10 +358,12 @@ controls_tick() {
* DO NOT REMOVE OR ALTER STEP 1!
*/
if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(
conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(
conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
} else {
/* in the configured dead zone, output zero */
@ -347,6 +377,7 @@ controls_tick() {
/* and update the scaled/mapped version */
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
if (mapped < PX4IO_CONTROL_CHANNELS) {
/* invert channel if pitch - pulling the lever down means pitching up by convention */
@ -360,6 +391,7 @@ controls_tick() {
if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) ||
((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
@ -389,6 +421,7 @@ controls_tick() {
/* if we have enough channels (5) to control the vehicle, the mapping is ok */
if (assigned_channels > 4) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
}
@ -408,9 +441,9 @@ controls_tick() {
/* clear the input-kind flags here */
r_status_flags &= ~(
PX4IO_P_STATUS_FLAGS_RC_PPM |
PX4IO_P_STATUS_FLAGS_RC_DSM |
PX4IO_P_STATUS_FLAGS_RC_SBUS);
PX4IO_P_STATUS_FLAGS_RC_PPM |
PX4IO_P_STATUS_FLAGS_RC_DSM |
PX4IO_P_STATUS_FLAGS_RC_SBUS);
}
@ -427,8 +460,8 @@ controls_tick() {
if (rc_input_lost) {
/* Clear the RC input status flag, clear manual override flag */
r_status_flags &= ~(
PX4IO_P_STATUS_FLAGS_OVERRIDE |
PX4IO_P_STATUS_FLAGS_RC_OK);
PX4IO_P_STATUS_FLAGS_OVERRIDE |
PX4IO_P_STATUS_FLAGS_RC_OK);
/* flag raw RC as lost */
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_RC_OK);
@ -451,9 +484,9 @@ controls_tick() {
* Override is enabled if either the hardcoded channel / value combination
* is selected, or the AP has requested it.
*/
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
bool override = false;
@ -464,8 +497,9 @@ controls_tick() {
* requested override.
*
*/
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH))
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) {
override = true;
}
/*
if the FMU is dead then enable override if we have a
@ -473,20 +507,23 @@ controls_tick() {
*/
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
(r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
override = true;
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
override = true;
}
if (override) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
/* mix new RC input control values to servos */
if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated)
if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated) {
mixer_tick();
}
} else {
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
}
} else {
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
}
@ -512,8 +549,10 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
/* PPM data exists, copy it */
*num_values = ppm_decoded_channels;
if (*num_values > PX4IO_RC_INPUT_CHANNELS)
if (*num_values > PX4IO_RC_INPUT_CHANNELS) {
*num_values = PX4IO_RC_INPUT_CHANNELS;
}
for (unsigned i = 0; ((i < *num_values) && (i < PPM_MAX_CHANNELS)); i++) {
values[i] = ppm_buffer[i];

21
src/modules/px4iofirmware/mixer.cpp
View File

@ -318,6 +318,13 @@ mixer_callback(uintptr_t handle,
case MIX_OVERRIDE:
if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) {
control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
if (control_group == 0 && control_index == 0) {
control += REG_TO_FLOAT(r_setup_trim_roll);
} else if (control_group == 0 && control_index == 1) {
control += REG_TO_FLOAT(r_setup_trim_pitch);
} else if (control_group == 0 && control_index == 2) {
control += REG_TO_FLOAT(r_setup_trim_yaw);
}
break;
}
return -1;
@ -326,6 +333,13 @@ mixer_callback(uintptr_t handle,
/* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */
if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) {
control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
if (control_group == 0 && control_index == 0) {
control += REG_TO_FLOAT(r_setup_trim_roll);
} else if (control_group == 0 && control_index == 1) {
control += REG_TO_FLOAT(r_setup_trim_pitch);
} else if (control_group == 0 && control_index == 2) {
control += REG_TO_FLOAT(r_setup_trim_yaw);
}
break;
} else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) {
control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
@ -339,6 +353,13 @@ mixer_callback(uintptr_t handle,
return -1;
}
/* limit output */
if (control > 1.0f) {
control = 1.0f;
} else if (control < -1.0f) {
control = -1.0f;
}
return 0;
}

3
src/modules/px4iofirmware/protocol.h
View File

@ -234,6 +234,9 @@ enum { /* DSM bind states */
#define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */
#define PX4IO_P_SETUP_PWM_REVERSE 15 /**< Bitmask to reverse PWM channels 1-8 */
#define PX4IO_P_SETUP_TRIM_ROLL 16 /**< Roll trim, in actuator units */
#define PX4IO_P_SETUP_TRIM_PITCH 17 /**< Pitch trim, in actuator units */
#define PX4IO_P_SETUP_TRIM_YAW 18 /**< Yaw trim, in actuator units */
/* autopilot control values, -10000..10000 */
#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */

4
src/modules/px4iofirmware/px4io.h
View File

@ -113,6 +113,10 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
#define r_setup_pwm_reverse r_page_setup[PX4IO_P_SETUP_PWM_REVERSE]
#define r_setup_trim_roll r_page_setup[PX4IO_P_SETUP_TRIM_ROLL]
#define r_setup_trim_pitch r_page_setup[PX4IO_P_SETUP_TRIM_PITCH]
#define r_setup_trim_yaw r_page_setup[PX4IO_P_SETUP_TRIM_YAW]
#define r_control_values (&r_page_controls[0])
/*

9
src/modules/px4iofirmware/registers.c
View File

@ -174,6 +174,9 @@ volatile uint16_t r_page_setup[] =
[PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0,
[PX4IO_P_SETUP_PWM_REVERSE] = 0,
[PX4IO_P_SETUP_TRIM_ROLL] = 0,
[PX4IO_P_SETUP_TRIM_PITCH] = 0,
[PX4IO_P_SETUP_TRIM_YAW] = 0
};
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
@ -627,6 +630,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
r_page_setup[PX4IO_P_SETUP_PWM_REVERSE] = value;
break;
case PX4IO_P_SETUP_TRIM_ROLL:
case PX4IO_P_SETUP_TRIM_PITCH:
case PX4IO_P_SETUP_TRIM_YAW:
r_page_setup[offset] = value;
break;
default:
return -1;
}

6
src/modules/systemlib/system_params.c
View File

@ -88,9 +88,9 @@ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2);
* Companion computer interface
*
* Configures the baud rate of the companion computer interface.
* Set to zero to disable, set to 921600 to enable.
* CURRENTLY ONLY SUPPORTS 921600 BAUD! Use extras.txt for
* other baud rates.
* Set to zero to disable, set to these values to enable (NO OTHER VALUES SUPPORTED!)
* 921600: enables onboard mode at 921600 baud, 8N1. 57600: enables onboard mode at 57600 baud, 8N1.
* 157600: enables OSD mode at 57600 baud, 8N1.
*
* @min 0
* @max 921600

70
src/modules/uORB/topics/navigation_capabilities.h
View File

@ -1,70 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file navigation_capabilities.h
*
* Definition of navigation capabilities uORB topic.
*/
#ifndef TOPIC_NAVIGATION_CAPABILITIES_H_
#define TOPIC_NAVIGATION_CAPABILITIES_H_
#include "../uORB.h"
#include <stdint.h>
/**
* @addtogroup topics
* @{
*/
/**
* Airspeed
*/
struct navigation_capabilities_s {
float turn_distance; /**< the optimal distance to a waypoint to switch to the next */
/* Landing parameters: see fw_pos_control_l1/landingslope.h */
float landing_horizontal_slope_displacement;
float landing_slope_angle_rad;
float landing_flare_length;
};
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(navigation_capabilities);
#endif

2
src/modules/uavcan/sensors/baro.hpp
View File

@ -58,7 +58,7 @@ public:
int init() override;
private:
ssize_t read(struct file *filp, char *buffer, size_t buflen);
ssize_t read(struct file *filp, char *buffer, size_t buflen);
int ioctl(struct file *filp, int cmd, unsigned long arg) override;
void air_pressure_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &msg);