dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge commit '2780dc39ce5d47f2d9dfa921062100a1dc86c2be' into mpc_track

This commit is contained in:
Anton Babushkin 2014-09-01 15:19:42 +02:00
parent f31c3243b0 2780dc39ce
commit 3c7b9ef94d
76 changed files with 2186 additions and 492 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
NuttX

@ -1 +1 @@
Subproject commit 088146b90eee5b614ea6386a64dae343a49a5172
Subproject commit 41fffa0df192e4a26d2325c7ac5d9b5d7ba0211c

3
ROMFS/px4fmu_common/init.d/3031_phantom
View File

@ -30,6 +30,9 @@ then
param set FW_RR_P 0.08
param set FW_R_LIM 50
param set FW_R_RMAX 0
# Bottom of bay and nominal zero-pitch attitude differ
# the payload bay is pitched up about 7 degrees
param set SENS_BOARD_Y_OFF 7.0
fi
set MIXER phantom

2
ROMFS/px4fmu_common/init.d/3035_viper
View File

@ -8,3 +8,5 @@
sh /etc/init.d/rc.fw_defaults
set MIXER Viper
set FAILSAFE "-c567 -p 1000"

5
ROMFS/px4fmu_common/init.d/rc.interface
View File

@ -77,4 +77,9 @@ then
pwm max -c $PWM_OUTPUTS -p $PWM_MAX
fi
fi
if [ $FAILSAFE != none ]
then
pwm failsafe -d $OUTPUT_DEV $FAILSAFE
fi
fi

18
ROMFS/px4fmu_common/init.d/rc.uavcan Normal file
View File

@ -0,0 +1,18 @@
#!nsh
#
# UAVCAN initialization script.
#
if param compare UAVCAN_ENABLE 1
then
if uavcan start
then
# First sensor publisher to initialize takes lowest instance ID
# This delay ensures that UAVCAN-interfaced sensors would be allocated on lowest instance IDs
sleep 1
echo "[init] UAVCAN started"
else
echo "[init] ERROR: Could not start UAVCAN"
tone_alarm $TUNE_OUT_ERROR
fi
fi

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

@ -66,6 +66,9 @@ then
#
sercon
# Try to get an USB console
nshterm /dev/ttyACM0 &
#
# Start the ORB (first app to start)
#
@ -96,11 +99,9 @@ then
#
if rgbled start
then
echo "[init] RGB Led"
else
if blinkm start
then
echo "[init] BlinkM"
blinkm systemstate
fi
fi
@ -129,6 +130,7 @@ then
set LOAD_DEFAULT_APPS yes
set GPS yes
set GPS_FAKE no
set FAILSAFE none
#
# Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts
@ -279,9 +281,6 @@ then
fi
fi
# Try to get an USB console
nshterm /dev/ttyACM0 &
#
# Start the datamanager (and do not abort boot if it fails)
#
@ -304,11 +303,10 @@ then
then
if [ $OUTPUT_MODE == uavcan_esc ]
then
if uavcan start 1
if param compare UAVCAN_ENABLE 0
then
echo "CAN UP"
else
echo "CAN ERR"
echo "[init] OVERRIDING UAVCAN_ENABLE = 1"
param set UAVCAN_ENABLE 1
fi
fi
@ -447,6 +445,11 @@ then
mavlink start $MAVLINK_FLAGS
#
# UAVCAN
#
sh /etc/init.d/rc.uavcan
#
# Sensors, Logging, GPS
#

11
ROMFS/px4fmu_common/mixers/FMU_AERT.mix
View File

@ -64,21 +64,22 @@ O: 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000
Gimbal / flaps / payload mixer for last four channels
Gimbal / flaps / payload mixer for last four channels,
using the payload control group
-----------------------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
S: 0 4 10000 10000 0 -10000 10000
S: 2 0 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 5 10000 10000 0 -10000 10000
S: 2 1 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 6 10000 10000 0 -10000 10000
S: 2 2 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 7 10000 10000 0 -10000 10000
S: 2 3 10000 10000 0 -10000 10000

13
ROMFS/px4fmu_common/mixers/Viper.mix
View File

@ -52,21 +52,20 @@ M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000
Gimbal / flaps / payload mixer for last four channels
Inputs to the mixer come from channel group 2 (payload), channels 0
(bay servo 1), 1 (bay servo 2) and 3 (drop release).
-----------------------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
S: 0 4 10000 10000 0 -10000 10000
S: 2 0 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 5 10000 10000 0 -10000 10000
S: 2 1 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 6 10000 10000 0 -10000 10000
S: 2 2 -10000 -10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 7 10000 10000 0 -10000 10000

6
Tools/px_uploader.py
View File

@ -178,9 +178,9 @@ class uploader(object):
MAVLINK_REBOOT_ID1 = bytearray(b'\xfe\x21\x72\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x01\x00\x00\x48\xf0')
MAVLINK_REBOOT_ID0 = bytearray(b'\xfe\x21\x45\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x00\x00\x00\xd7\xac')
def __init__(self, portname, baudrate):
def __init__(self, portname, baudrate, interCharTimeout=0.001, timeout=0.5):
# open the port, keep the default timeout short so we can poll quickly
self.port = serial.Serial(portname, baudrate, timeout=2.0)
self.port = serial.Serial(portname, baudrate)
self.otp = b''
self.sn = b''
@ -195,7 +195,7 @@ class uploader(object):
def __recv(self, count=1):
c = self.port.read(count)
if len(c) < 1:
raise RuntimeError("timeout waiting for data")
raise RuntimeError("timeout waiting for data (%u bytes)", count)
# print("recv " + binascii.hexlify(c))
return c

5
makefiles/config_px4fmu-v1_default.mk
View File

@ -25,7 +25,6 @@ MODULES += drivers/mpu6000
MODULES += drivers/hmc5883
MODULES += drivers/ms5611
MODULES += drivers/mb12xx
MODULES += drivers/ll40ls
MODULES += drivers/gps
MODULES += drivers/hil
MODULES += drivers/hott/hott_telemetry
@ -44,7 +43,6 @@ MODULES += modules/sensors
#
MODULES += systemcmds/mtd
MODULES += systemcmds/bl_update
MODULES += systemcmds/i2c
MODULES += systemcmds/mixer
MODULES += systemcmds/param
MODULES += systemcmds/perf
@ -152,5 +150,4 @@ endef
# command priority stack entrypoint
BUILTIN_COMMANDS := \
$(call _B, sercon, , 2048, sercon_main ) \
$(call _B, serdis, , 2048, serdis_main ) \
$(call _B, sysinfo, , 2048, sysinfo_main )
$(call _B, serdis, , 2048, serdis_main )

1
makefiles/config_px4fmu-v2_default.mk
View File

@ -41,6 +41,7 @@ MODULES += drivers/frsky_telemetry
MODULES += modules/sensors
MODULES += drivers/mkblctrl
MODULES += drivers/pca8574
MODULES += drivers/px4flow
# Needs to be burned to the ground and re-written; for now,

2
mavlink/include/mavlink/v1.0

@ -1 +1 @@
Subproject commit 4d7487c2bc5f5ccf87bca82970fb2c08d6d8bd50
Subproject commit 2423e47b4f9169e77f7194b36fa2118e018c94e2

2
nuttx-configs/aerocore/nsh/defconfig
View File

@ -314,7 +314,7 @@ CONFIG_STM32_I2CTIMEOTICKS=500
# CONFIG_ARCH_NOINTC is not set
# CONFIG_ARCH_VECNOTIRQ is not set
CONFIG_ARCH_DMA=y
CONFIG_ARCH_IRQPRIO=y
# CONFIG_ARCH_IRQPRIO is not set
# CONFIG_CUSTOM_STACK is not set
# CONFIG_ADDRENV is not set
CONFIG_ARCH_HAVE_VFORK=y

5
nuttx-configs/px4fmu-v1/nsh/defconfig
View File

@ -287,8 +287,7 @@ CONFIG_STM32_USART_SINGLEWIRE=y
#
# CONFIG_STM32_I2C_DYNTIMEO is not set
CONFIG_STM32_I2CTIMEOSEC=0
CONFIG_STM32_I2CTIMEOMS=10
CONFIG_STM32_I2CTIMEOTICKS=500
CONFIG_STM32_I2CTIMEOMS=1
# CONFIG_STM32_I2C_DUTY16_9 is not set
#
@ -309,7 +308,7 @@ CONFIG_STM32_I2CTIMEOTICKS=500
# CONFIG_ARCH_NOINTC is not set
# CONFIG_ARCH_VECNOTIRQ is not set
CONFIG_ARCH_DMA=y
CONFIG_ARCH_IRQPRIO=y
# CONFIG_ARCH_IRQPRIO is not set
# CONFIG_CUSTOM_STACK is not set
# CONFIG_ADDRENV is not set
CONFIG_ARCH_HAVE_VFORK=y

5
nuttx-configs/px4fmu-v2/nsh/defconfig
View File

@ -323,8 +323,7 @@ CONFIG_STM32_USART_SINGLEWIRE=y
#
# CONFIG_STM32_I2C_DYNTIMEO is not set
CONFIG_STM32_I2CTIMEOSEC=0
CONFIG_STM32_I2CTIMEOMS=10
CONFIG_STM32_I2CTIMEOTICKS=500
CONFIG_STM32_I2CTIMEOMS=1
# CONFIG_STM32_I2C_DUTY16_9 is not set
#
@ -350,7 +349,7 @@ CONFIG_SDIO_PRI=128
# CONFIG_ARCH_NOINTC is not set
# CONFIG_ARCH_VECNOTIRQ is not set
CONFIG_ARCH_DMA=y
CONFIG_ARCH_IRQPRIO=y
# CONFIG_ARCH_IRQPRIO is not set
# CONFIG_CUSTOM_STACK is not set
# CONFIG_ADDRENV is not set
CONFIG_ARCH_HAVE_VFORK=y

3
nuttx-configs/px4io-v1/nsh/defconfig
View File

@ -83,7 +83,6 @@ CONFIG_ARCH_BOARD="px4io-v1"
CONFIG_BOARD_LOOPSPERMSEC=2000
CONFIG_DRAM_SIZE=0x00002000
CONFIG_DRAM_START=0x20000000
CONFIG_ARCH_IRQPRIO=y
CONFIG_ARCH_INTERRUPTSTACK=n
CONFIG_ARCH_STACKDUMP=y
CONFIG_ARCH_BOOTLOADER=n
@ -134,6 +133,8 @@ CONFIG_STM32_USART2=y
CONFIG_STM32_USART3=y
CONFIG_STM32_I2C1=y
CONFIG_STM32_I2C2=n
CONFIG_STM32_I2CTIMEOSEC=0
CONFIG_STM32_I2CTIMEOMS=1
CONFIG_STM32_BKP=n
CONFIG_STM32_PWR=n
CONFIG_STM32_DAC=n

1
nuttx-configs/px4io-v2/nsh/defconfig
View File

@ -79,7 +79,6 @@ CONFIG_ARCH_BOARD_PX4IO_V2=y
CONFIG_BOARD_LOOPSPERMSEC=2000
CONFIG_DRAM_SIZE=0x00002000
CONFIG_DRAM_START=0x20000000
CONFIG_ARCH_IRQPRIO=y
CONFIG_ARCH_INTERRUPTSTACK=n
CONFIG_ARCH_STACKDUMP=y
CONFIG_ARCH_BOOTLOADER=n

7
src/drivers/airspeed/airspeed.cpp
View File

@ -165,7 +165,7 @@ Airspeed::probe()
*/
_retries = 4;
int ret = measure();
_retries = 2;
_retries = 0;
return ret;
}
@ -381,7 +381,10 @@ Airspeed::cycle_trampoline(void *arg)
Airspeed *dev = (Airspeed *)arg;
dev->cycle();
dev->update_status();
// XXX we do not know if this is
// really helping - do not update the
// subsys state right now
//dev->update_status();
}
void

5
src/drivers/drv_pwm_output.h
View File

@ -94,6 +94,11 @@ __BEGIN_DECLS
*/
#define PWM_LOWEST_MAX 1700
/**
* Do not output a channel with this value
*/
#define PWM_IGNORE_THIS_CHANNEL UINT16_MAX
/**
* Servo output signal type, value is actual servo output pulse
* width in microseconds.

31
src/drivers/drv_px4flow.h
View File

@ -46,37 +46,6 @@
#define PX4FLOW_DEVICE_PATH "/dev/px4flow"
/**
* @addtogroup topics
* @{
*/
/**
* Optical flow in NED body frame in SI units.
*
* @see http://en.wikipedia.org/wiki/International_System_of_Units
*
* @warning If possible the usage of the raw flow and performing rotation-compensation
* using the autopilot angular rate estimate is recommended.
*/
struct px4flow_report {
uint64_t timestamp; /**< in microseconds since system start */
int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */
int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */
float flow_comp_x_m; /**< speed over ground in meters per second, rotation-compensated */
float flow_comp_y_m; /**< speed over ground in meters per second, rotation-compensated */
float ground_distance_m; /**< Altitude / distance to ground in meters */
uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */
uint8_t sensor_id; /**< id of the sensor emitting the flow value */
};
/**
* @}
*/
/*
* ObjDev tag for px4flow data.
*/

