Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-11 18:03:41 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/master' into offboard2_externalsetpointmessages

Browse Source 
Conflicts:
	src/modules/mavlink/mavlink_main.cpp
This commit is contained in:
Thomas Gubler
2014-07-23 09:31:28 +02:00
parent 8bb1f9a4bf a7d2963e2b
commit 825b84fa3b
75 changed files with 2163 additions and 557 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitmodules
+3
View File
@@ -4,3 +4,6 @@
[submodule "NuttX"]
path = NuttX
url = git://github.com/PX4/NuttX.git
[submodule "uavcan"]
path = uavcan
url = git://github.com/pavel-kirienko/uavcan.git
Makefile
+3
View File
@@ -212,6 +212,9 @@ endif
$(NUTTX_SRC):
$(Q) ($(PX4_BASE)/Tools/check_submodules.sh)
$(UAVCAN_DIR):
$(Q) (./Tools/check_submodules.sh)
.PHONY: checksubmodules
checksubmodules:
$(Q) ($(PX4_BASE)/Tools/check_submodules.sh)
ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil
-9
View File
@@ -26,15 +26,6 @@ then
param set FW_RR_P 0.1
param set FW_R_LIM 45
param set FW_R_RMAX 0
param set FW_T_CLMB_MAX 5
param set FW_T_HRATE_P 0.02
param set FW_T_PTCH_DAMP 0
param set FW_T_RLL2THR 15
param set FW_T_SINK_MAX 5
param set FW_T_SINK_MIN 2
param set FW_T_SRATE_P 0.01
param set FW_T_TIME_CONST 3
param set FW_T_VERT_ACC 7
param set FW_YR_FF 0.0
param set FW_YR_I 0
param set FW_YR_IMAX 0.2
ROMFS/px4fmu_common/init.d/3031_phantom
-4
View File
@@ -30,10 +30,6 @@ then
param set FW_RR_P 0.08
param set FW_R_LIM 50
param set FW_R_RMAX 0
param set FW_T_HRATE_P 0.01
param set FW_T_RLL2THR 15
param set FW_T_SRATE_P 0.01
param set FW_T_TIME_CONST 5
fi
set MIXER phantom
ROMFS/px4fmu_common/init.d/3032_skywalker_x5
-4
View File
@@ -30,10 +30,6 @@ then
param set FW_RR_P 0.03
param set FW_R_LIM 60
param set FW_R_RMAX 0
param set FW_T_HRATE_P 0.01
param set FW_T_RLL2THR 15
param set FW_T_SRATE_P 0.01
param set FW_T_TIME_CONST 5
fi
set MIXER FMU_X5
ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha
-4
View File
@@ -33,10 +33,6 @@ then
param set FW_RR_P 0.03
param set FW_R_LIM 60
param set FW_R_RMAX 0
param set FW_T_HRATE_P 0.01
param set FW_T_RLL2THR 15
param set FW_T_SRATE_P 0.01
param set FW_T_TIME_CONST 5
fi
set MIXER FMU_Q
ROMFS/px4fmu_common/init.d/4012_quad_x_can
+27
View File
@@ -0,0 +1,27 @@
#!nsh
#
# F450-sized quadrotor with CAN
#
# Lorenz Meier <lm@inf.ethz.ch>
#
sh /etc/init.d/4001_quad_x
if [ $DO_AUTOCONFIG == yes ]
then
# TODO REVIEW
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.1
param set MC_ROLLRATE_I 0.0
param set MC_ROLLRATE_D 0.003
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.1
param set MC_PITCHRATE_I 0.0
param set MC_PITCHRATE_D 0.003
param set MC_YAW_P 2.8
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_D 0.0
fi
set OUTPUT_MODE uavcan_esc
ROMFS/px4fmu_common/init.d/rc.autostart
+5
View File
@@ -136,6 +136,11 @@ then
sh /etc/init.d/4011_dji_f450
fi
if param compare SYS_AUTOSTART 4012
then
sh /etc/init.d/4012_quad_x_can
fi
if param compare SYS_AUTOSTART 4020
then
sh /etc/init.d/4020_hk_micro_pcb
ROMFS/px4fmu_common/init.d/rc.interface
+5
View File
@@ -24,6 +24,11 @@ then
else
set OUTPUT_DEV /dev/pwm_output
fi
if [ $OUTPUT_MODE == uavcan_esc ]
then
set OUTPUT_DEV /dev/uavcan/esc
fi
if mixer load $OUTPUT_DEV $MIXER_FILE
then
ROMFS/px4fmu_common/init.d/rcS
+11 -1
View File
@@ -297,7 +297,17 @@ then
# If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output
if [ $OUTPUT_MODE != none ]
then
if [ $OUTPUT_MODE == io ]
if [ $OUTPUT_MODE == uavcan_esc ]
then
if uavcan start 1
then
echo "CAN UP"
else
echo "CAN ERR"
fi
fi
if [ $OUTPUT_MODE == io -o $OUTPUT_MODE == uavcan_esc ]
then
echo "[init] Use PX4IO PWM as primary output"
if px4io start
Tools/check_submodules.sh
+24
View File
@@ -53,4 +53,28 @@ else
git submodule update;
fi
if [ -d uavcan ]
then
STATUSRETVAL=$(git submodule summary | grep -A20 -i uavcan | grep "<")
if [ -z "$STATUSRETVAL" ]
then
echo "Checked uavcan submodule, correct version found"
else
echo ""
echo ""
echo "uavcan sub repo not at correct version. Try 'git submodule update'"
echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
echo ""
echo ""
echo "New commits required:"
echo "$(git submodule summary)"
echo ""
exit 1
fi
else
git submodule init
git submodule update
fi
exit 0
Tools/tests-host/data/fit_linear_voltage.m
+14
View File
@@ -0,0 +1,14 @@
close all;
clear all;
M = importdata('px4io_v1.3.csv');
voltage = M.data(:, 1);
counts = M.data(:, 2);
plot(counts, voltage, 'b*-', 'LineWidth', 2, 'MarkerSize', 15);
coeffs = polyfit(counts, voltage, 1);
fittedC = linspace(min(counts), max(counts), 500);
fittedV = polyval(coeffs, fittedC);
hold on
plot(fittedC, fittedV, 'r-', 'LineWidth', 3);
slope = coeffs(1)
y_intersection = coeffs(2)
Tools/tests-host/data/px4io_v1.3.csv
+70
View File
@@ -0,0 +1,70 @@
voltage, counts
4.3, 950
4.4, 964
4.5, 986
4.6, 1009
4.7, 1032
4.8, 1055
4.9, 1078
5.0, 1101
5.2, 1124
5.3, 1148
5.4, 1171
5.5, 1195
6.0, 1304
6.1, 1329
6.2, 1352
7.0, 1517
7.1, 1540
7.2, 1564
7.3, 1585
7.4, 1610
7.5, 1636
8.0, 1728
8.1, 1752
8.2, 1755
8.3, 1798
8.4, 1821
9.0, 1963
9.1, 1987
9.3, 2010
9.4, 2033
10.0, 2174
10.1, 2198
10.2, 2221
10.3, 2245
10.4, 2268
11.0, 2385
11.1, 2409
11.2, 2432
11.3, 2456
11.4, 2480
11.5, 2502
11.6, 2526
11.7, 2550
11.8, 2573
11.9, 2597
12.0, 2621
12.1, 2644
12.3, 2668
12.4, 2692
12.5, 2716
12.6, 2737
12.7, 2761
13.0, 2832
13.5, 2950
13.6, 2973
14.1, 3068
14.2, 3091
14.7, 3209
15.0, 3280
15.1, 3304
15.5, 3374
15.6, 3397
15.7, 3420
16.0, 3492
16.1, 3514
16.2, 3538
16.9, 3680
17.0, 3704
17.1, 3728
1 voltage counts
2 4.3 950
3 4.4 964
4 4.5 986
5 4.6 1009
6 4.7 1032
7 4.8 1055
8 4.9 1078
9 5.0 1101
10 5.2 1124
11 5.3 1148
12 5.4 1171
13 5.5 1195
14 6.0 1304
15 6.1 1329
16 6.2 1352
17 7.0 1517
18 7.1 1540
19 7.2 1564
20 7.3 1585
21 7.4 1610
22 7.5 1636
23 8.0 1728
24 8.1 1752
25 8.2 1755
26 8.3 1798
27 8.4 1821
28 9.0 1963
29 9.1 1987
30 9.3 2010
31 9.4 2033
32 10.0 2174
33 10.1 2198
34 10.2 2221
35 10.3 2245
36 10.4 2268
37 11.0 2385
38 11.1 2409
39 11.2 2432
40 11.3 2456
41 11.4 2480
42 11.5 2502
43 11.6 2526
44 11.7 2550
45 11.8 2573
46 11.9 2597
47 12.0 2621
48 12.1 2644
49 12.3 2668
50 12.4 2692
51 12.5 2716
52 12.6 2737
53 12.7 2761
54 13.0 2832
55 13.5 2950
56 13.6 2973
57 14.1 3068
58 14.2 3091
59 14.7 3209
60 15.0 3280
61 15.1 3304
62 15.5 3374
63 15.6 3397
64 15.7 3420
65 16.0 3492
66 16.1 3514
67 16.2 3538
68 16.9 3680
69 17.0 3704
70 17.1 3728
makefiles/config_px4fmu-v2_default.mk
+1
View File
@@ -74,6 +74,7 @@ MODULES += modules/commander
MODULES += modules/navigator
MODULES += modules/mavlink
MODULES += modules/gpio_led
MODULES += modules/uavcan
#
# Estimation modules (EKF/ SO3 / other filters)
makefiles/nuttx.mk
+1
View File
@@ -64,6 +64,7 @@ LDSCRIPT += $(NUTTX_EXPORT_DIR)build/ld.script
# Add directories from the NuttX export to the relevant search paths
#
INCLUDE_DIRS += $(NUTTX_EXPORT_DIR)include \
$(NUTTX_EXPORT_DIR)include/cxx \
$(NUTTX_EXPORT_DIR)arch/chip \
$(NUTTX_EXPORT_DIR)arch/common
makefiles/setup.mk
+1
View File
@@ -49,6 +49,7 @@ export NUTTX_SRC = $(abspath $(PX4_BASE)/NuttX/nuttx)/
export MAVLINK_SRC = $(abspath $(PX4_BASE)/mavlink/include/mavlink/v1.0)/
export NUTTX_APP_SRC = $(abspath $(PX4_BASE)/NuttX/apps)/
export MAVLINK_SRC = $(abspath $(PX4_BASE)/mavlink)/
export UAVCAN_DIR = $(abspath $(PX4_BASE)/uavcan)/
export ROMFS_SRC = $(abspath $(PX4_BASE)/ROMFS)/
export IMAGE_DIR = $(abspath $(PX4_BASE)/Images)/
export BUILD_DIR = $(abspath $(PX4_BASE)/Build)/
makefiles/toolchain_gnu-arm-eabi.mk
+1 -1
View File
@@ -121,7 +121,7 @@ INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH))
# Language-specific flags
#
ARCHCFLAGS = -std=gnu99
ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x
ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics
# Generic warnings
#
mavlink/include/mavlink/v1.0
+1 -1
Submodule mavlink/include/mavlink/v1.0 updated: d1ebe85eb6...04b1ad5b28
src/drivers/airspeed/module.mk
+2
View File
@@ -36,3 +36,5 @@
#
SRCS = airspeed.cpp
MAXOPTIMIZATION = -Os
src/drivers/bma180/module.mk
+2
View File
@@ -38,3 +38,5 @@
MODULE_COMMAND = bma180
SRCS = bma180.cpp
MAXOPTIMIZATION = -Os
src/drivers/drv_pwm_output.h
+3
View File
@@ -202,6 +202,9 @@ ORB_DECLARE(output_pwm);
/** force safety switch off (to disable use of safety switch) */
#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23)
/** force failsafe mode (failsafe values are set immediately even if failsafe condition not met) */
#define PWM_SERVO_SET_FORCE_FAILSAFE _IOC(_PWM_SERVO_BASE, 24)
/*
*
*
src/drivers/ets_airspeed/module.mk
+2
View File
@@ -40,3 +40,5 @@ MODULE_COMMAND = ets_airspeed
SRCS = ets_airspeed.cpp
MODULE_STACKSIZE = 1200
MAXOPTIMIZATION = -Os
src/drivers/hil/module.mk
+2
View File
@@ -38,3 +38,5 @@
MODULE_COMMAND = hil
SRCS = hil.cpp
MAXOPTIMIZATION = -Os
src/drivers/led/module.mk
+2
View File
@@ -36,3 +36,5 @@
#
SRCS = led.cpp
MAXOPTIMIZATION = -Os
src/drivers/md25/module.mk
+2
View File
@@ -40,3 +40,5 @@ MODULE_COMMAND = md25
SRCS = md25.cpp \
md25_main.cpp
MAXOPTIMIZATION = -Os
src/drivers/meas_airspeed/module.mk
+2
View File
@@ -42,3 +42,5 @@ SRCS = meas_airspeed.cpp
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++
MAXOPTIMIZATION = -Os
src/drivers/ms5611/module.mk
+2
View File
@@ -38,3 +38,5 @@
MODULE_COMMAND = ms5611
SRCS = ms5611.cpp ms5611_spi.cpp ms5611_i2c.cpp
MAXOPTIMIZATION = -Os
src/drivers/px4flow/module.mk
+2
View File
@@ -38,3 +38,5 @@
MODULE_COMMAND = px4flow
SRCS = px4flow.cpp
MAXOPTIMIZATION = -Os
src/drivers/px4io/px4io.cpp
+28 -9
View File
@@ -1149,6 +1149,12 @@ PX4IO::io_set_arming_state()
clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
}
if (armed.force_failsafe) {
set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
} else {
clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
}
if (armed.ready_to_arm) {
set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
@@ -2002,7 +2008,7 @@ PX4IO::print_status(bool extended_status)
((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "")
);
uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
printf("arming 0x%04x%s%s%s%s%s%s%s\n",
printf("arming 0x%04x%s%s%s%s%s%s%s%s\n",
arming,
((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"),
((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"),
@@ -2010,7 +2016,9 @@ PX4IO::print_status(bool extended_status)
((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""),
((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""),
((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : ""));
((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : ""),
((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : "")
);
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
@@ -2222,6 +2230,17 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
break;
case PWM_SERVO_SET_FORCE_FAILSAFE:
/* force failsafe mode instantly */
if (arg == 0) {
/* clear force failsafe flag */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE, 0);
} else {
/* set force failsafe flag */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE);
}
break;
case DSM_BIND_START:
/* only allow DSM2, DSM-X and DSM-X with more than 7 channels */
@@ -2424,7 +2443,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
break;
case PX4IO_CHECK_CRC: {
/* check IO firmware CRC against passed value */
/* check IO firmware CRC against passed value */
uint32_t io_crc = 0;
ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC, (uint16_t *)&io_crc, 2);
if (ret != OK)
@@ -2684,7 +2703,7 @@ checkcrc(int argc, char *argv[])
int fd = open(argv[1], O_RDONLY);
if (fd == -1) {
printf("open of %s failed - %d\n", argv[1], errno);
exit(1);
exit(1);
}
const uint32_t app_size_max = 0xf000;
uint32_t fw_crc = 0;
@@ -2699,7 +2718,7 @@ checkcrc(int argc, char *argv[])
close(fd);
while (nbytes < app_size_max) {
uint8_t b = 0xff;
fw_crc = crc32part(&b, 1, fw_crc);
fw_crc = crc32part(&b, 1, fw_crc);
nbytes++;
}
@@ -2712,7 +2731,7 @@ checkcrc(int argc, char *argv[])
if (ret != OK) {
printf("check CRC failed - %d\n", ret);
exit(1);
exit(1);
}
printf("CRCs match\n");
exit(0);
@@ -2742,12 +2761,12 @@ bind(int argc, char *argv[])
pulses = DSMX_BIND_PULSES;
else if (!strcmp(argv[2], "dsmx8"))
pulses = DSMX8_BIND_PULSES;
else
else
errx(1, "unknown parameter %s, use dsm2, dsmx or dsmx8", argv[2]);
// Test for custom pulse parameter
if (argc > 3)
pulses = atoi(argv[3]);
if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
errx(1, "system must not be armed");
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
@@ -2949,7 +2968,7 @@ lockdown(int argc, char *argv[])
(void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 0);
warnx("ACTUATORS ARE NOW SAFE IN HIL.");
}
} else {
errx(1, "driver not loaded, exiting");
}
src/drivers/roboclaw/module.mk
+2
View File
@@ -39,3 +39,5 @@ MODULE_COMMAND = roboclaw
SRCS = roboclaw_main.cpp \
RoboClaw.cpp
MAXOPTIMIZATION = -Os
src/modules/commander/commander.cpp
+26 -18
View File
@@ -546,24 +546,19 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
}
break;
#if 0
/* Flight termination */
case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
//XXX: to enable the parachute, a param needs to be set
//xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
if (armed_local->armed && cmd->param3 > 0.5 && parachute_enabled) {
transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
case VEHICLE_CMD_DO_FLIGHTTERMINATION: {
if (cmd->param1 > 0.5f) {
//XXX update state machine?
armed_local->force_failsafe = true;
warnx("forcing failsafe");
} else {
/* reject parachute depoyment not armed */
cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
armed_local->force_failsafe = false;
warnx("disabling failsafe");
}
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
}
break;
#endif
case VEHICLE_CMD_DO_SET_HOME: {
bool use_current = cmd->param1 > 0.5f;
@@ -940,6 +935,11 @@ int commander_thread_main(int argc, char *argv[])
struct system_power_s system_power;
memset(&system_power, 0, sizeof(system_power));
/* Subscribe to actuator controls (outputs) */
int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
struct actuator_controls_s actuator_controls;
memset(&actuator_controls, 0, sizeof(actuator_controls));
control_status_leds(&status, &armed, true);
/* now initialized */
@@ -1221,13 +1221,17 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
/* only consider battery voltage if system has been running 2s and battery voltage is valid */
if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
status.battery_voltage = battery.voltage_filtered_v;
status.battery_current = battery.current_a;
status.condition_battery_voltage_valid = true;
status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah);
/* get throttle (if armed), as we only care about energy negative throttle also counts */
float throttle = (armed.armed) ? fabsf(actuator_controls.control[3]) : 0.0f;
status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah, throttle);
}
}
@@ -1289,13 +1293,13 @@ int commander_thread_main(int argc, char *argv[])
}
/* if battery voltage is getting lower, warn using buzzer, etc. */
if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) {
if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) {
low_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.09f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
/* critical battery voltage, this is rather an emergency, change state machine */
critical_battery_voltage_actions_done = true;
mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
@@ -1434,8 +1438,12 @@ int commander_thread_main(int argc, char *argv[])
arming_state_changed = true;
} else if (arming_ret == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
mavlink_log_critical(mavlink_fd, "arming state transition denied");
/*
* the arming transition can be denied to a number of reasons:
* - pre-flight check failed (sensors not ok or not calibrated)
* - safety not disabled
* - system not in manual mode
*/
tune_negative(true);
}
src/modules/commander/commander_helper.cpp
+11 -6
View File
@@ -281,15 +281,17 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern)
}
}
float battery_remaining_estimate_voltage(float voltage, float discharged)
float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized)
{
float ret = 0;
static param_t bat_v_empty_h;
static param_t bat_v_full_h;
static param_t bat_n_cells_h;
static param_t bat_capacity_h;
static float bat_v_empty = 3.2f;
static float bat_v_full = 4.0f;
static param_t bat_v_load_drop_h;
static float bat_v_empty = 3.4f;
static float bat_v_full = 4.2f;
static float bat_v_load_drop = 0.06f;
static int bat_n_cells = 3;
static float bat_capacity = -1.0f;
static bool initialized = false;
@@ -297,23 +299,26 @@ float battery_remaining_estimate_voltage(float voltage, float discharged)
if (!initialized) {
bat_v_empty_h = param_find("BAT_V_EMPTY");
bat_v_full_h = param_find("BAT_V_FULL");
bat_v_full_h = param_find("BAT_V_CHARGED");
bat_n_cells_h = param_find("BAT_N_CELLS");
bat_capacity_h = param_find("BAT_CAPACITY");
bat_v_load_drop_h = param_find("BAT_V_LOAD_DROP");
initialized = true;
}
if (counter % 100 == 0) {
param_get(bat_v_empty_h, &bat_v_empty);
param_get(bat_v_full_h, &bat_v_full);
param_get(bat_v_load_drop_h, &bat_v_load_drop);
param_get(bat_n_cells_h, &bat_n_cells);
param_get(bat_capacity_h, &bat_capacity);
}
counter++;
/* remaining charge estimate based on voltage */
float remaining_voltage = (voltage - bat_n_cells * bat_v_empty) / (bat_n_cells * (bat_v_full - bat_v_empty));
/* remaining charge estimate based on voltage and internal resistance (drop under load) */
float bat_v_full_dynamic = bat_v_full - (bat_v_load_drop * throttle_normalized);
float remaining_voltage = (voltage - (bat_n_cells * bat_v_empty)) / (bat_n_cells * (bat_v_full_dynamic - bat_v_empty));
if (bat_capacity > 0.0f) {
/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */
src/modules/commander/commander_helper.h
+2 -1
View File
@@ -80,8 +80,9 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern);
*
* @param voltage the current battery voltage
* @param discharged the discharged capacity
* @param throttle_normalized the normalized throttle magnitude from 0 to 1. Negative throttle should be converted to this range as well, as it consumes energy.
* @return the estimated remaining capacity in 0..1
*/
float battery_remaining_estimate_voltage(float voltage, float discharged);
float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized);
#endif /* COMMANDER_HELPER_H_ */
src/modules/commander/commander_params.c
+11 -1
View File
@@ -65,7 +65,17 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
*
* @group Battery Calibration
*/
PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f);
PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f);
/**
* Voltage drop per cell on 100% load
*
* This implicitely defines the internal resistance
* to maximum current ratio and assumes linearity.
*
* @group Battery Calibration
*/
PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f);
/**
* Number of cells.
src/modules/commander/state_machine_helper.cpp
+17 -9
View File
@@ -110,8 +110,8 @@ arming_state_transition(struct vehicle_status_s *status, /// current
ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
transition_result_t ret = TRANSITION_DENIED;
arming_state_t current_arming_state = status->arming_state;
bool feedback_provided = false;
/* only check transition if the new state is actually different from the current one */
if (new_arming_state == current_arming_state) {
@@ -156,13 +156,15 @@ arming_state_transition(struct vehicle_status_s *status, /// current
// Fail transition if pre-arm check fails
if (prearm_ret) {
/* the prearm check already prints the reject reason */
feedback_provided = true;
valid_transition = false;
// Fail transition if we need safety switch press
} else if (safety->safety_switch_available && !safety->safety_off) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch!");
mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!");
feedback_provided = true;
valid_transition = false;
}
@@ -173,6 +175,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current
if (!status->condition_power_input_valid) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
feedback_provided = true;
valid_transition = false;
}
@@ -182,6 +185,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current
(status->avionics_power_rail_voltage < 4.9f))) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
valid_transition = false;
}
}
@@ -200,6 +204,8 @@ arming_state_transition(struct vehicle_status_s *status, /// current
/* Sensors need to be initialized for STANDBY state */
if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational.");
feedback_provided = true;
valid_transition = false;
}
@@ -216,11 +222,14 @@ arming_state_transition(struct vehicle_status_s *status, /// current
}
if (ret == TRANSITION_DENIED) {
static const char *errMsg = "INVAL: %s - %s";
const char * str = "INVAL: %s - %s";
/* only print to console here by default as this is too technical to be useful during operation */
warnx(str, state_names[status->arming_state], state_names[new_arming_state]);
mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]);
/* print to MAVLink if we didn't provide any feedback yet */
if (!feedback_provided) {
mavlink_log_critical(mavlink_fd, str, state_names[status->arming_state], state_names[new_arming_state]);
}
}
return ret;
@@ -648,8 +657,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE");
mavlink_log_critical(mavlink_fd, "hold still while arming");
mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE, hold still");
/* this is frickin' fatal */
failed = true;
goto system_eval;
src/modules/controllib/block/Block.hpp
+5
View File
@@ -93,6 +93,11 @@ protected:
List<uORB::SubscriptionBase *> _subscriptions;
List<uORB::PublicationBase *> _publications;
List<BlockParamBase *> _params;
private:
/* this class has pointer data members and should not be copied (private constructor) */
Block(const control::Block&);
Block operator=(const control::Block&);
};
class __EXPORT SuperBlock :
src/modules/controllib/blocks.cpp
+12 -1
View File
@@ -293,7 +293,18 @@ int blockIntegralTrapTest()
float BlockDerivative::update(float input)
{
float output = _lowPass.update((input - getU()) / getDt());
float output;
if (_initialized) {
output = _lowPass.update((input - getU()) / getDt());
} else {
// if this is the first call to update
// we have no valid derivative
// and so we use the assumption the
// input value is not changing much,
// which is the best we can do here.
output = 0.0f;
_initialized = true;
}
setU(input);
return output;
}
src/modules/controllib/blocks.hpp
+17
View File
@@ -238,9 +238,25 @@ public:
BlockDerivative(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_u(0),
_initialized(false),
_lowPass(this, "LP")
{};
virtual ~BlockDerivative() {};
/**
* Update the state and get current derivative
*
* This call updates the state and gets the current
* derivative. As the derivative is only valid
* on the second call to update, it will return
* no change (0) on the first. To get a closer
* estimate of the derivative on the first call,
* call setU() one time step before using the
* return value of update().
*
* @param input the variable to calculate the derivative of
* @return the current derivative
*/
float update(float input);
// accessors
void setU(float u) {_u = u;}
@@ -249,6 +265,7 @@ public:
protected:
// attributes
float _u; /**< previous input */
bool _initialized;
BlockLowPass _lowPass; /**< low pass filter */
};
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+9 -193
View File
@@ -136,7 +136,6 @@ private:
int _global_pos_sub;
int _pos_sp_triplet_sub;
int _att_sub; /**< vehicle attitude subscription */
int _attitude_sub; /**< raw rc channels data subscription */
int _airspeed_sub; /**< airspeed subscription */
int _control_mode_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
@@ -160,18 +159,6 @@ private:
perf_counter_t _loop_perf; /**< loop performance counter */
/** manual control states */
float _altctrl_hold_heading; /**< heading the system should hold in altctrl mode */
double _loiter_hold_lat;
double _loiter_hold_lon;
float _loiter_hold_alt;
bool _loiter_hold;
double _launch_lat;
double _launch_lon;
float _launch_alt;
bool _launch_valid;
/* land states */
/* not in non-abort mode for landing yet */
bool land_noreturn_horizontal;
@@ -188,8 +175,8 @@ private:
/* Landingslope object */
Landingslope landingslope;
float flare_curve_alt_rel_last;
/* heading hold */
float target_bearing;
@@ -212,19 +199,6 @@ private:
float l1_period;
float l1_damping;
float time_const;
float min_sink_rate;
float max_sink_rate;
float max_climb_rate;
float throttle_damp;
float integrator_gain;
float vertical_accel_limit;
float height_comp_filter_omega;
float speed_comp_filter_omega;
float roll_throttle_compensation;
float speed_weight;
float pitch_damping;
float airspeed_min;
float airspeed_trim;
float airspeed_max;
@@ -238,9 +212,6 @@ private:
float throttle_land_max;
float heightrate_p;
float speedrate_p;
float land_slope_angle;
float land_H1_virt;
float land_flare_alt_relative;
@@ -255,19 +226,6 @@ private:
param_t l1_period;
param_t l1_damping;
param_t time_const;
param_t min_sink_rate;
param_t max_sink_rate;
param_t max_climb_rate;
param_t throttle_damp;
param_t integrator_gain;
param_t vertical_accel_limit;
param_t height_comp_filter_omega;
param_t speed_comp_filter_omega;
param_t roll_throttle_compensation;
param_t speed_weight;
param_t pitch_damping;
param_t airspeed_min;
param_t airspeed_trim;
param_t airspeed_max;
@@ -281,9 +239,6 @@ private:
param_t throttle_land_max;
param_t heightrate_p;
param_t speedrate_p;
param_t land_slope_angle;
param_t land_H1_virt;
param_t land_flare_alt_relative;
@@ -416,12 +371,14 @@ FixedwingPositionControl::FixedwingPositionControl() :
_control_mode_sub(-1),
_params_sub(-1),
_manual_control_sub(-1),
_sensor_combined_sub(-1),
_range_finder_sub(-1),
/* publications */
_attitude_sp_pub(-1),
_nav_capabilities_pub(-1),
/* states */
_att(),
_att_sp(),
_nav_capabilities(),
@@ -436,8 +393,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
/* states */
_loiter_hold(false),
land_noreturn_horizontal(false),
land_noreturn_vertical(false),
land_stayonground(false),
@@ -446,12 +401,16 @@ FixedwingPositionControl::FixedwingPositionControl() :
launch_detected(false),
usePreTakeoffThrust(false),
last_manual(false),
landingslope(),
flare_curve_alt_rel_last(0.0f),
target_bearing(0.0f),
launchDetector(),
_airspeed_error(0.0f),
_airspeed_valid(false),
_airspeed_last_valid(0),
_groundspeed_undershoot(0.0f),
_global_pos_valid(false),
_l1_control(),
_mTecs(),
_was_pos_control_mode(false)
{
@@ -479,21 +438,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
_parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT");
_parameter_handles.time_const = param_find("FW_T_TIME_CONST");
_parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN");
_parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX");
_parameter_handles.max_climb_rate = param_find("FW_T_CLMB_MAX");
_parameter_handles.throttle_damp = param_find("FW_T_THR_DAMP");
_parameter_handles.integrator_gain = param_find("FW_T_INTEG_GAIN");
_parameter_handles.vertical_accel_limit = param_find("FW_T_VERT_ACC");
_parameter_handles.height_comp_filter_omega = param_find("FW_T_HGT_OMEGA");
_parameter_handles.speed_comp_filter_omega = param_find("FW_T_SPD_OMEGA");
_parameter_handles.roll_throttle_compensation = param_find("FW_T_RLL2THR");
_parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT");
_parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP");
_parameter_handles.heightrate_p = param_find("FW_T_HRATE_P");
_parameter_handles.speedrate_p = param_find("FW_T_SRATE_P");
/* fetch initial parameter values */
parameters_update();
}
@@ -544,22 +488,6 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max));
param_get(_parameter_handles.time_const, &(_parameters.time_const));
param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate));
param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate));
param_get(_parameter_handles.throttle_damp, &(_parameters.throttle_damp));
param_get(_parameter_handles.integrator_gain, &(_parameters.integrator_gain));
param_get(_parameter_handles.vertical_accel_limit, &(_parameters.vertical_accel_limit));
param_get(_parameter_handles.height_comp_filter_omega, &(_parameters.height_comp_filter_omega));
param_get(_parameter_handles.speed_comp_filter_omega, &(_parameters.speed_comp_filter_omega));
param_get(_parameter_handles.roll_throttle_compensation, &(_parameters.roll_throttle_compensation));
param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight));
param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping));
param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate));
param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p));
param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p));
param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle));
param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));
@@ -609,17 +537,7 @@ FixedwingPositionControl::vehicle_control_mode_poll()
orb_check(_control_mode_sub, &vstatus_updated);
if (vstatus_updated) {
bool was_armed = _control_mode.flag_armed;
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
if (!was_armed && _control_mode.flag_armed) {
_launch_lat = _global_pos.lat;
_launch_lon = _global_pos.lon;
_launch_alt = _global_pos.alt;
_launch_valid = true;
}
}
}
@@ -815,9 +733,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
/* filter speed and altitude for controller */
math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
/* define altitude error */
float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
/* no throttle limit as default */
@@ -949,7 +865,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
float airspeed_land = 1.3f * _parameters.airspeed_min;
float airspeed_approach = 1.3f * _parameters.airspeed_min;
/* Calculate altitude of last ordinary waypoint L */
/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f;
float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
@@ -1115,15 +1031,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
}
// warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(),
// (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing());
// warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp(0), (double)prev_wp(1),
// (double)next_wp(0), (double)next_wp(1), (pos_sp_triplet.previous_valid) ? "valid" : "invalid");
// XXX at this point we always want no loiter hold if a
// mission is active
_loiter_hold = false;
/* reset landing state */
if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) {
reset_landing_state();
@@ -1139,89 +1046,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_att_sp.roll_reset_integral = true;
}
} else if (0/* posctrl mode enabled */) {
_was_pos_control_mode = false;
/** POSCTRL FLIGHT **/
if (0/* switched from another mode to posctrl */) {
_altctrl_hold_heading = _att.yaw;
}
if (0/* posctrl on and manual control yaw non-zero */) {
_altctrl_hold_heading = _att.yaw + _manual.r;
}
//XXX not used
/* climb out control */
// bool climb_out = false;
//
// /* user wants to climb out */
// if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
// climb_out = true;
// }
/* if in altctrl mode, set airspeed based on manual control */
// XXX check if ground speed undershoot should be applied here
float altctrl_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.z;
_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
} else if (0/* altctrl mode enabled */) {
_was_pos_control_mode = false;
/** ALTCTRL FLIGHT **/
if (0/* switched from another mode to altctrl */) {
_altctrl_hold_heading = _att.yaw;
}
if (0/* altctrl on and manual control yaw non-zero */) {
_altctrl_hold_heading = _att.yaw + _manual.r;
}
/* if in altctrl mode, set airspeed based on manual control */
// XXX check if ground speed undershoot should be applied here
float altctrl_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.z;
/* user switched off throttle */
if (_manual.z < 0.1f) {
throttle_max = 0.0f;
}
/* climb out control */
bool climb_out = false;
/* user wants to climb out */
if (_manual.x > 0.3f && _manual.z > 0.8f) {
climb_out = true;
}
_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d);
_att_sp.roll_body = _manual.y;
_att_sp.yaw_body = _manual.r;
tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
climb_out, math::radians(_parameters.pitch_limit_min),
_global_pos.alt, ground_speed);
} else {
_was_pos_control_mode = false;
@@ -1339,14 +1163,6 @@ FixedwingPositionControl::task_main()
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
}
static uint64_t last_run = 0;
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
/* guard against too large deltaT's */
if (deltaT > 1.0f)
deltaT = 0.01f;
/* load local copies */
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
-176
View File
@@ -154,182 +154,6 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
*/
PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f);
/**
* Maximum climb rate
*
* This is the best climb rate that the aircraft can achieve with
* the throttle set to THR_MAX and the airspeed set to the
* default value. For electric aircraft make sure this number can be
* achieved towards the end of flight when the battery voltage has reduced.
* The setting of this parameter can be checked by commanding a positive
* altitude change of 100m in loiter, RTL or guided mode. If the throttle
* required to climb is close to THR_MAX and the aircraft is maintaining
* airspeed, then this parameter is set correctly. If the airspeed starts
* to reduce, then the parameter is set to high, and if the throttle
* demand required to climb and maintain speed is noticeably less than
* FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or
* FW_THR_MAX reduced.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
/**
* Minimum descent rate
*
* This is the sink rate of the aircraft with the throttle
* set to THR_MIN and flown at the same airspeed as used
* to measure FW_T_CLMB_MAX.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
/**
* Maximum descent rate
*
* This sets the maximum descent rate that the controller will use.
* If this value is too large, the aircraft can over-speed on descent.
* This should be set to a value that can be achieved without
* exceeding the lower pitch angle limit and without over-speeding
* the aircraft.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
/**
* TECS time constant
*
* This is the time constant of the TECS control algorithm (in seconds).
* Smaller values make it faster to respond, larger values make it slower
* to respond.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
/**
* Throttle damping factor
*
* This is the damping gain for the throttle demand loop.
* Increase to add damping to correct for oscillations in speed and height.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
/**
* Integrator gain
*
* This is the integrator gain on the control loop.
* Increasing this gain increases the speed at which speed
* and height offsets are trimmed out, but reduces damping and
* increases overshoot.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
/**
* Maximum vertical acceleration
*
* This is the maximum vertical acceleration (in metres/second square)
* either up or down that the controller will use to correct speed
* or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g)
* allows for reasonably aggressive pitch changes if required to recover
* from under-speed conditions.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
/**
* Complementary filter "omega" parameter for height
*
* This is the cross-over frequency (in radians/second) of the complementary
* filter used to fuse vertical acceleration and barometric height to obtain
* an estimate of height rate and height. Increasing this frequency weights
* the solution more towards use of the barometer, whilst reducing it weights
* the solution more towards use of the accelerometer data.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
/**
* Complementary filter "omega" parameter for speed
*
* This is the cross-over frequency (in radians/second) of the complementary
* filter used to fuse longitudinal acceleration and airspeed to obtain an
* improved airspeed estimate. Increasing this frequency weights the solution
* more towards use of the arispeed sensor, whilst reducing it weights the
* solution more towards use of the accelerometer data.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
/**
* Roll -> Throttle feedforward
*
* Increasing this gain turn increases the amount of throttle that will
* be used to compensate for the additional drag created by turning.
* Ideally this should be set to approximately 10 x the extra sink rate
* in m/s created by a 45 degree bank turn. Increase this gain if
* the aircraft initially loses energy in turns and reduce if the
* aircraft initially gains energy in turns. Efficient high aspect-ratio
* aircraft (eg powered sailplanes) can use a lower value, whereas
* inefficient low aspect-ratio models (eg delta wings) can use a higher value.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
/**
* Speed <--> Altitude priority
*
* This parameter adjusts the amount of weighting that the pitch control
* applies to speed vs height errors. Setting it to 0.0 will cause the
* pitch control to control height and ignore speed errors. This will
* normally improve height accuracy but give larger airspeed errors.
* Setting it to 2.0 will cause the pitch control loop to control speed
* and ignore height errors. This will normally reduce airspeed errors,
* but give larger height errors. The default value of 1.0 allows the pitch
* control to simultaneously control height and speed.
* Note to Glider Pilots - set this parameter to 2.0 (The glider will
* adjust its pitch angle to maintain airspeed, ignoring changes in height).
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
/**
* Pitch damping factor
*
* This is the damping gain for the pitch demand loop. Increase to add
* damping to correct for oscillations in height. The default value of 0.0
* will work well provided the pitch to servo controller has been tuned
* properly.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
/**
* Height rate P factor
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
/**
* Speed rate P factor
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f);
/**
* Landing slope angle
*
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+8 -2
View File
@@ -59,6 +59,7 @@ mTecs::mTecs() :
_controlAltitude(this, "FPA", true),
_controlAirSpeed(this, "ACC"),
_flightPathAngleLowpass(this, "FPA_LP"),
_altitudeLowpass(this, "ALT_LP"),
_airspeedLowpass(this, "A_LP"),
_airspeedDerivative(this, "AD"),
_throttleSp(0.0f),
@@ -93,18 +94,23 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
/* time measurement */
updateTimeMeasurement();
/* Filter altitude */
float altitudeFiltered = _altitudeLowpass.update(altitude);
/* calculate flight path angle setpoint from altitude setpoint */
float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude);
float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitudeFiltered);
/* Debug output */
if (_counter % 10 == 0) {
debug("***");
debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp);
debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, altitude filtered %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)altitudeFiltered, (double)flightPathAngleSp);
}
/* Write part of the status message */
_status.altitudeSp = altitudeSp;
_status.altitude = altitude;
_status.altitudeFiltered = altitudeFiltered;
/* use flightpath angle setpoint for total energy control */
src/modules/fw_pos_control_l1/mtecs/mTecs.h
+1
View File
@@ -115,6 +115,7 @@ protected:
/* Other calculation Blocks */
control::BlockLowPass _flightPathAngleLowpass; /**< low pass filter for the flight path angle */
control::BlockLowPass _altitudeLowpass; /**< low pass filter for altitude */
control::BlockLowPass _airspeedLowpass; /**< low pass filter for airspeed */
control::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */
src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
+3 -3
View File
@@ -72,8 +72,8 @@ public:
* @return: true if the limit is applied, false otherwise
*/
bool limit(float& value, float& difference) {
float minimum = isAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin();
float maximum = isAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax();
float minimum = getIsAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin();
float maximum = getIsAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax();
if (value < minimum) {
difference = value - minimum;
value = minimum;
@@ -86,7 +86,7 @@ public:
return false;
}
//accessor:
bool isAngularLimit() {return _isAngularLimit ;}
bool getIsAngularLimit() {return _isAngularLimit ;}
float getMin() { return _min.get(); }
float getMax() { return _max.get(); }
void setMin(float value) { _min.set(value); }
src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
+7
View File
@@ -174,6 +174,13 @@ PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f);
*/
PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f);
/**
* Lowpass (cutoff freq.) for altitude
*
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_ALT_LP, 1.0f);
/**
* Lowpass (cutoff freq.) for the flight path angle
*
src/modules/mavlink/mavlink_main.cpp
+146 -51
View File
@@ -81,7 +81,7 @@
#include "mavlink_commands.h"
#ifndef MAVLINK_CRC_EXTRA
#error MAVLINK_CRC_EXTRA has to be defined on PX4 systems
#error MAVLINK_CRC_EXTRA has to be defined on PX4 systems
#endif
/* oddly, ERROR is not defined for c++ */
@@ -154,7 +154,8 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
instance = Mavlink::get_instance(6);
break;
#endif
default:
default:
return;
}
@@ -169,17 +170,16 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
int buf_free = 0;
if (instance->get_flow_control_enabled()
&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
/* Disable hardware flow control:
* if no successful write since a defined time
* and if the last try was not the last successful write
*/
if (last_write_try_times[(unsigned)channel] != 0 &&
hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL &&
last_write_success_times[(unsigned)channel] !=
last_write_try_times[(unsigned)channel])
{
hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL &&
last_write_success_times[(unsigned)channel] !=
last_write_try_times[(unsigned)channel]) {
warnx("DISABLING HARDWARE FLOW CONTROL");
instance->enable_flow_control(false);
}
@@ -197,15 +197,20 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
if (buf_free < desired) {
/* we don't want to send anything just in half, so return */
instance->count_txerr();
instance->count_txerrbytes(desired);
return;
}
}
ssize_t ret = write(uart, ch, desired);
if (ret != desired) {
instance->count_txerr();
instance->count_txerrbytes(desired);
} else {
last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
instance->count_txbytes(desired);
}
}
}
@@ -233,9 +238,9 @@ Mavlink::Mavlink() :
_mission_result_sub(-1),
_mode(MAVLINK_MODE_NORMAL),
_channel(MAVLINK_COMM_0),
_logbuffer{},
_logbuffer {},
_total_counter(0),
_receive_thread{},
_receive_thread {},
_verbose(false),
_forwarding_on(false),
_passing_on(false),
@@ -248,8 +253,16 @@ Mavlink::Mavlink() :
_subscribe_to_stream(nullptr),
_subscribe_to_stream_rate(0.0f),
_flow_control_enabled(true),
_message_buffer{},
_message_buffer_mutex{},
_bytes_tx(0),
_bytes_txerr(0),
_bytes_rx(0),
_bytes_timestamp(0),
_rate_tx(0.0f),
_rate_txerr(0.0f),
_rate_rx(0.0f),
_rstatus {},
_message_buffer {},
_message_buffer_mutex {},
_param_initialized(false),
_param_system_id(0),
_param_component_id(0),
@@ -257,7 +270,7 @@ Mavlink::Mavlink() :
_param_use_hil_gps(0),
_param_forward_externalsp(0),
/* performance counters */
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
_txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
{
@@ -426,6 +439,27 @@ Mavlink::destroy_all_instances()
return OK;
}
int
Mavlink::get_status_all_instances()
{
Mavlink *inst = ::_mavlink_instances;
unsigned iterations = 0;
while (inst != nullptr) {
printf("\ninstance #%u:\n", iterations);
inst->display_status();
/* move on */
inst = inst->next;
iterations++;
}
/* return an error if there are no instances */
return (iterations == 0);
}
bool
Mavlink::instance_exists(const char *device_name, Mavlink *self)
{
@@ -623,7 +657,8 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
case 921600: speed = B921600; break;
default:
warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", baud);
warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n",
baud);
return -EINVAL;
}
@@ -853,8 +888,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
mavlink_param_request_list_t req;
mavlink_msg_param_request_list_decode(msg, &req);
if (req.target_system == mavlink_system.sysid &&
(req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) {
(req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) {
/* Start sending parameters */
mavlink_pm_start_queued_send();
send_statustext_info("[pm] sending list");
@@ -869,7 +905,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
mavlink_param_set_t mavlink_param_set;
mavlink_msg_param_set_decode(msg, &mavlink_param_set);
if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) {
if (mavlink_param_set.target_system == mavlink_system.sysid
&& ((mavlink_param_set.target_component == mavlink_system.compid)
|| (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) {
/* local name buffer to enforce null-terminated string */
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
@@ -896,7 +934,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
mavlink_param_request_read_t mavlink_param_request_read;
mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read);
if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) {
if (mavlink_param_request_read.target_system == mavlink_system.sysid
&& ((mavlink_param_request_read.target_component == mavlink_system.compid)
|| (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) {
/* when no index is given, loop through string ids and compare them */
if (mavlink_param_request_read.param_index == -1) {
/* local name buffer to enforce null-terminated string */
@@ -959,15 +999,17 @@ Mavlink::send_statustext(unsigned severity, const char *string)
/* Map severity */
switch (severity) {
case MAVLINK_IOC_SEND_TEXT_INFO:
statustext.severity = MAV_SEVERITY_INFO;
break;
case MAVLINK_IOC_SEND_TEXT_CRITICAL:
statustext.severity = MAV_SEVERITY_CRITICAL;
break;
case MAVLINK_IOC_SEND_TEXT_EMERGENCY:
statustext.severity = MAV_SEVERITY_EMERGENCY;
break;
case MAVLINK_IOC_SEND_TEXT_INFO:
statustext.severity = MAV_SEVERITY_INFO;
break;
case MAVLINK_IOC_SEND_TEXT_CRITICAL:
statustext.severity = MAV_SEVERITY_CRITICAL;
break;
case MAVLINK_IOC_SEND_TEXT_EMERGENCY:
statustext.severity = MAV_SEVERITY_EMERGENCY;
break;
}
mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text);
@@ -1084,12 +1126,14 @@ Mavlink::message_buffer_init(int size)
_message_buffer.size = size;
_message_buffer.write_ptr = 0;
_message_buffer.read_ptr = 0;
_message_buffer.data = (char*)malloc(_message_buffer.size);
_message_buffer.data = (char *)malloc(_message_buffer.size);
int ret;
if (_message_buffer.data == 0) {
ret = ERROR;
_message_buffer.size = 0;
} else {
ret = OK;
}
@@ -1403,7 +1447,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
configure_stream("HIGHRES_IMU", 1.0f * rate_mult);
configure_stream("ATTITUDE", 10.0f * rate_mult);
configure_stream("VFR_HUD", 10.0f * rate_mult);
configure_stream("VFR_HUD", 8.0f * rate_mult);
configure_stream("GPS_RAW_INT", 1.0f * rate_mult);
configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult);
configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult);
@@ -1461,7 +1505,8 @@ Mavlink::task_main(int argc, char *argv[])
if (_subscribe_to_stream != nullptr) {
if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
if (_subscribe_to_stream_rate > 0.0f) {
warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, (double)_subscribe_to_stream_rate);
warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name,
(double)_subscribe_to_stream_rate);
} else {
warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name);
@@ -1500,48 +1545,63 @@ Mavlink::task_main(int argc, char *argv[])
bool is_part;
uint8_t *read_ptr;
uint8_t *write_ptr;
uint8_t *write_ptr;
pthread_mutex_lock(&_message_buffer_mutex);
int available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
int available = message_buffer_get_ptr((void **)&read_ptr, &is_part);
pthread_mutex_unlock(&_message_buffer_mutex);
if (available > 0) {
// Reconstruct message from buffer
// Reconstruct message from buffer
mavlink_message_t msg;
write_ptr = (uint8_t*)&msg;
write_ptr = (uint8_t *)&msg;
// Pull a single message from the buffer
size_t read_count = available;
if (read_count > sizeof(mavlink_message_t)) {
read_count = sizeof(mavlink_message_t);
}
// Pull a single message from the buffer
size_t read_count = available;
memcpy(write_ptr, read_ptr, read_count);
if (read_count > sizeof(mavlink_message_t)) {
read_count = sizeof(mavlink_message_t);
}
// We hold the mutex until after we complete the second part of the buffer. If we don't
// we may end up breaking the empty slot overflow detection semantics when we mark the
// possibly partial read below.
pthread_mutex_lock(&_message_buffer_mutex);
memcpy(write_ptr, read_ptr, read_count);
// We hold the mutex until after we complete the second part of the buffer. If we don't
// we may end up breaking the empty slot overflow detection semantics when we mark the
// possibly partial read below.
pthread_mutex_lock(&_message_buffer_mutex);
message_buffer_mark_read(read_count);
/* write second part of buffer if there is some */
if (is_part && read_count < sizeof(mavlink_message_t)) {
write_ptr += read_count;
available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
read_count = sizeof(mavlink_message_t) - read_count;
memcpy(write_ptr, read_ptr, read_count);
write_ptr += read_count;
available = message_buffer_get_ptr((void **)&read_ptr, &is_part);
read_count = sizeof(mavlink_message_t) - read_count;
memcpy(write_ptr, read_ptr, read_count);
message_buffer_mark_read(available);
}
pthread_mutex_unlock(&_message_buffer_mutex);
pthread_mutex_unlock(&_message_buffer_mutex);
_mavlink_resend_uart(_channel, &msg);
_mavlink_resend_uart(_channel, &msg);
}
}
/* update TX/RX rates*/
if (t > _bytes_timestamp + 1000000) {
if (_bytes_timestamp != 0) {
float dt = (t - _bytes_timestamp) / 1000.0f;
_rate_tx = _bytes_tx / dt;
_rate_txerr = _bytes_txerr / dt;
_rate_rx = _bytes_rx / dt;
_bytes_tx = 0;
_bytes_txerr = 0;
_bytes_rx = 0;
}
_bytes_timestamp = t;
}
perf_end(_loop_perf);
}
@@ -1592,6 +1652,7 @@ Mavlink::task_main(int argc, char *argv[])
message_buffer_destroy();
pthread_mutex_destroy(&_message_buffer_mutex);
}
/* destroy log buffer */
mavlink_logbuffer_destroy(&_logbuffer);
@@ -1613,6 +1674,7 @@ int Mavlink::start_helper(int argc, char *argv[])
/* out of memory */
res = -ENOMEM;
warnx("OUT OF MEM");
} else {
/* this will actually only return once MAVLink exits */
res = instance->task_main(argc, argv);
@@ -1672,7 +1734,40 @@ Mavlink::start(int argc, char *argv[])
void
Mavlink::display_status()
{
warnx("running");
if (_rstatus.heartbeat_time > 0) {
printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time));
}
if (_rstatus.timestamp > 0) {
printf("\ttype:\t\t");
switch (_rstatus.type) {
case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO:
printf("3DR RADIO\n");
break;
default:
printf("UNKNOWN RADIO\n");
break;
}
printf("\trssi:\t\t%d\n", _rstatus.rssi);
printf("\tremote rssi:\t%u\n", _rstatus.remote_rssi);
printf("\ttxbuf:\t\t%u\n", _rstatus.txbuf);
printf("\tnoise:\t\t%d\n", _rstatus.noise);
printf("\tremote noise:\t%u\n", _rstatus.remote_noise);
printf("\trx errors:\t%u\n", _rstatus.rxerrors);
printf("\tfixed:\t\t%u\n", _rstatus.fixed);
} else {
printf("\tno telem status.\n");
}
printf("\trates:\n");
printf("\ttx: %.3f kB/s\n", (double)_rate_tx);
printf("\ttxerr: %.3f kB/s\n", (double)_rate_txerr);
printf("\trx: %.3f kB/s\n", (double)_rate_rx);
}
int
@@ -1760,8 +1855,8 @@ int mavlink_main(int argc, char *argv[])
} else if (!strcmp(argv[1], "stop-all")) {
return Mavlink::destroy_all_instances();
// } else if (!strcmp(argv[1], "status")) {
// mavlink::g_mavlink->status();
} else if (!strcmp(argv[1], "status")) {
return Mavlink::get_status_all_instances();
} else if (!strcmp(argv[1], "stream")) {
return Mavlink::stream_command(argc, argv);
src/modules/mavlink/mavlink_main.h
+37 -4
View File
@@ -51,6 +51,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/telemetry_status.h>
#include "mavlink_bridge_header.h"
#include "mavlink_orb_subscription.h"
@@ -97,6 +98,8 @@ public:
static int destroy_all_instances();
static int get_status_all_instances();
static bool instance_exists(const char *device_name, Mavlink *self);
static void forward_message(const mavlink_message_t *msg, Mavlink *self);
@@ -231,6 +234,26 @@ public:
*/
void count_txerr();
/**
* Count transmitted bytes
*/
void count_txbytes(unsigned n) { _bytes_tx += n; };
/**
* Count bytes not transmitted because of errors
*/
void count_txerrbytes(unsigned n) { _bytes_txerr += n; };
/**
* Count received bytes
*/
void count_rxbytes(unsigned n) { _bytes_rx += n; };
/**
* Get the receive status of this MAVLink link
*/
struct telemetry_status_s& get_rx_status() { return _rstatus; }
protected:
Mavlink *next;
@@ -253,13 +276,13 @@ private:
MavlinkOrbSubscription *_subscriptions;
MavlinkStream *_streams;
MavlinkMissionManager *_mission_manager;
MavlinkMissionManager *_mission_manager;
orb_advert_t _mission_pub;
orb_advert_t _mission_pub;
int _mission_result_sub;
MAVLINK_MODE _mode;
MAVLINK_MODE _mode;
mavlink_channel_t _channel;
mavlink_channel_t _channel;
struct mavlink_logbuffer _logbuffer;
unsigned int _total_counter;
@@ -288,6 +311,16 @@ private:
bool _flow_control_enabled;
unsigned _bytes_tx;
unsigned _bytes_txerr;
unsigned _bytes_rx;
uint64_t _bytes_timestamp;
float _rate_tx;
float _rate_txerr;
float _rate_rx;
struct telemetry_status_s _rstatus; ///< receive status
struct mavlink_message_buffer {
int write_ptr;
int read_ptr;
src/modules/mavlink/mavlink_receiver.cpp
+16 -10
View File
@@ -112,7 +112,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_telemetry_status_pub(-1),
_rc_pub(-1),
_manual_pub(-1),
_telemetry_heartbeat_time(0),
_radio_status_available(false),
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
_hil_frames(0),
@@ -594,11 +593,11 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
mavlink_radio_status_t rstatus;
mavlink_msg_radio_status_decode(msg, &rstatus);
struct telemetry_status_s tstatus;
memset(&tstatus, 0, sizeof(tstatus));
struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
tstatus.timestamp = hrt_absolute_time();
tstatus.heartbeat_time = _telemetry_heartbeat_time;
tstatus.telem_time = tstatus.timestamp;
/* tstatus.heartbeat_time is set by system heartbeats */
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
tstatus.rssi = rstatus.rssi;
tstatus.remote_rssi = rstatus.remrssi;
@@ -655,16 +654,20 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
/* ignore own heartbeats, accept only heartbeats from GCS */
if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
_telemetry_heartbeat_time = hrt_absolute_time();
struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
hrt_abstime tnow = hrt_absolute_time();
/* always set heartbeat, publish only if telemetry link not up */
tstatus.heartbeat_time = tnow;
/* if no radio status messages arrive, lets at least publish that heartbeats were received */
if (!_radio_status_available) {
struct telemetry_status_s tstatus;
memset(&tstatus, 0, sizeof(tstatus));
tstatus.timestamp = _telemetry_heartbeat_time;
tstatus.heartbeat_time = _telemetry_heartbeat_time;
tstatus.timestamp = tnow;
/* telem_time indicates the timestamp of a telemetry status packet and we got none */
tstatus.telem_time = 0;
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
if (_telemetry_status_pub < 0) {
@@ -1149,6 +1152,9 @@ MavlinkReceiver::receive_thread(void *arg)
_mavlink->handle_message(&msg);
}
}
/* count received bytes */
_mavlink->count_rxbytes(nread);
}
}
src/modules/mavlink/mavlink_receiver.h
-1
View File
@@ -152,7 +152,6 @@ private:
orb_advert_t _telemetry_status_pub;
orb_advert_t _rc_pub;
orb_advert_t _manual_pub;
hrt_abstime _telemetry_heartbeat_time;
bool _radio_status_available;
int _control_mode_sub;
int _hil_frames;
src/modules/navigator/mission_feasibility_checker.cpp
+6 -3
View File
@@ -135,12 +135,15 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
}
}
if (home_alt > missionitem.altitude) {
/* calculate the global waypoint altitude */
float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude;
if (home_alt > wp_alt) {
if (throw_error) {
mavlink_log_info(_mavlink_fd, "Waypoint %d below home", i);
mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i);
return false;
} else {
mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i);
mavlink_log_critical(_mavlink_fd, "Warning: Waypoint %d below home", i);
return true;
}
}
src/modules/navigator/module.mk
+2
View File
@@ -54,3 +54,5 @@ SRCS = navigator_main.cpp \
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++

Some files were not shown because too many files have changed in this diff Show More