forked from Archive/PX4-Autopilot
Merge remote-tracking branch 'upstream/master' into obcfailsafe
This commit is contained in:
Thomas Gubler
commit
ed19faf428
|
|
@ -4,3 +4,6 @@
|
||||||
[submodule "NuttX"]
|
[submodule "NuttX"]
|
||||||
path = NuttX
|
path = NuttX
|
||||||
url = git://github.com/PX4/NuttX.git
|
url = git://github.com/PX4/NuttX.git
|
||||||
|
[submodule "uavcan"]
|
||||||
|
path = uavcan
|
||||||
|
url = git://github.com/pavel-kirienko/uavcan.git
|
||||||
|
|
|
||||||
3
Makefile
3
Makefile
|
|
@ -212,6 +212,9 @@ endif
|
||||||
$(NUTTX_SRC):
|
$(NUTTX_SRC):
|
||||||
$(Q) ($(PX4_BASE)/Tools/check_submodules.sh)
|
$(Q) ($(PX4_BASE)/Tools/check_submodules.sh)
|
||||||
|
|
||||||
|
$(UAVCAN_DIR):
|
||||||
|
$(Q) (./Tools/check_submodules.sh)
|
||||||
|
|
||||||
.PHONY: checksubmodules
|
.PHONY: checksubmodules
|
||||||
checksubmodules:
|
checksubmodules:
|
||||||
$(Q) ($(PX4_BASE)/Tools/check_submodules.sh)
|
$(Q) ($(PX4_BASE)/Tools/check_submodules.sh)
|
||||||
|
|
|
||||||
|
|
@ -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
|
||||||
|
|
@ -136,6 +136,11 @@ then
|
||||||
sh /etc/init.d/4011_dji_f450
|
sh /etc/init.d/4011_dji_f450
|
||||||
fi
|
fi
|
||||||
|
|
||||||
|
if param compare SYS_AUTOSTART 4012
|
||||||
|
then
|
||||||
|
sh /etc/init.d/4012_quad_x_can
|
||||||
|
fi
|
||||||
|
|
||||||
if param compare SYS_AUTOSTART 4020
|
if param compare SYS_AUTOSTART 4020
|
||||||
then
|
then
|
||||||
sh /etc/init.d/4020_hk_micro_pcb
|
sh /etc/init.d/4020_hk_micro_pcb
|
||||||
|
|
|
||||||
|
|
@ -24,6 +24,11 @@ then
|
||||||
else
|
else
|
||||||
set OUTPUT_DEV /dev/pwm_output
|
set OUTPUT_DEV /dev/pwm_output
|
||||||
fi
|
fi
|
||||||
|
|
||||||
|
if [ $OUTPUT_MODE == uavcan_esc ]
|
||||||
|
then
|
||||||
|
set OUTPUT_DEV /dev/uavcan/esc
|
||||||
|
fi
|
||||||
|
|
||||||
if mixer load $OUTPUT_DEV $MIXER_FILE
|
if mixer load $OUTPUT_DEV $MIXER_FILE
|
||||||
then
|
then
|
||||||
|
|
|
||||||
|
|
@ -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 something is wrong with setup and we shouldn't try to enable output
|
||||||
if [ $OUTPUT_MODE != none ]
|
if [ $OUTPUT_MODE != none ]
|
||||||
then
|
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
|
then
|
||||||
echo "[init] Use PX4IO PWM as primary output"
|
echo "[init] Use PX4IO PWM as primary output"
|
||||||
if px4io start
|
if px4io start
|
||||||
|
|
|
||||||
|
|
@ -53,4 +53,28 @@ else
|
||||||
git submodule update;
|
git submodule update;
|
||||||
fi
|
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
|
exit 0
|
||||||
|
|
|
||||||
|
|
@ -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)
|
||||||
|
|
@ -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
|
||||||
|
|
|
@ -74,6 +74,7 @@ MODULES += modules/commander
|
||||||
MODULES += modules/navigator
|
MODULES += modules/navigator
|
||||||
MODULES += modules/mavlink
|
MODULES += modules/mavlink
|
||||||
MODULES += modules/gpio_led
|
MODULES += modules/gpio_led
|
||||||
|
#MODULES += modules/uavcan
|
||||||
|
|
||||||
#
|
#
|
||||||
# Estimation modules (EKF/ SO3 / other filters)
|
# Estimation modules (EKF/ SO3 / other filters)
|
||||||
|
|
|
||||||
|
|
@ -64,6 +64,7 @@ LDSCRIPT += $(NUTTX_EXPORT_DIR)build/ld.script
|
||||||
# Add directories from the NuttX export to the relevant search paths
|
# Add directories from the NuttX export to the relevant search paths
|
||||||
#
|
#
|
||||||
INCLUDE_DIRS += $(NUTTX_EXPORT_DIR)include \
|
INCLUDE_DIRS += $(NUTTX_EXPORT_DIR)include \
|
||||||
|
$(NUTTX_EXPORT_DIR)include/cxx \
|
||||||
$(NUTTX_EXPORT_DIR)arch/chip \
|
$(NUTTX_EXPORT_DIR)arch/chip \
|
||||||
$(NUTTX_EXPORT_DIR)arch/common
|
$(NUTTX_EXPORT_DIR)arch/common
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -49,6 +49,7 @@ export NUTTX_SRC = $(abspath $(PX4_BASE)/NuttX/nuttx)/
|
||||||
export MAVLINK_SRC = $(abspath $(PX4_BASE)/mavlink/include/mavlink/v1.0)/
|
export MAVLINK_SRC = $(abspath $(PX4_BASE)/mavlink/include/mavlink/v1.0)/
|
||||||
export NUTTX_APP_SRC = $(abspath $(PX4_BASE)/NuttX/apps)/
|
export NUTTX_APP_SRC = $(abspath $(PX4_BASE)/NuttX/apps)/
|
||||||
export MAVLINK_SRC = $(abspath $(PX4_BASE)/mavlink)/
|
export MAVLINK_SRC = $(abspath $(PX4_BASE)/mavlink)/
|
||||||
|
export UAVCAN_DIR = $(abspath $(PX4_BASE)/uavcan)/
|
||||||
export ROMFS_SRC = $(abspath $(PX4_BASE)/ROMFS)/
|
export ROMFS_SRC = $(abspath $(PX4_BASE)/ROMFS)/
|
||||||
export IMAGE_DIR = $(abspath $(PX4_BASE)/Images)/
|
export IMAGE_DIR = $(abspath $(PX4_BASE)/Images)/
|
||||||
export BUILD_DIR = $(abspath $(PX4_BASE)/Build)/
|
export BUILD_DIR = $(abspath $(PX4_BASE)/Build)/
|
||||||
|
|
|
||||||
|
|
@ -121,7 +121,7 @@ INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH))
|
||||||
# Language-specific flags
|
# Language-specific flags
|
||||||
#
|
#
|
||||||
ARCHCFLAGS = -std=gnu99
|
ARCHCFLAGS = -std=gnu99
|
||||||
ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x
|
ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics
|
||||||
|
|
||||||
# Generic warnings
|
# Generic warnings
|
||||||
#
|
#
|
||||||
|
|
|
||||||
|
|
@ -913,6 +913,11 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
struct system_power_s system_power;
|
struct system_power_s system_power;
|
||||||
memset(&system_power, 0, sizeof(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);
|
control_status_leds(&status, &armed, true);
|
||||||
|
|
||||||
/* now initialized */
|
/* now initialized */
|
||||||
|
|
@ -1196,13 +1201,17 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
|
|
||||||
if (updated) {
|
if (updated) {
|
||||||
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
|
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 */
|
/* 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) {
|
if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
|
||||||
status.battery_voltage = battery.voltage_filtered_v;
|
status.battery_voltage = battery.voltage_filtered_v;
|
||||||
status.battery_current = battery.current_a;
|
status.battery_current = battery.current_a;
|
||||||
status.condition_battery_voltage_valid = true;
|
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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -1272,13 +1281,13 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
}
|
}
|
||||||
|
|
||||||
/* if battery voltage is getting lower, warn using buzzer, etc. */
|
/* if battery voltage is getting lower, warn using buzzer, etc. */
|
||||||
if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) {
|
if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) {
|
||||||
low_battery_voltage_actions_done = true;
|
low_battery_voltage_actions_done = true;
|
||||||
mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
|
mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
|
||||||
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
|
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
|
||||||
status_changed = true;
|
status_changed = true;
|
||||||
|
|
||||||
} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
|
} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.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, this is rather an emergency, change state machine */
|
||||||
critical_battery_voltage_actions_done = true;
|
critical_battery_voltage_actions_done = true;
|
||||||
mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
|
mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
|
||||||
|
|
|
||||||
|
|
@ -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;
|
float ret = 0;
|
||||||
static param_t bat_v_empty_h;
|
static param_t bat_v_empty_h;
|
||||||
static param_t bat_v_full_h;
|
static param_t bat_v_full_h;
|
||||||
static param_t bat_n_cells_h;
|
static param_t bat_n_cells_h;
|
||||||
static param_t bat_capacity_h;
|
static param_t bat_capacity_h;
|
||||||
static float bat_v_empty = 3.2f;
|
static param_t bat_v_load_drop_h;
|
||||||
static float bat_v_full = 4.0f;
|
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 int bat_n_cells = 3;
|
||||||
static float bat_capacity = -1.0f;
|
static float bat_capacity = -1.0f;
|
||||||
static bool initialized = false;
|
static bool initialized = false;
|
||||||
|
|
@ -297,23 +299,26 @@ float battery_remaining_estimate_voltage(float voltage, float discharged)
|
||||||
|
|
||||||
if (!initialized) {
|
if (!initialized) {
|
||||||
bat_v_empty_h = param_find("BAT_V_EMPTY");
|
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_n_cells_h = param_find("BAT_N_CELLS");
|
||||||
bat_capacity_h = param_find("BAT_CAPACITY");
|
bat_capacity_h = param_find("BAT_CAPACITY");
|
||||||
|
bat_v_load_drop_h = param_find("BAT_V_LOAD_DROP");
|
||||||
initialized = true;
|
initialized = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (counter % 100 == 0) {
|
if (counter % 100 == 0) {
|
||||||
param_get(bat_v_empty_h, &bat_v_empty);
|
param_get(bat_v_empty_h, &bat_v_empty);
|
||||||
param_get(bat_v_full_h, &bat_v_full);
|
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_n_cells_h, &bat_n_cells);
|
||||||
param_get(bat_capacity_h, &bat_capacity);
|
param_get(bat_capacity_h, &bat_capacity);
|
||||||
}
|
}
|
||||||
|
|
||||||
counter++;
|
counter++;
|
||||||
|
|
||||||
/* remaining charge estimate based on voltage */
|
/* remaining charge estimate based on voltage and internal resistance (drop under load) */
|
||||||
float remaining_voltage = (voltage - bat_n_cells * bat_v_empty) / (bat_n_cells * (bat_v_full - bat_v_empty));
|
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 (bat_capacity > 0.0f) {
|
||||||
/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */
|
/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */
|
||||||
|
|
|
||||||
|
|
@ -80,8 +80,9 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern);
|
||||||
*
|
*
|
||||||
* @param voltage the current battery voltage
|
* @param voltage the current battery voltage
|
||||||
* @param discharged the discharged capacity
|
* @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
|
* @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_ */
|
#endif /* COMMANDER_HELPER_H_ */
|
||||||
|
|
|
||||||
|
|
@ -65,7 +65,17 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
|
||||||
*
|
*
|
||||||
* @group Battery Calibration
|
* @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.
|
* Number of cells.
|
||||||
|
|
|
||||||
|
|
@ -739,30 +739,19 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
|
||||||
{
|
{
|
||||||
/*
|
/*
|
||||||
* Coefficients here derived by measurement of the 5-16V
|
* Coefficients here derived by measurement of the 5-16V
|
||||||
* range on one unit:
|
* range on one unit, validated on sample points of another unit
|
||||||
*
|
*
|
||||||
* V counts
|
* Data in Tools/tests-host/data folder.
|
||||||
* 5 1001
|
|
||||||
* 6 1219
|
|
||||||
* 7 1436
|
|
||||||
* 8 1653
|
|
||||||
* 9 1870
|
|
||||||
* 10 2086
|
|
||||||
* 11 2303
|
|
||||||
* 12 2522
|
|
||||||
* 13 2738
|
|
||||||
* 14 2956
|
|
||||||
* 15 3172
|
|
||||||
* 16 3389
|
|
||||||
*
|
*
|
||||||
* slope = 0.0046067
|
* measured slope = 0.004585267878277 (int: 4585)
|
||||||
* intercept = 0.3863
|
* nominal theoretic slope: 0.00459340659 (int: 4593)
|
||||||
|
* intercept = 0.016646394188076 (int: 16646)
|
||||||
|
* nominal theoretic intercept: 0.00 (int: 0)
|
||||||
*
|
*
|
||||||
* Intercept corrected for best results @ 12V.
|
|
||||||
*/
|
*/
|
||||||
unsigned counts = adc_measure(ADC_VBATT);
|
unsigned counts = adc_measure(ADC_VBATT);
|
||||||
if (counts != 0xffff) {
|
if (counts != 0xffff) {
|
||||||
unsigned mV = (4150 + (counts * 46)) / 10 - 200;
|
unsigned mV = (166460 + (counts * 45934)) / 10000;
|
||||||
unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
|
unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
|
||||||
|
|
||||||
r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
|
r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1 @@
|
||||||
|
./dsdlc_generated/
|
||||||
|
|
@ -0,0 +1,138 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2014 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 esc_controller.cpp
|
||||||
|
*
|
||||||
|
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "esc_controller.hpp"
|
||||||
|
#include <systemlib/err.h>
|
||||||
|
|
||||||
|
UavcanEscController::UavcanEscController(uavcan::INode &node) :
|
||||||
|
_node(node),
|
||||||
|
_uavcan_pub_raw_cmd(node),
|
||||||
|
_uavcan_sub_status(node),
|
||||||
|
_orb_timer(node)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
UavcanEscController::~UavcanEscController()
|
||||||
|
{
|
||||||
|
perf_free(_perfcnt_invalid_input);
|
||||||
|
perf_free(_perfcnt_scaling_error);
|
||||||
|
}
|
||||||
|
|
||||||
|
int UavcanEscController::init()
|
||||||
|
{
|
||||||
|
// ESC status subscription
|
||||||
|
int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb));
|
||||||
|
if (res < 0)
|
||||||
|
{
|
||||||
|
warnx("ESC status sub failed %i", res);
|
||||||
|
return res;
|
||||||
|
}
|
||||||
|
|
||||||
|
// ESC status will be relayed from UAVCAN bus into ORB at this rate
|
||||||
|
_orb_timer.setCallback(TimerCbBinder(this, &UavcanEscController::orb_pub_timer_cb));
|
||||||
|
_orb_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / ESC_STATUS_UPDATE_RATE_HZ));
|
||||||
|
|
||||||
|
return res;
|
||||||
|
}
|
||||||
|
|
||||||
|
void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
|
||||||
|
{
|
||||||
|
if ((outputs == nullptr) || (num_outputs > MAX_ESCS)) {
|
||||||
|
perf_count(_perfcnt_invalid_input);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Rate limiting - we don't want to congest the bus
|
||||||
|
*/
|
||||||
|
const auto timestamp = _node.getMonotonicTime();
|
||||||
|
if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
_prev_cmd_pub = timestamp;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Fill the command message
|
||||||
|
* If unarmed, we publish an empty message anyway
|
||||||
|
*/
|
||||||
|
uavcan::equipment::esc::RawCommand msg;
|
||||||
|
|
||||||
|
if (_armed) {
|
||||||
|
for (unsigned i = 0; i < num_outputs; i++) {
|
||||||
|
|
||||||
|
float scaled = (outputs[i] + 1.0F) * 0.5F * uavcan::equipment::esc::RawCommand::CMD_MAX;
|
||||||
|
if (scaled < 1.0F) {
|
||||||
|
scaled = 1.0F; // Since we're armed, we don't want to stop it completely
|
||||||
|
}
|
||||||
|
|
||||||
|
if (scaled < uavcan::equipment::esc::RawCommand::CMD_MIN) {
|
||||||
|
scaled = uavcan::equipment::esc::RawCommand::CMD_MIN;
|
||||||
|
perf_count(_perfcnt_scaling_error);
|
||||||
|
} else if (scaled > uavcan::equipment::esc::RawCommand::CMD_MAX) {
|
||||||
|
scaled = uavcan::equipment::esc::RawCommand::CMD_MAX;
|
||||||
|
perf_count(_perfcnt_scaling_error);
|
||||||
|
} else {
|
||||||
|
; // Correct value
|
||||||
|
}
|
||||||
|
|
||||||
|
msg.cmd.push_back(static_cast<unsigned>(scaled));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Publish the command message to the bus
|
||||||
|
* Note that for a quadrotor it takes one CAN frame
|
||||||
|
*/
|
||||||
|
(void)_uavcan_pub_raw_cmd.broadcast(msg);
|
||||||
|
}
|
||||||
|
|
||||||
|
void UavcanEscController::arm_esc(bool arm)
|
||||||
|
{
|
||||||
|
_armed = arm;
|
||||||
|
}
|
||||||
|
|
||||||
|
void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
|
||||||
|
{
|
||||||
|
// TODO save status into a local storage; publish to ORB later from orb_pub_timer_cb()
|
||||||
|
}
|
||||||
|
|
||||||
|
void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&)
|
||||||
|
{
|
||||||
|
// TODO publish to ORB
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,107 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2014 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 esc_controller.hpp
|
||||||
|
*
|
||||||
|
* UAVCAN <--> ORB bridge for ESC messages:
|
||||||
|
* uavcan.equipment.esc.RawCommand
|
||||||
|
* uavcan.equipment.esc.RPMCommand
|
||||||
|
* uavcan.equipment.esc.Status
|
||||||
|
*
|
||||||
|
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <uavcan/uavcan.hpp>
|
||||||
|
#include <uavcan/equipment/esc/RawCommand.hpp>
|
||||||
|
#include <uavcan/equipment/esc/Status.hpp>
|
||||||
|
#include <systemlib/perf_counter.h>
|
||||||
|
|
||||||
|
class UavcanEscController
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
UavcanEscController(uavcan::INode& node);
|
||||||
|
~UavcanEscController();
|
||||||
|
|
||||||
|
int init();
|
||||||
|
|
||||||
|
void update_outputs(float *outputs, unsigned num_outputs);
|
||||||
|
|
||||||
|
void arm_esc(bool arm);
|
||||||
|
|
||||||
|
private:
|
||||||
|
/**
|
||||||
|
* ESC status message reception will be reported via this callback.
|
||||||
|
*/
|
||||||
|
void esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* ESC status will be published to ORB from this callback (fixed rate).
|
||||||
|
*/
|
||||||
|
void orb_pub_timer_cb(const uavcan::TimerEvent &event);
|
||||||
|
|
||||||
|
|
||||||
|
static constexpr unsigned MAX_RATE_HZ = 100; ///< XXX make this configurable
|
||||||
|
static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 5;
|
||||||
|
static constexpr unsigned MAX_ESCS = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize;
|
||||||
|
|
||||||
|
typedef uavcan::MethodBinder<UavcanEscController*,
|
||||||
|
void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)>
|
||||||
|
StatusCbBinder;
|
||||||
|
|
||||||
|
typedef uavcan::MethodBinder<UavcanEscController*, void (UavcanEscController::*)(const uavcan::TimerEvent&)>
|
||||||
|
TimerCbBinder;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* libuavcan related things
|
||||||
|
*/
|
||||||
|
uavcan::MonotonicTime _prev_cmd_pub; ///< rate limiting
|
||||||
|
uavcan::INode &_node;
|
||||||
|
uavcan::Publisher<uavcan::equipment::esc::RawCommand> _uavcan_pub_raw_cmd;
|
||||||
|
uavcan::Subscriber<uavcan::equipment::esc::Status, StatusCbBinder> _uavcan_sub_status;
|
||||||
|
uavcan::TimerEventForwarder<TimerCbBinder> _orb_timer;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* ESC states
|
||||||
|
*/
|
||||||
|
bool _armed = false;
|
||||||
|
uavcan::equipment::esc::Status _states[MAX_ESCS];
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Perf counters
|
||||||
|
*/
|
||||||
|
perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input");
|
||||||
|
perf_counter_t _perfcnt_scaling_error = perf_alloc(PC_COUNT, "uavcan_esc_scaling_error");
|
||||||
|
};
|
||||||
|
|
@ -0,0 +1,153 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2014 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 gnss_receiver.cpp
|
||||||
|
*
|
||||||
|
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||||
|
* @author Andrew Chambers <achamber@gmail.com>
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "gnss_receiver.hpp"
|
||||||
|
#include <systemlib/err.h>
|
||||||
|
#include <mathlib/mathlib.h>
|
||||||
|
|
||||||
|
#define MM_PER_CM 10 // Millimeters per centimeter
|
||||||
|
|
||||||
|
UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) :
|
||||||
|
_node(node),
|
||||||
|
_uavcan_sub_status(node),
|
||||||
|
_report_pub(-1)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
int UavcanGnssReceiver::init()
|
||||||
|
{
|
||||||
|
int res = -1;
|
||||||
|
|
||||||
|
// GNSS fix subscription
|
||||||
|
res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssReceiver::gnss_fix_sub_cb));
|
||||||
|
if (res < 0)
|
||||||
|
{
|
||||||
|
warnx("GNSS fix sub failed %i", res);
|
||||||
|
return res;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Clear the uORB GPS report
|
||||||
|
memset(&_report, 0, sizeof(_report));
|
||||||
|
|
||||||
|
return res;
|
||||||
|
}
|
||||||
|
|
||||||
|
void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
|
||||||
|
{
|
||||||
|
_report.timestamp_position = hrt_absolute_time();
|
||||||
|
_report.lat = msg.lat_1e7;
|
||||||
|
_report.lon = msg.lon_1e7;
|
||||||
|
_report.alt = msg.alt_1e2 * MM_PER_CM; // Convert from centimeter (1e2) to millimeters (1e3)
|
||||||
|
|
||||||
|
_report.timestamp_variance = _report.timestamp_position;
|
||||||
|
|
||||||
|
|
||||||
|
// Check if the msg contains valid covariance information
|
||||||
|
const bool valid_position_covariance = !msg.position_covariance.empty();
|
||||||
|
const bool valid_velocity_covariance = !msg.velocity_covariance.empty();
|
||||||
|
|
||||||
|
if (valid_position_covariance) {
|
||||||
|
float pos_cov[9];
|
||||||
|
msg.position_covariance.unpackSquareMatrix(pos_cov);
|
||||||
|
|
||||||
|
// Horizontal position uncertainty
|
||||||
|
const float horizontal_pos_variance = math::max(pos_cov[0], pos_cov[4]);
|
||||||
|
_report.eph = (horizontal_pos_variance > 0) ? sqrtf(horizontal_pos_variance) : -1.0F;
|
||||||
|
|
||||||
|
// Vertical position uncertainty
|
||||||
|
_report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F;
|
||||||
|
} else {
|
||||||
|
_report.eph = -1.0F;
|
||||||
|
_report.epv = -1.0F;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (valid_velocity_covariance) {
|
||||||
|
float vel_cov[9];
|
||||||
|
msg.velocity_covariance.unpackSquareMatrix(vel_cov);
|
||||||
|
_report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]);
|
||||||
|
|
||||||
|
/* There is a nonlinear relationship between the velocity vector and the heading.
|
||||||
|
* Use Jacobian to transform velocity covariance to heading covariance
|
||||||
|
*
|
||||||
|
* Nonlinear equation:
|
||||||
|
* heading = atan2(vel_e_m_s, vel_n_m_s)
|
||||||
|
* For math, see http://en.wikipedia.org/wiki/Atan2#Derivative
|
||||||
|
*
|
||||||
|
* To calculate the variance of heading from the variance of velocity,
|
||||||
|
* cov(heading) = J(velocity)*cov(velocity)*J(velocity)^T
|
||||||
|
*/
|
||||||
|
float vel_n = msg.ned_velocity[0];
|
||||||
|
float vel_e = msg.ned_velocity[1];
|
||||||
|
float vel_n_sq = vel_n * vel_n;
|
||||||
|
float vel_e_sq = vel_e * vel_e;
|
||||||
|
_report.c_variance_rad =
|
||||||
|
(vel_e_sq * vel_cov[0] +
|
||||||
|
-2 * vel_n * vel_e * vel_cov[1] + // Covariance matrix is symmetric
|
||||||
|
vel_n_sq* vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq));
|
||||||
|
|
||||||
|
} else {
|
||||||
|
_report.s_variance_m_s = -1.0F;
|
||||||
|
_report.c_variance_rad = -1.0F;
|
||||||
|
}
|
||||||
|
|
||||||
|
_report.fix_type = msg.status;
|
||||||
|
|
||||||
|
_report.timestamp_velocity = _report.timestamp_position;
|
||||||
|
_report.vel_n_m_s = msg.ned_velocity[0];
|
||||||
|
_report.vel_e_m_s = msg.ned_velocity[1];
|
||||||
|
_report.vel_d_m_s = msg.ned_velocity[2];
|
||||||
|
_report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s);
|
||||||
|
_report.cog_rad = atan2f(_report.vel_e_m_s, _report.vel_n_m_s);
|
||||||
|
_report.vel_ned_valid = true;
|
||||||
|
|
||||||
|
_report.timestamp_time = _report.timestamp_position;
|
||||||
|
_report.time_gps_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec(); // Convert to microseconds
|
||||||
|
|
||||||
|
_report.satellites_used = msg.sats_used;
|
||||||
|
|
||||||
|
if (_report_pub > 0) {
|
||||||
|
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,84 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2014 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 gnss_receiver.hpp
|
||||||
|
*
|
||||||
|
* UAVCAN --> ORB bridge for GNSS messages:
|
||||||
|
* uavcan.equipment.gnss.Fix
|
||||||
|
*
|
||||||
|
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||||
|
* @author Andrew Chambers <achamber@gmail.com>
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <drivers/drv_hrt.h>
|
||||||
|
|
||||||
|
#include <uORB/uORB.h>
|
||||||
|
#include <uORB/topics/vehicle_gps_position.h>
|
||||||
|
|
||||||
|
#include <uavcan/uavcan.hpp>
|
||||||
|
#include <uavcan/equipment/gnss/Fix.hpp>
|
||||||
|
|
||||||
|
class UavcanGnssReceiver
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
UavcanGnssReceiver(uavcan::INode& node);
|
||||||
|
|
||||||
|
int init();
|
||||||
|
|
||||||
|
private:
|
||||||
|
/**
|
||||||
|
* GNSS fix message will be reported via this callback.
|
||||||
|
*/
|
||||||
|
void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg);
|
||||||
|
|
||||||
|
|
||||||
|
typedef uavcan::MethodBinder<UavcanGnssReceiver*,
|
||||||
|
void (UavcanGnssReceiver::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>&)>
|
||||||
|
FixCbBinder;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* libuavcan related things
|
||||||
|
*/
|
||||||
|
uavcan::INode &_node;
|
||||||
|
uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _uavcan_sub_status;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* uORB
|
||||||
|
*/
|
||||||
|
struct vehicle_gps_position_s _report; ///< uORB topic for gnss position
|
||||||
|
orb_advert_t _report_pub; ///< uORB pub for gnss position
|
||||||
|
|
||||||
|
};
|
||||||
|
|
@ -0,0 +1,74 @@
|
||||||
|
############################################################################
|
||||||
|
#
|
||||||
|
# Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||||
|
# Author: Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||||
|
#
|
||||||
|
# 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.
|
||||||
|
#
|
||||||
|
############################################################################
|
||||||
|
|
||||||
|
#
|
||||||
|
# UAVCAN <--> uORB bridge
|
||||||
|
#
|
||||||
|
|
||||||
|
MODULE_COMMAND = uavcan
|
||||||
|
|
||||||
|
MAXOPTIMIZATION = -Os
|
||||||
|
|
||||||
|
SRCS += uavcan_main.cpp \
|
||||||
|
uavcan_clock.cpp \
|
||||||
|
esc_controller.cpp \
|
||||||
|
gnss_receiver.cpp
|
||||||
|
|
||||||
|
#
|
||||||
|
# libuavcan
|
||||||
|
#
|
||||||
|
include $(UAVCAN_DIR)/libuavcan/include.mk
|
||||||
|
SRCS += $(LIBUAVCAN_SRC)
|
||||||
|
INCLUDE_DIRS += $(LIBUAVCAN_INC)
|
||||||
|
# Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile
|
||||||
|
# because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode.
|
||||||
|
override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS
|
||||||
|
|
||||||
|
#
|
||||||
|
# libuavcan drivers for STM32
|
||||||
|
#
|
||||||
|
include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk
|
||||||
|
SRCS += $(LIBUAVCAN_STM32_SRC)
|
||||||
|
INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC)
|
||||||
|
override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2
|
||||||
|
|
||||||
|
#
|
||||||
|
# Invoke DSDL compiler
|
||||||
|
# TODO: Add make target for this, or invoke dsdlc manually.
|
||||||
|
# The second option assumes that the generated headers shall be saved
|
||||||
|
# under the version control, which may be undesirable.
|
||||||
|
# The first option requires any Python and the Python Mako library for the sources to be built.
|
||||||
|
#
|
||||||
|
$(info $(shell $(LIBUAVCAN_DSDLC) $(UAVCAN_DSDL_DIR)))
|
||||||
|
INCLUDE_DIRS += dsdlc_generated
|
||||||
|
|
@ -0,0 +1,79 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2014 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.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#include <uavcan_stm32/uavcan_stm32.hpp>
|
||||||
|
#include <drivers/drv_hrt.h>
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file uavcan_clock.cpp
|
||||||
|
*
|
||||||
|
* Implements a clock for the CAN node.
|
||||||
|
*
|
||||||
|
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||||
|
*/
|
||||||
|
|
||||||
|
namespace uavcan_stm32
|
||||||
|
{
|
||||||
|
namespace clock
|
||||||
|
{
|
||||||
|
|
||||||
|
uavcan::MonotonicTime getMonotonic()
|
||||||
|
{
|
||||||
|
return uavcan::MonotonicTime::fromUSec(hrt_absolute_time());
|
||||||
|
}
|
||||||
|
|
||||||
|
uavcan::UtcTime getUtc()
|
||||||
|
{
|
||||||
|
return uavcan::UtcTime();
|
||||||
|
}
|
||||||
|
|
||||||
|
void adjustUtc(uavcan::UtcDuration adjustment)
|
||||||
|
{
|
||||||
|
(void)adjustment;
|
||||||
|
}
|
||||||
|
|
||||||
|
uavcan::uint64_t getUtcUSecFromCanInterrupt()
|
||||||
|
{
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace clock
|
||||||
|
|
||||||
|
SystemClock &SystemClock::instance()
|
||||||
|
{
|
||||||
|
static SystemClock inst;
|
||||||
|
return inst;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
@ -0,0 +1,582 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2014 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.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#include <nuttx/config.h>
|
||||||
|
|
||||||
|
#include <cstdlib>
|
||||||
|
#include <cstring>
|
||||||
|
#include <fcntl.h>
|
||||||
|
#include <systemlib/err.h>
|
||||||
|
#include <systemlib/systemlib.h>
|
||||||
|
#include <systemlib/mixer/mixer.h>
|
||||||
|
#include <arch/board/board.h>
|
||||||
|
#include <arch/chip/chip.h>
|
||||||
|
|
||||||
|
#include <drivers/drv_hrt.h>
|
||||||
|
#include <drivers/drv_pwm_output.h>
|
||||||
|
|
||||||
|
#include "uavcan_main.hpp"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file uavcan_main.cpp
|
||||||
|
*
|
||||||
|
* Implements basic functinality of UAVCAN node.
|
||||||
|
*
|
||||||
|
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||||
|
*/
|
||||||
|
|
||||||
|
/*
|
||||||
|
* UavcanNode
|
||||||
|
*/
|
||||||
|
UavcanNode *UavcanNode::_instance;
|
||||||
|
|
||||||
|
UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
|
||||||
|
CDev("uavcan", UAVCAN_DEVICE_PATH),
|
||||||
|
_node(can_driver, system_clock),
|
||||||
|
_esc_controller(_node),
|
||||||
|
_gnss_receiver(_node)
|
||||||
|
{
|
||||||
|
_control_topics[0] = ORB_ID(actuator_controls_0);
|
||||||
|
_control_topics[1] = ORB_ID(actuator_controls_1);
|
||||||
|
_control_topics[2] = ORB_ID(actuator_controls_2);
|
||||||
|
_control_topics[3] = ORB_ID(actuator_controls_3);
|
||||||
|
|
||||||
|
// memset(_controls, 0, sizeof(_controls));
|
||||||
|
// memset(_poll_fds, 0, sizeof(_poll_fds));
|
||||||
|
}
|
||||||
|
|
||||||
|
UavcanNode::~UavcanNode()
|
||||||
|
{
|
||||||
|
if (_task != -1) {
|
||||||
|
/* tell the task we want it to go away */
|
||||||
|
_task_should_exit = true;
|
||||||
|
|
||||||
|
unsigned i = 10;
|
||||||
|
|
||||||
|
do {
|
||||||
|
/* wait 5ms - it should wake every 10ms or so worst-case */
|
||||||
|
usleep(5000);
|
||||||
|
|
||||||
|
/* if we have given up, kill it */
|
||||||
|
if (--i == 0) {
|
||||||
|
task_delete(_task);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
} while (_task != -1);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* clean up the alternate device node */
|
||||||
|
// unregister_driver(PWM_OUTPUT_DEVICE_PATH);
|
||||||
|
|
||||||
|
::close(_armed_sub);
|
||||||
|
|
||||||
|
_instance = nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
|
||||||
|
{
|
||||||
|
if (_instance != nullptr) {
|
||||||
|
warnx("Already started");
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* GPIO config.
|
||||||
|
* Forced pull up on CAN2 is required for Pixhawk v1 where the second interface lacks a transceiver.
|
||||||
|
* If no transceiver is connected, the RX pin will float, occasionally causing CAN controller to
|
||||||
|
* fail during initialization.
|
||||||
|
*/
|
||||||
|
stm32_configgpio(GPIO_CAN1_RX);
|
||||||
|
stm32_configgpio(GPIO_CAN1_TX);
|
||||||
|
stm32_configgpio(GPIO_CAN2_RX | GPIO_PULLUP);
|
||||||
|
stm32_configgpio(GPIO_CAN2_TX);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* CAN driver init
|
||||||
|
*/
|
||||||
|
static CanInitHelper can;
|
||||||
|
static bool can_initialized = false;
|
||||||
|
|
||||||
|
if (!can_initialized) {
|
||||||
|
const int can_init_res = can.init(bitrate);
|
||||||
|
|
||||||
|
if (can_init_res < 0) {
|
||||||
|
warnx("CAN driver init failed %i", can_init_res);
|
||||||
|
return can_init_res;
|
||||||
|
}
|
||||||
|
|
||||||
|
can_initialized = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Node init
|
||||||
|
*/
|
||||||
|
_instance = new UavcanNode(can.driver, uavcan_stm32::SystemClock::instance());
|
||||||
|
|
||||||
|
if (_instance == nullptr) {
|
||||||
|
warnx("Out of memory");
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
const int node_init_res = _instance->init(node_id);
|
||||||
|
|
||||||
|
if (node_init_res < 0) {
|
||||||
|
delete _instance;
|
||||||
|
_instance = nullptr;
|
||||||
|
warnx("Node init failed %i", node_init_res);
|
||||||
|
return node_init_res;
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Start the task. Normally it should never exit.
|
||||||
|
*/
|
||||||
|
static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();};
|
||||||
|
_instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize,
|
||||||
|
static_cast<main_t>(run_trampoline), nullptr);
|
||||||
|
|
||||||
|
if (_instance->_task < 0) {
|
||||||
|
warnx("start failed: %d", errno);
|
||||||
|
return -errno;
|
||||||
|
}
|
||||||
|
|
||||||
|
return OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
int UavcanNode::init(uavcan::NodeID node_id)
|
||||||
|
{
|
||||||
|
int ret = -1;
|
||||||
|
|
||||||
|
/* do regular cdev init */
|
||||||
|
ret = CDev::init();
|
||||||
|
|
||||||
|
if (ret != OK)
|
||||||
|
return ret;
|
||||||
|
|
||||||
|
ret = _esc_controller.init();
|
||||||
|
if (ret < 0)
|
||||||
|
return ret;
|
||||||
|
|
||||||
|
ret = _gnss_receiver.init();
|
||||||
|
if (ret < 0)
|
||||||
|
return ret;
|
||||||
|
|
||||||
|
uavcan::protocol::SoftwareVersion swver;
|
||||||
|
swver.major = 12; // TODO fill version info
|
||||||
|
swver.minor = 34;
|
||||||
|
_node.setSoftwareVersion(swver);
|
||||||
|
|
||||||
|
uavcan::protocol::HardwareVersion hwver;
|
||||||
|
hwver.major = 42; // TODO fill version info
|
||||||
|
hwver.minor = 42;
|
||||||
|
_node.setHardwareVersion(hwver);
|
||||||
|
|
||||||
|
_node.setName("org.pixhawk"); // Huh?
|
||||||
|
|
||||||
|
_node.setNodeID(node_id);
|
||||||
|
|
||||||
|
return _node.start();
|
||||||
|
}
|
||||||
|
|
||||||
|
void UavcanNode::node_spin_once()
|
||||||
|
{
|
||||||
|
const int spin_res = _node.spin(uavcan::MonotonicTime());
|
||||||
|
if (spin_res < 0) {
|
||||||
|
warnx("node spin error %i", spin_res);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
int UavcanNode::run()
|
||||||
|
{
|
||||||
|
const unsigned PollTimeoutMs = 50;
|
||||||
|
|
||||||
|
// XXX figure out the output count
|
||||||
|
_output_count = 2;
|
||||||
|
|
||||||
|
|
||||||
|
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
|
||||||
|
|
||||||
|
actuator_outputs_s outputs;
|
||||||
|
memset(&outputs, 0, sizeof(outputs));
|
||||||
|
|
||||||
|
const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0);
|
||||||
|
if (busevent_fd < 0)
|
||||||
|
{
|
||||||
|
warnx("Failed to open %s", uavcan_stm32::BusEvent::DevName);
|
||||||
|
_task_should_exit = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* XXX Mixing logic/subscriptions shall be moved into UavcanEscController::update();
|
||||||
|
* IO multiplexing shall be done here.
|
||||||
|
*/
|
||||||
|
|
||||||
|
_node.setStatusOk();
|
||||||
|
|
||||||
|
while (!_task_should_exit) {
|
||||||
|
|
||||||
|
if (_groups_subscribed != _groups_required) {
|
||||||
|
subscribe();
|
||||||
|
_groups_subscribed = _groups_required;
|
||||||
|
/*
|
||||||
|
* This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
|
||||||
|
* Please note that with such multiplexing it is no longer possible to rely only on
|
||||||
|
* the value returned from poll() to detect whether actuator control has timed out or not.
|
||||||
|
* Instead, all ORB events need to be checked individually (see below).
|
||||||
|
*/
|
||||||
|
_poll_fds[_poll_fds_num] = ::pollfd();
|
||||||
|
_poll_fds[_poll_fds_num].fd = busevent_fd;
|
||||||
|
_poll_fds[_poll_fds_num].events = POLLIN;
|
||||||
|
_poll_fds_num += 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
|
||||||
|
|
||||||
|
node_spin_once(); // Non-blocking
|
||||||
|
|
||||||
|
// this would be bad...
|
||||||
|
if (poll_ret < 0) {
|
||||||
|
log("poll error %d", errno);
|
||||||
|
continue;
|
||||||
|
} else {
|
||||||
|
// get controls for required topics
|
||||||
|
bool controls_updated = false;
|
||||||
|
unsigned poll_id = 0;
|
||||||
|
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
|
||||||
|
if (_control_subs[i] > 0) {
|
||||||
|
if (_poll_fds[poll_id].revents & POLLIN) {
|
||||||
|
controls_updated = true;
|
||||||
|
orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
|
||||||
|
}
|
||||||
|
poll_id++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!controls_updated) {
|
||||||
|
// timeout: no control data, switch to failsafe values
|
||||||
|
// XXX trigger failsafe
|
||||||
|
}
|
||||||
|
|
||||||
|
//can we mix?
|
||||||
|
if (controls_updated && (_mixers != nullptr)) {
|
||||||
|
|
||||||
|
// XXX one output group has 8 outputs max,
|
||||||
|
// but this driver could well serve multiple groups.
|
||||||
|
unsigned num_outputs_max = 8;
|
||||||
|
|
||||||
|
// Do mixing
|
||||||
|
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs_max);
|
||||||
|
outputs.timestamp = hrt_absolute_time();
|
||||||
|
|
||||||
|
// iterate actuators
|
||||||
|
for (unsigned i = 0; i < outputs.noutputs; i++) {
|
||||||
|
// last resort: catch NaN, INF and out-of-band errors
|
||||||
|
if (!isfinite(outputs.output[i])) {
|
||||||
|
/*
|
||||||
|
* Value is NaN, INF or out of band - set to the minimum value.
|
||||||
|
* This will be clearly visible on the servo status and will limit the risk of accidentally
|
||||||
|
* spinning motors. It would be deadly in flight.
|
||||||
|
*/
|
||||||
|
outputs.output[i] = -1.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
// limit outputs to valid range
|
||||||
|
|
||||||
|
// never go below min
|
||||||
|
if (outputs.output[i] < -1.0f) {
|
||||||
|
outputs.output[i] = -1.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
// never go below max
|
||||||
|
if (outputs.output[i] > 1.0f) {
|
||||||
|
outputs.output[i] = 1.0f;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Output to the bus
|
||||||
|
_esc_controller.update_outputs(outputs.output, outputs.noutputs);
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// Check arming state
|
||||||
|
bool updated = false;
|
||||||
|
orb_check(_armed_sub, &updated);
|
||||||
|
|
||||||
|
if (updated) {
|
||||||
|
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
|
||||||
|
|
||||||
|
// Update the armed status and check that we're not locked down
|
||||||
|
bool set_armed = _armed.armed && !_armed.lockdown;
|
||||||
|
|
||||||
|
arm_actuators(set_armed);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
teardown();
|
||||||
|
warnx("exiting.");
|
||||||
|
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
UavcanNode::control_callback(uintptr_t handle,
|
||||||
|
uint8_t control_group,
|
||||||
|
uint8_t control_index,
|
||||||
|
float &input)
|
||||||
|
{
|
||||||
|
const actuator_controls_s *controls = (actuator_controls_s *)handle;
|
||||||
|
|
||||||
|
input = controls[control_group].control[control_index];
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
UavcanNode::teardown()
|
||||||
|
{
|
||||||
|
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
|
||||||
|
if (_control_subs[i] > 0) {
|
||||||
|
::close(_control_subs[i]);
|
||||||
|
_control_subs[i] = -1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return ::close(_armed_sub);
|
||||||
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
UavcanNode::arm_actuators(bool arm)
|
||||||
|
{
|
||||||
|
_is_armed = arm;
|
||||||
|
_esc_controller.arm_esc(arm);
|
||||||
|
return OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
void
|
||||||
|
UavcanNode::subscribe()
|
||||||
|
{
|
||||||
|
// Subscribe/unsubscribe to required actuator control groups
|
||||||
|
uint32_t sub_groups = _groups_required & ~_groups_subscribed;
|
||||||
|
uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
|
||||||
|
_poll_fds_num = 0;
|
||||||
|
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
|
||||||
|
if (sub_groups & (1 << i)) {
|
||||||
|
warnx("subscribe to actuator_controls_%d", i);
|
||||||
|
_control_subs[i] = orb_subscribe(_control_topics[i]);
|
||||||
|
}
|
||||||
|
if (unsub_groups & (1 << i)) {
|
||||||
|
warnx("unsubscribe from actuator_controls_%d", i);
|
||||||
|
::close(_control_subs[i]);
|
||||||
|
_control_subs[i] = -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (_control_subs[i] > 0) {
|
||||||
|
_poll_fds[_poll_fds_num].fd = _control_subs[i];
|
||||||
|
_poll_fds[_poll_fds_num].events = POLLIN;
|
||||||
|
_poll_fds_num++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
|
||||||
|
{
|
||||||
|
int ret = OK;
|
||||||
|
|
||||||
|
lock();
|
||||||
|
|
||||||
|
switch (cmd) {
|
||||||
|
case PWM_SERVO_ARM:
|
||||||
|
arm_actuators(true);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case PWM_SERVO_SET_ARM_OK:
|
||||||
|
case PWM_SERVO_CLEAR_ARM_OK:
|
||||||
|
case PWM_SERVO_SET_FORCE_SAFETY_OFF:
|
||||||
|
// these are no-ops, as no safety switch
|
||||||
|
break;
|
||||||
|
|
||||||
|
case PWM_SERVO_DISARM:
|
||||||
|
arm_actuators(false);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MIXERIOCGETOUTPUTCOUNT:
|
||||||
|
*(unsigned *)arg = _output_count;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MIXERIOCRESET:
|
||||||
|
if (_mixers != nullptr) {
|
||||||
|
delete _mixers;
|
||||||
|
_mixers = nullptr;
|
||||||
|
_groups_required = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MIXERIOCLOADBUF: {
|
||||||
|
const char *buf = (const char *)arg;
|
||||||
|
unsigned buflen = strnlen(buf, 1024);
|
||||||
|
|
||||||
|
if (_mixers == nullptr)
|
||||||
|
_mixers = new MixerGroup(control_callback, (uintptr_t)_controls);
|
||||||
|
|
||||||
|
if (_mixers == nullptr) {
|
||||||
|
_groups_required = 0;
|
||||||
|
ret = -ENOMEM;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
|
||||||
|
ret = _mixers->load_from_buf(buf, buflen);
|
||||||
|
|
||||||
|
if (ret != 0) {
|
||||||
|
warnx("mixer load failed with %d", ret);
|
||||||
|
delete _mixers;
|
||||||
|
_mixers = nullptr;
|
||||||
|
_groups_required = 0;
|
||||||
|
ret = -EINVAL;
|
||||||
|
} else {
|
||||||
|
|
||||||
|
_mixers->groups_required(_groups_required);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
default:
|
||||||
|
ret = -ENOTTY;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
unlock();
|
||||||
|
|
||||||
|
if (ret == -ENOTTY) {
|
||||||
|
ret = CDev::ioctl(filp, cmd, arg);
|
||||||
|
}
|
||||||
|
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
void
|
||||||
|
UavcanNode::print_info()
|
||||||
|
{
|
||||||
|
if (!_instance) {
|
||||||
|
warnx("not running, start first");
|
||||||
|
}
|
||||||
|
|
||||||
|
warnx("groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
|
||||||
|
warnx("mixer: %s", (_mixers == nullptr) ? "FAIL" : "OK");
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* App entry point
|
||||||
|
*/
|
||||||
|
static void print_usage()
|
||||||
|
{
|
||||||
|
warnx("usage: uavcan start <node_id> [can_bitrate]");
|
||||||
|
}
|
||||||
|
|
||||||
|
extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
|
||||||
|
|
||||||
|
int uavcan_main(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
constexpr unsigned DEFAULT_CAN_BITRATE = 1000000;
|
||||||
|
|
||||||
|
if (argc < 2) {
|
||||||
|
print_usage();
|
||||||
|
::exit(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!std::strcmp(argv[1], "start")) {
|
||||||
|
if (argc < 3) {
|
||||||
|
print_usage();
|
||||||
|
::exit(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Node ID
|
||||||
|
*/
|
||||||
|
const int node_id = atoi(argv[2]);
|
||||||
|
|
||||||
|
if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) {
|
||||||
|
warnx("Invalid Node ID %i", node_id);
|
||||||
|
::exit(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* CAN bitrate
|
||||||
|
*/
|
||||||
|
unsigned bitrate = 0;
|
||||||
|
|
||||||
|
if (argc > 3) {
|
||||||
|
bitrate = atol(argv[3]);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (bitrate <= 0) {
|
||||||
|
bitrate = DEFAULT_CAN_BITRATE;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (UavcanNode::instance()) {
|
||||||
|
errx(1, "already started");
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Start
|
||||||
|
*/
|
||||||
|
warnx("Node ID %u, bitrate %u", node_id, bitrate);
|
||||||
|
return UavcanNode::start(node_id, bitrate);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/* commands below require the app to be started */
|
||||||
|
UavcanNode *inst = UavcanNode::instance();
|
||||||
|
|
||||||
|
if (!inst) {
|
||||||
|
errx(1, "application not running");
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) {
|
||||||
|
|
||||||
|
inst->print_info();
|
||||||
|
return OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!std::strcmp(argv[1], "stop")) {
|
||||||
|
|
||||||
|
delete inst;
|
||||||
|
inst = nullptr;
|
||||||
|
return OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
print_usage();
|
||||||
|
::exit(1);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,123 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2014 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.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <nuttx/config.h>
|
||||||
|
|
||||||
|
#include <uavcan_stm32/uavcan_stm32.hpp>
|
||||||
|
#include <drivers/device/device.h>
|
||||||
|
|
||||||
|
#include <uORB/topics/actuator_controls.h>
|
||||||
|
#include <uORB/topics/actuator_outputs.h>
|
||||||
|
#include <uORB/topics/actuator_armed.h>
|
||||||
|
|
||||||
|
#include "esc_controller.hpp"
|
||||||
|
#include "gnss_receiver.hpp"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file uavcan_main.hpp
|
||||||
|
*
|
||||||
|
* Defines basic functinality of UAVCAN node.
|
||||||
|
*
|
||||||
|
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||||
|
*/
|
||||||
|
|
||||||
|
#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4
|
||||||
|
#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A UAVCAN node.
|
||||||
|
*/
|
||||||
|
class UavcanNode : public device::CDev
|
||||||
|
{
|
||||||
|
static constexpr unsigned MemPoolSize = 10752;
|
||||||
|
static constexpr unsigned RxQueueLenPerIface = 64;
|
||||||
|
static constexpr unsigned StackSize = 3000;
|
||||||
|
|
||||||
|
public:
|
||||||
|
typedef uavcan::Node<MemPoolSize> Node;
|
||||||
|
typedef uavcan_stm32::CanInitHelper<RxQueueLenPerIface> CanInitHelper;
|
||||||
|
|
||||||
|
UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock);
|
||||||
|
|
||||||
|
virtual ~UavcanNode();
|
||||||
|
|
||||||
|
virtual int ioctl(file *filp, int cmd, unsigned long arg);
|
||||||
|
|
||||||
|
static int start(uavcan::NodeID node_id, uint32_t bitrate);
|
||||||
|
|
||||||
|
Node& getNode() { return _node; }
|
||||||
|
|
||||||
|
static int control_callback(uintptr_t handle,
|
||||||
|
uint8_t control_group,
|
||||||
|
uint8_t control_index,
|
||||||
|
float &input);
|
||||||
|
|
||||||
|
void subscribe();
|
||||||
|
|
||||||
|
int teardown();
|
||||||
|
int arm_actuators(bool arm);
|
||||||
|
|
||||||
|
void print_info();
|
||||||
|
|
||||||
|
static UavcanNode* instance() { return _instance; }
|
||||||
|
|
||||||
|
private:
|
||||||
|
int init(uavcan::NodeID node_id);
|
||||||
|
void node_spin_once();
|
||||||
|
int run();
|
||||||
|
|
||||||
|
int _task = -1; ///< handle to the OS task
|
||||||
|
bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver
|
||||||
|
int _armed_sub = -1; ///< uORB subscription of the arming status
|
||||||
|
actuator_armed_s _armed; ///< the arming request of the system
|
||||||
|
bool _is_armed = false; ///< the arming status of the actuators on the bus
|
||||||
|
|
||||||
|
unsigned _output_count = 0; ///< number of actuators currently available
|
||||||
|
|
||||||
|
static UavcanNode *_instance; ///< singleton pointer
|
||||||
|
Node _node; ///< library instance
|
||||||
|
UavcanEscController _esc_controller;
|
||||||
|
UavcanGnssReceiver _gnss_receiver;
|
||||||
|
|
||||||
|
MixerGroup *_mixers = nullptr;
|
||||||
|
|
||||||
|
uint32_t _groups_required = 0;
|
||||||
|
uint32_t _groups_subscribed = 0;
|
||||||
|
int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
|
||||||
|
actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
|
||||||
|
orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
|
||||||
|
pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {}; ///< +1 for /dev/uavcan/busevent
|
||||||
|
unsigned _poll_fds_num = 0;
|
||||||
|
};
|
||||||
|
|
@ -0,0 +1 @@
|
||||||
|
Subproject commit dca2611c3186eaa1cac42557f07b013e2dc633d3
|
||||||
Loading…
Reference in New Issue