20
src/drivers/ets_airspeed/ets_airspeed.cpp
View File

@ -155,7 +155,6 @@ ETSAirspeed::collect()
}
uint16_t diff_pres_pa_raw = val[1] << 8 | val[0];
uint16_t diff_pres_pa;
if (diff_pres_pa_raw == 0) {
// a zero value means the pressure sensor cannot give us a
// value. We need to return, and not report a value or the
@ -166,28 +165,21 @@ ETSAirspeed::collect()
return -1;
}
if (diff_pres_pa_raw < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
diff_pres_pa = 0;
} else {
diff_pres_pa = diff_pres_pa_raw - _diff_pres_offset;
}
// The raw value still should be compensated for the known offset
diff_pres_pa_raw -= _diff_pres_offset;
// Track maximum differential pressure measured (so we can work out top speed).
if (diff_pres_pa > _max_differential_pressure_pa) {
_max_differential_pressure_pa = diff_pres_pa;
if (diff_pres_pa_raw > _max_differential_pressure_pa) {
_max_differential_pressure_pa = diff_pres_pa_raw;
}
differential_pressure_s report;
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.differential_pressure_pa = (float)diff_pres_pa;
// XXX we may want to smooth out the readings to remove noise.
report.differential_pressure_filtered_pa = (float)diff_pres_pa;
report.differential_pressure_raw_pa = (float)diff_pres_pa_raw;
report.differential_pressure_filtered_pa = diff_pres_pa_raw;
report.differential_pressure_raw_pa = diff_pres_pa_raw;
report.temperature = -1000.0f;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
@ -369,7 +361,7 @@ test()
err(1, "immediate read failed");
warnx("single read");
warnx("diff pressure: %f pa", (double)report.differential_pressure_pa);
warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
@ -394,7 +386,7 @@ test()
err(1, "periodic read failed");
warnx("periodic read %u", i);
warnx("diff pressure: %f pa", (double)report.differential_pressure_pa);
warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);
}
/* reset the sensor polling to its default rate */

3
src/drivers/hmc5883/hmc5883.cpp
View File

@ -1283,14 +1283,13 @@ int HMC5883::set_excitement(unsigned enable)
if (OK != ret)
perf_count(_comms_errors);
_conf_reg &= ~0x03; // reset previous excitement mode
if (((int)enable) < 0) {
_conf_reg |= 0x01;
} else if (enable > 0) {
_conf_reg |= 0x02;
} else {
_conf_reg &= ~0x03;
}
// ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)_conf_reg);

37
src/drivers/meas_airspeed/meas_airspeed.cpp
View File

@ -228,44 +228,23 @@ MEASAirspeed::collect()
// the raw value still should be compensated for the known offset
diff_press_pa_raw -= _diff_pres_offset;
float diff_press_pa = fabsf(diff_press_pa_raw);
/*
note that we return both the absolute value with offset
applied and a raw value without the offset applied. This
makes it possible for higher level code to detect if the
user has the tubes connected backwards, and also makes it
possible to correctly use offsets calculated by a higher
level airspeed driver.
With the above calculation the MS4525 sensor will produce a
positive number when the top port is used as a dynamic port
and bottom port is used as the static port
Also note that the _diff_pres_offset is applied before the
fabsf() not afterwards. It needs to be done this way to
prevent a bias at low speeds, but this also means that when
setting a offset you must set it based on the raw value, not
the offset value
*/
struct differential_pressure_s report;
/* track maximum differential pressure measured (so we can work out top speed). */
if (diff_press_pa > _max_differential_pressure_pa) {
_max_differential_pressure_pa = diff_press_pa;
if (diff_press_pa_raw > _max_differential_pressure_pa) {
_max_differential_pressure_pa = diff_press_pa_raw;
}
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.temperature = temperature;
report.differential_pressure_pa = diff_press_pa;
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa);
/* the dynamics of the filter can make it overshoot into the negative range */
if (report.differential_pressure_filtered_pa < 0.0f) {
report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa);
}
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw);
report.differential_pressure_raw_pa = diff_press_pa_raw;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
@ -345,7 +324,7 @@ MEASAirspeed::cycle()
/**
correct for 5V rail voltage if the system_power ORB topic is
available
See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of
offset versus voltage for 3 sensors
*/
@ -394,7 +373,7 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
if (voltage_diff < -1.0f) {
voltage_diff = -1.0f;
}
temperature -= voltage_diff * temp_slope;
temperature -= voltage_diff * temp_slope;
#endif // CONFIG_ARCH_BOARD_PX4FMU_V2
}
@ -523,7 +502,7 @@ test()
}
warnx("single read");
warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
warnx("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
@ -551,7 +530,7 @@ test()
}
warnx("periodic read %u", i);
warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
warnx("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa);
warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature);
}

4
src/drivers/ms5611/ms5611.cpp
View File

@ -130,7 +130,7 @@ protected:
float _T;
/* altitude conversion calibration */
unsigned _msl_pressure; /* in kPa */
unsigned _msl_pressure; /* in Pa */
orb_advert_t _baro_topic;
@ -466,7 +466,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
irqrestore(flags);
return OK;
}

29
src/drivers/pca9685/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt Normal file
View File

@ -0,0 +1,29 @@
The following license agreement covers re-used code from the arduino driver
for the Adafruit I2C to PWM converter.
Software License Agreement (BSD License)
Copyright (c) 2012, Adafruit Industries
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 of the copyright holders 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 ''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 HOLDER 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.

42
src/drivers/pca9685/module.mk Normal file
View File

@ -0,0 +1,42 @@
############################################################################
#
# Copyright (c) 2012-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.
#
############################################################################
#
# Driver for the PCA9685 I2C PWM controller
# The chip is used on the adafruit I2C PWM converter,
# which allows to control servos via I2C.
# https://www.adafruit.com/product/815
MODULE_COMMAND = pca9685
SRCS = pca9685.cpp

651
src/drivers/pca9685/pca9685.cpp Normal file
View File

@ -0,0 +1,651 @@
/****************************************************************************
*
* Copyright (c) 2012-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 pca9685.cpp
*
* Driver for the PCA9685 I2C PWM module
* The chip is used on the Adafruit I2C/PWM converter https://www.adafruit.com/product/815
*
* Parts of the code are adapted from the arduino library for the board
* https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library
* for the license of these parts see the
* arduino_Adafruit_PWM_Servo_Driver_Library_license.txt file
* see https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library for contributors
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <nuttx/config.h>
#include <drivers/device/i2c.h>
#include <sys/types.h>
#include <stdint.h>
#include <string.h>
#include <stdlib.h>
#include <stdbool.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdio.h>
#include <ctype.h>
#include <math.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <uORB/uORB.h>
#include <uORB/topics/actuator_controls.h>
#include <board_config.h>
#include <drivers/drv_io_expander.h>
#define PCA9685_SUBADR1 0x2
#define PCA9685_SUBADR2 0x3
#define PCA9685_SUBADR3 0x4
#define PCA9685_MODE1 0x0
#define PCA9685_PRESCALE 0xFE
#define LED0_ON_L 0x6
#define LED0_ON_H 0x7
#define LED0_OFF_L 0x8
#define LED0_OFF_H 0x9
#define ALLLED_ON_L 0xFA
#define ALLLED_ON_H 0xFB
#define ALLLED_OFF_L 0xFC
#define ALLLED_OF
#define ADDR 0x40 // I2C adress
#define PCA9685_DEVICE_PATH "/dev/pca9685"
#define PCA9685_BUS PX4_I2C_BUS_EXPANSION
#define PCA9685_PWMFREQ 60.0f
#define PCA9685_NCHANS 16 // total amount of pwm outputs
#define PCA9685_PWMMIN 150 // this is the 'minimum' pulse length count (out of 4096)
#define PCA9685_PWMMAX 600 // this is the 'maximum' pulse length count (out of 4096)_PWMFREQ 60.0f
#define PCA9685_PWMCENTER ((PCA9685_PWMMAX + PCA9685_PWMMIN)/2)
#define PCA9685_MAXSERVODEG 90.0f /* maximal servo deflection in degrees
PCA9685_PWMMIN <--> -PCA9685_MAXSERVODEG
PCA9685_PWMMAX <--> PCA9685_MAXSERVODEG
*/
#define PCA9685_SCALE ((PCA9685_PWMMAX - PCA9685_PWMCENTER)/(M_DEG_TO_RAD_F * PCA9685_MAXSERVODEG)) // scales from rad to PWM
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
class PCA9685 : public device::I2C
{
public:
PCA9685(int bus=PCA9685_BUS, uint8_t address=ADDR);
virtual ~PCA9685();
virtual int init();
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
virtual int info();
virtual int reset();
bool is_running() { return _running; }
private:
work_s _work;
enum IOX_MODE _mode;
bool _running;
int _i2cpwm_interval;
bool _should_run;
perf_counter_t _comms_errors;
uint8_t _msg[6];
int _actuator_controls_sub;
struct actuator_controls_s _actuator_controls;
uint16_t _current_values[NUM_ACTUATOR_CONTROLS]; /**< stores the current pwm output
values as sent to the setPin() */
bool _mode_on_initialized; /** Set to true after the first call of i2cpwm in mode IOX_MODE_ON */
static void i2cpwm_trampoline(void *arg);
void i2cpwm();
/**
* Helper function to set the pwm frequency
*/
int setPWMFreq(float freq);
/**
* Helper function to set the demanded pwm value
* @param num pwm output number
*/
int setPWM(uint8_t num, uint16_t on, uint16_t off);
/**
* Sets pin without having to deal with on/off tick placement and properly handles
* a zero value as completely off. Optional invert parameter supports inverting
* the pulse for sinking to ground.
* @param num pwm output number
* @param val should be a value from 0 to 4095 inclusive.
*/
int setPin(uint8_t num, uint16_t val, bool invert = false);
/* Wrapper to read a byte from addr */
int read8(uint8_t addr, uint8_t &value);
/* Wrapper to wite a byte to addr */
int write8(uint8_t addr, uint8_t value);
};
/* for now, we only support one board */
namespace
{
PCA9685 *g_pca9685;
}
void pca9685_usage();
extern "C" __EXPORT int pca9685_main(int argc, char *argv[]);
PCA9685::PCA9685(int bus, uint8_t address) :
I2C("pca9685", PCA9685_DEVICE_PATH, bus, address, 100000),
_mode(IOX_MODE_OFF),
_running(false),
_i2cpwm_interval(SEC2TICK(1.0f/60.0f)),
_should_run(false),
_comms_errors(perf_alloc(PC_COUNT, "actuator_controls_2_comms_errors")),
_actuator_controls_sub(-1),
_actuator_controls(),
_mode_on_initialized(false)
{
memset(&_work, 0, sizeof(_work));
memset(_msg, 0, sizeof(_msg));
memset(_current_values, 0, sizeof(_current_values));
}
PCA9685::~PCA9685()
{
}
int
PCA9685::init()
{
int ret;
ret = I2C::init();
if (ret != OK) {
return ret;
}
ret = reset();
if (ret != OK) {
return ret;
}
ret = setPWMFreq(PCA9685_PWMFREQ);
return ret;
}
int
PCA9685::ioctl(struct file *filp, int cmd, unsigned long arg)
{
int ret = -EINVAL;
switch (cmd) {
case IOX_SET_MODE:
if (_mode != (IOX_MODE)arg) {
switch ((IOX_MODE)arg) {
case IOX_MODE_OFF:
warnx("shutting down");
break;
case IOX_MODE_ON:
warnx("starting");
break;
case IOX_MODE_TEST_OUT:
warnx("test starting");
break;
default:
return -1;
}
_mode = (IOX_MODE)arg;
}
// if not active, kick it
if (!_running) {
_running = true;
work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, 1);
}
return OK;
default:
// see if the parent class can make any use of it
ret = CDev::ioctl(filp, cmd, arg);
break;
}
return ret;
}
int
PCA9685::info()
{
int ret = OK;
if (is_running()) {
warnx("Driver is running, mode: %u", _mode);
} else {
warnx("Driver started but not running");
}
return ret;
}
void
PCA9685::i2cpwm_trampoline(void *arg)
{
PCA9685 *i2cpwm = reinterpret_cast<PCA9685 *>(arg);
i2cpwm->i2cpwm();
}
/**
* Main loop function
*/
void
PCA9685::i2cpwm()
{
if (_mode == IOX_MODE_TEST_OUT) {
setPin(0, PCA9685_PWMCENTER);
_should_run = true;
} else if (_mode == IOX_MODE_OFF) {
_should_run = false;
} else {
if (!_mode_on_initialized) {
/* Subscribe to actuator control 2 (payload group for gimbal) */
_actuator_controls_sub = orb_subscribe(ORB_ID(actuator_controls_2));
/* set the uorb update interval lower than the driver pwm interval */
orb_set_interval(_actuator_controls_sub, 1000.0f / PCA9685_PWMFREQ - 5);
_mode_on_initialized = true;
}
/* Read the servo setpoints from the actuator control topics (gimbal) */
bool updated;
orb_check(_actuator_controls_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_controls_2), _actuator_controls_sub, &_actuator_controls);
for (int i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
/* Scale the controls to PWM, first multiply by pi to get rad,
* the control[i] values are on the range -1 ... 1 */
uint16_t new_value = PCA9685_PWMCENTER +
(_actuator_controls.control[i] * M_PI_F * PCA9685_SCALE);
debug("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value,
(double)_actuator_controls.control[i]);
if (new_value != _current_values[i] &&
isfinite(new_value) &&
new_value >= PCA9685_PWMMIN &&
new_value <= PCA9685_PWMMAX) {
/* This value was updated, send the command to adjust the PWM value */
setPin(i, new_value);
_current_values[i] = new_value;
}
}
}
_should_run = true;
}
// check if any activity remains, else stop
if (!_should_run) {
_running = false;
return;
}
// re-queue ourselves to run again later
_running = true;
work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, _i2cpwm_interval);
}
int
PCA9685::setPWM(uint8_t num, uint16_t on, uint16_t off)
{
int ret;
/* convert to correct message */
_msg[0] = LED0_ON_L + 4 * num;
_msg[1] = on;
_msg[2] = on >> 8;
_msg[3] = off;
_msg[4] = off >> 8;
/* try i2c transfer */
ret = transfer(_msg, 5, nullptr, 0);
if (OK != ret) {
perf_count(_comms_errors);
log("i2c::transfer returned %d", ret);
}
return ret;
}
int
PCA9685::setPin(uint8_t num, uint16_t val, bool invert)
{
// Clamp value between 0 and 4095 inclusive.
if (val > 4095) {
val = 4095;
}
if (invert) {
if (val == 0) {
// Special value for signal fully on.
return setPWM(num, 4096, 0);
} else if (val == 4095) {
// Special value for signal fully off.
return setPWM(num, 0, 4096);
} else {
return setPWM(num, 0, 4095-val);
}
} else {
if (val == 4095) {
// Special value for signal fully on.
return setPWM(num, 4096, 0);
} else if (val == 0) {
// Special value for signal fully off.
return setPWM(num, 0, 4096);
} else {
return setPWM(num, 0, val);
}
}
return ERROR;
}
int
PCA9685::setPWMFreq(float freq)
{
int ret = OK;
freq *= 0.9f; /* Correct for overshoot in the frequency setting (see issue
https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library/issues/11). */
float prescaleval = 25000000;
prescaleval /= 4096;
prescaleval /= freq;
prescaleval -= 1;
uint8_t prescale = uint8_t(prescaleval + 0.5f); //implicit floor()
uint8_t oldmode;
ret = read8(PCA9685_MODE1, oldmode);
if (ret != OK) {
return ret;
}
uint8_t newmode = (oldmode&0x7F) | 0x10; // sleep
ret = write8(PCA9685_MODE1, newmode); // go to sleep
if (ret != OK) {
return ret;
}
ret = write8(PCA9685_PRESCALE, prescale); // set the prescaler
if (ret != OK) {
return ret;
}
ret = write8(PCA9685_MODE1, oldmode);
if (ret != OK) {
return ret;
}
usleep(5000); //5ms delay (from arduino driver)
ret = write8(PCA9685_MODE1, oldmode | 0xa1); // This sets the MODE1 register to turn on auto increment.
if (ret != OK) {
return ret;
}
return ret;
}
/* Wrapper to read a byte from addr */
int
PCA9685::read8(uint8_t addr, uint8_t &value)
{
int ret = OK;
/* send addr */
ret = transfer(&addr, sizeof(addr), nullptr, 0);
if (ret != OK) {
goto fail_read;
}
/* get value */
ret = transfer(nullptr, 0, &value, 1);
if (ret != OK) {
goto fail_read;
}
return ret;
fail_read:
perf_count(_comms_errors);
log("i2c::transfer returned %d", ret);
return ret;
}
int PCA9685::reset(void) {
warnx("resetting");
return write8(PCA9685_MODE1, 0x0);
}
/* Wrapper to wite a byte to addr */
int
PCA9685::write8(uint8_t addr, uint8_t value) {
int ret = OK;
_msg[0] = addr;
_msg[1] = value;
/* send addr and value */
ret = transfer(_msg, 2, nullptr, 0);
if (ret != OK) {
perf_count(_comms_errors);
log("i2c::transfer returned %d", ret);
}
return ret;
}
void
pca9685_usage()
{
warnx("missing command: try 'start', 'test', 'stop', 'info'");
warnx("options:");
warnx(" -b i2cbus (%d)", PX4_I2C_BUS_EXPANSION);
warnx(" -a addr (0x%x)", ADDR);
}
int
pca9685_main(int argc, char *argv[])
{
int i2cdevice = -1;
int i2caddr = ADDR; // 7bit
int ch;
// jump over start/off/etc and look at options first
while ((ch = getopt(argc, argv, "a:b:")) != EOF) {
switch (ch) {
case 'a':
i2caddr = strtol(optarg, NULL, 0);
break;
case 'b':
i2cdevice = strtol(optarg, NULL, 0);
break;
default:
pca9685_usage();
exit(0);
}
}
if (optind >= argc) {
pca9685_usage();
exit(1);
}
const char *verb = argv[optind];
int fd;
int ret;
if (!strcmp(verb, "start")) {
if (g_pca9685 != nullptr) {
errx(1, "already started");
}
if (i2cdevice == -1) {
// try the external bus first
i2cdevice = PX4_I2C_BUS_EXPANSION;
g_pca9685 = new PCA9685(PX4_I2C_BUS_EXPANSION, i2caddr);
if (g_pca9685 != nullptr && OK != g_pca9685->init()) {
delete g_pca9685;
g_pca9685 = nullptr;
}
if (g_pca9685 == nullptr) {
errx(1, "init failed");
}
}
if (g_pca9685 == nullptr) {
g_pca9685 = new PCA9685(i2cdevice, i2caddr);
if (g_pca9685 == nullptr) {
errx(1, "new failed");
}
if (OK != g_pca9685->init()) {
delete g_pca9685;
g_pca9685 = nullptr;
errx(1, "init failed");
}
}
fd = open(PCA9685_DEVICE_PATH, 0);
if (fd == -1) {
errx(1, "Unable to open " PCA9685_DEVICE_PATH);
}
ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_ON);
close(fd);
exit(0);
}
// need the driver past this point
if (g_pca9685 == nullptr) {
warnx("not started, run pca9685 start");
exit(1);
}
if (!strcmp(verb, "info")) {
g_pca9685->info();
exit(0);
}
if (!strcmp(verb, "reset")) {
g_pca9685->reset();
exit(0);
}
if (!strcmp(verb, "test")) {
fd = open(PCA9685_DEVICE_PATH, 0);
if (fd == -1) {
errx(1, "Unable to open " PCA9685_DEVICE_PATH);
}
ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_TEST_OUT);
close(fd);
exit(ret);
}
if (!strcmp(verb, "stop")) {
fd = open(PCA9685_DEVICE_PATH, 0);
if (fd == -1) {
errx(1, "Unable to open " PCA9685_DEVICE_PATH);
}
ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF);
close(fd);
// wait until we're not running any more
for (unsigned i = 0; i < 15; i++) {
if (!g_pca9685->is_running()) {
break;
}
usleep(50000);
printf(".");
fflush(stdout);
}
printf("\n");
fflush(stdout);
if (!g_pca9685->is_running()) {
delete g_pca9685;
g_pca9685= nullptr;
warnx("stopped, exiting");
exit(0);
} else {
warnx("stop failed.");
exit(1);
}
}
pca9685_usage();
exit(0);
}

68
src/drivers/px4flow/px4flow.cpp
View File

@ -37,7 +37,7 @@
*
* Driver for the PX4FLOW module connected via I2C.
*/
#include <nuttx/config.h>
#include <drivers/device/i2c.h>
@ -68,7 +68,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
//#include <uORB/topics/optical_flow.h>
#include <uORB/topics/optical_flow.h>
#include <board_config.h>
@ -80,7 +80,7 @@
/* PX4FLOW Registers addresses */
#define PX4FLOW_REG 0x00 /* Measure Register */
#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz
#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz */
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
@ -115,17 +115,17 @@ class PX4FLOW : public device::I2C
public:
PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS);
virtual ~PX4FLOW();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
protected:
virtual int probe();
@ -136,13 +136,13 @@ private:
bool _sensor_ok;
int _measure_ticks;
bool _collect_phase;
orb_advert_t _px4flow_topic;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
/**
* Test whether the device supported by the driver is present at a
* specific address.
@ -151,7 +151,7 @@ private:
* @return True if the device is present.
*/
int probe_address(uint8_t address);
/**
* Initialise the automatic measurement state machine and start it.
*
@ -159,12 +159,12 @@ private:
* to make it more aggressive about resetting the bus in case of errors.
*/
void start();
/**
* Stop the automatic measurement state machine.
*/
void stop();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
@ -179,8 +179,8 @@ private:
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
};
/*
@ -201,7 +201,7 @@ PX4FLOW::PX4FLOW(int bus, int address) :
{
// enable debug() calls
_debug_enabled = true;
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
}
@ -226,13 +226,13 @@ PX4FLOW::init()
goto out;
/* allocate basic report buffers */
_reports = new RingBuffer(2, sizeof(px4flow_report));
_reports = new RingBuffer(2, sizeof(struct optical_flow_s));
if (_reports == nullptr)
goto out;
/* get a publish handle on the px4flow topic */
struct px4flow_report zero_report;
struct optical_flow_s zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report);
@ -323,24 +323,24 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
return -EINVAL;
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _reports->size();
case SENSORIOCRESET:
/* XXX implement this */
return -EINVAL;
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
@ -350,8 +350,8 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
ssize_t
PX4FLOW::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct px4flow_report);
struct px4flow_report *rbuf = reinterpret_cast<struct px4flow_report *>(buffer);
unsigned count = buflen / sizeof(struct optical_flow_s);
struct optical_flow_s *rbuf = reinterpret_cast<struct optical_flow_s *>(buffer);
int ret = 0;
/* buffer must be large enough */
@ -425,7 +425,7 @@ PX4FLOW::measure()
return ret;
}
ret = OK;
return ret;
}
@ -433,14 +433,14 @@ int
PX4FLOW::collect()
{
int ret = -EIO;
/* read from the sensor */
uint8_t val[22] = {0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0};
perf_begin(_sample_perf);
ret = transfer(nullptr, 0, &val[0], 22);
if (ret < 0)
{
log("error reading from sensor: %d", ret);
@ -448,7 +448,7 @@ PX4FLOW::collect()
perf_end(_sample_perf);
return ret;
}
// f.frame_count = val[1] << 8 | val[0];
// f.pixel_flow_x_sum= val[3] << 8 | val[2];
// f.pixel_flow_y_sum= val[5] << 8 | val[4];
@ -466,7 +466,7 @@ PX4FLOW::collect()
int16_t flowcy = val[9] << 8 | val[8];
int16_t gdist = val[21] << 8 | val[20];
struct px4flow_report report;
struct optical_flow_s report;
report.flow_comp_x_m = float(flowcx)/1000.0f;
report.flow_comp_y_m = float(flowcy)/1000.0f;
report.flow_raw_x= val[3] << 8 | val[2];
@ -503,7 +503,7 @@ PX4FLOW::start()
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1);
/* notify about state change */
struct subsystem_info_s info = {
true,
@ -644,7 +644,7 @@ start()
fail:
if (g_dev != nullptr)
if (g_dev != nullptr)
{
delete g_dev;
g_dev = nullptr;
@ -678,7 +678,7 @@ void stop()
void
test()
{
struct px4flow_report report;
struct optical_flow_s report;
ssize_t sz;
int ret;
@ -777,7 +777,7 @@ px4flow_main(int argc, char *argv[])
*/
if (!strcmp(argv[1], "start"))
px4flow::start();
/*
* Stop the driver
*/

4
src/drivers/px4fmu/fmu.cpp
View File

@ -1272,7 +1272,9 @@ PX4FMU::write(file *filp, const char *buffer, size_t len)
memcpy(values, buffer, count * 2);
for (uint8_t i = 0; i < count; i++) {
up_pwm_servo_set(i, values[i]);
if (values[i] != PWM_IGNORE_THIS_CHANNEL) {
up_pwm_servo_set(i, values[i]);
}
}
return count * 2;

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

@ -1968,7 +1968,7 @@ PX4IO::print_status(bool extended_status)
printf("actuators");
for (unsigned i = 0; i < _max_actuators; i++)
printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i));
printf(" %hi", int16_t(io_reg_get(PX4IO_PAGE_ACTUATORS, i)));
printf("\n");
printf("servos");

41
src/drivers/sf0x/sf0x.cpp
View File

@ -520,11 +520,9 @@ SF0X::collect()
/* clear buffer if last read was too long ago */
uint64_t read_elapsed = hrt_elapsed_time(&_last_read);
/* timed out - retry */
if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
_linebuf_index = 0;
} else if (_linebuf_index > 0) {
/* increment to next read position */
_linebuf_index++;
}
/* the buffer for read chars is buflen minus null termination */
@ -550,18 +548,19 @@ SF0X::collect()
return -EAGAIN;
}
/* we did increment the index to the next position already, so just add the additional fields */
_linebuf_index += (ret - 1);
/* let the write pointer point to the next free entry */
_linebuf_index += ret;
_last_read = hrt_absolute_time();
if (_linebuf_index < 1) {
/* we need at least the two end bytes to make sense of this string */
/* require a reasonable amount of minimum bytes */
if (_linebuf_index < 6) {
/* we need at this format: x.xx\r\n */
return -EAGAIN;
} else if (_linebuf[_linebuf_index - 1] != '\r' || _linebuf[_linebuf_index] != '\n') {
} else if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') {
if (_linebuf_index >= readlen - 1) {
if (_linebuf_index == readlen) {
/* we have a full buffer, but no line ending - abort */
_linebuf_index = 0;
perf_count(_comms_errors);
@ -577,9 +576,7 @@ SF0X::collect()
bool valid;
/* enforce line ending */
unsigned lend = (_linebuf_index < (sizeof(_linebuf) - 1)) ? _linebuf_index : (sizeof(_linebuf) - 1);
_linebuf[lend] = '\0';
_linebuf[_linebuf_index] = '\0';
if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') {
si_units = -1.0f;
@ -591,14 +588,16 @@ SF0X::collect()
valid = false;
/* wipe out partially read content from last cycle(s), check for dot */
for (unsigned i = 0; i < (lend - 2); i++) {
for (unsigned i = 0; i < (_linebuf_index - 2); i++) {
if (_linebuf[i] == '\n') {
char buf[sizeof(_linebuf)];
memcpy(buf, &_linebuf[i+1], (lend + 1) - (i + 1));
memcpy(_linebuf, buf, (lend + 1) - (i + 1));
/* wipe out any partial measurements */
for (unsigned j = 0; j <= i; j++) {
_linebuf[j] = ' ';
}
}
if (_linebuf[i] == '.') {
/* we need a digit before the dot and a dot for a valid number */
if (i > 0 && ((_linebuf[i - 1] >= '0') && (_linebuf[i - 1] <= '9')) && (_linebuf[i] == '.')) {
valid = true;
}
}
@ -606,7 +605,7 @@ SF0X::collect()
if (valid) {
si_units = strtod(_linebuf, &end);
/* we require at least 3 characters for a valid number */
/* we require at least four characters for a valid number */
if (end > _linebuf + 3) {
valid = true;
} else {
@ -616,7 +615,7 @@ SF0X::collect()
}
}
debug("val (float): %8.4f, raw: %s, valid: %s\n", (double)si_units, _linebuf, ((valid) ? "OK" : "NO"));
debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO"));
/* done with this chunk, resetting - even if invalid */
_linebuf_index = 0;
@ -708,12 +707,12 @@ SF0X::cycle()
int collect_ret = collect();
if (collect_ret == -EAGAIN) {
/* reschedule to grab the missing bits, time to transmit 10 bytes @9600 bps */
/* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */
work_queue(HPWORK,
&_work,
(worker_t)&SF0X::cycle_trampoline,
this,
USEC2TICK(1100));
USEC2TICK(1042 * 8));
return;
}

6
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
View File

@ -169,7 +169,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
float id = _rate_error * dt;
float id = _rate_error * dt * scaler;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
@ -190,7 +190,9 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
/* Apply PI rate controller and store non-limited output */
_last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed
_last_output = _bodyrate_setpoint * _k_ff * scaler +
_rate_error * _k_p * scaler * scaler
+ integrator_constrained; //scaler is proportional to 1/airspeed
// warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
// warnx("roll: _last_output %.4f", (double)_last_output);
return math::constrain(_last_output, -1.0f, 1.0f);

6
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
View File

@ -135,7 +135,7 @@ float ECL_RollController::control_bodyrate(float pitch,
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
float id = _rate_error * dt;
float id = _rate_error * dt * scaler;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
@ -157,7 +157,9 @@ float ECL_RollController::control_bodyrate(float pitch,
//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);
/* Apply PI rate controller and store non-limited output */
_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed
_last_output = _bodyrate_setpoint * _k_ff * scaler +
_rate_error * _k_p * scaler * scaler
+ integrator_constrained; //scaler is proportional to 1/airspeed
return math::constrain(_last_output, -1.0f, 1.0f);
}

5
src/lib/external_lgpl/tecs/tecs.cpp
View File

@ -252,6 +252,11 @@ void TECS::_update_height_demand(float demand, float state)
void TECS::_detect_underspeed(void)
{
if (!_detect_underspeed_enabled) {
_underspeed = false;
return;
}
if (((_integ5_state < _TASmin * 0.9f) && (_throttle_dem >= _THRmaxf * 0.95f)) || ((_integ3_state < _hgt_dem_adj) && _underspeed)) {
_underspeed = true;

10
src/lib/external_lgpl/tecs/tecs.h
View File

@ -66,6 +66,9 @@ public:
_hgt_dem_prev(0.0f),
_TAS_dem_adj(0.0f),
_STEdotErrLast(0.0f),
_underspeed(false),
_detect_underspeed_enabled(true),
_badDescent(false),
_climbOutDem(false),
_SPE_dem(0.0f),
_SKE_dem(0.0f),
@ -221,6 +224,10 @@ public:
_speedrate_p = speedrate_p;
}
void set_detect_underspeed_enabled(bool enabled) {
_detect_underspeed_enabled = enabled;
}
private:
struct tecs_state _tecs_state;
@ -323,6 +330,9 @@ private:
// Underspeed condition
bool _underspeed;
// Underspeed detection enabled
bool _detect_underspeed_enabled;
// Bad descent condition caused by unachievable airspeed demand
bool _badDescent;

4
src/lib/launchdetection/CatapultLaunchMethod.cpp
View File

@ -66,10 +66,10 @@ void CatapultLaunchMethod::update(float accel_x)
last_timestamp = hrt_absolute_time();
if (accel_x > threshold_accel.get()) {
integrator += accel_x * dt;
integrator += dt;
// warnx("*** integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f",
// (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt);
if (integrator > threshold_accel.get() * threshold_time.get()) {
if (integrator > threshold_time.get()) {
launchDetected = true;
}

18
src/modules/commander/airspeed_calibration.cpp
View File

@ -180,11 +180,13 @@ int do_airspeed_calibration(int mavlink_fd)
return ERROR;
}
mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset);
mavlink_log_critical(mavlink_fd, "Offset of %d Pascal", (int)diff_pres_offset);
/* wait 500 ms to ensure parameter propagated through the system */
usleep(500 * 1000);
mavlink_log_critical(mavlink_fd, "Create airflow now");
calibration_counter = 0;
const unsigned maxcount = 3000;
@ -204,18 +206,18 @@ int do_airspeed_calibration(int mavlink_fd)
calibration_counter++;
if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
if (calibration_counter % 100 == 0) {
mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)",
(int)diff_pres.differential_pressure_raw_pa);
if (calibration_counter % 500 == 0) {
mavlink_log_info(mavlink_fd, "Create airflow! (%d, wanted: 50 Pa)",
(int)diff_pres.differential_pressure_raw_pa);
}
continue;
}
/* do not allow negative values */
if (diff_pres.differential_pressure_raw_pa < 0.0f) {
mavlink_log_info(mavlink_fd, "negative pressure: ERROR (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa);
mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!");
mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
close(diff_pres_sub);
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
@ -236,7 +238,7 @@ int do_airspeed_calibration(int mavlink_fd)
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
return ERROR;
} else {
mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)",
mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
break;
}

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

@ -129,7 +129,7 @@ extern struct system_load_s system_load;
#define POSITION_TIMEOUT (2 * 1000 * 1000) /**< consider the local or global position estimate invalid after 600ms */
#define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
#define RC_TIMEOUT 500000
#define DL_TIMEOUT 5 * 1000* 1000
#define DL_TIMEOUT (10 * 1000 * 1000)
#define OFFBOARD_TIMEOUT 500000
#define DIFFPRESS_TIMEOUT 2000000
@ -615,6 +615,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
case VEHICLE_CMD_PREFLIGHT_STORAGE:
case VEHICLE_CMD_CUSTOM_0:
case VEHICLE_CMD_CUSTOM_1:
case VEHICLE_CMD_CUSTOM_2:
case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY:
case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY:
/* ignore commands that handled in low prio loop */
break;

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

@ -182,12 +182,19 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
// Fail transition if power levels on the avionics rail
// are measured but are insufficient
if (status->condition_power_input_valid && ((status->avionics_power_rail_voltage > 0.0f) &&
(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;
if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) {
// Check avionics rail voltages
if (status->avionics_power_rail_voltage < 4.75f) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
valid_transition = false;
} else if (status->avionics_power_rail_voltage < 4.9f) {
mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
} else if (status->avionics_power_rail_voltage > 5.4f) {
mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
}
}
}

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

@ -145,6 +145,7 @@ private:
perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */
bool _setpoint_valid; /**< flag if the position control setpoint is valid */
bool _debug; /**< if set to true, print debug output */
struct {
float tconst;
@ -324,7 +325,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")),
_nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")),
/* states */
_setpoint_valid(false)
_setpoint_valid(false),
_debug(false)
{
/* safely initialize structs */
_att = {};
@ -700,7 +702,8 @@ FixedwingAttitudeControl::task_main()
perf_count(_nonfinite_input_perf);
}
} else {
airspeed = _airspeed.true_airspeed_m_s;
/* prevent numerical drama by requiring 0.5 m/s minimal speed */
airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s);
}
/*
@ -785,7 +788,7 @@ FixedwingAttitudeControl::task_main()
speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d;
speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d;
} else {
if (loop_counter % 10 == 0) {
if (_debug && loop_counter % 10 == 0) {
warnx("Did not get a valid R\n");
}
}
@ -808,7 +811,7 @@ FixedwingAttitudeControl::task_main()
_roll_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (loop_counter % 10 == 0) {
if (_debug && loop_counter % 10 == 0) {
warnx("roll_u %.4f", (double)roll_u);
}
}
@ -821,7 +824,7 @@ FixedwingAttitudeControl::task_main()
if (!isfinite(pitch_u)) {
_pitch_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (loop_counter % 10 == 0) {
if (_debug && loop_counter % 10 == 0) {
warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
" airspeed %.4f, airspeed_scaling %.4f,"
" roll_sp %.4f, pitch_sp %.4f,"
@ -845,7 +848,7 @@ FixedwingAttitudeControl::task_main()
if (!isfinite(yaw_u)) {
_yaw_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (loop_counter % 10 == 0) {
if (_debug && loop_counter % 10 == 0) {
warnx("yaw_u %.4f", (double)yaw_u);
}
}
@ -853,13 +856,13 @@ FixedwingAttitudeControl::task_main()
/* throttle passed through */
_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
if (!isfinite(throttle_sp)) {
if (loop_counter % 10 == 0) {
if (_debug && loop_counter % 10 == 0) {
warnx("throttle_sp %.4f", (double)throttle_sp);
}
}
} else {
perf_count(_nonfinite_input_perf);
if (loop_counter % 10 == 0) {
if (_debug && loop_counter % 10 == 0) {
warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
}
}

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

@ -569,6 +569,12 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));
param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative));
/* check if negative value for 2/3 of flare altitude is set for throttle cut */
if (_parameters.land_thrust_lim_alt_relative < 0.0f) {
_parameters.land_thrust_lim_alt_relative = 0.66f * _parameters.land_flare_alt_relative;
}
param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance));
param_get(_parameter_handles.range_finder_rel_alt, &(_parameters.range_finder_rel_alt));
@ -823,7 +829,7 @@ float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt,
* the measurement is valid
* the estimated relative altitude (from global altitude estimate and landing waypoint) <= range_finder_use_relative_alt
*/
if (range_finder_use_relative_alt < 0 || !range_finder.valid || rel_alt_estimated > range_finder_use_relative_alt ) {
if (range_finder_use_relative_alt < 0 || !range_finder.valid || range_finder.distance > range_finder_use_relative_alt ) {
return rel_alt_estimated;
}
@ -1380,6 +1386,9 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
limitOverride);
} else {
/* No underspeed protection in landing mode */
_tecs.set_detect_underspeed_enabled(!(mode == TECS_MODE_LAND || mode == TECS_MODE_LAND_THROTTLELIM));
/* Using tecs library */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp,
_airspeed.indicated_airspeed_m_s, eas2tas,

13
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
View File

@ -379,18 +379,23 @@ PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f);
PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f);
/**
* Landing flare altitude (relative)
* Landing flare altitude (relative to landing altitude)
*
* @unit meter
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f);
PARAM_DEFINE_FLOAT(FW_LND_FLALT, 8.0f);
/**
* Landing throttle limit altitude (relative)
* Landing throttle limit altitude (relative landing altitude)
*
* Default of -1.0f lets the system default to applying throttle
* limiting at 2/3 of the flare altitude.
*
* @unit meter
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f);
PARAM_DEFINE_FLOAT(FW_LND_TLALT, -1.0f);
/**
* Landing heading hold horizontal distance

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

@ -114,7 +114,7 @@ MavlinkFTP::_worker(Request *req)
uint32_t messageCRC;
// basic sanity checks; must validate length before use
if ((hdr->magic != kProtocolMagic) || (hdr->size > kMaxDataLength)) {
if (hdr->size > kMaxDataLength) {
errorCode = kErrNoRequest;
goto out;
}
@ -122,6 +122,9 @@ MavlinkFTP::_worker(Request *req)
// check request CRC to make sure this is one of ours
messageCRC = hdr->crc32;
hdr->crc32 = 0;
hdr->padding[0] = 0;
hdr->padding[1] = 0;
hdr->padding[2] = 0;
if (crc32(req->rawData(), req->dataSize()) != messageCRC) {
errorCode = kErrNoRequest;
goto out;
@ -199,10 +202,13 @@ MavlinkFTP::_reply(Request *req)
{
auto hdr = req->header();
hdr->magic = kProtocolMagic;
hdr->seqNumber = req->header()->seqNumber + 1;
// message is assumed to be already constructed in the request buffer, so generate the CRC
hdr->crc32 = 0;
hdr->padding[0] = 0;
hdr->padding[1] = 0;
hdr->padding[2] = 0;
hdr->crc32 = crc32(req->rawData(), req->dataSize());
// then pack and send the reply back to the request source

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

@ -38,9 +38,6 @@
*
* MAVLink remote file server.
*
* Messages are wrapped in ENCAPSULATED_DATA messages. Every message includes
* a session ID and sequence number.
*
* A limited number of requests (currently 2) may be outstanding at a time.
* Additional messages will be discarded.
*
@ -74,16 +71,19 @@ private:
static MavlinkFTP *_server;
/// @brief Trying to pack structures across differing compilers is questionable for Clients, so we pad the
/// structure ourselves to 32 bit alignment which should get us what we want.
struct RequestHeader
{
uint8_t magic;
uint8_t session;
uint8_t opcode;
uint8_t size;
uint32_t crc32;
uint32_t offset;
{
uint16_t seqNumber; ///< sequence number for message
uint8_t session; ///< Session id for read and write commands
uint8_t opcode; ///< Command opcode
uint8_t size; ///< Size of data
uint8_t padding[3];
uint32_t crc32; ///< CRC for entire Request structure, with crc32 and padding set to 0
uint32_t offset; ///< Offsets for List and Read commands
uint8_t data[];
};
};
enum Opcode : uint8_t
{
@ -131,10 +131,11 @@ private:
};
bool decode(Mavlink *mavlink, mavlink_message_t *fromMessage) {
if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) {
if (fromMessage->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
_systemId = fromMessage->sysid;
_mavlink = mavlink;
mavlink_msg_encapsulated_data_decode(fromMessage, &_message);
return true;
mavlink_msg_file_transfer_protocol_decode(fromMessage, &_message);
return _message.target_system == _mavlink->get_system_id();
}
return false;
}
@ -145,8 +146,14 @@ private:
// flat memory architecture, as we're operating between threads here.
mavlink_message_t msg;
msg.checksum = 0;
unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(),
_mavlink->get_channel(), &msg, sequence()+1, rawData());
unsigned len = mavlink_msg_file_transfer_protocol_pack_chan(_mavlink->get_system_id(), // Sender system id
_mavlink->get_component_id(), // Sender component id
_mavlink->get_channel(), // Channel to send on
&msg, // Message to pack payload into
0, // Target network
_systemId, // Target system id
0, // Target component id
rawData()); // Payload to pack into message
_mavlink->lockMessageBufferMutex();
bool success = _mavlink->message_buffer_write(&msg, len);
@ -167,26 +174,25 @@ private:
#endif
}
uint8_t *rawData() { return &_message.data[0]; }
RequestHeader *header() { return reinterpret_cast<RequestHeader *>(&_message.data[0]); }
uint8_t *rawData() { return &_message.payload[0]; }
RequestHeader *header() { return reinterpret_cast<RequestHeader *>(&_message.payload[0]); }
uint8_t *requestData() { return &(header()->data[0]); }
unsigned dataSize() { return header()->size + sizeof(RequestHeader); }
uint16_t sequence() const { return _message.seqnr; }
mavlink_channel_t channel() { return _mavlink->get_channel(); }
char *dataAsCString();
private:
Mavlink *_mavlink;
mavlink_encapsulated_data_t _message;
mavlink_file_transfer_protocol_t _message;
uint8_t _systemId;
};
static const uint8_t kProtocolMagic = 'f';
static const char kDirentFile = 'F';
static const char kDirentDir = 'D';
static const char kDirentUnknown = 'U';
static const uint8_t kMaxDataLength = MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN - sizeof(RequestHeader);
static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(RequestHeader);
/// Request worker; runs on the low-priority work queue to service
/// remote requests.

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

@ -134,44 +134,44 @@ Mavlink::Mavlink() :
_mode(MAVLINK_MODE_NORMAL),
_channel(MAVLINK_COMM_0),
_logbuffer {},
_total_counter(0),
_receive_thread {},
_verbose(false),
_forwarding_on(false),
_passing_on(false),
_ftp_on(false),
_uart_fd(-1),
_baudrate(57600),
_datarate(1000),
_datarate_events(500),
_rate_mult(1.0f),
_mavlink_param_queue_index(0),
mavlink_link_termination_allowed(false),
_subscribe_to_stream(nullptr),
_subscribe_to_stream_rate(0.0f),
_flow_control_enabled(true),
_last_write_success_time(0),
_last_write_try_time(0),
_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 {},
_send_mutex {},
_param_initialized(false),
_param_system_id(0),
_param_component_id(0),
_param_system_type(0),
_param_use_hil_gps(0),
_total_counter(0),
_receive_thread {},
_verbose(false),
_forwarding_on(false),
_passing_on(false),
_ftp_on(false),
_uart_fd(-1),
_baudrate(57600),
_datarate(1000),
_datarate_events(500),
_rate_mult(1.0f),
_mavlink_param_queue_index(0),
mavlink_link_termination_allowed(false),
_subscribe_to_stream(nullptr),
_subscribe_to_stream_rate(0.0f),
_flow_control_enabled(true),
_last_write_success_time(0),
_last_write_try_time(0),
_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 {},
_send_mutex {},
_param_initialized(false),
_param_system_id(0),
_param_component_id(0),
_param_system_type(0),
_param_use_hil_gps(0),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
_txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
_txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
{
fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl;
@ -217,6 +217,8 @@ Mavlink::Mavlink() :
errx(1, "instance ID is out of range");
break;
}
_rstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
}
Mavlink::~Mavlink()
@ -1227,7 +1229,10 @@ Mavlink::task_main(int argc, char *argv[])
_mode = MAVLINK_MODE_CUSTOM;
} else if (strcmp(optarg, "camera") == 0) {
_mode = MAVLINK_MODE_CAMERA;
// left in here for compatibility
_mode = MAVLINK_MODE_ONBOARD;
} else if (strcmp(optarg, "onboard") == 0) {
_mode = MAVLINK_MODE_ONBOARD;
}
break;
@ -1287,8 +1292,8 @@ Mavlink::task_main(int argc, char *argv[])
warnx("mode: CUSTOM");
break;
case MAVLINK_MODE_CAMERA:
warnx("mode: CAMERA");
case MAVLINK_MODE_ONBOARD:
warnx("mode: ONBOARD");
break;
default:
@ -1391,13 +1396,14 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
configure_stream("ATTITUDE_TARGET", 3.0f);
configure_stream("DISTANCE_SENSOR", 0.5f);
configure_stream("OPTICAL_FLOW", 20.0f);
break;
case MAVLINK_MODE_CAMERA:
case MAVLINK_MODE_ONBOARD:
configure_stream("SYS_STATUS", 1.0f);
configure_stream("ATTITUDE", 15.0f);
configure_stream("GLOBAL_POSITION_INT", 15.0f);
configure_stream("CAMERA_CAPTURE", 1.0f);
configure_stream("ATTITUDE", 50.0f);
configure_stream("GLOBAL_POSITION_INT", 50.0f);
configure_stream("CAMERA_CAPTURE", 2.0f);
break;
default:
@ -1653,6 +1659,8 @@ Mavlink::display_status()
printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time));
}
printf("\tmavlink chan: #%u\n", _channel);
if (_rstatus.timestamp > 0) {
printf("\ttype:\t\t");

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

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

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

@ -774,6 +774,9 @@ private:
MavlinkOrbSubscription *_airspeed_sub;
uint64_t _airspeed_time;
MavlinkOrbSubscription *_sensor_combined_sub;
uint64_t _sensor_combined_time;
/* do not allow top copying this class */
MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &);
MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &);
@ -789,7 +792,9 @@ protected:
_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))),
_act_time(0),
_airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))),
_airspeed_time(0)
_airspeed_time(0),
_sensor_combined_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))),
_sensor_combined_time(0)
{}
void send(const hrt_abstime t)
@ -799,12 +804,14 @@ protected:
struct actuator_armed_s armed;
struct actuator_controls_s act;
struct airspeed_s airspeed;
struct sensor_combined_s sensor_combined;
bool updated = _att_sub->update(&_att_time, &att);
updated |= _pos_sub->update(&_pos_time, &pos);
updated |= _armed_sub->update(&_armed_time, &armed);
updated |= _act_sub->update(&_act_time, &act);
updated |= _airspeed_sub->update(&_airspeed_time, &airspeed);
updated |= _sensor_combined_sub->update(&_sensor_combined_time, &sensor_combined);
if (updated) {
mavlink_vfr_hud_t msg;
@ -813,7 +820,7 @@ protected:
msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
msg.alt = pos.alt;
msg.alt = sensor_combined.baro_alt_meter;
msg.climb = -pos.vel_d;
_mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg);
@ -879,8 +886,8 @@ protected:
msg.eph = cm_uint16_from_m_float(gps.eph);
msg.epv = cm_uint16_from_m_float(gps.epv);
msg.vel = cm_uint16_from_m_float(gps.vel_m_s),
msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
msg.satellites_visible = gps.satellites_used;
msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
msg.satellites_visible = gps.satellites_used;
_mavlink->send_message(MAVLINK_MSG_ID_GPS_RAW_INT, &msg);
}
@ -950,11 +957,11 @@ protected:
msg.lat = pos.lat * 1e7;
msg.lon = pos.lon * 1e7;
msg.alt = pos.alt * 1000.0f;
msg.relative_alt = (pos.alt - home.alt) * 1000.0f;
msg.vx = pos.vel_n * 100.0f;
msg.vy = pos.vel_e * 100.0f;
msg.vz = pos.vel_d * 100.0f;
msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f;
msg.relative_alt = (pos.alt - home.alt) * 1000.0f;
msg.vx = pos.vel_n * 100.0f;
msg.vy = pos.vel_e * 100.0f;
msg.vz = pos.vel_d * 100.0f;
msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f;
_mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &msg);
}
@ -1015,9 +1022,9 @@ protected:
msg.x = pos.x;
msg.y = pos.y;
msg.z = pos.z;
msg.vx = pos.vx;
msg.vy = pos.vy;
msg.vz = pos.vz;
msg.vx = pos.vx;
msg.vy = pos.vy;
msg.vz = pos.vz;
_mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED, &msg);
}
@ -1078,9 +1085,9 @@ protected:
msg.x = pos.x;
msg.y = pos.y;
msg.z = pos.z;
msg.roll = pos.roll;
msg.pitch = pos.pitch;
msg.yaw = pos.yaw;
msg.roll = pos.roll;
msg.pitch = pos.pitch;
msg.yaw = pos.yaw;
_mavlink->send_message(MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, &msg);
}

21
src/modules/mavlink/mavlink_mission.cpp
View File

@ -58,6 +58,10 @@
#endif
static const int ERROR = -1;
#define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && \
((_msg.target_component == mavlink_system.compid) || \
(_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \
(_msg.target_component == MAV_COMP_ID_ALL)))
MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink),
_state(MAVLINK_WPM_STATE_IDLE),
@ -79,8 +83,7 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(m
_mission_result_sub(-1),
_offboard_mission_pub(-1),
_slow_rate_limiter(_interval / 10.0f),
_verbose(false),
_comp_id(MAV_COMP_ID_MISSIONPLANNER)
_verbose(false)
{
_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
_mission_result_sub = orb_subscribe(ORB_ID(mission_result));
@ -384,7 +387,7 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg)
mavlink_mission_ack_t wpa;
mavlink_msg_mission_ack_decode(msg, &wpa);
if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) {
if (CHECK_SYSID_COMPID_MISSION(wpa)) {
if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) {
if (_state == MAVLINK_WPM_STATE_SENDLIST) {
_time_last_recv = hrt_absolute_time();
@ -416,7 +419,7 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg)
mavlink_mission_set_current_t wpc;
mavlink_msg_mission_set_current_decode(msg, &wpc);
if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) {
if (CHECK_SYSID_COMPID_MISSION(wpc)) {
if (_state == MAVLINK_WPM_STATE_IDLE) {
_time_last_recv = hrt_absolute_time();
@ -451,7 +454,7 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg)
mavlink_mission_request_list_t wprl;
mavlink_msg_mission_request_list_decode(msg, &wprl);
if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) {
if (CHECK_SYSID_COMPID_MISSION(wprl)) {
if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) {
_time_last_recv = hrt_absolute_time();
@ -487,7 +490,7 @@ MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg)
mavlink_mission_request_t wpr;
mavlink_msg_mission_request_decode(msg, &wpr);
if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) {
if (CHECK_SYSID_COMPID_MISSION(wpr)) {
if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) {
if (_state == MAVLINK_WPM_STATE_SENDLIST) {
_time_last_recv = hrt_absolute_time();
@ -558,7 +561,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
mavlink_mission_count_t wpc;
mavlink_msg_mission_count_decode(msg, &wpc);
if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) {
if (CHECK_SYSID_COMPID_MISSION(wpc)) {
if (_state == MAVLINK_WPM_STATE_IDLE) {
_time_last_recv = hrt_absolute_time();
@ -624,7 +627,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
mavlink_mission_item_t wp;
mavlink_msg_mission_item_decode(msg, &wp);
if (wp.target_system == mavlink_system.sysid && wp.target_component == _comp_id) {
if (CHECK_SYSID_COMPID_MISSION(wp)) {
if (_state == MAVLINK_WPM_STATE_GETLIST) {
_time_last_recv = hrt_absolute_time();
@ -710,7 +713,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
mavlink_mission_clear_all_t wpca;
mavlink_msg_mission_clear_all_decode(msg, &wpca);
if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) {
if (CHECK_SYSID_COMPID_MISSION(wpca)) {
if (_state == MAVLINK_WPM_STATE_IDLE) {
/* don't touch mission items storage itself, but only items count in mission state */

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

@ -126,8 +126,6 @@ private:
bool _verbose;
uint8_t _comp_id;
/* do not allow top copying this class */
MavlinkMissionManager(MavlinkMissionManager &);
MavlinkMissionManager& operator = (const MavlinkMissionManager &);

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

@ -112,7 +112,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_telemetry_status_pub(-1),
_rc_pub(-1),
_manual_pub(-1),
_radio_status_available(false),
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
_hil_frames(0),
_old_timestamp(0),
@ -137,6 +136,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_command_long(msg);
break;
case MAVLINK_MSG_ID_COMMAND_INT:
handle_message_command_int(msg);
break;
case MAVLINK_MSG_ID_OPTICAL_FLOW:
handle_message_optical_flow(msg);
break;
@ -169,7 +172,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_request_data_stream(msg);
break;
case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
MavlinkFTP::getServer()->handle_message(_mavlink, msg);
break;
@ -276,6 +279,62 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
}
}
void
MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
{
/* command */
mavlink_command_int_t cmd_mavlink;
mavlink_msg_command_int_decode(msg, &cmd_mavlink);
if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
//check for MAVLINK terminate command
if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
/* This is the link shutdown command, terminate mavlink */
warnx("terminated by remote command");
fflush(stdout);
usleep(50000);
/* terminate other threads and this thread */
_mavlink->_task_should_exit = true;
} else {
if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID",
mavlink_system.sysid, mavlink_system.compid);
return;
}
struct vehicle_command_s vcmd;
memset(&vcmd, 0, sizeof(vcmd));
/* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */
vcmd.param1 = cmd_mavlink.param1;
vcmd.param2 = cmd_mavlink.param2;
vcmd.param3 = cmd_mavlink.param3;
vcmd.param4 = cmd_mavlink.param4;
/* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */
vcmd.param5 = ((double)cmd_mavlink.x) / 1e7;
vcmd.param6 = ((double)cmd_mavlink.y) / 1e7;
vcmd.param7 = cmd_mavlink.z;
// XXX do proper translation
vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command;
vcmd.target_system = cmd_mavlink.target_system;
vcmd.target_component = cmd_mavlink.target_component;
vcmd.source_system = msg->sysid;
vcmd.source_component = msg->compid;
if (_cmd_pub < 0) {
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
} else {
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
}
}
}
}
void
MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
{
@ -430,9 +489,6 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
} else {
orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
}
/* this means that heartbeats alone won't be published to the radio status no more */
_radio_status_available = true;
}
}
@ -474,25 +530,17 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
hrt_abstime tnow = hrt_absolute_time();
/* set heartbeat time and topic time and publish -
* the telem status also gets updated on telemetry events
*/
tstatus.timestamp = hrt_absolute_time();
tstatus.heartbeat_time = tstatus.timestamp;
/* always set heartbeat, publish only if telemetry link not up */
tstatus.heartbeat_time = tnow;
if (_telemetry_status_pub < 0) {
_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
/* if no radio status messages arrive, lets at least publish that heartbeats were received */
if (!_radio_status_available) {
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) {
_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
} else {
orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
}
} else {
orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
}
}
}

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

@ -110,6 +110,7 @@ private:
void handle_message(mavlink_message_t *msg);
void handle_message_command_long(mavlink_message_t *msg);
void handle_message_command_int(mavlink_message_t *msg);
void handle_message_optical_flow(mavlink_message_t *msg);
void handle_message_set_mode(mavlink_message_t *msg);
void handle_message_vicon_position_estimate(mavlink_message_t *msg);
@ -151,7 +152,6 @@ private:
orb_advert_t _telemetry_status_pub;
orb_advert_t _rc_pub;
orb_advert_t _manual_pub;
bool _radio_status_available;
int _control_mode_sub;
int _hil_frames;
uint64_t _old_timestamp;

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

@ -353,6 +353,8 @@ Navigator::task_main()
case NAVIGATION_STATE_ACRO:
case NAVIGATION_STATE_ALTCTL:
case NAVIGATION_STATE_POSCTL:
case NAVIGATION_STATE_LAND:
case NAVIGATION_STATE_TERMINATION:
_navigation_mode = nullptr;
_can_loiter_at_sp = false;
break;
@ -368,8 +370,6 @@ Navigator::task_main()
case NAVIGATION_STATE_AUTO_RTGS:
_navigation_mode = &_rtl; /* TODO: change this to something else */
break;
case NAVIGATION_STATE_LAND:
case NAVIGATION_STATE_TERMINATION:
case NAVIGATION_STATE_OFFBOARD:
_navigation_mode = &_offboard;
break;

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

@ -285,7 +285,9 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
/* XXX range-check value? */
r_page_servos[offset] = *values;
if (*values != PWM_IGNORE_THIS_CHANNEL) {
r_page_servos[offset] = *values;
}
offset++;
num_values--;

21
src/modules/sdlog2/sdlog2.c
View File

@ -76,6 +76,7 @@
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/satellite_info.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vision_position_estimate.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/battery_status.h>
@ -934,6 +935,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vehicle_global_position_s global_pos;
struct position_setpoint_triplet_s triplet;
struct vehicle_vicon_position_s vicon_pos;
struct vision_position_estimate vision_pos;
struct optical_flow_s flow;
struct rc_channels_s rc;
struct differential_pressure_s diff_pres;
@ -984,6 +986,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_EST1_s log_EST1;
struct log_PWR_s log_PWR;
struct log_VICN_s log_VICN;
struct log_VISN_s log_VISN;
struct log_GS0A_s log_GS0A;
struct log_GS0B_s log_GS0B;
struct log_GS1A_s log_GS1A;
@ -1013,6 +1016,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int gps_pos_sub;
int sat_info_sub;
int vicon_pos_sub;
int vision_pos_sub;
int flow_sub;
int rc_sub;
int airspeed_sub;
@ -1043,6 +1047,7 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
subs.vision_pos_sub = orb_subscribe(ORB_ID(vision_position_estimate));
subs.flow_sub = orb_subscribe(ORB_ID(optical_flow));
subs.rc_sub = orb_subscribe(ORB_ID(rc_channels));
subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
@ -1459,6 +1464,22 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw;
LOGBUFFER_WRITE_AND_COUNT(VICN);
}
/* --- VISION POSITION --- */
if (copy_if_updated(ORB_ID(vision_position_estimate), subs.vision_pos_sub, &buf.vision_pos)) {
log_msg.msg_type = LOG_VISN_MSG;
log_msg.body.log_VISN.x = buf.vision_pos.x;
log_msg.body.log_VISN.y = buf.vision_pos.y;
log_msg.body.log_VISN.z = buf.vision_pos.z;
log_msg.body.log_VISN.vx = buf.vision_pos.vx;
log_msg.body.log_VISN.vy = buf.vision_pos.vy;
log_msg.body.log_VISN.vz = buf.vision_pos.vz;
log_msg.body.log_VISN.qx = buf.vision_pos.q[0];
log_msg.body.log_VISN.qy = buf.vision_pos.q[1];
log_msg.body.log_VISN.qz = buf.vision_pos.q[2];
log_msg.body.log_VISN.qw = buf.vision_pos.q[3];
LOGBUFFER_WRITE_AND_COUNT(VISN);
}
/* --- FLOW --- */
if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) {

15
src/modules/sdlog2/sdlog2_messages.h
View File

@ -391,6 +391,20 @@ struct log_TEL_s {
uint64_t heartbeat_time;
};
/* --- VISN - VISION POSITION --- */
#define LOG_VISN_MSG 38
struct log_VISN_s {
float x;
float y;
float z;
float vx;
float vy;
float vz;
float qx;
float qy;
float qz;
float qw;
};
/********** SYSTEM MESSAGES, ID > 0x80 **********/
@ -449,6 +463,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"),
LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"),
LOG_FORMAT(VISN, "ffffffffff", "X,Y,Z,VX,VY,VZ,QuatX,QuatY,QuatZ,QuatW"),
LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),

10
src/modules/sensors/sensor_params.c
View File

@ -214,6 +214,16 @@ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f);
*/
PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0);
/**
* QNH for barometer
*
* @min 500
* @max 1500
* @group Sensor Calibration
* @unit hPa
*/
PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f);
/**
* Board rotation

67
src/modules/sensors/sensors.cpp
View File

@ -143,6 +143,12 @@
#define STICK_ON_OFF_LIMIT 0.75f
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
/**
* Sensor app start / stop handling function
*
@ -235,7 +241,7 @@ private:
math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
math::Matrix<3, 3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
bool _mag_is_external; /**< true if the active mag is on an external board */
uint64_t _battery_discharged; /**< battery discharged current in mA*ms */
hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */
@ -258,7 +264,7 @@ private:
int board_rotation;
int external_mag_rotation;
float board_offset[3];
int rc_map_roll;
@ -301,6 +307,8 @@ private:
float battery_voltage_scaling;
float battery_current_scaling;
float baro_qnh;
} _parameters; /**< local copies of interesting parameters */
struct {
@ -354,9 +362,11 @@ private:
param_t board_rotation;
param_t external_mag_rotation;
param_t board_offset[3];
param_t baro_qnh;
} _parameter_handles; /**< handles for interesting parameters */
@ -462,12 +472,6 @@ private:
namespace sensors
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
Sensors *g_sensors = nullptr;
}
@ -611,12 +615,15 @@ Sensors::Sensors() :
/* rotations */
_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
_parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
/* rotation offsets */
_parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF");
_parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF");
_parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF");
/* Barometer QNH */
_parameter_handles.baro_qnh = param_find("SENS_BARO_QNH");
/* fetch initial parameter values */
parameters_update();
}
@ -828,19 +835,37 @@ Sensors::parameters_update()
get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0]));
param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1]));
param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2]));
/** fine tune board offset on parameter update **/
math::Matrix<3, 3> board_rotation_offset;
math::Matrix<3, 3> board_rotation_offset;
board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0],
M_DEG_TO_RAD_F * _parameters.board_offset[1],
M_DEG_TO_RAD_F * _parameters.board_offset[2]);
_board_rotation = _board_rotation * board_rotation_offset;
/* update barometer qnh setting */
param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh));
int fd;
fd = open(BARO_DEVICE_PATH, 0);
if (fd < 0) {
warn("%s", BARO_DEVICE_PATH);
errx(1, "FATAL: no barometer found");
} else {
int ret = ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100));
if (ret) {
warnx("qnh could not be set");
close(fd);
return ERROR;
}
close(fd);
}
return OK;
}
@ -1204,16 +1229,18 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
if (updated) {
orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
raw.differential_pressure_pa = _diff_pres.differential_pressure_pa;
raw.differential_pressure_pa = _diff_pres.differential_pressure_raw_pa;
raw.differential_pressure_timestamp = _diff_pres.timestamp;
raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;
float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
_airspeed.timestamp = _diff_pres.timestamp;
_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa);
_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
raw.baro_pres_mbar * 1e2f, air_temperature_celsius);
/* don't risk to feed negative airspeed into the system */
_airspeed.indicated_airspeed_m_s = math::max(0.0f, calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa));
_airspeed.true_airspeed_m_s = math::max(0.0f, calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
raw.baro_pres_mbar * 1e2f, air_temperature_celsius));
_airspeed.air_temperature_celsius = air_temperature_celsius;
/* announce the airspeed if needed, just publish else */
@ -1432,12 +1459,10 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0.0f)) {
float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa;
float diff_pres_pa = (diff_pres_pa_raw > 0.0f) ? diff_pres_pa_raw : 0.0f;
_diff_pres.timestamp = t;
_diff_pres.differential_pressure_pa = diff_pres_pa;
_diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw;
_diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa * 0.1f);
_diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa_raw * 0.1f);
_diff_pres.temperature = -1000.0f;
/* announce the airspeed if needed, just publish else */

1
src/modules/uORB/topics/differential_pressure.h
View File

@ -54,7 +54,6 @@
struct differential_pressure_s {
uint64_t timestamp; /**< Microseconds since system boot, needed to integrate */
uint64_t error_count; /**< Number of errors detected by driver */
float differential_pressure_pa; /**< Differential pressure reading */
float differential_pressure_raw_pa; /**< Raw differential pressure reading (may be negative) */
float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */
float max_differential_pressure_pa; /**< Maximum differential pressure reading */

19
src/modules/uORB/topics/vehicle_command.h
View File

@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* Copyright (C) 2012-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
@ -37,6 +34,10 @@
/**
* @file vehicle_command.h
* Definition of the vehicle command uORB topic.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#ifndef TOPIC_VEHICLE_COMMAND_H_
@ -52,6 +53,9 @@
* but can contain additional ones.
*/
enum VEHICLE_CMD {
VEHICLE_CMD_CUSTOM_0 = 0, /* test command */
VEHICLE_CMD_CUSTOM_1 = 1, /* test command */
VEHICLE_CMD_CUSTOM_2 = 2, /* test command */
VEHICLE_CMD_NAV_WAYPOINT = 16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */
VEHICLE_CMD_NAV_LOITER_UNLIM = 17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */
VEHICLE_CMD_NAV_LOITER_TURNS = 18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */
@ -87,7 +91,8 @@ enum VEHICLE_CMD {
VEHICLE_CMD_MISSION_START = 300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */
VEHICLE_CMD_COMPONENT_ARM_DISARM = 400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */
VEHICLE_CMD_START_RX_PAIR = 500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */
VEHICLE_CMD_ENUM_END = 501, /* | */
VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001, /**< Prepare a payload deployment in the flight plan */
VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 /**< Control a pre-programmed payload deployment */
};
/**
@ -115,8 +120,8 @@ struct vehicle_command_s {
float param2; /**< Parameter 2, as defined by MAVLink VEHICLE_CMD enum. */
float param3; /**< Parameter 3, as defined by MAVLink VEHICLE_CMD enum. */
float param4; /**< Parameter 4, as defined by MAVLink VEHICLE_CMD enum. */
float param5; /**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum. */
float param6; /**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum. */
double param5; /**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum. */
double param6; /**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum. */
float param7; /**< Parameter 7, as defined by MAVLink VEHICLE_CMD enum. */
enum VEHICLE_CMD command; /**< Command ID, as defined MAVLink by VEHICLE_CMD enum. */
uint8_t target_system; /**< System which should execute the command */

4
src/modules/uavcan/esc_controller.cpp → src/modules/uavcan/actuators/esc.cpp
View File

@ -32,12 +32,12 @@
****************************************************************************/
/**
* @file esc_controller.cpp
* @file esc.cpp
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include "esc_controller.hpp"
#include "esc.hpp"
#include <systemlib/err.h>
UavcanEscController::UavcanEscController(uavcan::INode &node) :

2
src/modules/uavcan/esc_controller.hpp → src/modules/uavcan/actuators/esc.hpp
View File

@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file esc_controller.hpp
* @file esc.hpp
*
* UAVCAN <--> ORB bridge for ESC messages:
* uavcan.equipment.esc.RawCommand

17
src/modules/uavcan/module.mk
View File

@ -40,10 +40,19 @@ MODULE_COMMAND = uavcan
MAXOPTIMIZATION = -Os
SRCS += uavcan_main.cpp \
uavcan_clock.cpp \
esc_controller.cpp \
gnss_receiver.cpp
# Main
SRCS += uavcan_main.cpp \
uavcan_clock.cpp \
uavcan_params.c
# Actuators
SRCS += actuators/esc.cpp
# Sensors
SRCS += sensors/sensor_bridge.cpp \
sensors/gnss.cpp \
sensors/mag.cpp \
sensors/baro.cpp
#
# libuavcan

117
src/modules/uavcan/sensors/baro.cpp Normal file
View File

@ -0,0 +1,117 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include "baro.hpp"
#include <cmath>
static const orb_id_t BARO_TOPICS[2] = {
ORB_ID(sensor_baro0),
ORB_ID(sensor_baro1)
};
const char *const UavcanBarometerBridge::NAME = "baro";
UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) :
UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, BARO_TOPICS),
_sub_air_data(node)
{
}
int UavcanBarometerBridge::init()
{
int res = device::CDev::init();
if (res < 0) {
return res;
}
res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb));
if (res < 0) {
log("failed to start uavcan sub: %d", res);
return res;
}
return 0;
}
int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case BAROIOCSMSLPRESSURE: {
if ((arg < 80000) || (arg > 120000)) {
return -EINVAL;
} else {
log("new msl pressure %u", _msl_pressure);
_msl_pressure = arg;
return OK;
}
}
case BAROIOCGMSLPRESSURE: {
return _msl_pressure;
}
default: {
return CDev::ioctl(filp, cmd, arg);
}
}
}
void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData> &msg)
{
auto report = ::baro_report();
report.timestamp = msg.getUtcTimestamp().toUSec();
if (report.timestamp == 0) {
report.timestamp = msg.getMonotonicTimestamp().toUSec();
}
report.temperature = msg.static_temperature;
report.pressure = msg.static_pressure / 100.0F; // Convert to millibar
/*
* Altitude computation
* Refer to the MS5611 driver for details
*/
const double T1 = 15.0 + 273.15; // temperature at base height in Kelvin
const double a = -6.5 / 1000; // temperature gradient in degrees per metre
const double g = 9.80665; // gravity constant in m/s/s
const double R = 287.05; // ideal gas constant in J/kg/K
const double p1 = _msl_pressure / 1000.0; // current pressure at MSL in kPa
const double p = double(msg.static_pressure) / 1000.0; // measured pressure in kPa
report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
publish(msg.getSrcNodeID().get(), &report);
}

68
src/modules/uavcan/sensors/baro.hpp Normal file
View File

@ -0,0 +1,68 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#pragma once
#include "sensor_bridge.hpp"
#include <drivers/drv_baro.h>
#include <uavcan/equipment/air_data/StaticAirData.hpp>
class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase
{
public:
static const char *const NAME;
UavcanBarometerBridge(uavcan::INode& node);
const char *get_name() const override { return NAME; }
int init() override;
private:
int ioctl(struct file *filp, int cmd, unsigned long arg) override;
void air_data_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData> &msg);
typedef uavcan::MethodBinder<UavcanBarometerBridge*,
void (UavcanBarometerBridge::*)
(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData>&)>
AirDataCbBinder;
uavcan::Subscriber<uavcan::equipment::air_data::StaticAirData, AirDataCbBinder> _sub_air_data;
unsigned _msl_pressure = 101325;
};

102
src/modules/uavcan/gnss_receiver.cpp → src/modules/uavcan/sensors/gnss.cpp
View File

@ -32,52 +32,74 @@
****************************************************************************/
/**
* @file gnss_receiver.cpp
* @file gnss.cpp
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
* @author Andrew Chambers <achamber@gmail.com>
*
*/
#include "gnss_receiver.hpp"
#include "gnss.hpp"
#include <systemlib/err.h>
#include <mathlib/mathlib.h>
#define MM_PER_CM 10 // Millimeters per centimeter
UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) :
const char *const UavcanGnssBridge::NAME = "gnss";
UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
_node(node),
_uavcan_sub_status(node),
_sub_fix(node),
_report_pub(-1)
{
}
int UavcanGnssReceiver::init()
int UavcanGnssBridge::init()
{
int res = -1;
// GNSS fix subscription
res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssReceiver::gnss_fix_sub_cb));
int res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::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)
unsigned UavcanGnssBridge::get_num_redundant_channels() const
{
_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)
return (_receiver_node_id < 0) ? 0 : 1;
}
_report.timestamp_variance = _report.timestamp_position;
void UavcanGnssBridge::print_status() const
{
printf("RX errors: %d, receiver node id: ", _sub_fix.getFailureCount());
if (_receiver_node_id < 0) {
printf("N/A\n");
} else {
printf("%d\n", _receiver_node_id);
}
}
void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
{
// This bridge does not support redundant GNSS receivers yet.
if (_receiver_node_id < 0) {
_receiver_node_id = msg.getSrcNodeID().get();
warnx("GNSS receiver node ID: %d", _receiver_node_id);
} else {
if (_receiver_node_id != msg.getSrcNodeID().get()) {
return; // This GNSS receiver is the redundant one, ignore it.
}
}
auto report = ::vehicle_gps_position_s();
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
@ -90,19 +112,19 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uav
// 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;
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;
report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F;
} else {
_report.eph = -1.0F;
_report.epv = -1.0F;
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]);
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
@ -118,36 +140,36 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uav
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 =
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.s_variance_m_s = -1.0F;
report.c_variance_rad = -1.0F;
}
_report.fix_type = msg.status;
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_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.timestamp_time = report.timestamp_position;
report.time_gps_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec(); // Convert to microseconds
_report.satellites_used = msg.sats_used;
report.satellites_used = msg.sats_used;
if (_report_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &report);
} else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &report);
}
}

37
src/modules/uavcan/gnss_receiver.hpp → src/modules/uavcan/sensors/gnss.hpp
View File

@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file gnss_receiver.hpp
* @file gnss.hpp
*
* UAVCAN --> ORB bridge for GNSS messages:
* uavcan.equipment.gnss.Fix
@ -51,12 +51,22 @@
#include <uavcan/uavcan.hpp>
#include <uavcan/equipment/gnss/Fix.hpp>
class UavcanGnssReceiver
#include "sensor_bridge.hpp"
class UavcanGnssBridge : public IUavcanSensorBridge
{
public:
UavcanGnssReceiver(uavcan::INode& node);
static const char *const NAME;
int init();
UavcanGnssBridge(uavcan::INode& node);
const char *get_name() const override { return NAME; }
int init() override;
unsigned get_num_redundant_channels() const override;
void print_status() const override;
private:
/**
@ -64,21 +74,14 @@ private:
*/
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>&)>
typedef uavcan::MethodBinder<UavcanGnssBridge*,
void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>&)>
FixCbBinder;
/*
* libuavcan related things
*/
uavcan::INode &_node;
uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _uavcan_sub_status;
uavcan::INode &_node;
uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _sub_fix;
int _receiver_node_id = -1;
/*
* uORB
*/
struct vehicle_gps_position_s _report; ///< uORB topic for gnss position
orb_advert_t _report_pub; ///< uORB pub for gnss position
orb_advert_t _report_pub; ///< uORB pub for gnss position
};

123
src/modules/uavcan/sensors/mag.cpp Normal file
View File

@ -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.
*
****************************************************************************/
/**
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include "mag.hpp"
static const orb_id_t MAG_TOPICS[3] = {
ORB_ID(sensor_mag0),
ORB_ID(sensor_mag1),
ORB_ID(sensor_mag2)
};
const char *const UavcanMagnetometerBridge::NAME = "mag";
UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) :
UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS),
_sub_mag(node)
{
_scale.x_scale = 1.0F;
_scale.y_scale = 1.0F;
_scale.z_scale = 1.0F;
}
int UavcanMagnetometerBridge::init()
{
int res = device::CDev::init();
if (res < 0) {
return res;
}
res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb));
if (res < 0) {
log("failed to start uavcan sub: %d", res);
return res;
}
return 0;
}
int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case MAGIOCSSCALE: {
std::memcpy(&_scale, reinterpret_cast<const void*>(arg), sizeof(_scale));
return 0;
}
case MAGIOCGSCALE: {
std::memcpy(reinterpret_cast<void*>(arg), &_scale, sizeof(_scale));
return 0;
}
case MAGIOCSELFTEST: {
return 0; // Nothing to do
}
case MAGIOCGEXTERNAL: {
return 0; // We don't want anyone to transform the coordinate frame, so we declare it onboard
}
case MAGIOCSSAMPLERATE: {
return 0; // Pretend that this stuff is supported to keep the sensor app happy
}
case MAGIOCCALIBRATE:
case MAGIOCGSAMPLERATE:
case MAGIOCSRANGE:
case MAGIOCGRANGE:
case MAGIOCSLOWPASS:
case MAGIOCEXSTRAP:
case MAGIOCGLOWPASS: {
return -EINVAL;
}
default: {
return CDev::ioctl(filp, cmd, arg);
}
}
}
void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg)
{
auto report = ::mag_report();
report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything
report.timestamp = msg.getUtcTimestamp().toUSec();
if (report.timestamp == 0) {
report.timestamp = msg.getMonotonicTimestamp().toUSec();
}
report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale;
report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale;
report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale;
publish(msg.getSrcNodeID().get(), &report);
}

68
src/modules/uavcan/sensors/mag.hpp Normal file
View File

@ -0,0 +1,68 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#pragma once
#include "sensor_bridge.hpp"
#include <drivers/drv_mag.h>
#include <uavcan/equipment/ahrs/Magnetometer.hpp>
class UavcanMagnetometerBridge : public UavcanCDevSensorBridgeBase
{
public:
static const char *const NAME;
UavcanMagnetometerBridge(uavcan::INode& node);
const char *get_name() const override { return NAME; }
int init() override;
private:
int ioctl(struct file *filp, int cmd, unsigned long arg) override;
void mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg);
typedef uavcan::MethodBinder<UavcanMagnetometerBridge*,
void (UavcanMagnetometerBridge::*)
(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer>&)>
MagCbBinder;
uavcan::Subscriber<uavcan::equipment::ahrs::Magnetometer, MagCbBinder> _sub_mag;
mag_scale _scale = {};
};

158
src/modules/uavcan/sensors/sensor_bridge.cpp Normal file
View File

@ -0,0 +1,158 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include "sensor_bridge.hpp"
#include <cassert>
#include "gnss.hpp"
#include "mag.hpp"
#include "baro.hpp"
/*
* IUavcanSensorBridge
*/
void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list)
{
list.add(new UavcanBarometerBridge(node));
list.add(new UavcanMagnetometerBridge(node));
list.add(new UavcanGnssBridge(node));
}
/*
* UavcanCDevSensorBridgeBase
*/
UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase()
{
for (unsigned i = 0; i < _max_channels; i++) {
if (_channels[i].node_id >= 0) {
(void)unregister_class_devname(_class_devname, _channels[i].class_instance);
}
}
delete [] _orb_topics;
delete [] _channels;
}
void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
{
assert(report != nullptr);
Channel *channel = nullptr;
// Checking if such channel already exists
for (unsigned i = 0; i < _max_channels; i++) {
if (_channels[i].node_id == node_id) {
channel = _channels + i;
break;
}
}
// No such channel - try to create one
if (channel == nullptr) {
if (_out_of_channels) {
return; // Give up immediately - saves some CPU time
}
log("adding channel %d...", node_id);
// Search for the first free channel
for (unsigned i = 0; i < _max_channels; i++) {
if (_channels[i].node_id < 0) {
channel = _channels + i;
break;
}
}
// No free channels left
if (channel == nullptr) {
_out_of_channels = true;
log("out of channels");
return;
}
// Ask the CDev helper which class instance we can take
const int class_instance = register_class_devname(_class_devname);
if (class_instance < 0 || class_instance >= int(_max_channels)) {
_out_of_channels = true;
log("out of class instances");
(void)unregister_class_devname(_class_devname, class_instance);
return;
}
// Publish to the appropriate topic, abort on failure
channel->orb_id = _orb_topics[class_instance];
channel->node_id = node_id;
channel->class_instance = class_instance;
channel->orb_advert = orb_advertise(channel->orb_id, report);
if (channel->orb_advert < 0) {
log("ADVERTISE FAILED");
(void)unregister_class_devname(_class_devname, class_instance);
*channel = Channel();
return;
}
log("channel %d class instance %d ok", channel->node_id, channel->class_instance);
}
assert(channel != nullptr);
(void)orb_publish(channel->orb_id, channel->orb_advert, report);
}
unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const
{
unsigned out = 0;
for (unsigned i = 0; i < _max_channels; i++) {
if (_channels[i].node_id >= 0) {
out += 1;
}
}
return out;
}
void UavcanCDevSensorBridgeBase::print_status() const
{
printf("devname: %s\n", _class_devname);
for (unsigned i = 0; i < _max_channels; i++) {
if (_channels[i].node_id >= 0) {
printf("channel %d: node id %d --> class instance %d\n",
i, _channels[i].node_id, _channels[i].class_instance);
} else {
printf("channel %d: empty\n", i);
}
}
}

131
src/modules/uavcan/sensors/sensor_bridge.hpp Normal file
View File

@ -0,0 +1,131 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#pragma once
#include <containers/List.hpp>
#include <uavcan/uavcan.hpp>
#include <drivers/device/device.h>
#include <drivers/drv_orb_dev.h>
/**
* A sensor bridge class must implement this interface.
*/
class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode<IUavcanSensorBridge*>
{
public:
static constexpr unsigned MAX_NAME_LEN = 20;
virtual ~IUavcanSensorBridge() { }
/**
* Returns ASCII name of the bridge.
*/
virtual const char *get_name() const = 0;
/**
* Starts the bridge.
* @return Non-negative value on success, negative on error.
*/
virtual int init() = 0;
/**
* Returns number of active redundancy channels.
*/
virtual unsigned get_num_redundant_channels() const = 0;
/**
* Prints current status in a human readable format to stdout.
*/
virtual void print_status() const = 0;
/**
* Sensor bridge factory.
* Creates a bridge object by its ASCII name, e.g. "gnss", "mag".
* @return nullptr if such bridge can't be created.
*/
static void make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list);
};
/**
* This is the base class for redundant sensors with an independent ORB topic per each redundancy channel.
* For example, sensor_mag0, sensor_mag1, etc.
*/
class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CDev
{
struct Channel
{
int node_id = -1;
orb_id_t orb_id = nullptr;
orb_advert_t orb_advert = -1;
int class_instance = -1;
};
const unsigned _max_channels;
const char *const _class_devname;
orb_id_t *const _orb_topics;
Channel *const _channels;
bool _out_of_channels = false;
protected:
template <unsigned MaxChannels>
UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname,
const orb_id_t (&orb_topics)[MaxChannels]) :
device::CDev(name, devname),
_max_channels(MaxChannels),
_class_devname(class_devname),
_orb_topics(new orb_id_t[MaxChannels]),
_channels(new Channel[MaxChannels])
{
memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels);
}
/**
* Sends one measurement into appropriate ORB topic.
* New redundancy channels will be registered automatically.
* @param node_id Sensor's Node ID
* @param report Pointer to ORB message object
*/
void publish(const int node_id, const void *report);
public:
virtual ~UavcanCDevSensorBridgeBase();
unsigned get_num_redundant_channels() const override;
void print_status() const override;
};

2
src/modules/uavcan/uavcan_clock.cpp
View File

@ -62,6 +62,8 @@ void adjustUtc(uavcan::UtcDuration adjustment)
(void)adjustment;
}
uavcan::uint64_t getUtcUSecFromCanInterrupt();
uavcan::uint64_t getUtcUSecFromCanInterrupt()
{
return 0;

135
src/modules/uavcan/uavcan_main.cpp
View File

@ -38,8 +38,10 @@
#include <fcntl.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/mixer/mixer.h>
#include <systemlib/board_serial.h>
#include <systemlib/scheduling_priorities.h>
#include <version/version.h>
#include <arch/board/board.h>
#include <arch/chip/chip.h>
@ -65,16 +67,18 @@ 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)
_node_mutex(),
_esc_controller(_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));
const int res = pthread_mutex_init(&_node_mutex, nullptr);
if (res < 0) {
std::abort();
}
}
UavcanNode::~UavcanNode()
@ -99,10 +103,18 @@ UavcanNode::~UavcanNode()
}
/* clean up the alternate device node */
// unregister_driver(PWM_OUTPUT_DEVICE_PATH);
// unregister_driver(PWM_OUTPUT_DEVICE_PATH);
::close(_armed_sub);
// Removing the sensor bridges
auto br = _sensor_bridges.getHead();
while (br != nullptr) {
auto next = br->getSibling();
delete br;
br = next;
}
_instance = nullptr;
}
@ -164,7 +176,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
* 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,
_instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize,
static_cast<main_t>(run_trampoline), nullptr);
if (_instance->_task < 0) {
@ -214,11 +226,12 @@ int UavcanNode::init(uavcan::NodeID node_id)
{
int ret = -1;
/* do regular cdev init */
// Do regular cdev init
ret = CDev::init();
if (ret != OK)
if (ret != OK) {
return ret;
}
_node.setName("org.pixhawk.pixhawk");
@ -226,14 +239,24 @@ int UavcanNode::init(uavcan::NodeID node_id)
fill_node_info();
/* initializing the bridges UAVCAN <--> uORB */
// Actuators
ret = _esc_controller.init();
if (ret < 0)
if (ret < 0) {
return ret;
}
ret = _gnss_receiver.init();
if (ret < 0)
return ret;
// Sensor bridges
IUavcanSensorBridge::make_all(_node, _sensor_bridges);
auto br = _sensor_bridges.getHead();
while (br != nullptr) {
ret = br->init();
if (ret < 0) {
warnx("cannot init sensor bridge '%s' (%d)", br->get_name(), ret);
return ret;
}
warnx("sensor bridge '%s' init ok", br->get_name());
br = br->getSibling();
}
return _node.start();
}
@ -248,6 +271,8 @@ void UavcanNode::node_spin_once()
int UavcanNode::run()
{
(void)pthread_mutex_lock(&_node_mutex);
const unsigned PollTimeoutMs = 50;
// XXX figure out the output count
@ -291,8 +316,13 @@ int UavcanNode::run()
_groups_subscribed = _groups_required;
}
// Mutex is unlocked while the thread is blocked on IO multiplexing
(void)pthread_mutex_unlock(&_node_mutex);
const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
(void)pthread_mutex_lock(&_node_mutex);
node_spin_once(); // Non-blocking
// this would be bad...
@ -352,7 +382,6 @@ int UavcanNode::run()
// Output to the bus
_esc_controller.update_outputs(outputs.output, outputs.noutputs);
}
}
// Check arming state
@ -376,10 +405,7 @@ int UavcanNode::run()
}
int
UavcanNode::control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input)
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;
@ -520,8 +546,23 @@ UavcanNode::print_info()
warnx("not running, start first");
}
warnx("actuators control groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK");
(void)pthread_mutex_lock(&_node_mutex);
// ESC mixer status
printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n",
(unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK");
// Sensor bridges
auto br = _sensor_bridges.getHead();
while (br != nullptr) {
printf("Sensor '%s':\n", br->get_name());
br->print_status();
printf("\n");
br = br->getSibling();
}
(void)pthread_mutex_unlock(&_node_mutex);
}
/*
@ -529,79 +570,57 @@ UavcanNode::print_info()
*/
static void print_usage()
{
warnx("usage: uavcan start <node_id> [can_bitrate]");
warnx("usage: \n"
"\tuavcan {start|status|stop}");
}
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);
if (UavcanNode::instance()) {
errx(1, "already started");
}
/*
* Node ID
*/
const int node_id = atoi(argv[2]);
// Node ID
int32_t node_id = 0;
(void)param_get(param_find("UAVCAN_NODE_ID"), &node_id);
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;
// CAN bitrate
int32_t bitrate = 0;
(void)param_get(param_find("UAVCAN_BITRATE"), &bitrate);
if (argc > 3) {
bitrate = atol(argv[3]);
}
if (bitrate <= 0) {
bitrate = DEFAULT_CAN_BITRATE;
}
if (UavcanNode::instance()) {
errx(1, "already started");
}
/*
* Start
*/
// 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();
UavcanNode *const 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;
inst->print_info();
::exit(0);
}
if (!std::strcmp(argv[1], "stop")) {
delete inst;
inst = nullptr;
return OK;
delete inst;
::exit(0);
}
print_usage();

17
src/modules/uavcan/uavcan_main.hpp
View File

@ -42,8 +42,8 @@
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#include "esc_controller.hpp"
#include "gnss_receiver.hpp"
#include "actuators/esc.hpp"
#include "sensors/sensor_bridge.hpp"
/**
* @file uavcan_main.hpp
@ -77,12 +77,10 @@ public:
static int start(uavcan::NodeID node_id, uint32_t bitrate);
Node& getNode() { return _node; }
Node& get_node() { return _node; }
static int control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input);
// TODO: move the actuator mixing stuff into the ESC controller class
static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input);
void subscribe();
@ -109,8 +107,11 @@ private:
static UavcanNode *_instance; ///< singleton pointer
Node _node; ///< library instance
pthread_mutex_t _node_mutex;
UavcanEscController _esc_controller;
UavcanGnssReceiver _gnss_receiver;
List<IUavcanSensorBridge*> _sensor_bridges; ///< List of active sensor bridges
MixerGroup *_mixers = nullptr;

73
src/modules/uavcan/uavcan_params.c Normal file
View File

@ -0,0 +1,73 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/**
* Enable UAVCAN.
*
* Enables support for UAVCAN-interfaced actuators and sensors.
*
* @min 0
* @max 1
* @group UAVCAN
*/
PARAM_DEFINE_INT32(UAVCAN_ENABLE, 0);
/**
* UAVCAN Node ID.
*
* Read the specs at http://uavcan.org to learn more about Node ID.
*
* @min 1
* @max 125
* @group UAVCAN
*/
PARAM_DEFINE_INT32(UAVCAN_NODE_ID, 1);
/**
* UAVCAN CAN bus bitrate.
*
* @min 20000
* @max 1000000
* @group UAVCAN
*/
PARAM_DEFINE_INT32(UAVCAN_BITRATE, 1000000);

2
src/systemcmds/tests/test_sensors.c
View File

@ -331,7 +331,7 @@ mag(int argc, char *argv[])
float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z);
if (len < 1.0f || len > 3.0f) {
if (len < 0.25f || len > 3.0f) {
warnx("MAG scale error!");
return ERROR;
}

2
uavcan

@ -1 +1 @@
Subproject commit 6980ee824074aa2f7a62221bf6040ee411119520
Subproject commit c4c14c60fbbd9acd281ee97d5bb2a4027d0ae2d9