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

Merge branch 'master' of https://github.com/mazahner/Firmware

Browse Source
This commit is contained in:
mazahner
2014-11-10 17:39:04 +01:00
parent d15203de91 4e8e6e653b
commit 6ec0833545
235 changed files with 13957 additions and 2789 deletions
Download Patch File Download Diff File Expand all files Collapse all files
LICENSE.md
+41
View File
@@ -0,0 +1,41 @@
The PX4 firmware is licensed generally under a permissive 3-clause BSD license. Contributions are required
to be made under the same license. Any exception to this general rule is listed below.
/****************************************************************************
*
* 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.
*
****************************************************************************/
- PX4 middleware: BSD 3-clause
- PX4 flight control stack: BSD 3-clause
- NuttX operating system: BSD 3-clause
- Exceptions: Currently only this [400 LOC file](https://github.com/PX4/Firmware/blob/master/src/lib/external_lgpl/tecs/tecs.cpp) remains LGPL, but will be replaced with a BSD implementation.
NuttX
+1 -1
Submodule NuttX updated: 088146b90e...5ee4b2b2c2
README.md
+10
View File
@@ -0,0 +1,10 @@
## PX4 Aerial Middleware and Flight Control Stack ##
* Official Website: http://px4.io
* License: BSD 3-clause (see LICENSE.md)
* Supported airframes:
* Multicopters
* Fixed wing
* Binaries (always up-to-date from master):
* [Downloads](https://pixhawk.org/downloads)
* Mailing list: [Google Groups](http://groups.google.com/group/px4users)
ROMFS/px4fmu_common/init.d/10015_tbs_discovery
+1
View File
@@ -27,3 +27,4 @@ fi
set MIXER FMU_quad_w
set PWM_OUTPUTS 1234
set PWM_MIN 1200
ROMFS/px4fmu_common/init.d/3031_phantom
+3
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
ROMFS/px4fmu_common/init.d/3035_viper
+11
View File
@@ -0,0 +1,11 @@
#!nsh
#
# Viper
#
# Simon Wilks <sjwilks@gmail.com>
#
sh /etc/init.d/rc.fw_defaults
set MIXER Viper
ROMFS/px4fmu_common/init.d/rc.autostart
+11
View File
@@ -21,6 +21,12 @@
# Simulation setups
#
if param compare SYS_AUTOSTART 901
then
sh /etc/init.d/901_bottle_drop_test.hil
set MODE custom
fi
if param compare SYS_AUTOSTART 1000
then
sh /etc/init.d/1000_rc_fw_easystar.hil
@@ -103,6 +109,11 @@ then
sh /etc/init.d/3034_fx79
fi
if param compare SYS_AUTOSTART 3035 35
then
sh /etc/init.d/3035_viper
fi
if param compare SYS_AUTOSTART 3100
then
sh /etc/init.d/3100_tbs_caipirinha
ROMFS/px4fmu_common/init.d/rc.fw_apps
+2
View File
@@ -13,3 +13,5 @@ ekf_att_pos_estimator start
#
fw_att_control start
fw_pos_control_l1 start
bottle_drop start
ROMFS/px4fmu_common/init.d/rc.interface
+5
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
ROMFS/px4fmu_common/init.d/rc.sensors
+5
View File
@@ -68,6 +68,11 @@ else
fi
fi
# Check for flow sensor
if px4flow start
then
fi
#
# Start the sensor collection task.
# IMPORTANT: this also loads param offsets
ROMFS/px4fmu_common/init.d/rc.uavcan
+18
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
ROMFS/px4fmu_common/init.d/rc.usb
+2 -2
View File
@@ -3,7 +3,7 @@
# USB MAVLink start
#
mavlink start -r 10000 -d /dev/ttyACM0 -x
mavlink start -r 20000 -d /dev/ttyACM0 -x
# Enable a number of interesting streams we want via USB
mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 200
usleep 100000
@@ -23,7 +23,7 @@ mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS_RAW -r 5
usleep 100000
mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20
usleep 100000
mavlink stream -d /dev/ttyACM0 -s GLOBAL_POSITION_SETPOINT_INT -r 20
mavlink stream -d /dev/ttyACM0 -s POSITION_TARGET_GLOBAL_INT -r 10
usleep 100000
# Exit shell to make it available to MAVLink
ROMFS/px4fmu_common/init.d/rcS
+21 -13
View File
@@ -66,6 +66,9 @@ then
#
sercon
# Try to get an USB console
nshterm /dev/ttyACM0 &
#
# Start the ORB (first app to start)
#
@@ -83,9 +86,12 @@ then
param select $PARAM_FILE
if param load
then
echo "[init] Params loaded: $PARAM_FILE"
echo "[param] Loaded: $PARAM_FILE"
else
echo "[init] ERROR: Params loading failed: $PARAM_FILE"
echo "[param] FAILED loading $PARAM_FILE"
if param reset
then
fi
fi
#
@@ -93,11 +99,9 @@ then
#
if rgbled start
then
echo "[init] RGB Led"
else
if blinkm start
then
echo "[init] BlinkM"
blinkm systemstate
fi
fi
@@ -126,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
@@ -276,13 +281,12 @@ then
fi
fi
# Try to get an USB console
nshterm /dev/ttyACM0 &
#
# Start the datamanager
# Start the datamanager (and do not abort boot if it fails)
#
dataman start
if dataman start
then
fi
#
# Start the Commander (needs to be this early for in-air-restarts)
@@ -299,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
@@ -442,6 +445,11 @@ then
mavlink start $MAVLINK_FLAGS
#
# UAVCAN
#
sh /etc/init.d/rc.uavcan
#
# Sensors, Logging, GPS
#
ROMFS/px4fmu_common/mixers/FMU_AERT.mix
+6 -5
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
ROMFS/px4fmu_common/mixers/FMU_FX79.mix
+7 -10
View File
@@ -27,12 +27,12 @@ for the elevons.
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 5000 8000 0 -10000 10000
S: 0 0 7500 7500 0 -10000 10000
S: 0 1 8000 8000 0 -10000 10000
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 8000 5000 0 -10000 10000
S: 0 0 7500 7500 0 -10000 10000
S: 0 1 -8000 -8000 0 -10000 10000
Output 2
@@ -52,21 +52,18 @@ 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
M: 1
O: 10000 10000 0 -10000 10000
S: 0 7 10000 10000 0 -10000 10000
S: 2 2 -10000 -10000 0 -10000 10000
ROMFS/px4fmu_common/mixers/Viper.mix
Executable
+71
View File
@@ -0,0 +1,71 @@
Viper Delta-wing mixer
=================================
Designed for Viper.
TODO (sjwilks): Add mixers for flaps.
This file defines mixers suitable for controlling a delta wing aircraft using
PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU
servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is
assumed to be unused.
Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
(roll), 1 (pitch) and 3 (thrust).
See the README for more information on the scaler format.
Elevon mixers
-------------
Three scalers total (output, roll, pitch).
On the assumption that the two elevon servos are physically reversed, the pitch
input is inverted between the two servos.
The scaling factor for roll inputs is adjusted to implement differential travel
for the elevons.
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 7500 7500 0 -10000 10000
S: 0 1 8000 8000 0 -10000 10000
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 7500 7500 0 -10000 10000
S: 0 1 -8000 -8000 0 -10000 10000
Output 2
--------
This mixer is empty.
Z:
Motor speed mixer
-----------------
Two scalers total (output, thrust).
This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
range. Inputs below zero are treated as zero.
M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000
Inputs to the mixer come from channel group 2 (payload), channels 0
(bay servo 1), 1 (bay servo 2) and 3 (drop release).
-----------------------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
S: 2 0 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 2 1 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 2 2 -8000 -8000 0 -10000 10000
ROMFS/px4fmu_test/init.d/rcS
+30
View File
@@ -75,3 +75,33 @@ if [ -f /fs/microsd/mount_test_cmds.txt ]
then
tests mount
fi
#
# Run unit tests at board boot, reporting failure as needed.
# Add new unit tests using the same pattern as below.
#
set unit_test_failure 0
if mavlink_tests
then
else
set unit_test_failure 1
set unit_test_failure_list "${unit_test_failure_list} mavlink_tests"
fi
if commander_tests
then
else
set unit_test_failure 1
set unit_test_failure_list "${unit_test_failure_list} commander_tests"
fi
if [ $unit_test_failure == 0 ]
then
echo
echo "All Unit Tests PASSED"
else
echo
echo "Some Unit Tests FAILED:${unit_test_failure_list}"
fi
ROMFS/px4fmu_test/unit_test_data/mavlink_tests/empty_dir/.gitignore
View File
ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_238.data
BIN
View File
Binary file not shown.
ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_239.data
BIN
View File
Binary file not shown.
ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_240.data
BIN
View File
Binary file not shown.
Tools/px_romfs_pruner.py
+1 -1
View File
@@ -57,7 +57,7 @@ def main():
for (root, dirs, files) in os.walk(args.folder):
for file in files:
# only prune text files
if ".zip" in file or ".bin" in file or ".swp" in file:
if ".zip" in file or ".bin" in file or ".swp" in file or ".data" in file:
continue
file_path = os.path.join(root, file)
Tools/px_uploader.py
+5 -1
View File
@@ -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
@@ -459,6 +459,7 @@ if os.path.exists("/usr/sbin/ModemManager"):
# Load the firmware file
fw = firmware(args.firmware)
print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision')))
print("If the board does not respond within 1-2 seconds, unplug and re-plug the USB connector.")
# Spin waiting for a device to show up
while True:
@@ -508,9 +509,12 @@ while True:
except Exception:
# most probably a timeout talking to the port, no bootloader, try to reboot the board
print("attempting reboot on %s..." % port)
print("if the board does not respond, unplug and re-plug the USB connector.")
up.send_reboot()
# wait for the reboot, without we might run into Serial I/O Error 5
time.sleep(0.5)
# always close the port
up.close()
continue
try:
Tools/sdlog2/sdlog2_dump.py
Regular → Executable
+2 -2
View File
@@ -154,8 +154,8 @@ class SDLog2Parser:
first_data_msg = False
self.__parseMsg(msg_descr)
bytes_read += self.__ptr
if not self.__debug_out and self.__time_msg != None and self.__csv_updated:
self.__printCSVRow()
if not self.__debug_out and self.__time_msg != None and self.__csv_updated:
self.__printCSVRow()
f.close()
def __bytesLeft(self):
Tools/tests-host/.gitignore
+2
View File
@@ -1,4 +1,6 @@
./obj/*
mixer_test
sf0x_test
sbus2_test
autodeclination_test
st24_test
Tools/tests-host/Makefile
+18 -3
View File
@@ -3,7 +3,7 @@ CC=g++
CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers \
-I../../src -I../../src/lib -D__EXPORT="" -Dnullptr="0" -lm
all: mixer_test sbus2_test autodeclination_test
all: mixer_test sbus2_test autodeclination_test st24_test sf0x_test
MIXER_FILES=../../src/systemcmds/tests/test_mixer.cpp \
../../src/systemcmds/tests/test_conv.cpp \
@@ -20,7 +20,16 @@ SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \
hrt.cpp \
sbus2_test.cpp
AUTODECLINATION_FILES= ../../src/lib/geo/geo_mag_declination.c \
ST24_FILES=../../src/lib/rc/st24.c \
hrt.cpp \
st24_test.cpp
SF0X_FILES= \
hrt.cpp \
sf0x_test.cpp \
../../src/drivers/sf0x/sf0x_parser.cpp
AUTODECLINATION_FILES= ../../src/lib/geo_lookup/geo_mag_declination.c \
hrt.cpp \
autodeclination_test.cpp
@@ -30,10 +39,16 @@ mixer_test: $(MIXER_FILES)
sbus2_test: $(SBUS2_FILES)
$(CC) -o sbus2_test $(SBUS2_FILES) $(CFLAGS)
sf0x_test: $(SF0X_FILES)
$(CC) -o sf0x_test $(SF0X_FILES) $(CFLAGS)
autodeclination_test: $(SBUS2_FILES)
$(CC) -o autodeclination_test $(AUTODECLINATION_FILES) $(CFLAGS)
st24_test: $(ST24_FILES)
$(CC) -o st24_test $(ST24_FILES) $(CFLAGS)
.PHONY: clean
clean:
rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test
rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test st24_test sf0x_test
Tools/tests-host/sbus2_test.cpp
+2 -1
View File
@@ -29,7 +29,8 @@ int main(int argc, char *argv[]) {
// Trash the first 20 lines
for (unsigned i = 0; i < 20; i++) {
(void)fscanf(fp, "%f,%x,,", &f, &x);
char buf[200];
(void)fgets(buf, sizeof(buf), fp);
}
// Init the parser
Tools/tests-host/sf0x_test.cpp
+65
View File
@@ -0,0 +1,65 @@
#include <unistd.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <drivers/sf0x/sf0x_parser.h>
int main(int argc, char *argv[])
{
warnx("SF0X test started");
int ret = 0;
const char LINE_MAX = 20;
char _linebuf[LINE_MAX];
_linebuf[0] = '\0';
const char *lines[] = {"0.01\r\n",
"0.02\r\n",
"0.03\r\n",
"0.04\r\n",
"0",
".",
"0",
"5",
"\r",
"\n",
"0",
"3\r",
"\n"
"\r\n",
"0.06",
"\r\n"
};
enum SF0X_PARSE_STATE state = SF0X_PARSE_STATE0_UNSYNC;
float dist_m;
char _parserbuf[LINE_MAX];
unsigned _parsebuf_index = 0;
for (unsigned l = 0; l < sizeof(lines) / sizeof(lines[0]); l++) {
printf("\n%s", _linebuf);
int parse_ret;
for (int i = 0; i < strlen(lines[l]); i++) {
parse_ret = sf0x_parser(lines[l][i], _parserbuf, &_parsebuf_index, &state, &dist_m);
if (parse_ret == 0) {
printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : "");
}
}
printf("%s", lines[l]);
}
warnx("test finished");
return ret;
}
Tools/tests-host/st24_test.cpp
+76
View File
@@ -0,0 +1,76 @@
#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <rc/st24.h>
#include "../../src/systemcmds/tests/tests.h"
int main(int argc, char *argv[])
{
warnx("ST24 test started");
if (argc < 2) {
errx(1, "Need a filename for the input file");
}
warnx("loading data from: %s", argv[1]);
FILE *fp;
fp = fopen(argv[1], "rt");
if (!fp) {
errx(1, "failed opening file");
}
float f;
unsigned x;
int ret;
// Trash the first 20 lines
for (unsigned i = 0; i < 20; i++) {
char buf[200];
(void)fgets(buf, sizeof(buf), fp);
}
float last_time = 0;
while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
if (((f - last_time) * 1000 * 1000) > 3000) {
// warnx("FRAME RESET\n\n");
}
uint8_t b = static_cast<uint8_t>(x);
last_time = f;
// Pipe the data into the parser
hrt_abstime now = hrt_absolute_time();
uint8_t rssi;
uint8_t rx_count;
uint16_t channel_count;
uint16_t channels[20];
if (!st24_decode(b, &rssi, &rx_count, &channel_count, channels, sizeof(channels) / sizeof(channels[0]))) {
warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count);
for (unsigned i = 0; i < channel_count; i++) {
int16_t val = channels[i];
warnx("channel %u: %d 0x%03X", i, static_cast<int>(val), static_cast<int>(val));
}
}
}
if (ret == EOF) {
warnx("Test finished, reached end of file");
} else {
warnx("Test aborted, errno: %d", ret);
}
}
Tools/upload.sh
Executable
+28
View File
@@ -0,0 +1,28 @@
#!/bin/bash
EXEDIR=`pwd`
BASEDIR=$(dirname $0)
SYSTYPE=`uname -s`
#
# Serial port defaults.
#
# XXX The uploader should be smarter than this.
#
if [ $SYSTYPE=Darwin ];
then
SERIAL_PORTS="/dev/tty.usbmodemPX*,/dev/tty.usbmodem*"
fi
if [ $SYSTYPE=Linux ];
then
SERIAL_PORTS="/dev/serial/by-id/usb-3D_Robotics*"
fi
if [ $SYSTYPE="" ];
then
SERIAL_PORTS="COM32,COM31,COM30,COM29,COM28,COM27,COM26,COM25,COM24,COM23,COM22,COM21,COM20,COM19,COM18,COM17,COM16,COM15,COM14,COM13,COM12,COM11,COM10,COM9,COM8,COM7,COM6,COM5,COM4,COM3,COM2,COM1,COM0"
fi
python $BASEDIR/px_uploader.py --port $SERIAL_PORTS $1
Tools/usb_serialload.py
+55
View File
@@ -0,0 +1,55 @@
import serial, time
port = serial.Serial('/dev/ttyACM0', baudrate=57600, timeout=2)
data = '01234567890123456789012345678901234567890123456789'
#data = 'hellohello'
outLine = 'echo %s\n' % data
port.write('\n\n\n')
port.write('free\n')
line = port.readline(80)
while line != '':
print(line)
line = port.readline(80)
i = 0
bytesOut = 0
bytesIn = 0
startTime = time.time()
lastPrint = startTime
while True:
bytesOut += port.write(outLine)
line = port.readline(80)
bytesIn += len(line)
# check command line echo
if (data not in line):
print('command error %d: %s' % (i,line))
#break
# read echo output
line = port.readline(80)
if (data not in line):
print('echo output error %d: %s' % (i,line))
#break
bytesIn += len(line)
#print('%d: %s' % (i,line))
#print('%d: bytesOut: %d, bytesIn: %d' % (i, bytesOut, bytesIn))
elapsedT = time.time() - lastPrint
if (time.time() - lastPrint >= 5):
outRate = bytesOut / elapsedT
inRate = bytesIn / elapsedT
usbRate = (bytesOut + bytesIn) / elapsedT
lastPrint = time.time()
print('elapsed time: %f' % (time.time() - startTime))
print('data rates (bytes/sec): out: %f, in: %f, total: %f' % (outRate, inRate, usbRate))
bytesOut = 0
bytesIn = 0
i += 1
#if (i > 2): break
makefiles/config_px4fmu-v1_default.mk
+5 -11
View File
@@ -24,27 +24,22 @@ MODULES += drivers/l3gd20
MODULES += drivers/mpu6000
MODULES += drivers/hmc5883
MODULES += drivers/ms5611
MODULES += drivers/mb12xx
MODULES += drivers/ll40ls
#MODULES += drivers/mb12xx
MODULES += drivers/gps
MODULES += drivers/hil
MODULES += drivers/hott/hott_telemetry
MODULES += drivers/hott/hott_sensors
MODULES += drivers/blinkm
#MODULES += drivers/blinkm
MODULES += drivers/rgbled
MODULES += drivers/mkblctrl
MODULES += drivers/airspeed
MODULES += drivers/ets_airspeed
#MODULES += drivers/ets_airspeed
MODULES += drivers/meas_airspeed
MODULES += drivers/frsky_telemetry
#MODULES += drivers/frsky_telemetry
MODULES += modules/sensors
#
# System commands
#
MODULES += systemcmds/mtd
MODULES += systemcmds/bl_update
MODULES += systemcmds/i2c
MODULES += systemcmds/mixer
MODULES += systemcmds/param
MODULES += systemcmds/perf
@@ -152,5 +147,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 )
makefiles/config_px4fmu-v2_default.mk
+6
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,
@@ -127,6 +128,11 @@ MODULES += lib/geo_lookup
MODULES += lib/conversion
MODULES += lib/launchdetection
#
# OBC challenge
#
MODULES += modules/bottle_drop
#
# Demo apps
#
makefiles/config_px4fmu-v2_test.mk
+4
View File
@@ -54,6 +54,10 @@ MODULES += lib/conversion
#
LIBRARIES += lib/mathlib/CMSIS
MODULES += modules/unit_test
MODULES += modules/mavlink/mavlink_tests
MODULES += modules/commander/commander_tests
#
# Transitional support - add commands from the NuttX export archive.
#
mavlink/include/mavlink/v1.0
+1 -1
Submodule mavlink/include/mavlink/v1.0 updated: 04b1ad5b28...ad5e5a645d
nuttx-configs/aerocore/nsh/defconfig
+1 -1
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
nuttx-configs/px4fmu-v1/nsh/defconfig
+1 -2
View File
@@ -288,7 +288,6 @@ 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_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
nuttx-configs/px4fmu-v2/nsh/defconfig
+2 -3
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
nuttx-configs/px4io-v1/nsh/defconfig
+2 -1
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
nuttx-configs/px4io-v2/nsh/defconfig
-1
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
src/drivers/airspeed/airspeed.cpp
+5 -2
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
src/drivers/boards/aerocore/aerocore_init.c
+13
View File
@@ -93,6 +93,19 @@
# endif
#endif
/*
* Ideally we'd be able to get these from up_internal.h,
* but since we want to be able to disable the NuttX use
* of leds for system indication at will and there is no
* separate switch, we need to build independent of the
* CONFIG_ARCH_LEDS configuration switch.
*/
__BEGIN_DECLS
extern void led_init(void);
extern void led_on(int led);
extern void led_off(int led);
__END_DECLS
/****************************************************************************
* Protected Functions
****************************************************************************/
src/drivers/boards/aerocore/board_config.h
+19
View File
@@ -171,6 +171,25 @@ __BEGIN_DECLS
extern void stm32_spiinitialize(void);
/****************************************************************************
* Name: nsh_archinitialize
*
* Description:
* Perform architecture specific initialization for NSH.
*
* CONFIG_NSH_ARCHINIT=y :
* Called from the NSH library
*
* CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, &&
* CONFIG_NSH_ARCHINIT=n :
* Called from board_initialize().
*
****************************************************************************/
#ifdef CONFIG_NSH_LIBRARY
int nsh_archinitialize(void);
#endif
#endif /* __ASSEMBLY__ */
__END_DECLS
src/drivers/boards/px4fmu-v1/board_config.h
+21
View File
@@ -209,6 +209,27 @@ __BEGIN_DECLS
extern void stm32_spiinitialize(void);
extern void stm32_usbinitialize(void);
/****************************************************************************
* Name: nsh_archinitialize
*
* Description:
* Perform architecture specific initialization for NSH.
*
* CONFIG_NSH_ARCHINIT=y :
* Called from the NSH library
*
* CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, &&
* CONFIG_NSH_ARCHINIT=n :
* Called from board_initialize().
*
****************************************************************************/
#ifdef CONFIG_NSH_LIBRARY
int nsh_archinitialize(void);
#endif
#endif /* __ASSEMBLY__ */
__END_DECLS
src/drivers/boards/px4fmu-v2/board_config.h
+21
View File
@@ -229,6 +229,27 @@ __BEGIN_DECLS
extern void stm32_spiinitialize(void);
extern void stm32_usbinitialize(void);
/****************************************************************************
* Name: nsh_archinitialize
*
* Description:
* Perform architecture specific initialization for NSH.
*
* CONFIG_NSH_ARCHINIT=y :
* Called from the NSH library
*
* CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, &&
* CONFIG_NSH_ARCHINIT=n :
* Called from board_initialize().
*
****************************************************************************/
#ifdef CONFIG_NSH_LIBRARY
int nsh_archinitialize(void);
#endif
#endif /* __ASSEMBLY__ */
__END_DECLS
src/drivers/boards/px4fmu-v2/px4fmu2_init.c
+13
View File
@@ -94,6 +94,19 @@
# endif
#endif
/*
* Ideally we'd be able to get these from up_internal.h,
* but since we want to be able to disable the NuttX use
* of leds for system indication at will and there is no
* separate switch, we need to build independent of the
* CONFIG_ARCH_LEDS configuration switch.
*/
__BEGIN_DECLS
extern void led_init(void);
extern void led_on(int led);
extern void led_off(int led);
__END_DECLS
/****************************************************************************
* Protected Functions
****************************************************************************/
src/drivers/boards/px4io-v2/board_config.h
+1
View File
@@ -77,6 +77,7 @@
#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15)
#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
#define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11)
/* Safety switch button *******************************************************/
src/drivers/boards/px4io-v2/px4iov2_init.c
+1
View File
@@ -108,6 +108,7 @@ __EXPORT void stm32_boardinitialize(void)
stm32_configgpio(GPIO_LED1);
stm32_configgpio(GPIO_LED2);
stm32_configgpio(GPIO_LED3);
stm32_configgpio(GPIO_LED4);
stm32_configgpio(GPIO_BTN_SAFETY);
src/drivers/device/device.cpp
+1
View File
@@ -97,6 +97,7 @@ Device::Device(const char *name,
/* setup a default device ID. When bus_type is UNKNOWN the
other fields are invalid */
_device_id.devid = 0;
_device_id.devid_s.bus_type = DeviceBusType_UNKNOWN;
_device_id.devid_s.bus = 0;
_device_id.devid_s.address = 0;
src/drivers/drv_gps.h
+2 -1
View File
@@ -57,7 +57,8 @@
typedef enum {
GPS_DRIVER_MODE_NONE = 0,
GPS_DRIVER_MODE_UBX,
GPS_DRIVER_MODE_MTK
GPS_DRIVER_MODE_MTK,
GPS_DRIVER_MODE_ASHTECH
} gps_driver_mode_t;
src/drivers/drv_pwm_output.h
+14 -3
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.
@@ -160,7 +165,7 @@ ORB_DECLARE(output_pwm);
#define DSM2_BIND_PULSES 3 /* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */
#define DSMX_BIND_PULSES 7 /* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */
#define DSMX8_BIND_PULSES 10 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */
#define DSMX8_BIND_PULSES 9 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */
/** power up DSM receiver */
#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11)
@@ -200,10 +205,16 @@ ORB_DECLARE(output_pwm);
#define PWM_SERVO_GET_DISABLE_LOCKDOWN _IOC(_PWM_SERVO_BASE, 22)
/** force safety switch off (to disable use of safety switch) */
#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23)
#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23)
/** force failsafe mode (failsafe values are set immediately even if failsafe condition not met) */
#define PWM_SERVO_SET_FORCE_FAILSAFE _IOC(_PWM_SERVO_BASE, 24)
#define PWM_SERVO_SET_FORCE_FAILSAFE _IOC(_PWM_SERVO_BASE, 24)
/** make failsafe non-recoverable (termination) if it occurs */
#define PWM_SERVO_SET_TERMINATION_FAILSAFE _IOC(_PWM_SERVO_BASE, 25)
/** force safety switch on (to enable use of safety switch) */
#define PWM_SERVO_SET_FORCE_SAFETY_ON _IOC(_PWM_SERVO_BASE, 26)
/*
*
src/drivers/drv_px4flow.h
-31
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.
*/
src/drivers/drv_rc_input.h
+2 -1
View File
@@ -83,7 +83,8 @@ enum RC_INPUT_SOURCE {
RC_INPUT_SOURCE_PX4FMU_PPM,
RC_INPUT_SOURCE_PX4IO_PPM,
RC_INPUT_SOURCE_PX4IO_SPEKTRUM,
RC_INPUT_SOURCE_PX4IO_SBUS
RC_INPUT_SOURCE_PX4IO_SBUS,
RC_INPUT_SOURCE_PX4IO_ST24
};
/**
src/drivers/drv_tone_alarm.h
+1
View File
@@ -150,6 +150,7 @@ enum {
TONE_ARMING_FAILURE_TUNE,
TONE_PARACHUTE_RELEASE_TUNE,
TONE_EKF_WARNING_TUNE,
TONE_BARO_WARNING_TUNE,
TONE_NUMBER_OF_TUNES
};
src/drivers/ets_airspeed/ets_airspeed.cpp
+6 -14
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 */
src/drivers/gps/ashtech.cpp
+641
View File
@@ -0,0 +1,641 @@
#include "ashtech.h"
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <poll.h>
#include <math.h>
#include <string.h>
#include <assert.h>
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/satellite_info.h>
#include <drivers/drv_hrt.h>
#include <fcntl.h>
#include <math.h>
ASHTECH::ASHTECH(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info):
_fd(fd),
_satellite_info(satellite_info),
_gps_position(gps_position)
{
decode_init();
_decode_state = NME_DECODE_UNINIT;
_rx_buffer_bytes = 0;
}
ASHTECH::~ASHTECH()
{
}
/*
* All NMEA descriptions are taken from
* http://www.trimble.com/OEM_ReceiverHelp/V4.44/en/NMEA-0183messages_MessageOverview.html
*/
int ASHTECH::handle_message(int len)
{
char * endp;
if (len < 7) { return 0; }
int uiCalcComma = 0;
for (int i = 0 ; i < len; i++) {
if (_rx_buffer[i] == ',') { uiCalcComma++; }
}
char *bufptr = (char *)(_rx_buffer + 6);
if ((memcmp(_rx_buffer + 3, "ZDA,", 3) == 0) && (uiCalcComma == 6)) {
/*
UTC day, month, and year, and local time zone offset
An example of the ZDA message string is:
$GPZDA,172809.456,12,07,1996,00,00*45
ZDA message fields
Field Meaning
0 Message ID $GPZDA
1 UTC
2 Day, ranging between 01 and 31
3 Month, ranging between 01 and 12
4 Year
5 Local time zone offset from GMT, ranging from 00 through 13 hours
6 Local time zone offset from GMT, ranging from 00 through 59 minutes
7 The checksum data, always begins with *
Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT.
*/
double ashtech_time = 0.0;
int day = 0, month = 0, year = 0, local_time_off_hour __attribute__((unused)) = 0, local_time_off_min __attribute__((unused)) = 0;
if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { day = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { month = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { year = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { local_time_off_hour = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { local_time_off_min = strtol(bufptr, &endp, 10); bufptr = endp; }
int ashtech_hour = ashtech_time / 10000;
int ashtech_minute = (ashtech_time - ashtech_hour * 10000) / 100;
double ashtech_sec = ashtech_time - ashtech_hour * 10000 - ashtech_minute * 100;
/*
* convert to unix timestamp
*/
struct tm timeinfo;
timeinfo.tm_year = year - 1900;
timeinfo.tm_mon = month - 1;
timeinfo.tm_mday = day;
timeinfo.tm_hour = ashtech_hour;
timeinfo.tm_min = ashtech_minute;
timeinfo.tm_sec = int(ashtech_sec);
time_t epoch = mktime(&timeinfo);
_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
_gps_position->time_gps_usec += (uint64_t)((ashtech_sec - int(ashtech_sec)) * 1e6);
_gps_position->timestamp_time = hrt_absolute_time();
}
else if ((memcmp(_rx_buffer + 3, "GGA,", 3) == 0) && (uiCalcComma == 14)) {
/*
Time, position, and fix related data
An example of the GBS message string is:
$GPGGA,172814.0,3723.46587704,N,12202.26957864,W,2,6,1.2,18.893,M,-25.669,M,2.0,0031*4F
Note - The data string exceeds the ASHTECH standard length.
GGA message fields
Field Meaning
0 Message ID $GPGGA
1 UTC of position fix
2 Latitude
3 Direction of latitude:
N: North
S: South
4 Longitude
5 Direction of longitude:
E: East
W: West
6 GPS Quality indicator:
0: Fix not valid
1: GPS fix
2: Differential GPS fix, OmniSTAR VBS
4: Real-Time Kinematic, fixed integers
5: Real-Time Kinematic, float integers, OmniSTAR XP/HP or Location RTK
7 Number of SVs in use, range from 00 through to 24+
8 HDOP
9 Orthometric height (MSL reference)
10 M: unit of measure for orthometric height is meters
11 Geoid separation
12 M: geoid separation measured in meters
13 Age of differential GPS data record, Type 1 or Type 9. Null field when DGPS is not used.
14 Reference station ID, range 0000-4095. A null field when any reference station ID is selected and no corrections are received1.
15
The checksum data, always begins with *
Note - If a user-defined geoid model, or an inclined
*/
double ashtech_time __attribute__((unused)) = 0.0, lat = 0.0, lon = 0.0, alt = 0.0;
int num_of_sv __attribute__((unused)) = 0, fix_quality = 0;
double hdop __attribute__((unused)) = 99.9;
char ns = '?', ew = '?';
if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { lat = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); }
if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); }
if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { num_of_sv = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { hdop = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { alt = strtod(bufptr, &endp); bufptr = endp; }
if (ns == 'S') {
lat = -lat;
}
if (ew == 'W') {
lon = -lon;
}
_gps_position->lat = (int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000;
_gps_position->lon = (int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000;
_gps_position->alt = alt * 1000;
_rate_count_lat_lon++;
if (fix_quality <= 0) {
_gps_position->fix_type = 0;
} else {
/*
* in this NMEA message float integers (value 5) mode has higher value than fixed integers (value 4), whereas it provides lower quality,
* and since value 3 is not being used, I "moved" value 5 to 3 to add it to _gps_position->fix_type
*/
if (fix_quality == 5) { fix_quality = 3; }
/*
* fix quality 1 means just a normal 3D fix, so I'm subtracting 1 here. This way we'll have 3 for auto, 4 for DGPS, 5 for floats, 6 for fixed.
*/
_gps_position->fix_type = 3 + fix_quality - 1;
}
_gps_position->timestamp_position = hrt_absolute_time();
_gps_position->vel_m_s = 0; /**< GPS ground speed (m/s) */
_gps_position->vel_n_m_s = 0; /**< GPS ground speed in m/s */
_gps_position->vel_e_m_s = 0; /**< GPS ground speed in m/s */
_gps_position->vel_d_m_s = 0; /**< GPS ground speed in m/s */
_gps_position->cog_rad =
0; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */
_gps_position->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */
_gps_position->c_variance_rad = 0.1;
_gps_position->timestamp_velocity = hrt_absolute_time();
return 1;
} else if ((memcmp(_rx_buffer, "$PASHR,POS,", 11) == 0) && (uiCalcComma == 18)) {
/*
Example $PASHR,POS,2,10,125410.00,5525.8138702,N,03833.9587380,E,131.555,1.0,0.0,0.007,-0.001,2.0,1.0,1.7,1.0,*34
$PASHR,POS,d1,d2,m3,m4,c5,m6,c7,f8,f9,f10,f11,f12,f13,f14,f15,f16,s17*cc
Parameter Description Range
d1 Position mode 0: standalone
1: differential
2: RTK float
3: RTK fixed
5: Dead reckoning
9: SBAS (see NPT setting)
d2 Number of satellite used in position fix 0-99
m3 Current UTC time of position fix (hhmmss.ss) 000000.00-235959.99
m4 Latitude of position (ddmm.mmmmmm) 0-90 degrees 00-59.9999999 minutes
c5 Latitude sector N, S
m6 Longitude of position (dddmm.mmmmmm) 0-180 degrees 00-59.9999999 minutes
c7 Longitude sector E,W
f8 Altitude above ellipsoid +9999.000
f9 Differential age (data link age), seconds 0.0-600.0
f10 True track/course over ground in degrees 0.0-359.9
f11 Speed over ground in knots 0.0-999.9
f12 Vertical velocity in decimeters per second +999.9
f13 PDOP 0-99.9
f14 HDOP 0-99.9
f15 VDOP 0-99.9
f16 TDOP 0-99.9
s17 Reserved no data
*cc Checksum
*/
bufptr = (char *)(_rx_buffer + 10);
/*
* Ashtech would return empty space as coordinate (lat, lon or alt) if it doesn't have a fix yet
*/
int coordinatesFound = 0;
double ashtech_time __attribute__((unused)) = 0.0, lat = 0.0, lon = 0.0, alt = 0.0;
int num_of_sv __attribute__((unused)) = 0, fix_quality = 0;
double track_true = 0.0, ground_speed = 0.0 , age_of_corr __attribute__((unused)) = 0.0;
double hdop __attribute__((unused)) = 99.9, vdop __attribute__((unused)) = 99.9, pdop __attribute__((unused)) = 99.9, tdop __attribute__((unused)) = 99.9, vertic_vel = 0.0;
char ns = '?', ew = '?';
if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { num_of_sv = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') {
/*
* if a coordinate is skipped (i.e. no fix), it either won't get into this block (two commas in a row)
* or strtod won't find anything and endp will point exactly where bufptr is. The same is for lon and alt.
*/
lat = strtod(bufptr, &endp);
if (bufptr != endp) {coordinatesFound++;}
bufptr = endp;
}
if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); }
if (bufptr && *(++bufptr) != ',') {
lon = strtod(bufptr, &endp);
if (bufptr != endp) {coordinatesFound++;}
bufptr = endp;
}
if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); }
if (bufptr && *(++bufptr) != ',') {
alt = strtod(bufptr, &endp);
if (bufptr != endp) {coordinatesFound++;}
bufptr = endp;
}
if (bufptr && *(++bufptr) != ',') { age_of_corr = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { track_true = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { ground_speed = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { vertic_vel = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { pdop = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { hdop = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { vdop = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { tdop = strtod(bufptr, &endp); bufptr = endp; }
if (ns == 'S') {
lat = -lat;
}
if (ew == 'W') {
lon = -lon;
}
_gps_position->lat = (int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000;
_gps_position->lon = (int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000;
_gps_position->alt = alt * 1000;
_rate_count_lat_lon++;
if (coordinatesFound < 3) {
_gps_position->fix_type = 0;
} else {
_gps_position->fix_type = 3 + fix_quality;
}
_gps_position->timestamp_position = hrt_absolute_time();
double track_rad = track_true * M_PI / 180.0;
double velocity_ms = ground_speed / 1.9438445; /** knots to m/s */
double velocity_north = velocity_ms * cos(track_rad);
double velocity_east = velocity_ms * sin(track_rad);
_gps_position->vel_m_s = velocity_ms; /**< GPS ground speed (m/s) */
_gps_position->vel_n_m_s = velocity_north; /**< GPS ground speed in m/s */
_gps_position->vel_e_m_s = velocity_east; /**< GPS ground speed in m/s */
_gps_position->vel_d_m_s = -vertic_vel; /**< GPS ground speed in m/s */
_gps_position->cog_rad =
track_rad; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */
_gps_position->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */
_gps_position->c_variance_rad = 0.1;
_gps_position->timestamp_velocity = hrt_absolute_time();
return 1;
} else if ((memcmp(_rx_buffer + 3, "GST,", 3) == 0) && (uiCalcComma == 8)) {
/*
Position error statistics
An example of the GST message string is:
$GPGST,172814.0,0.006,0.023,0.020,273.6,0.023,0.020,0.031*6A
The Talker ID ($--) will vary depending on the satellite system used for the position solution:
$GP - GPS only
$GL - GLONASS only
$GN - Combined
GST message fields
Field Meaning
0 Message ID $GPGST
1 UTC of position fix
2 RMS value of the pseudorange residuals; includes carrier phase residuals during periods of RTK (float) and RTK (fixed) processing
3 Error ellipse semi-major axis 1 sigma error, in meters
4 Error ellipse semi-minor axis 1 sigma error, in meters
5 Error ellipse orientation, degrees from true north
6 Latitude 1 sigma error, in meters
7 Longitude 1 sigma error, in meters
8 Height 1 sigma error, in meters
9 The checksum data, always begins with *
*/
double ashtech_time __attribute__((unused)) = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0;
double min_err __attribute__((unused)) = 0.0, maj_err __attribute__((unused)) = 0.0, deg_from_north __attribute__((unused)) = 0.0, rms_err __attribute__((unused)) = 0.0;
if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { rms_err = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { maj_err = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { min_err = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { deg_from_north = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { lat_err = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { lon_err = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { alt_err = strtod(bufptr, &endp); bufptr = endp; }
_gps_position->eph = sqrt(lat_err * lat_err + lon_err * lon_err);
_gps_position->epv = alt_err;
_gps_position->s_variance_m_s = 0;
_gps_position->timestamp_variance = hrt_absolute_time();
} else if ((memcmp(_rx_buffer + 3, "GSV,", 3) == 0)) {
/*
The GSV message string identifies the number of SVs in view, the PRN numbers, elevations, azimuths, and SNR values. An example of the GSV message string is:
$GPGSV,4,1,13,02,02,213,,03,-3,000,,11,00,121,,14,13,172,05*67
GSV message fields
Field Meaning
0 Message ID $GPGSV
1 Total number of messages of this type in this cycle
2 Message number
3 Total number of SVs visible
4 SV PRN number
5 Elevation, in degrees, 90 maximum
6 Azimuth, degrees from True North, 000 through 359
7 SNR, 00 through 99 dB (null when not tracking)
8-11 Information about second SV, same format as fields 4 through 7
12-15 Information about third SV, same format as fields 4 through 7
16-19 Information about fourth SV, same format as fields 4 through 7
20 The checksum data, always begins with *
*/
/*
* currently process only gps, because do not know what
* Global satellite ID I should use for non GPS sats
*/
bool bGPS = false;
if (memcmp(_rx_buffer, "$GP", 3) != 0) {
return 0;
} else {
bGPS = true;
}
int all_msg_num = 0, this_msg_num = 0, tot_sv_visible = 0;
struct gsv_sat {
int svid;
int elevation;
int azimuth;
int snr;
} sat[4];
memset(sat, 0, sizeof(sat));
if (bufptr && *(++bufptr) != ',') { all_msg_num = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { this_msg_num = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { tot_sv_visible = strtol(bufptr, &endp, 10); bufptr = endp; }
if ((this_msg_num < 1) || (this_msg_num > all_msg_num)) {
return 0;
}
if ((this_msg_num == 0) && (bGPS == true)) {
memset(_satellite_info->svid, 0, sizeof(_satellite_info->svid));
memset(_satellite_info->used, 0, sizeof(_satellite_info->used));
memset(_satellite_info->snr, 0, sizeof(_satellite_info->snr));
memset(_satellite_info->elevation, 0, sizeof(_satellite_info->elevation));
memset(_satellite_info->azimuth, 0, sizeof(_satellite_info->azimuth));
}
int end = 4;
if (this_msg_num == all_msg_num) {
end = tot_sv_visible - (this_msg_num - 1) * 4;
_gps_position->satellites_used = tot_sv_visible;
_satellite_info->count = SAT_INFO_MAX_SATELLITES;
_satellite_info->timestamp = hrt_absolute_time();
}
for (int y = 0 ; y < end ; y++) {
if (bufptr && *(++bufptr) != ',') { sat[y].svid = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { sat[y].elevation = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { sat[y].azimuth = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { sat[y].snr = strtol(bufptr, &endp, 10); bufptr = endp; }
_satellite_info->svid[y + (this_msg_num - 1) * 4] = sat[y].svid;
_satellite_info->used[y + (this_msg_num - 1) * 4] = ((sat[y].snr > 0) ? true : false);
_satellite_info->snr[y + (this_msg_num - 1) * 4] = sat[y].snr;
_satellite_info->elevation[y + (this_msg_num - 1) * 4] = sat[y].elevation;
_satellite_info->azimuth[y + (this_msg_num - 1) * 4] = sat[y].azimuth;
}
}
return 0;
}
int ASHTECH::receive(unsigned timeout)
{
{
/* poll descriptor */
pollfd fds[1];
fds[0].fd = _fd;
fds[0].events = POLLIN;
uint8_t buf[32];
/* timeout additional to poll */
uint64_t time_started = hrt_absolute_time();
int j = 0;
ssize_t bytes_count = 0;
while (true) {
/* pass received bytes to the packet decoder */
while (j < bytes_count) {
int l = 0;
if ((l = parse_char(buf[j])) > 0) {
/* return to configure during configuration or to the gps driver during normal work
* if a packet has arrived */
if (handle_message(l) > 0) {
return 1;
}
}
/* in case we keep trying but only get crap from GPS */
if (time_started + timeout * 1000 * 2 < hrt_absolute_time()) {
return -1;
}
j++;
}
/* everything is read */
j = bytes_count = 0;
/* then poll for new data */
int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout * 2);
if (ret < 0) {
/* something went wrong when polling */
return -1;
} else if (ret == 0) {
/* Timeout */
return -1;
} else if (ret > 0) {
/* if we have new data from GPS, go handle it */
if (fds[0].revents & POLLIN) {
/*
* We are here because poll says there is some data, so this
* won't block even on a blocking device. If more bytes are
* available, we'll go back to poll() again...
*/
bytes_count = ::read(_fd, buf, sizeof(buf));
}
}
}
}
}
#define HEXDIGIT_CHAR(d) ((char)((d) + (((d) < 0xA) ? '0' : 'A'-0xA)))
int ASHTECH::parse_char(uint8_t b)
{
int iRet = 0;
switch (_decode_state) {
/* First, look for sync1 */
case NME_DECODE_UNINIT:
if (b == '$') {
_decode_state = NME_DECODE_GOT_SYNC1;
_rx_buffer_bytes = 0;
_rx_buffer[_rx_buffer_bytes++] = b;
}
break;
case NME_DECODE_GOT_SYNC1:
if (b == '$') {
_decode_state = NME_DECODE_GOT_SYNC1;
_rx_buffer_bytes = 0;
} else if (b == '*') {
_decode_state = NME_DECODE_GOT_ASTERIKS;
}
if (_rx_buffer_bytes >= (sizeof(_rx_buffer) - 5)) {
_decode_state = NME_DECODE_UNINIT;
_rx_buffer_bytes = 0;
} else {
_rx_buffer[_rx_buffer_bytes++] = b;
}
break;
case NME_DECODE_GOT_ASTERIKS:
_rx_buffer[_rx_buffer_bytes++] = b;
_decode_state = NME_DECODE_GOT_FIRST_CS_BYTE;
break;
case NME_DECODE_GOT_FIRST_CS_BYTE:
_rx_buffer[_rx_buffer_bytes++] = b;
uint8_t checksum = 0;
uint8_t *buffer = _rx_buffer + 1;
uint8_t *bufend = _rx_buffer + _rx_buffer_bytes - 3;
for (; buffer < bufend; buffer++) { checksum ^= *buffer; }
if ((HEXDIGIT_CHAR(checksum >> 4) == *(_rx_buffer + _rx_buffer_bytes - 2)) &&
(HEXDIGIT_CHAR(checksum & 0x0F) == *(_rx_buffer + _rx_buffer_bytes - 1))) {
iRet = _rx_buffer_bytes;
}
_decode_state = NME_DECODE_UNINIT;
_rx_buffer_bytes = 0;
break;
}
return iRet;
}
void ASHTECH::decode_init(void)
{
}
/*
* ashtech board configuration script
*/
const char comm[] = "$PASHS,POP,20\r\n"\
"$PASHS,NME,ZDA,B,ON,3\r\n"\
"$PASHS,NME,GGA,B,OFF\r\n"\
"$PASHS,NME,GST,B,ON,3\r\n"\
"$PASHS,NME,POS,B,ON,0.05\r\n"\
"$PASHS,NME,GSV,B,ON,3\r\n"\
"$PASHS,SPD,A,8\r\n"\
"$PASHS,SPD,B,9\r\n";
int ASHTECH::configure(unsigned &baudrate)
{
/* try different baudrates */
const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200};
for (unsigned int baud_i = 0; baud_i < sizeof(baudrates_to_try) / sizeof(baudrates_to_try[0]); baud_i++) {
baudrate = baudrates_to_try[baud_i];
set_baudrate(_fd, baudrate);
write(_fd, (uint8_t *)comm, sizeof(comm));
}
set_baudrate(_fd, 115200);
return 0;
}
src/drivers/gps/ashtech.h
+96
View File
@@ -0,0 +1,96 @@
/****************************************************************************
*
* Copyright (C) 2013. All rights reserved.
* Author: Boriskin Aleksey <a.d.boriskin@gmail.com>
* Kistanov Alexander <akistanov@gramant.ru>
*
* 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 ASHTECH protocol definitions */
#ifndef ASHTECH_H_
#define ASHTECH_H_
#include "gps_helper.h"
#ifndef RECV_BUFFER_SIZE
#define RECV_BUFFER_SIZE 512
#define SAT_INFO_MAX_SATELLITES 20
#endif
class ASHTECH : public GPS_Helper
{
enum ashtech_decode_state_t {
NME_DECODE_UNINIT,
NME_DECODE_GOT_SYNC1,
NME_DECODE_GOT_ASTERIKS,
NME_DECODE_GOT_FIRST_CS_BYTE
};
int _fd;
struct satellite_info_s *_satellite_info;
struct vehicle_gps_position_s *_gps_position;
int ashtechlog_fd;
ashtech_decode_state_t _decode_state;
uint8_t _rx_buffer[RECV_BUFFER_SIZE];
uint16_t _rx_buffer_bytes;
bool _parse_error; /** parse error flag */
char *_parse_pos; /** parse position */
bool _gsv_in_progress; /**< Indicates that gsv data parsing is in progress */
/* int _satellites_count; **< Number of satellites info parsed. */
uint8_t count; /**< Number of satellites in satellite info */
uint8_t svid[SAT_INFO_MAX_SATELLITES]; /**< Space vehicle ID [1..255], see scheme below */
uint8_t used[SAT_INFO_MAX_SATELLITES]; /**< 0: Satellite not used, 1: used for navigation */
uint8_t elevation[SAT_INFO_MAX_SATELLITES]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
uint8_t azimuth[SAT_INFO_MAX_SATELLITES]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
uint8_t snr[SAT_INFO_MAX_SATELLITES]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */
public:
ASHTECH(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info);
~ASHTECH();
int receive(unsigned timeout);
int configure(unsigned &baudrate);
void decode_init(void);
int handle_message(int len);
int parse_char(uint8_t b);
/** Read int ASHTECH parameter */
int32_t read_int();
/** Read float ASHTECH parameter */
double read_float();
/** Read char ASHTECH parameter */
char read_char();
};
#endif /* ASHTECH_H_ */
src/drivers/gps/gps.cpp
+17
View File
@@ -69,6 +69,7 @@
#include "ubx.h"
#include "mtk.h"
#include "ashtech.h"
#define TIMEOUT_5HZ 500
@@ -341,6 +342,10 @@ GPS::task_main()
_Helper = new MTK(_serial_fd, &_report_gps_pos);
break;
case GPS_DRIVER_MODE_ASHTECH:
_Helper = new ASHTECH(_serial_fd, &_report_gps_pos, _p_report_sat_info);
break;
default:
break;
}
@@ -402,6 +407,10 @@ GPS::task_main()
mode_str = "MTK";
break;
case GPS_DRIVER_MODE_ASHTECH:
mode_str = "ASHTECH";
break;
default:
break;
}
@@ -429,6 +438,10 @@ GPS::task_main()
break;
case GPS_DRIVER_MODE_MTK:
_mode = GPS_DRIVER_MODE_ASHTECH;
break;
case GPS_DRIVER_MODE_ASHTECH:
_mode = GPS_DRIVER_MODE_UBX;
break;
@@ -475,6 +488,10 @@ GPS::print_info()
warnx("protocol: MTK");
break;
case GPS_DRIVER_MODE_ASHTECH:
warnx("protocol: ASHTECH");
break;
default:
break;
}
src/drivers/gps/module.mk
+1
View File
@@ -40,6 +40,7 @@ MODULE_COMMAND = gps
SRCS = gps.cpp \
gps_helper.cpp \
mtk.cpp \
ashtech.cpp \
ubx.cpp
MODULE_STACKSIZE = 1200
src/drivers/gps/ubx.cpp
+12
View File
@@ -189,6 +189,18 @@ UBX::configure(unsigned &baudrate)
return 1;
}
#ifdef UBX_CONFIGURE_SBAS
/* send a SBAS message to set the SBAS options */
memset(&_buf.payload_tx_cfg_sbas, 0, sizeof(_buf.payload_tx_cfg_sbas));
_buf.payload_tx_cfg_sbas.mode = UBX_TX_CFG_SBAS_MODE;
send_message(UBX_MSG_CFG_SBAS, _buf.raw, sizeof(_buf.payload_tx_cfg_sbas));
if (wait_for_ack(UBX_MSG_CFG_SBAS, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
#endif
/* configure message rates */
/* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */
src/drivers/gps/ubx.h
+17
View File
@@ -73,6 +73,7 @@
#define UBX_ID_CFG_MSG 0x01
#define UBX_ID_CFG_RATE 0x08
#define UBX_ID_CFG_NAV5 0x24
#define UBX_ID_CFG_SBAS 0x16
#define UBX_ID_MON_VER 0x04
#define UBX_ID_MON_HW 0x09
@@ -89,6 +90,7 @@
#define UBX_MSG_CFG_MSG ((UBX_CLASS_CFG) | UBX_ID_CFG_MSG << 8)
#define UBX_MSG_CFG_RATE ((UBX_CLASS_CFG) | UBX_ID_CFG_RATE << 8)
#define UBX_MSG_CFG_NAV5 ((UBX_CLASS_CFG) | UBX_ID_CFG_NAV5 << 8)
#define UBX_MSG_CFG_SBAS ((UBX_CLASS_CFG) | UBX_ID_CFG_SBAS << 8)
#define UBX_MSG_MON_HW ((UBX_CLASS_MON) | UBX_ID_MON_HW << 8)
#define UBX_MSG_MON_VER ((UBX_CLASS_MON) | UBX_ID_MON_VER << 8)
@@ -128,6 +130,11 @@
#define UBX_TX_CFG_NAV5_DYNMODEL 7 /**< 0 Portable, 2 Stationary, 3 Pedestrian, 4 Automotive, 5 Sea, 6 Airborne <1g, 7 Airborne <2g, 8 Airborne <4g */
#define UBX_TX_CFG_NAV5_FIXMODE 2 /**< 1 2D only, 2 3D only, 3 Auto 2D/3D */
/* TX CFG-SBAS message contents */
#define UBX_TX_CFG_SBAS_MODE_ENABLED 1 /**< SBAS enabled */
#define UBX_TX_CFG_SBAS_MODE_DISABLED 0 /**< SBAS disabled */
#define UBX_TX_CFG_SBAS_MODE UBX_TX_CFG_SBAS_MODE_DISABLED /**< SBAS enabled or disabled */
/* TX CFG-MSG message contents */
#define UBX_TX_CFG_MSG_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
#define UBX_TX_CFG_MSG_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
@@ -383,6 +390,15 @@ typedef struct {
uint32_t reserved4;
} ubx_payload_tx_cfg_nav5_t;
/* tx cfg-sbas */
typedef struct {
uint8_t mode;
uint8_t usage;
uint8_t maxSBAS;
uint8_t scanmode2;
uint32_t scanmode1;
} ubx_payload_tx_cfg_sbas_t;
/* Tx CFG-MSG */
typedef struct {
union {
@@ -413,6 +429,7 @@ typedef union {
ubx_payload_tx_cfg_prt_t payload_tx_cfg_prt;
ubx_payload_tx_cfg_rate_t payload_tx_cfg_rate;
ubx_payload_tx_cfg_nav5_t payload_tx_cfg_nav5;
ubx_payload_tx_cfg_sbas_t payload_tx_cfg_sbas;
ubx_payload_tx_cfg_msg_t payload_tx_cfg_msg;
uint8_t raw[];
} ubx_buf_t;
src/drivers/hil/hil.cpp
+2 -1
View File
@@ -392,7 +392,8 @@ HIL::task_main()
if (fds[0].revents & POLLIN) {
/* get controls - must always do this to avoid spinning */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1), _t_actuators, &_controls);
/* can we mix? */
if (_mixers != nullptr) {
src/drivers/hmc5883/hmc5883.cpp
+16 -36
View File
@@ -1049,11 +1049,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
* LSM/Ga, giving 1.16 and 1.08 */
float expected_cal[3] = { 1.16f, 1.08f, 1.08f };
warnx("starting mag scale calibration");
/* start the sensor polling at 50 Hz */
if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
warn("failed to set 2Hz poll rate");
warn("FAILED: SENSORIOCSPOLLRATE 2Hz");
ret = 1;
goto out;
}
@@ -1061,25 +1059,25 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
/* Set to 2.5 Gauss. We ask for 3 to get the right part of
* the chained if statement above. */
if (OK != ioctl(filp, MAGIOCSRANGE, 3)) {
warnx("failed to set 2.5 Ga range");
warnx("FAILED: MAGIOCSRANGE 3.3 Ga");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) {
warnx("failed to enable sensor calibration mode");
warnx("FAILED: MAGIOCEXSTRAP 1");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) {
warn("WARNING: failed to get scale / offsets for mag");
warn("FAILED: MAGIOCGSCALE 1");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
warn("WARNING: failed to set null scale / offsets for mag");
warn("FAILED: MAGIOCSSCALE 1");
ret = 1;
goto out;
}
@@ -1094,7 +1092,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
ret = ::poll(&fds, 1, 2000);
if (ret != 1) {
warn("timed out waiting for sensor data");
warn("ERROR: TIMEOUT 1");
goto out;
}
@@ -1102,7 +1100,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
sz = ::read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
warn("periodic read failed");
warn("ERROR: READ 1");
ret = -EIO;
goto out;
}
@@ -1118,7 +1116,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
ret = ::poll(&fds, 1, 2000);
if (ret != 1) {
warn("timed out waiting for sensor data");
warn("ERROR: TIMEOUT 2");
goto out;
}
@@ -1126,7 +1124,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
sz = ::read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
warn("periodic read failed");
warn("ERROR: READ 2");
ret = -EIO;
goto out;
}
@@ -1142,33 +1140,19 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
sum_excited[1] += cal[1];
sum_excited[2] += cal[2];
}
//warnx("periodic read %u", i);
//warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
//warnx("cal: %.6f %.6f %.6f", (double)cal[0], (double)cal[1], (double)cal[2]);
}
if (good_count < 5) {
warn("failed calibration");
ret = -EIO;
goto out;
}
#if 0
warnx("measurement avg: %.6f %.6f %.6f",
(double)sum_excited[0]/good_count,
(double)sum_excited[1]/good_count,
(double)sum_excited[2]/good_count);
#endif
float scaling[3];
scaling[0] = sum_excited[0] / good_count;
scaling[1] = sum_excited[1] / good_count;
scaling[2] = sum_excited[2] / good_count;
warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
/* set scaling in device */
mscale_previous.x_scale = scaling[0];
mscale_previous.y_scale = scaling[1];
@@ -1179,29 +1163,26 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
out:
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
warn("failed to set new scale / offsets for mag");
warn("FAILED: MAGIOCSSCALE 2");
}
/* set back to normal mode */
/* Set to 1.1 Gauss */
if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
warnx("failed to set 1.1 Ga range");
warnx("FAILED: MAGIOCSRANGE 1.1 Ga");
}
if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
warnx("failed to disable sensor calibration mode");
warnx("FAILED: MAGIOCEXSTRAP 0");
}
if (ret == OK) {
if (!check_scale()) {
warnx("mag scale calibration successfully finished.");
} else {
warnx("mag scale calibration finished with invalid results.");
if (check_scale()) {
/* failed */
warnx("FAILED: SCALE");
ret = ERROR;
}
} else {
warnx("mag scale calibration failed.");
}
return ret;
@@ -1283,14 +1264,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);
src/drivers/hott/messages.cpp
+7 -7
View File
@@ -115,9 +115,9 @@ publish_gam_message(const uint8_t *buffer)
esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT;
esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10;
esc.esc[0].esc_temperature = msg.temperature1 - 20;
esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff));
esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff));
esc.esc[0].esc_temperature = static_cast<float>(msg.temperature1) - 20.0F;
esc.esc[0].esc_voltage = static_cast<float>((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)) * 0.1F;
esc.esc[0].esc_current = static_cast<float>((msg.current_H << 8) | (msg.current_L & 0xff)) * 0.1F;
/* announce the esc if needed, just publish else */
if (_esc_pub > 0) {
@@ -186,18 +186,18 @@ build_gam_response(uint8_t *buffer, size_t *size)
msg.gam_sensor_id = GAM_SENSOR_ID;
msg.sensor_text_id = GAM_SENSOR_TEXT_ID;
msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20);
msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20.0F);
msg.temperature2 = 20; // 0 deg. C.
uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage);
const uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage * 10.0F);
msg.main_voltage_L = (uint8_t)voltage & 0xff;
msg.main_voltage_H = (uint8_t)(voltage >> 8) & 0xff;
uint16_t current = (uint16_t)(esc.esc[0].esc_current);
const uint16_t current = (uint16_t)(esc.esc[0].esc_current * 10.0F);
msg.current_L = (uint8_t)current & 0xff;
msg.current_H = (uint8_t)(current >> 8) & 0xff;
uint16_t rpm = (uint16_t)(esc.esc[0].esc_rpm * 0.1f);
const uint16_t rpm = (uint16_t)(esc.esc[0].esc_rpm * 0.1f);
msg.rpm_L = (uint8_t)rpm & 0xff;
msg.rpm_H = (uint8_t)(rpm >> 8) & 0xff;
src/drivers/l3gd20/l3gd20.cpp
+8 -1
View File
@@ -176,6 +176,7 @@ static const int ERROR = -1;
#define L3G4200D_DEFAULT_RATE 800
#define L3GD20_DEFAULT_RANGE_DPS 2000
#define L3GD20_DEFAULT_FILTER_FREQ 30
#define L3GD20_TEMP_OFFSET_CELSIUS 40
#ifndef SENSOR_BOARD_ROTATION_DEFAULT
#define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG
@@ -856,7 +857,7 @@ L3GD20::measure()
#pragma pack(push, 1)
struct {
uint8_t cmd;
uint8_t temp;
int8_t temp;
uint8_t status;
int16_t x;
int16_t y;
@@ -930,6 +931,8 @@ L3GD20::measure()
report.z_raw = raw_report.z;
report.temperature_raw = raw_report.temp;
report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
@@ -938,6 +941,8 @@ L3GD20::measure()
report.y = _gyro_filter_y.apply(report.y);
report.z = _gyro_filter_z.apply(report.z);
report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp;
// apply user specified rotation
rotate_3f(_rotation, report.x, report.y, report.z);
@@ -1091,9 +1096,11 @@ test()
warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x);
warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y);
warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z);
warnx("temp: \t%d\tC", (int)g_report.temperature);
warnx("gyro x: \t%d\traw", (int)g_report.x_raw);
warnx("gyro y: \t%d\traw", (int)g_report.y_raw);
warnx("gyro z: \t%d\traw", (int)g_report.z_raw);
warnx("temp: \t%d\traw", (int)g_report.temperature_raw);
warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
(int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
src/drivers/ll40ls/ll40ls.cpp
+187 -59
View File
@@ -73,14 +73,19 @@
/* Configuration Constants */
#define LL40LS_BUS PX4_I2C_BUS_EXPANSION
#define LL40LS_BASEADDR 0x42 /* 7-bit address */
#define LL40LS_DEVICE_PATH "/dev/ll40ls"
#define LL40LS_BASEADDR 0x62 /* 7-bit address */
#define LL40LS_BASEADDR_OLD 0x42 /* previous 7-bit address */
#define LL40LS_DEVICE_PATH_INT "/dev/ll40ls_int"
#define LL40LS_DEVICE_PATH_EXT "/dev/ll40ls_ext"
/* LL40LS Registers addresses */
#define LL40LS_MEASURE_REG 0x00 /* Measure range register */
#define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */
#define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */
#define LL40LS_DISTHIGH_REG 0x8F /* High byte of distance register, auto increment */
#define LL40LS_WHO_AM_I_REG 0x11
#define LL40LS_WHO_AM_I_REG_VAL 0xCA
#define LL40LS_SIGNAL_STRENGTH_REG 0x5b
/* Device limits */
#define LL40LS_MIN_DISTANCE (0.00f)
@@ -101,7 +106,7 @@ static const int ERROR = -1;
class LL40LS : public device::I2C
{
public:
LL40LS(int bus = LL40LS_BUS, int address = LL40LS_BASEADDR);
LL40LS(int bus, const char *path, int address = LL40LS_BASEADDR);
virtual ~LL40LS();
virtual int init();
@@ -116,6 +121,7 @@ public:
protected:
virtual int probe();
virtual int read_reg(uint8_t reg, uint8_t &val);
private:
float _min_distance;
@@ -132,6 +138,10 @@ private:
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
uint16_t _last_distance;
/**< the bus the device is connected to */
int _bus;
/**
* Test whether the device supported by the driver is present at a
@@ -188,8 +198,8 @@ private:
*/
extern "C" __EXPORT int ll40ls_main(int argc, char *argv[]);
LL40LS::LL40LS(int bus, int address) :
I2C("LL40LS", LL40LS_DEVICE_PATH, bus, address, 100000),
LL40LS::LL40LS(int bus, const char *path, int address) :
I2C("LL40LS", path, bus, address, 100000),
_min_distance(LL40LS_MIN_DISTANCE),
_max_distance(LL40LS_MAX_DISTANCE),
_reports(nullptr),
@@ -200,10 +210,12 @@ LL40LS::LL40LS(int bus, int address) :
_range_finder_topic(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")),
_comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows"))
_buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")),
_last_distance(0),
_bus(bus)
{
// up the retries since the device misses the first measure attempts
I2C::_retries = 3;
_retries = 3;
// enable debug() calls
_debug_enabled = false;
@@ -270,9 +282,51 @@ out:
return ret;
}
int
LL40LS::read_reg(uint8_t reg, uint8_t &val)
{
return transfer(&reg, 1, &val, 1);
}
int
LL40LS::probe()
{
// cope with both old and new I2C bus address
const uint8_t addresses[2] = {LL40LS_BASEADDR, LL40LS_BASEADDR_OLD};
// more retries for detection
_retries = 10;
for (uint8_t i=0; i<sizeof(addresses); i++) {
uint8_t val=0, who_am_i=0;
// set the I2C bus address
set_address(addresses[i]);
if (read_reg(LL40LS_WHO_AM_I_REG, who_am_i) == OK && who_am_i == LL40LS_WHO_AM_I_REG_VAL) {
// it is responding correctly to a WHO_AM_I
goto ok;
}
if (read_reg(LL40LS_SIGNAL_STRENGTH_REG, val) == OK && val != 0) {
// very likely to be a ll40ls. px4flow does not
// respond to this
goto ok;
}
debug("WHO_AM_I byte mismatch 0x%02x should be 0x%02x val=0x%02x\n",
(unsigned)who_am_i,
LL40LS_WHO_AM_I_REG_VAL,
(unsigned)val);
}
// not found on any address
return -EIO;
ok:
_retries = 3;
// start a measurement
return measure();
}
@@ -521,6 +575,8 @@ LL40LS::collect()
float si_units = distance * 0.01f; /* cm to m */
struct range_finder_report report;
_last_distance = distance;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
@@ -648,6 +704,8 @@ LL40LS::print_info()
perf_print_counter(_buffer_overflows);
printf("poll interval: %u ticks\n", _measure_ticks);
_reports->print_info("report queue");
printf("distance: %ucm (0x%04x)\n",
(unsigned)_last_distance, (unsigned)_last_distance);
}
/**
@@ -662,55 +720,89 @@ namespace ll40ls
#endif
const int ERROR = -1;
LL40LS *g_dev;
LL40LS *g_dev_int;
LL40LS *g_dev_ext;
void start();
void stop();
void test();
void reset();
void info();
void start(int bus);
void stop(int bus);
void test(int bus);
void reset(int bus);
void info(int bus);
void usage();
/**
* Start the driver.
*/
void
start()
start(int bus)
{
int fd;
if (g_dev != nullptr) {
errx(1, "already started");
/* create the driver, attempt expansion bus first */
if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) {
if (g_dev_ext != nullptr)
errx(0, "already started external");
g_dev_ext = new LL40LS(PX4_I2C_BUS_EXPANSION, LL40LS_DEVICE_PATH_EXT);
if (g_dev_ext != nullptr && OK != g_dev_ext->init()) {
delete g_dev_ext;
g_dev_ext = nullptr;
}
}
/* create the driver */
g_dev = new LL40LS(LL40LS_BUS);
#ifdef PX4_I2C_BUS_ONBOARD
/* if this failed, attempt onboard sensor */
if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) {
if (g_dev_int != nullptr)
errx(0, "already started internal");
g_dev_int = new LL40LS(PX4_I2C_BUS_ONBOARD, LL40LS_DEVICE_PATH_INT);
if (g_dev_int != nullptr && OK != g_dev_int->init()) {
/* tear down the failing onboard instance */
delete g_dev_int;
g_dev_int = nullptr;
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
goto fail;
if (bus == PX4_I2C_BUS_ONBOARD) {
goto fail;
}
}
if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) {
goto fail;
}
}
#endif
/* set the poll rate to default, starts automatic data collection */
fd = open(LL40LS_DEVICE_PATH, O_RDONLY);
if (g_dev_int != nullptr) {
int fd = open(LL40LS_DEVICE_PATH_INT, O_RDONLY);
if (fd == -1) {
goto fail;
}
int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
close(fd);
if (ret < 0) {
goto fail;
}
}
if (fd < 0) {
goto fail;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail;
}
if (g_dev_ext != nullptr) {
int fd = open(LL40LS_DEVICE_PATH_EXT, O_RDONLY);
if (fd == -1) {
goto fail;
}
int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
close(fd);
if (ret < 0) {
goto fail;
}
}
exit(0);
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) {
delete g_dev_int;
g_dev_int = nullptr;
}
if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) {
delete g_dev_ext;
g_dev_ext = nullptr;
}
errx(1, "driver start failed");
@@ -719,11 +811,12 @@ fail:
/**
* Stop the driver
*/
void stop()
void stop(int bus)
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
LL40LS **g_dev = (bus == PX4_I2C_BUS_ONBOARD?&g_dev_int:&g_dev_ext);
if (*g_dev != nullptr) {
delete *g_dev;
*g_dev = nullptr;
} else {
errx(1, "driver not running");
@@ -738,16 +831,17 @@ void stop()
* and automatic modes.
*/
void
test()
test(int bus)
{
struct range_finder_report report;
ssize_t sz;
int ret;
const char *path = (bus==PX4_I2C_BUS_ONBOARD?LL40LS_DEVICE_PATH_INT:LL40LS_DEVICE_PATH_EXT);
int fd = open(LL40LS_DEVICE_PATH, O_RDONLY);
int fd = open(path, O_RDONLY);
if (fd < 0) {
err(1, "%s open failed (try 'll40ls start' if the driver is not running", LL40LS_DEVICE_PATH);
err(1, "%s open failed (try 'll40ls start' if the driver is not running", path);
}
/* do a simple demand read */
@@ -803,9 +897,10 @@ test()
* Reset the driver.
*/
void
reset()
reset(int bus)
{
int fd = open(LL40LS_DEVICE_PATH, O_RDONLY);
const char *path = (bus==PX4_I2C_BUS_ONBOARD?LL40LS_DEVICE_PATH_INT:LL40LS_DEVICE_PATH_EXT);
int fd = open(path, O_RDONLY);
if (fd < 0) {
err(1, "failed ");
@@ -826,8 +921,9 @@ reset()
* Print a little info about the driver.
*/
void
info()
info(int bus)
{
LL40LS *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext);
if (g_dev == nullptr) {
errx(1, "driver not running");
}
@@ -838,44 +934,76 @@ info()
exit(0);
}
void
usage()
{
warnx("missing command: try 'start', 'stop', 'info', 'test', 'reset', 'info'");
warnx("options:");
warnx(" -X only external bus");
#ifdef PX4_I2C_BUS_ONBOARD
warnx(" -I only internal bus");
#endif
}
} // namespace
int
ll40ls_main(int argc, char *argv[])
{
int ch;
int bus = -1;
while ((ch = getopt(argc, argv, "XI")) != EOF) {
switch (ch) {
#ifdef PX4_I2C_BUS_ONBOARD
case 'I':
bus = PX4_I2C_BUS_ONBOARD;
break;
#endif
case 'X':
bus = PX4_I2C_BUS_EXPANSION;
break;
default:
ll40ls::usage();
exit(0);
}
}
const char *verb = argv[optind];
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start")) {
ll40ls::start();
if (!strcmp(verb, "start")) {
ll40ls::start(bus);
}
/*
* Stop the driver
*/
if (!strcmp(argv[1], "stop")) {
ll40ls::stop();
if (!strcmp(verb, "stop")) {
ll40ls::stop(bus);
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test")) {
ll40ls::test();
if (!strcmp(verb, "test")) {
ll40ls::test(bus);
}
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset")) {
ll40ls::reset();
if (!strcmp(verb, "reset")) {
ll40ls::reset(bus);
}
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
ll40ls::info();
if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
ll40ls::info(bus);
}
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
src/drivers/lsm303d/lsm303d.cpp
+4 -2
View File
@@ -1519,8 +1519,10 @@ LSM303D::measure()
{
// if the accel doesn't have any data ready then re-schedule
// for 100 microseconds later. This ensures we don't double
// read a value and then miss the next value
if (stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) {
// read a value and then miss the next value.
// Note that DRDY is not available when the lsm303d is
// connected on the external bus
if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) {
perf_count(_accel_reschedules);
hrt_call_delay(&_accel_call, 100);
return;
src/drivers/meas_airspeed/meas_airspeed.cpp
+9 -30
View File
@@ -103,7 +103,7 @@
/* Measurement rate is 100Hz */
#define MEAS_RATE 100
#define MEAS_DRIVER_FILTER_FREQ 1.5f
#define MEAS_DRIVER_FILTER_FREQ 1.2f
#define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */
class MEASAirspeed : public Airspeed
@@ -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);
}
src/drivers/mkblctrl/mkblctrl.cpp
+3 -3
View File
@@ -600,8 +600,8 @@ MK::task_main()
esc.esc[i].esc_address = (uint8_t) BLCTRL_BASE_ADDR + i;
esc.esc[i].esc_vendor = ESC_VENDOR_MIKROKOPTER;
esc.esc[i].esc_version = (uint16_t) Motor[i].Version;
esc.esc[i].esc_voltage = (uint16_t) 0;
esc.esc[i].esc_current = (uint16_t) Motor[i].Current;
esc.esc[i].esc_voltage = 0.0F;
esc.esc[i].esc_current = static_cast<float>(Motor[i].Current) * 0.1F;
esc.esc[i].esc_rpm = (uint16_t) 0;
esc.esc[i].esc_setpoint = (float) Motor[i].SetPoint_PX4;
@@ -614,7 +614,7 @@ MK::task_main()
esc.esc[i].esc_setpoint_raw = (uint16_t) Motor[i].SetPoint;
}
esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature;
esc.esc[i].esc_temperature = static_cast<float>(Motor[i].Temperature);
esc.esc[i].esc_state = (uint16_t) Motor[i].State;
esc.esc[i].esc_errorcount = (uint16_t) 0;
src/drivers/mpu6000/mpu6000.cpp
+18 -10
View File
@@ -229,6 +229,7 @@ private:
perf_counter_t _gyro_reads;
perf_counter_t _sample_perf;
perf_counter_t _bad_transfers;
perf_counter_t _good_transfers;
math::LowPassFilter2p _accel_filter_x;
math::LowPassFilter2p _accel_filter_y;
@@ -404,6 +405,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
_gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")),
_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")),
_bad_transfers(perf_alloc(PC_COUNT, "mpu6000_bad_transfers")),
_good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")),
_accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
@@ -456,6 +458,7 @@ MPU6000::~MPU6000()
perf_free(_accel_reads);
perf_free(_gyro_reads);
perf_free(_bad_transfers);
perf_free(_good_transfers);
}
int
@@ -910,12 +913,14 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
// adjust filters
float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq();
float sample_rate = 1.0e6f/ticks;
_set_dlpf_filter(cutoff_freq_hz);
_accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
_accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
_accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq();
_set_dlpf_filter(cutoff_freq_hz_gyro);
_gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
_gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
_gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
@@ -968,11 +973,9 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
return _accel_filter_x.get_cutoff_freq();
case ACCELIOCSLOWPASS:
if (arg == 0) {
// allow disabling of on-chip filter using
// zero as desired filter frequency
_set_dlpf_filter(0);
}
// set hardware filtering
_set_dlpf_filter(arg);
// set software filtering
_accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
_accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
_accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
@@ -1053,14 +1056,11 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
case GYROIOCGLOWPASS:
return _gyro_filter_x.get_cutoff_freq();
case GYROIOCSLOWPASS:
// set hardware filtering
_set_dlpf_filter(arg);
_gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
_gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
_gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
if (arg == 0) {
// allow disabling of on-chip filter using 0
// as desired frequency
_set_dlpf_filter(0);
}
return OK;
case GYROIOCSSCALE:
@@ -1282,8 +1282,14 @@ MPU6000::measure()
// all zero data - probably a SPI bus error
perf_count(_bad_transfers);
perf_end(_sample_perf);
// note that we don't call reset() here as a reset()
// costs 20ms with interrupts disabled. That means if
// the mpu6k does go bad it would cause a FMU failure,
// regardless of whether another sensor is available,
return;
}
perf_count(_good_transfers);
/*
@@ -1402,6 +1408,8 @@ MPU6000::print_info()
perf_print_counter(_sample_perf);
perf_print_counter(_accel_reads);
perf_print_counter(_gyro_reads);
perf_print_counter(_bad_transfers);
perf_print_counter(_good_transfers);
_accel_reports->print_info("accel queue");
_gyro_reports->print_info("gyro queue");
}
src/drivers/ms5611/ms5611.cpp
+2 -2
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;
}
src/drivers/pca9685/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt
+29
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.
src/drivers/pca9685/module.mk
+42
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
src/drivers/pca9685/pca9685.cpp
+651
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);
}
src/drivers/px4flow/px4flow.cpp
+144 -101
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>
@@ -76,11 +76,11 @@
#define PX4FLOW_BUS PX4_I2C_BUS_EXPANSION
#define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84
//range 0x42 - 0x49
/* 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));
}
@@ -212,8 +212,9 @@ PX4FLOW::~PX4FLOW()
stop();
/* free any existing reports */
if (_reports != nullptr)
if (_reports != nullptr) {
delete _reports;
}
}
int
@@ -222,22 +223,25 @@ PX4FLOW::init()
int ret = ERROR;
/* do I2C init (and probe) first */
if (I2C::init() != OK)
if (I2C::init() != OK) {
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)
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);
if (_px4flow_topic < 0)
if (_px4flow_topic < 0) {
debug("failed to create px4flow object. Did you start uOrb?");
}
ret = OK;
/* sensor is ok, but we don't really know if it is within range */
@@ -249,6 +253,17 @@ out:
int
PX4FLOW::probe()
{
uint8_t val[22];
// to be sure this is not a ll40ls Lidar (which can also be on
// 0x42) we check if a 22 byte transfer works from address
// 0. The ll40ls gives an error for that, whereas the flow
// happily returns some data
if (transfer(nullptr, 0, &val[0], 22) != OK) {
return -EIO;
}
// that worked, so start a measurement cycle
return measure();
}
@@ -260,20 +275,20 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_measure_ticks = 0;
return OK;
/* external signalling (DRDY) not supported */
/* external signalling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
@@ -283,13 +298,14 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
_measure_ticks = USEC2TICK(PX4FLOW_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (want_start)
if (want_start) {
start();
}
return OK;
}
/* adjust to a legal polling interval in Hz */
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
@@ -298,15 +314,17 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL))
if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) {
return -EINVAL;
}
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (want_start)
if (want_start) {
start();
}
return OK;
}
@@ -314,33 +332,37 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
}
case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0)
if (_measure_ticks == 0) {
return SENSOR_POLLRATE_MANUAL;
}
return (1000 / _measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* 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)) {
/* 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 -ENOMEM;
return OK;
}
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,13 +372,14 @@ 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 */
if (count < 1)
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is enabled */
if (_measure_ticks > 0) {
@@ -417,15 +440,15 @@ PX4FLOW::measure()
uint8_t cmd = PX4FLOW_REG;
ret = transfer(&cmd, 1, nullptr, 0);
if (OK != ret)
{
if (OK != ret) {
perf_count(_comms_errors);
log("i2c::transfer returned %d", ret);
printf("i2c::transfer flow returned %d");
printf("i2c::transfer flow returned %d");
return ret;
}
ret = OK;
return ret;
}
@@ -433,22 +456,21 @@ 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};
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)
{
if (ret < 0) {
log("error reading from sensor: %d", ret);
perf_count(_comms_errors);
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,13 +488,13 @@ PX4FLOW::collect()
int16_t flowcy = val[9] << 8 | val[8];
int16_t gdist = val[21] << 8 | val[20];
struct px4flow_report 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];
report.flow_raw_y= val[5] << 8 | val[4];
report.ground_distance_m =float(gdist)/1000.0f;
report.quality= val[10];
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];
report.flow_raw_y = val[5] << 8 | val[4];
report.ground_distance_m = float(gdist) / 1000.0f;
report.quality = val[10];
report.sensor_id = 0;
report.timestamp = hrt_absolute_time();
@@ -503,17 +525,19 @@ 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,
true,
true,
SUBSYSTEM_TYPE_OPTICALFLOW};
SUBSYSTEM_TYPE_OPTICALFLOW
};
static orb_advert_t pub = -1;
if (pub > 0) {
orb_publish(ORB_ID(subsystem_info), pub, &info);
} else {
pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
@@ -567,8 +591,9 @@ PX4FLOW::cycle()
}
/* measurement phase */
if (OK != measure())
if (OK != measure()) {
log("measure error");
}
/* next phase is collection */
_collect_phase = true;
@@ -619,33 +644,37 @@ start()
{
int fd;
if (g_dev != nullptr)
if (g_dev != nullptr) {
errx(1, "already started");
}
/* create the driver */
g_dev = new PX4FLOW(PX4FLOW_BUS);
if (g_dev == nullptr)
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init())
if (OK != g_dev->init()) {
goto fail;
}
/* set the poll rate to default, starts automatic data collection */
fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY);
if (fd < 0)
if (fd < 0) {
goto fail;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0)
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) {
goto fail;
}
exit(0);
fail:
if (g_dev != nullptr)
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
@@ -658,15 +687,14 @@ fail:
*/
void stop()
{
if (g_dev != nullptr)
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
else
{
} else {
errx(1, "driver not running");
}
exit(0);
}
@@ -678,20 +706,23 @@ void stop()
void
test()
{
struct px4flow_report report;
struct optical_flow_s report;
ssize_t sz;
int ret;
int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY);
if (fd < 0)
if (fd < 0) {
err(1, "%s open failed (try 'px4flow start' if the driver is not running", PX4FLOW_DEVICE_PATH);
}
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report))
// err(1, "immediate read failed");
{
warnx("immediate read failed");
}
warnx("single read");
warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m);
@@ -700,8 +731,9 @@ test()
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
errx(1, "failed to set 2Hz poll rate");
}
/* read the sensor 5x and report each value */
for (unsigned i = 0; i < 5; i++) {
@@ -712,14 +744,16 @@ test()
fds.events = POLLIN;
ret = poll(&fds, 1, 2000);
if (ret != 1)
if (ret != 1) {
errx(1, "timed out waiting for sensor data");
}
/* now go get it */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report))
if (sz != sizeof(report)) {
err(1, "periodic read failed");
}
warnx("periodic read %u", i);
warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m);
@@ -740,14 +774,17 @@ reset()
{
int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY);
if (fd < 0)
if (fd < 0) {
err(1, "failed ");
}
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
err(1, "driver reset failed");
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "driver poll restart failed");
}
exit(0);
}
@@ -758,8 +795,9 @@ reset()
void
info()
{
if (g_dev == nullptr)
if (g_dev == nullptr) {
errx(1, "driver not running");
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
@@ -775,32 +813,37 @@ px4flow_main(int argc, char *argv[])
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start"))
if (!strcmp(argv[1], "start")) {
px4flow::start();
/*
* Stop the driver
*/
if (!strcmp(argv[1], "stop"))
px4flow::stop();
}
/*
* Stop the driver
*/
if (!strcmp(argv[1], "stop")) {
px4flow::stop();
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test"))
if (!strcmp(argv[1], "test")) {
px4flow::test();
}
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset"))
if (!strcmp(argv[1], "reset")) {
px4flow::reset();
}
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
px4flow::info();
}
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
}
src/drivers/px4fmu/fmu.cpp
+4 -1
View File
@@ -829,6 +829,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_SET_ARM_OK:
case PWM_SERVO_CLEAR_ARM_OK:
case PWM_SERVO_SET_FORCE_SAFETY_OFF:
case PWM_SERVO_SET_FORCE_SAFETY_ON:
// these are no-ops, as no safety switch
break;
@@ -1272,7 +1273,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;
src/drivers/px4io/px4io.cpp
+91 -18
View File
@@ -135,6 +135,15 @@ public:
*/
virtual int init();
/**
* Initialize the PX4IO class.
*
* Retrieve relevant initial system parameters. Initialize PX4IO registers.
*
* @param disable_rc_handling set to true to forbid override / RC handling on IO
*/
int init(bool disable_rc_handling);
/**
* Detect if a PX4IO is connected.
*
@@ -286,6 +295,7 @@ private:
float _battery_amp_bias; ///< current sensor bias
float _battery_mamphour_total;///< amp hours consumed so far
uint64_t _battery_last_timestamp;///< last amp hour calculation timestamp
bool _cb_flighttermination; ///< true if the flight termination circuit breaker is enabled
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
bool _dsm_vcc_ctl; ///< true if relay 1 controls DSM satellite RX power
@@ -506,7 +516,8 @@ PX4IO::PX4IO(device::Device *interface) :
_battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor
_battery_amp_bias(0),
_battery_mamphour_total(0),
_battery_last_timestamp(0)
_battery_last_timestamp(0),
_cb_flighttermination(true)
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
, _dsm_vcc_ctl(false)
#endif
@@ -579,6 +590,12 @@ PX4IO::detect()
return 0;
}
int
PX4IO::init(bool rc_handling_disabled) {
_rc_handling_disabled = rc_handling_disabled;
return init();
}
int
PX4IO::init()
{
@@ -778,6 +795,11 @@ PX4IO::init()
if (_rc_handling_disabled) {
ret = io_disable_rc_handling();
if (ret != OK) {
log("failed disabling RC handling");
return ret;
}
} else {
/* publish RC config to IO */
ret = io_set_rc_config();
@@ -1031,6 +1053,9 @@ PX4IO::task_main()
}
}
/* Update Circuit breakers */
_cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY);
}
}
@@ -1149,12 +1174,21 @@ PX4IO::io_set_arming_state()
clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
}
if (armed.force_failsafe) {
/* Do not set failsafe if circuit breaker is enabled */
if (armed.force_failsafe && !_cb_flighttermination) {
set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
} else {
clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
}
// XXX this is for future support in the commander
// but can be removed if unneeded
// if (armed.termination_failsafe) {
// set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
// } else {
// clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
// }
if (armed.ready_to_arm) {
set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
@@ -1175,6 +1209,7 @@ PX4IO::io_set_arming_state()
int
PX4IO::disable_rc_handling()
{
_rc_handling_disabled = true;
return io_disable_rc_handling();
}
@@ -1212,28 +1247,40 @@ PX4IO::io_set_rc_config()
*/
param_get(param_find("RC_MAP_ROLL"), &ichan);
/* subtract one from 1-based index - this might be
* a negative number now
*/
ichan -= 1;
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
input_map[ichan - 1] = 0;
input_map[ichan] = 0;
param_get(param_find("RC_MAP_PITCH"), &ichan);
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
input_map[ichan - 1] = 1;
input_map[ichan] = 1;
param_get(param_find("RC_MAP_YAW"), &ichan);
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
input_map[ichan - 1] = 2;
input_map[ichan] = 2;
param_get(param_find("RC_MAP_THROTTLE"), &ichan);
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
input_map[ichan - 1] = 3;
input_map[ichan] = 3;
param_get(param_find("RC_MAP_FLAPS"), &ichan);
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
input_map[ichan] = 4;
param_get(param_find("RC_MAP_MODE_SW"), &ichan);
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
input_map[ichan - 1] = 4;
if ((ichan >= 0) && (ichan < (int)_max_rc_input)) {
/* use out of normal bounds index to indicate special channel */
input_map[ichan] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH;
}
/*
* Iterate all possible RC inputs.
@@ -1582,6 +1629,9 @@ PX4IO::io_publish_raw_rc()
} else if (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) {
rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SBUS;
} else if (_status & PX4IO_P_STATUS_FLAGS_RC_ST24) {
rc_val.input_source = RC_INPUT_SOURCE_PX4IO_ST24;
} else {
rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN;
@@ -1899,13 +1949,15 @@ PX4IO::print_status(bool extended_status)
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM));
uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
uint16_t io_status_flags = flags;
printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n",
printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
flags,
((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""),
((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"),
((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"),
((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""),
((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) ? " DSM" : ""),
((flags & PX4IO_P_STATUS_FLAGS_RC_ST24) ? " ST24" : ""),
((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""),
((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"),
((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PWM_PASSTHROUGH" : ""),
@@ -1947,7 +1999,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");
@@ -2017,7 +2069,8 @@ PX4IO::print_status(bool extended_status)
((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""),
((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : ""),
((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : "")
((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : ""),
((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : "")
);
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
@@ -2230,6 +2283,11 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
break;
case PWM_SERVO_SET_FORCE_SAFETY_ON:
/* force safety switch on */
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC);
break;
case PWM_SERVO_SET_FORCE_FAILSAFE:
/* force failsafe mode instantly */
if (arg == 0) {
@@ -2241,6 +2299,17 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
}
break;
case PWM_SERVO_SET_TERMINATION_FAILSAFE:
/* if failsafe occurs, do not allow the system to recover */
if (arg == 0) {
/* clear termination failsafe flag */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE, 0);
} else {
/* set termination failsafe flag */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE);
}
break;
case DSM_BIND_START:
/* only allow DSM2, DSM-X and DSM-X with more than 7 channels */
@@ -2418,6 +2487,9 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
} else if (status & PX4IO_P_STATUS_FLAGS_RC_SBUS) {
rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SBUS;
} else if (status & PX4IO_P_STATUS_FLAGS_RC_ST24) {
rc_val->input_source = RC_INPUT_SOURCE_PX4IO_ST24;
} else {
rc_val->input_source = RC_INPUT_SOURCE_UNKNOWN;
}
@@ -2613,24 +2685,25 @@ start(int argc, char *argv[])
errx(1, "driver alloc failed");
}
if (OK != g_dev->init()) {
delete g_dev;
g_dev = nullptr;
errx(1, "driver init failed");
}
bool rc_handling_disabled = false;
/* disable RC handling on request */
if (argc > 1) {
if (!strcmp(argv[1], "norc")) {
if (g_dev->disable_rc_handling())
warnx("Failed disabling RC handling");
rc_handling_disabled = true;
} else {
warnx("unknown argument: %s", argv[1]);
}
}
if (OK != g_dev->init(rc_handling_disabled)) {
delete g_dev;
g_dev = nullptr;
errx(1, "driver init failed");
}
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
int dsm_vcc_ctl;
src/drivers/sf0x/module.mk
+2 -1
View File
@@ -37,6 +37,7 @@
MODULE_COMMAND = sf0x
SRCS = sf0x.cpp
SRCS = sf0x.cpp \
sf0x_parser.cpp
MAXOPTIMIZATION = -Os
src/drivers/sf0x/sf0x.cpp
+18 -87
View File
@@ -72,6 +72,8 @@
#include <board_config.h>
#include "sf0x_parser.h"
/* Configuration Constants */
/* oddly, ERROR is not defined for c++ */
@@ -120,6 +122,7 @@ private:
int _fd;
char _linebuf[10];
unsigned _linebuf_index;
enum SF0X_PARSE_STATE _parse_state;
hrt_abstime _last_read;
orb_advert_t _range_finder_topic;
@@ -186,6 +189,7 @@ SF0X::SF0X(const char *port) :
_collect_phase(false),
_fd(-1),
_linebuf_index(0),
_parse_state(SF0X_PARSE_STATE0_UNSYNC),
_last_read(0),
_range_finder_topic(-1),
_consecutive_fail_count(0),
@@ -200,12 +204,6 @@ SF0X::SF0X(const char *port) :
warnx("FAIL: laser fd");
}
/* tell it to stop auto-triggering */
char stop_auto = ' ';
(void)::write(_fd, &stop_auto, 1);
usleep(100);
(void)::write(_fd, &stop_auto, 1);
struct termios uart_config;
int termios_state;
@@ -520,22 +518,15 @@ SF0X::collect()
/* clear buffer if last read was too long ago */
uint64_t read_elapsed = hrt_elapsed_time(&_last_read);
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 */
unsigned readlen = sizeof(_linebuf) - 1;
char readbuf[sizeof(_linebuf)];
unsigned readlen = sizeof(readbuf) - 1;
/* read from the sensor (uart buffer) */
ret = ::read(_fd, &_linebuf[_linebuf_index], readlen - _linebuf_index);
ret = ::read(_fd, &readbuf[0], readlen);
if (ret < 0) {
_linebuf[sizeof(_linebuf) - 1] = '\0';
debug("read err: %d lbi: %d buf: %s", ret, (int)_linebuf_index, _linebuf);
debug("read err: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
@@ -550,83 +541,23 @@ SF0X::collect()
return -EAGAIN;
}
/* we did increment the index to the next position already, so just add the additional fields */
_linebuf_index += (ret - 1);
_last_read = hrt_absolute_time();
if (_linebuf_index < 1) {
/* we need at least the two end bytes to make sense of this string */
return -EAGAIN;
} else if (_linebuf[_linebuf_index - 1] != '\r' || _linebuf[_linebuf_index] != '\n') {
if (_linebuf_index >= readlen - 1) {
/* we have a full buffer, but no line ending - abort */
_linebuf_index = 0;
perf_count(_comms_errors);
return -ENOMEM;
} else {
/* incomplete read, reschedule ourselves */
return -EAGAIN;
}
}
char *end;
float si_units;
bool valid;
/* enforce line ending */
unsigned lend = (_linebuf_index < (sizeof(_linebuf) - 1)) ? _linebuf_index : (sizeof(_linebuf) - 1);
_linebuf[lend] = '\0';
if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') {
si_units = -1.0f;
valid = false;
} else {
/* we need to find a dot in the string, as we're missing the meters part else */
valid = false;
/* wipe out partially read content from last cycle(s), check for dot */
for (unsigned i = 0; i < (lend - 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));
}
if (_linebuf[i] == '.') {
valid = true;
}
}
if (valid) {
si_units = strtod(_linebuf, &end);
/* we require at least 3 characters for a valid number */
if (end > _linebuf + 3) {
valid = true;
} else {
si_units = -1.0f;
valid = false;
}
bool valid = false;
for (int i = 0; i < ret; i++) {
if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &si_units)) {
valid = true;
}
}
debug("val (float): %8.4f, raw: %s, valid: %s\n", (double)si_units, _linebuf, ((valid) ? "OK" : "NO"));
/* done with this chunk, resetting - even if invalid */
_linebuf_index = 0;
/* if its invalid, there is no reason to forward the value */
if (!valid) {
perf_count(_comms_errors);
return -EINVAL;
return -EAGAIN;
}
debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO"));
struct range_finder_report report;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
@@ -708,12 +639,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;
}
src/drivers/sf0x/sf0x_parser.cpp
+155
View File
@@ -0,0 +1,155 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file sf0x_parser.cpp
* @author Lorenz Meier <lm@inf.ethz.ch>
*
* Driver for the Lightware SF0x laser rangefinder series
*/
#include "sf0x_parser.h"
#include <string.h>
#include <stdlib.h>
//#define SF0X_DEBUG
#ifdef SF0X_DEBUG
#include <stdio.h>
const char *parser_state[] = {
"0_UNSYNC",
"1_SYNC",
"2_GOT_DIGIT0",
"3_GOT_DOT",
"4_GOT_DIGIT1",
"5_GOT_DIGIT2",
"6_GOT_CARRIAGE_RETURN"
};
#endif
int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist)
{
int ret = -1;
char *end;
switch (*state) {
case SF0X_PARSE_STATE0_UNSYNC:
if (c == '\n') {
*state = SF0X_PARSE_STATE1_SYNC;
(*parserbuf_index) = 0;
}
break;
case SF0X_PARSE_STATE1_SYNC:
if (c >= '0' && c <= '9') {
*state = SF0X_PARSE_STATE2_GOT_DIGIT0;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
}
break;
case SF0X_PARSE_STATE2_GOT_DIGIT0:
if (c >= '0' && c <= '9') {
*state = SF0X_PARSE_STATE2_GOT_DIGIT0;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
} else if (c == '.') {
*state = SF0X_PARSE_STATE3_GOT_DOT;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
} else {
*state = SF0X_PARSE_STATE0_UNSYNC;
}
break;
case SF0X_PARSE_STATE3_GOT_DOT:
if (c >= '0' && c <= '9') {
*state = SF0X_PARSE_STATE4_GOT_DIGIT1;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
} else {
*state = SF0X_PARSE_STATE0_UNSYNC;
}
break;
case SF0X_PARSE_STATE4_GOT_DIGIT1:
if (c >= '0' && c <= '9') {
*state = SF0X_PARSE_STATE5_GOT_DIGIT2;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
} else {
*state = SF0X_PARSE_STATE0_UNSYNC;
}
break;
case SF0X_PARSE_STATE5_GOT_DIGIT2:
if (c == '\r') {
*state = SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN;
} else {
*state = SF0X_PARSE_STATE0_UNSYNC;
}
break;
case SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN:
if (c == '\n') {
parserbuf[*parserbuf_index] = '\0';
*dist = strtod(parserbuf, &end);
*state = SF0X_PARSE_STATE1_SYNC;
*parserbuf_index = 0;
ret = 0;
} else {
*state = SF0X_PARSE_STATE0_UNSYNC;
}
break;
}
#ifdef SF0X_DEBUG
printf("state: SF0X_PARSE_STATE%s\n", parser_state[*state]);
#endif
return ret;
}
src/drivers/sf0x/sf0x_parser.h
+51
View File
@@ -0,0 +1,51 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file sf0x_parser.cpp
* @author Lorenz Meier <lm@inf.ethz.ch>
*
* Declarations of parser for the Lightware SF0x laser rangefinder series
*/
enum SF0X_PARSE_STATE {
SF0X_PARSE_STATE0_UNSYNC = 0,
SF0X_PARSE_STATE1_SYNC,
SF0X_PARSE_STATE2_GOT_DIGIT0,
SF0X_PARSE_STATE3_GOT_DOT,
SF0X_PARSE_STATE4_GOT_DIGIT1,
SF0X_PARSE_STATE5_GOT_DIGIT2,
SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN
};
int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist);
src/drivers/stm32/tone_alarm/tone_alarm.cpp
+2
View File
@@ -337,6 +337,7 @@ ToneAlarm::ToneAlarm() :
_default_tunes[TONE_ARMING_FAILURE_TUNE] = "MFT255L4<<<BAP";
_default_tunes[TONE_PARACHUTE_RELEASE_TUNE] = "MFT255L16agagagag"; // parachute release
_default_tunes[TONE_EKF_WARNING_TUNE] = "MFT255L8ddd#d#eeff"; // ekf warning
_default_tunes[TONE_BARO_WARNING_TUNE] = "MFT255L4gf#fed#d"; // baro warning
_tune_names[TONE_STARTUP_TUNE] = "startup"; // startup tune
_tune_names[TONE_ERROR_TUNE] = "error"; // ERROR tone
@@ -350,6 +351,7 @@ ToneAlarm::ToneAlarm() :
_tune_names[TONE_ARMING_FAILURE_TUNE] = "arming_failure"; //fail to arm
_tune_names[TONE_PARACHUTE_RELEASE_TUNE] = "parachute_release"; // parachute release
_tune_names[TONE_EKF_WARNING_TUNE] = "ekf_warning"; // ekf warning
_tune_names[TONE_BARO_WARNING_TUNE] = "baro_warning"; // baro warning
}
ToneAlarm::~ToneAlarm()
src/lib/conversion/module.mk
+2
View File
@@ -36,3 +36,5 @@
#
SRCS = rotation.cpp
MAXOPTIMIZATION = -Os
src/lib/conversion/rotation.h
+3 -1
View File
@@ -74,6 +74,7 @@ enum Rotation {
ROTATION_ROLL_270_YAW_135 = 23,
ROTATION_PITCH_90 = 24,
ROTATION_PITCH_270 = 25,
ROTATION_ROLL_270_YAW_270 = 26,
ROTATION_MAX
};
@@ -109,7 +110,8 @@ const rot_lookup_t rot_lookup[] = {
{270, 0, 90 },
{270, 0, 135 },
{ 0, 90, 0 },
{ 0, 270, 0 }
{ 0, 270, 0 },
{270, 0, 270 }
};
/**
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+4 -2
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);
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+4 -2
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);
}
src/lib/external_lgpl/tecs/tecs.cpp
+41 -21
View File
@@ -236,9 +236,9 @@ void TECS::_update_height_demand(float demand, float state)
// // _hgt_rate_dem);
_hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last;
_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT;
_hgt_dem_adj_last = _hgt_dem_adj;
_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p;
// Limit height rate of change
if (_hgt_rate_dem > _maxClimbRate) {
_hgt_rate_dem = _maxClimbRate;
@@ -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;
@@ -294,11 +299,11 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
// Calculate throttle demand
// If underspeed condition is set, then demand full throttle
if (_underspeed) {
_throttle_dem_unc = 1.0f;
_throttle_dem = 1.0f;
} else {
// Calculate gain scaler from specific energy error to throttle
float K_STE2Thr = 1 / (_timeConst * (_STEdot_max - _STEdot_min));
float K_STE2Thr = 1 / (_timeConstThrot * (_STEdot_max - _STEdot_min));
// Calculate feed-forward throttle
float ff_throttle = 0;
@@ -327,9 +332,12 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
_throttle_dem = constrain(_throttle_dem,
_last_throttle_dem - thrRateIncr,
_last_throttle_dem + thrRateIncr);
_last_throttle_dem = _throttle_dem;
}
// Ensure _last_throttle_dem is always initialized properly
// Also: The throttle_slewrate limit is only applied to throttle_dem, but does not limit the integrator!!
_last_throttle_dem = _throttle_dem;
// Calculate integrator state upper and lower limits
// Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand
@@ -355,10 +363,10 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
} else {
_throttle_dem = ff_throttle;
}
}
// Constrain throttle demand
_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
// Constrain throttle demand
_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
}
}
void TECS::_detect_bad_descent(void)
@@ -551,18 +559,30 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
// Calculate pitch demand
_update_pitch();
// // Write internal variables to the log_tuning structure. This
// // structure will be logged in dataflash at 10Hz
// log_tuning.hgt_dem = _hgt_dem_adj;
// log_tuning.hgt = _integ3_state;
// log_tuning.dhgt_dem = _hgt_rate_dem;
// log_tuning.dhgt = _integ2_state;
// log_tuning.spd_dem = _TAS_dem_adj;
// log_tuning.spd = _integ5_state;
// log_tuning.dspd = _vel_dot;
// log_tuning.ithr = _integ6_state;
// log_tuning.iptch = _integ7_state;
// log_tuning.thr = _throttle_dem;
// log_tuning.ptch = _pitch_dem;
// log_tuning.dspd_dem = _TAS_rate_dem;
_tecs_state.timestamp = now;
if (_underspeed) {
_tecs_state.mode = ECL_TECS_MODE_UNDERSPEED;
} else if (_badDescent) {
_tecs_state.mode = ECL_TECS_MODE_BAD_DESCENT;
} else if (_climbOutDem) {
_tecs_state.mode = ECL_TECS_MODE_CLIMBOUT;
} else {
// If no error flag applies, conclude normal
_tecs_state.mode = ECL_TECS_MODE_NORMAL;
}
_tecs_state.hgt_dem = _hgt_dem_adj;
_tecs_state.hgt = _integ3_state;
_tecs_state.dhgt_dem = _hgt_rate_dem;
_tecs_state.dhgt = _integ2_state;
_tecs_state.spd_dem = _TAS_dem_adj;
_tecs_state.spd = _integ5_state;
_tecs_state.dspd = _vel_dot;
_tecs_state.ithr = _integ6_state;
_tecs_state.iptch = _integ7_state;
_tecs_state.thr = _throttle_dem;
_tecs_state.ptch = _pitch_dem;
_tecs_state.dspd_dem = _TAS_rate_dem;
}
src/lib/external_lgpl/tecs/tecs.h
+71 -20
View File
@@ -28,6 +28,28 @@ class __EXPORT TECS
{
public:
TECS() :
_tecs_state {},
_update_50hz_last_usec(0),
_update_speed_last_usec(0),
_update_pitch_throttle_last_usec(0),
// TECS tuning parameters
_hgtCompFiltOmega(0.0f),
_spdCompFiltOmega(0.0f),
_maxClimbRate(2.0f),
_minSinkRate(1.0f),
_maxSinkRate(2.0f),
_timeConst(5.0f),
_timeConstThrot(8.0f),
_ptchDamp(0.0f),
_thrDamp(0.0f),
_integGain(0.0f),
_vertAccLim(0.0f),
_rollComp(0.0f),
_spdWeight(0.5f),
_heightrate_p(0.0f),
_heightrate_ff(0.0f),
_speedrate_p(0.0f),
_throttle_dem(0.0f),
_pitch_dem(0.0f),
_integ1_state(0.0f),
_integ2_state(0.0f),
@@ -45,6 +67,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),
@@ -100,29 +125,42 @@ public:
return _spdWeight;
}
// log data on internal state of the controller. Called at 10Hz
// void log_data(DataFlash_Class &dataflash, uint8_t msgid);
enum ECL_TECS_MODE {
ECL_TECS_MODE_NORMAL = 0,
ECL_TECS_MODE_UNDERSPEED,
ECL_TECS_MODE_BAD_DESCENT,
ECL_TECS_MODE_CLIMBOUT
};
// struct PACKED log_TECS_Tuning {
// LOG_PACKET_HEADER;
// float hgt;
// float dhgt;
// float hgt_dem;
// float dhgt_dem;
// float spd_dem;
// float spd;
// float dspd;
// float ithr;
// float iptch;
// float thr;
// float ptch;
// float dspd_dem;
// } log_tuning;
struct tecs_state {
uint64_t timestamp;
float hgt;
float dhgt;
float hgt_dem;
float dhgt_dem;
float spd_dem;
float spd;
float dspd;
float ithr;
float iptch;
float thr;
float ptch;
float dspd_dem;
enum ECL_TECS_MODE mode;
};
void get_tecs_state(struct tecs_state& state) {
state = _tecs_state;
}
void set_time_const(float time_const) {
_timeConst = time_const;
}
void set_time_const_throt(float time_const_throt) {
_timeConstThrot = time_const_throt;
}
void set_min_sink_rate(float rate) {
_minSinkRate = rate;
}
@@ -183,11 +221,22 @@ public:
_heightrate_p = heightrate_p;
}
void set_heightrate_ff(float heightrate_ff) {
_heightrate_ff = heightrate_ff;
}
void set_speedrate_p(float speedrate_p) {
_speedrate_p = speedrate_p;
}
void set_detect_underspeed_enabled(bool enabled) {
_detect_underspeed_enabled = enabled;
}
private:
struct tecs_state _tecs_state;
// Last time update_50Hz was called
uint64_t _update_50hz_last_usec;
@@ -204,6 +253,7 @@ private:
float _minSinkRate;
float _maxSinkRate;
float _timeConst;
float _timeConstThrot;
float _ptchDamp;
float _thrDamp;
float _integGain;
@@ -211,6 +261,7 @@ private:
float _rollComp;
float _spdWeight;
float _heightrate_p;
float _heightrate_ff;
float _speedrate_p;
// throttle demand in the range from 0.0 to 1.0
@@ -285,15 +336,15 @@ private:
// Underspeed condition
bool _underspeed;
// Underspeed detection enabled
bool _detect_underspeed_enabled;
// Bad descent condition caused by unachievable airspeed demand
bool _badDescent;
// climbout mode
bool _climbOutDem;
// throttle demand before limiting
float _throttle_dem_unc;
// pitch demand before limiting
float _pitch_dem_unc;
src/lib/geo/geo.c
+195 -25
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012, 2014 PX4 Development Team. All rights reserved.
* 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
@@ -49,39 +49,124 @@
#include <stdio.h>
#include <math.h>
#include <stdbool.h>
#include <string.h>
#include <float.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
/*
* Azimuthal Equidistant Projection
* formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html
*/
__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
{
ref->lat = lat_0 / 180.0 * M_PI;
ref->lon = lon_0 / 180.0 * M_PI;
static struct map_projection_reference_s mp_ref = {0.0, 0.0, 0.0, 0.0, false, 0};
static struct globallocal_converter_reference_s gl_ref = {0.0f, false};
ref->sin_lat = sin(ref->lat);
ref->cos_lat = cos(ref->lat);
__EXPORT bool map_projection_global_initialized()
{
return map_projection_initialized(&mp_ref);
}
__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
__EXPORT bool map_projection_initialized(const struct map_projection_reference_s *ref)
{
double lat_rad = lat / 180.0 * M_PI;
double lon_rad = lon / 180.0 * M_PI;
return ref->init_done;
}
__EXPORT uint64_t map_projection_global_timestamp()
{
return map_projection_timestamp(&mp_ref);
}
__EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref)
{
return ref->timestamp;
}
__EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
{
if (strcmp("commander", getprogname()) == 0) {
return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp);
} else {
return -1;
}
}
__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
{
ref->lat_rad = lat_0 * M_DEG_TO_RAD;
ref->lon_rad = lon_0 * M_DEG_TO_RAD;
ref->sin_lat = sin(ref->lat_rad);
ref->cos_lat = cos(ref->lat_rad);
ref->timestamp = timestamp;
ref->init_done = true;
return 0;
}
__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
{
return map_projection_init_timestamped(ref, lat_0, lon_0, hrt_absolute_time());
}
__EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad)
{
return map_projection_reference(&mp_ref, ref_lat_rad, ref_lon_rad);
}
__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad)
{
if (!map_projection_initialized(ref)) {
return -1;
}
*ref_lat_rad = ref->lat_rad;
*ref_lon_rad = ref->lon_rad;
return 0;
}
__EXPORT int map_projection_global_project(double lat, double lon, float *x, float *y)
{
return map_projection_project(&mp_ref, lat, lon, x, y);
}
__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
{
if (!map_projection_initialized(ref)) {
return -1;
}
double lat_rad = lat * M_DEG_TO_RAD;
double lon_rad = lon * M_DEG_TO_RAD;
double sin_lat = sin(lat_rad);
double cos_lat = cos(lat_rad);
double cos_d_lon = cos(lon_rad - ref->lon);
double cos_d_lon = cos(lon_rad - ref->lon_rad);
double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon);
double k = (c == 0.0) ? 1.0 : (c / sin(c));
double k = (fabs(c) < DBL_EPSILON) ? 1.0 : (c / sin(c));
*x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH;
*y = k * cos_lat * sin(lon_rad - ref->lon) * CONSTANTS_RADIUS_OF_EARTH;
*y = k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH;
return 0;
}
__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
__EXPORT int map_projection_global_reproject(float x, float y, double *lat, double *lon)
{
return map_projection_reproject(&mp_ref, x, y, lat, lon);
}
__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
{
if (!map_projection_initialized(ref)) {
return -1;
}
double x_rad = x / CONSTANTS_RADIUS_OF_EARTH;
double y_rad = y / CONSTANTS_RADIUS_OF_EARTH;
double c = sqrtf(x_rad * x_rad + y_rad * y_rad);
@@ -91,19 +176,101 @@ __EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, f
double lat_rad;
double lon_rad;
if (c != 0.0) {
if (fabs(c) > DBL_EPSILON) {
lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c);
lon_rad = (ref->lon + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));
lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));
} else {
lat_rad = ref->lat;
lon_rad = ref->lon;
lat_rad = ref->lat_rad;
lon_rad = ref->lon_rad;
}
*lat = lat_rad * 180.0 / M_PI;
*lon = lon_rad * 180.0 / M_PI;
return 0;
}
__EXPORT int map_projection_global_getref(double *lat_0, double *lon_0)
{
if (!map_projection_global_initialized()) {
return -1;
}
if (lat_0 != NULL) {
*lat_0 = M_RAD_TO_DEG * mp_ref.lat_rad;
}
if (lon_0 != NULL) {
*lon_0 = M_RAD_TO_DEG * mp_ref.lon_rad;
}
return 0;
}
__EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp)
{
if (strcmp("commander", getprogname()) == 0) {
gl_ref.alt = alt_0;
if (!map_projection_global_init(lat_0, lon_0, timestamp))
{
gl_ref.init_done = true;
return 0;
} else {
gl_ref.init_done = false;
return -1;
}
} else {
return -1;
}
}
__EXPORT bool globallocalconverter_initialized()
{
return gl_ref.init_done && map_projection_global_initialized();
}
__EXPORT int globallocalconverter_tolocal(double lat, double lon, float alt, float *x, float *y, float *z)
{
if (!map_projection_global_initialized()) {
return -1;
}
map_projection_global_project(lat, lon, x, y);
*z = gl_ref.alt - alt;
return 0;
}
__EXPORT int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt)
{
if (!map_projection_global_initialized()) {
return -1;
}
map_projection_global_reproject(x, y, lat, lon);
*alt = gl_ref.alt - z;
return 0;
}
__EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0)
{
if (!map_projection_global_initialized()) {
return -1;
}
if (map_projection_global_getref(lat_0, lon_0))
{
return -1;
}
if (alt_0 != NULL) {
*alt_0 = gl_ref.alt;
}
return 0;
}
__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
{
@@ -195,8 +362,12 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d
crosstrack_error->distance = 0.0f;
crosstrack_error->bearing = 0.0f;
dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
// Return error if arguments are bad
if (lat_now == 0.0 || lon_now == 0.0 || lat_start == 0.0 || lon_start == 0.0 || lat_end == 0.0d || lon_end == 0.0d) { return return_value; }
if (dist_to_end < 0.1f) {
return ERROR;
}
bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
@@ -210,7 +381,6 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d
return return_value;
}
dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
crosstrack_error->distance = (dist_to_end) * sinf(bearing_diff);
if (sin(bearing_diff) >= 0) {
@@ -247,10 +417,10 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
crosstrack_error->bearing = 0.0f;
// Return error if arguments are bad
if (lat_now == 0.0 || lon_now == 0.0 || lat_center == 0.0 || lon_center == 0.0 || radius == 0.0f) { return return_value; }
if (radius < 0.1f) { return return_value; }
if (arc_sweep >= 0) {
if (arc_sweep >= 0.0f) {
bearing_sector_start = arc_start_bearing;
bearing_sector_end = arc_start_bearing + arc_sweep;
@@ -296,8 +466,8 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
double start_disp_x = (double)radius * sin(arc_start_bearing);
double start_disp_y = (double)radius * cos(arc_start_bearing);
double end_disp_x = (double)radius * sin(_wrapPI((double)(arc_start_bearing + arc_sweep)));
double end_disp_y = (double)radius * cos(_wrapPI((double)(arc_start_bearing + arc_sweep)));
double end_disp_x = (double)radius * sin(_wrap_pi((double)(arc_start_bearing + arc_sweep)));
double end_disp_y = (double)radius * cos(_wrap_pi((double)(arc_start_bearing + arc_sweep)));
double lon_start = lon_now + start_disp_x / 111111.0;
double lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0;
double lon_end = lon_now + end_disp_x / 111111.0;
@@ -317,7 +487,7 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
}
crosstrack_error->bearing = _wrapPI((double)crosstrack_error->bearing);
crosstrack_error->bearing = _wrap_pi((double)crosstrack_error->bearing);
return_value = OK;
return return_value;
}
src/lib/geo/geo.h
+132 -9
View File
@@ -69,39 +69,162 @@ struct crosstrack_error_s {
/* lat/lon are in radians */
struct map_projection_reference_s {
double lat;
double lon;
double lat_rad;
double lon_rad;
double sin_lat;
double cos_lat;
bool init_done;
uint64_t timestamp;
};
struct globallocal_converter_reference_s {
float alt;
bool init_done;
};
/**
* Initializes the map transformation.
* Checks if global projection was initialized
* @return true if map was initialized before, false else
*/
__EXPORT bool map_projection_global_initialized(void);
/**
* Checks if projection given as argument was initialized
* @return true if map was initialized before, false else
*/
__EXPORT bool map_projection_initialized(const struct map_projection_reference_s *ref);
/**
* Get the timestamp of the global map projection
* @return the timestamp of the map_projection
*/
__EXPORT uint64_t map_projection_global_timestamp(void);
/**
* Get the timestamp of the map projection given by the argument
* @return the timestamp of the map_projection
*/
__EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref);
/**
* Writes the reference values of the global projection to ref_lat and ref_lon
* @return 0 if map_projection_init was called before, -1 else
*/
__EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad);
/**
* Writes the reference values of the projection given by the argument to ref_lat and ref_lon
* @return 0 if map_projection_init was called before, -1 else
*/
__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad);
/**
* Initializes the global map transformation.
*
* Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane
* Initializes the transformation between the geographic coordinate system and
* the azimuthal equidistant plane
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0);
__EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp);
/**
* Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane
* Initializes the map transformation given by the argument.
*
* Initializes the transformation between the geographic coordinate system and
* the azimuthal equidistant plane
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref,
double lat_0, double lon_0, uint64_t timestamp);
/**
* Initializes the map transformation given by the argument and sets the timestamp to now.
*
* Initializes the transformation between the geographic coordinate system and
* the azimuthal equidistant plane
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0);
/**
* Transforms a point in the geographic coordinate system to the local
* azimuthal equidistant plane using the global projection
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
* @return 0 if map_projection_init was called before, -1 else
*/
__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y);
__EXPORT int map_projection_global_project(double lat, double lon, float *x, float *y);
/* Transforms a point in the geographic coordinate system to the local
* azimuthal equidistant plane using the projection given by the argument
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
* @return 0 if map_projection_init was called before, -1 else
*/
__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y);
/**
* Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system
* Transforms a point in the local azimuthal equidistant plane to the
* geographic coordinate system using the global projection
*
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
* @return 0 if map_projection_init was called before, -1 else
*/
__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon);
__EXPORT int map_projection_global_reproject(float x, float y, double *lat, double *lon);
/**
* Transforms a point in the local azimuthal equidistant plane to the
* geographic coordinate system using the projection given by the argument
*
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
* @return 0 if map_projection_init was called before, -1 else
*/
__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon);
/**
* Get reference position of the global map projection
*/
__EXPORT int map_projection_global_getref(double *lat_0, double *lon_0);
/**
* Initialize the global mapping between global position (spherical) and local position (NED).
*/
__EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp);
/**
* Checks if globallocalconverter was initialized
* @return true if map was initialized before, false else
*/
__EXPORT bool globallocalconverter_initialized(void);
/**
* Convert from global position coordinates to local position coordinates using the global reference
*/
__EXPORT int globallocalconverter_tolocal(double lat, double lon, float alt, float *x, float *y, float *z);
/**
* Convert from local position coordinates to global position coordinates using the global reference
*/
__EXPORT int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt);
/**
* Get reference position of the global to local converter
*/
__EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0);
/**
* Returns the distance to the next waypoint in meters.
src/lib/launchdetection/CatapultLaunchMethod.cpp
+52 -18
View File
@@ -49,9 +49,11 @@ CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) :
SuperBlock(parent, "CAT"),
last_timestamp(hrt_absolute_time()),
integrator(0.0f),
launchDetected(false),
threshold_accel(this, "A"),
threshold_time(this, "T")
state(LAUNCHDETECTION_RES_NONE),
thresholdAccel(this, "A"),
thresholdTime(this, "T"),
motorDelay(this, "MDEL"),
pitchMaxPreThrottle(this, "PMAX")
{
}
@@ -65,34 +67,66 @@ void CatapultLaunchMethod::update(float accel_x)
float dt = (float)hrt_elapsed_time(&last_timestamp) * 1e-6f;
last_timestamp = hrt_absolute_time();
if (accel_x > threshold_accel.get()) {
integrator += accel_x * 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()) {
launchDetected = true;
switch (state) {
case LAUNCHDETECTION_RES_NONE:
/* Detect a acceleration that is longer and stronger as the minimum given by the params */
if (accel_x > thresholdAccel.get()) {
integrator += dt;
if (integrator > thresholdTime.get()) {
if (motorDelay.get() > 0.0f) {
state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL;
warnx("Launch detected: state: enablecontrol, waiting %.2fs until using full"
" throttle", (double)motorDelay.get());
} else {
/* No motor delay set: go directly to enablemotors state */
state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
warnx("Launch detected: state: enablemotors (delay not activated)");
}
}
} else {
/* reset */
reset();
}
break;
case LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL:
/* Vehicle is currently controlling attitude but not with full throttle. Waiting undtil delay is
* over to allow full throttle */
motorDelayCounter += dt;
if (motorDelayCounter > motorDelay.get()) {
warnx("Launch detected: state enablemotors");
state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
}
break;
default:
break;
} else {
// 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);
/* reset integrator */
integrator = 0.0f;
launchDetected = false;
}
}
bool CatapultLaunchMethod::getLaunchDetected()
LaunchDetectionResult CatapultLaunchMethod::getLaunchDetected() const
{
return launchDetected;
return state;
}
void CatapultLaunchMethod::reset()
{
integrator = 0.0f;
launchDetected = false;
motorDelayCounter = 0.0f;
state = LAUNCHDETECTION_RES_NONE;
}
float CatapultLaunchMethod::getPitchMax(float pitchMaxDefault) {
/* If motor is turned on do not impose the extra limit on maximum pitch */
if (state == LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
return pitchMaxDefault;
} else {
return pitchMaxPreThrottle.get();
}
}
}
src/lib/launchdetection/CatapultLaunchMethod.h
+11 -4
View File
@@ -57,16 +57,23 @@ public:
~CatapultLaunchMethod();
void update(float accel_x);
bool getLaunchDetected();
LaunchDetectionResult getLaunchDetected() const;
void reset();
float getPitchMax(float pitchMaxDefault);
private:
hrt_abstime last_timestamp;
float integrator;
bool launchDetected;
float motorDelayCounter;
control::BlockParamFloat threshold_accel;
control::BlockParamFloat threshold_time;
LaunchDetectionResult state;
control::BlockParamFloat thresholdAccel;
control::BlockParamFloat thresholdTime;
control::BlockParamFloat motorDelay;
control::BlockParamFloat pitchMaxPreThrottle; /**< Upper pitch limit before throttle is turned on.
Can be used to make sure that the AC does not climb
too much while attached to a bungee */
};
src/lib/launchdetection/LaunchDetector.cpp
+41 -6
View File
@@ -46,6 +46,7 @@ namespace launchdetection
LaunchDetector::LaunchDetector() :
SuperBlock(NULL, "LAUN"),
activeLaunchDetectionMethodIndex(-1),
launchdetection_on(this, "ALL_ON"),
throttlePreTakeoff(this, "THR_PRE")
{
@@ -65,7 +66,14 @@ LaunchDetector::~LaunchDetector()
void LaunchDetector::reset()
{
/* Reset all detectors */
launchMethods[0]->reset();
for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
launchMethods[i]->reset();
}
/* Reset active launchdetector */
activeLaunchDetectionMethodIndex = -1;
}
void LaunchDetector::update(float accel_x)
@@ -77,17 +85,44 @@ void LaunchDetector::update(float accel_x)
}
}
bool LaunchDetector::getLaunchDetected()
LaunchDetectionResult LaunchDetector::getLaunchDetected()
{
if (launchdetection_on.get() == 1) {
for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
if(launchMethods[i]->getLaunchDetected()) {
return true;
if (activeLaunchDetectionMethodIndex < 0) {
/* None of the active launchmethods has detected a launch, check all launchmethods */
for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
if(launchMethods[i]->getLaunchDetected() != LAUNCHDETECTION_RES_NONE) {
warnx("selecting launchmethod %d", i);
activeLaunchDetectionMethodIndex = i; // from now on only check this method
return launchMethods[i]->getLaunchDetected();
}
}
} else {
return launchMethods[activeLaunchDetectionMethodIndex]->getLaunchDetected();
}
}
return false;
return LAUNCHDETECTION_RES_NONE;
}
float LaunchDetector::getPitchMax(float pitchMaxDefault) {
if (!launchdetection_on.get()) {
return pitchMaxDefault;
}
/* if a lauchdetectionmethod is active or only one exists return the pitch limit from this method,
* otherwise use the default limit */
if (activeLaunchDetectionMethodIndex < 0) {
if (sizeof(launchMethods)/sizeof(LaunchMethod) > 1) {
return pitchMaxDefault;
} else {
return launchMethods[0]->getPitchMax(pitchMaxDefault);
}
} else {
return launchMethods[activeLaunchDetectionMethodIndex]->getPitchMax(pitchMaxDefault);
}
}
}
src/lib/launchdetection/LaunchDetector.h
+10 -1
View File
@@ -59,14 +59,23 @@ public:
void reset();
void update(float accel_x);
bool getLaunchDetected();
LaunchDetectionResult getLaunchDetected();
bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); };
float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); }
/* Returns a maximum pitch in deg. Different launch methods may impose upper pitch limits during launch */
float getPitchMax(float pitchMaxDefault);
// virtual bool getLaunchDetected();
protected:
private:
int activeLaunchDetectionMethodIndex; /**< holds a index to the launchMethod in the array launchMethods
which detected a Launch. If no launchMethod has detected a launch yet the
value is -1. Once one launchMetthod has detected a launch only this
method is checked for further adavancing in the state machine (e.g. when
to power up the motors) */
LaunchMethod* launchMethods[1];
control::BlockParamInt launchdetection_on;
control::BlockParamFloat throttlePreTakeoff;
src/lib/launchdetection/LaunchMethod.h
+15 -1
View File
@@ -44,13 +44,27 @@
namespace launchdetection
{
enum LaunchDetectionResult {
LAUNCHDETECTION_RES_NONE = 0, /**< No launch has been detected */
LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL = 1, /**< Launch has been detected, the controller should
control the attitude. However any motors should not throttle
up and still be set to 'throttlePreTakeoff'.
For instance this is used to have a delay for the motor
when launching a fixed wing aircraft from a bungee */
LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS = 2 /**< Launch has been detected, teh controller should control
attitude and also throttle up the motors. */
};
class LaunchMethod
{
public:
virtual void update(float accel_x) = 0;
virtual bool getLaunchDetected() = 0;
virtual LaunchDetectionResult getLaunchDetected() const = 0;
virtual void reset() = 0;
/* Returns a upper pitch limit if required, otherwise returns pitchMaxDefault */
virtual float getPitchMax(float pitchMaxDefault) = 0;
protected:
private:
};
src/lib/launchdetection/launchdetection_params.c
+25
View File
@@ -77,6 +77,31 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f);
*/
PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f);
/**
* Motor delay
*
* Delay between starting attitude control and powering up the throttle (giving throttle control to the controller)
* Before this timespan is up the throttle will be set to LAUN_THR_PRE, set to 0 to deactivate
*
* @unit seconds
* @min 0
* @group Launch detection
*/
PARAM_DEFINE_FLOAT(LAUN_CAT_MDEL, 0.0f);
/**
* Maximum pitch before the throttle is powered up (during motor delay phase)
*
* This is an extra limit for the maximum pitch which is imposed in the phase before the throttle turns on.
* This allows to limit the maximum pitch angle during a bungee launch (make the launch less steep).
*
* @unit deg
* @min 0
* @max 45
* @group Launch detection
*/
PARAM_DEFINE_FLOAT(LAUN_CAT_PMAX, 30.0f);
/**
* Throttle setting while detecting launch.
*
src/lib/rc/module.mk
+40
View File
@@ -0,0 +1,40 @@
############################################################################
#
# 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.
#
############################################################################
#
# Yuntec ST24 transmitter protocol decoder
#
SRCS = st24.c
MAXOPTIMIZATION = -Os
src/lib/rc/st24.c
+253
View File
@@ -0,0 +1,253 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file st24.h
*
* RC protocol implementation for Yuneec ST24 transmitter.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <stdbool.h>
#include <stdio.h>
#include "st24.h"
enum ST24_DECODE_STATE {
ST24_DECODE_STATE_UNSYNCED = 0,
ST24_DECODE_STATE_GOT_STX1,
ST24_DECODE_STATE_GOT_STX2,
ST24_DECODE_STATE_GOT_LEN,
ST24_DECODE_STATE_GOT_TYPE,
ST24_DECODE_STATE_GOT_DATA
};
const char *decode_states[] = {"UNSYNCED",
"GOT_STX1",
"GOT_STX2",
"GOT_LEN",
"GOT_TYPE",
"GOT_DATA"
};
/* define range mapping here, -+100% -> 1000..2000 */
#define ST24_RANGE_MIN 0.0f
#define ST24_RANGE_MAX 4096.0f
#define ST24_TARGET_MIN 1000.0f
#define ST24_TARGET_MAX 2000.0f
/* pre-calculate the floating point stuff as far as possible at compile time */
#define ST24_SCALE_FACTOR ((ST24_TARGET_MAX - ST24_TARGET_MIN) / (ST24_RANGE_MAX - ST24_RANGE_MIN))
#define ST24_SCALE_OFFSET (int)(ST24_TARGET_MIN - (ST24_SCALE_FACTOR * ST24_RANGE_MIN + 0.5f))
static enum ST24_DECODE_STATE _decode_state = ST24_DECODE_STATE_UNSYNCED;
static unsigned _rxlen;
static ReceiverFcPacket _rxpacket;
uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len)
{
uint8_t i, crc ;
crc = 0;
while (len--) {
for (i = 0x80; i != 0; i >>= 1) {
if ((crc & 0x80) != 0) {
crc <<= 1;
crc ^= 0x07;
} else {
crc <<= 1;
}
if ((*ptr & i) != 0) {
crc ^= 0x07;
}
}
ptr++;
}
return (crc);
}
int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels,
uint16_t max_chan_count)
{
int ret = 1;
switch (_decode_state) {
case ST24_DECODE_STATE_UNSYNCED:
if (byte == ST24_STX1) {
_decode_state = ST24_DECODE_STATE_GOT_STX1;
} else {
ret = 3;
}
break;
case ST24_DECODE_STATE_GOT_STX1:
if (byte == ST24_STX2) {
_decode_state = ST24_DECODE_STATE_GOT_STX2;
} else {
_decode_state = ST24_DECODE_STATE_UNSYNCED;
}
break;
case ST24_DECODE_STATE_GOT_STX2:
/* ensure no data overflow failure or hack is possible */
if ((unsigned)byte <= sizeof(_rxpacket.length) + sizeof(_rxpacket.type) + sizeof(_rxpacket.st24_data)) {
_rxpacket.length = byte;
_rxlen = 0;
_decode_state = ST24_DECODE_STATE_GOT_LEN;
} else {
_decode_state = ST24_DECODE_STATE_UNSYNCED;
}
break;
case ST24_DECODE_STATE_GOT_LEN:
_rxpacket.type = byte;
_rxlen++;
_decode_state = ST24_DECODE_STATE_GOT_TYPE;
break;
case ST24_DECODE_STATE_GOT_TYPE:
_rxpacket.st24_data[_rxlen - 1] = byte;
_rxlen++;
if (_rxlen == (_rxpacket.length - 1)) {
_decode_state = ST24_DECODE_STATE_GOT_DATA;
}
break;
case ST24_DECODE_STATE_GOT_DATA:
_rxpacket.crc8 = byte;
_rxlen++;
if (st24_common_crc8((uint8_t *) & (_rxpacket.length), _rxlen) == _rxpacket.crc8) {
ret = 0;
/* decode the actual packet */
switch (_rxpacket.type) {
case ST24_PACKET_TYPE_CHANNELDATA12: {
ChannelData12 *d = (ChannelData12 *)_rxpacket.st24_data;
*rssi = d->rssi;
*rx_count = d->packet_count;
/* this can lead to rounding of the strides */
*channel_count = (max_chan_count < 12) ? max_chan_count : 12;
unsigned stride_count = (*channel_count * 3) / 2;
unsigned chan_index = 0;
for (unsigned i = 0; i < stride_count; i += 3) {
channels[chan_index] = ((uint16_t)d->channel[i] << 4);
channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4);
/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
chan_index++;
channels[chan_index] = ((uint16_t)d->channel[i + 2]);
channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8);
/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
chan_index++;
}
}
break;
case ST24_PACKET_TYPE_CHANNELDATA24: {
ChannelData24 *d = (ChannelData24 *)&_rxpacket.st24_data;
*rssi = d->rssi;
*rx_count = d->packet_count;
/* this can lead to rounding of the strides */
*channel_count = (max_chan_count < 24) ? max_chan_count : 24;
unsigned stride_count = (*channel_count * 3) / 2;
unsigned chan_index = 0;
for (unsigned i = 0; i < stride_count; i += 3) {
channels[chan_index] = ((uint16_t)d->channel[i] << 4);
channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4);
/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
chan_index++;
channels[chan_index] = ((uint16_t)d->channel[i + 2]);
channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8);
/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
chan_index++;
}
}
break;
case ST24_PACKET_TYPE_TRANSMITTERGPSDATA: {
// ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket.st24_data;
/* we silently ignore this data for now, as it is unused */
ret = 2;
}
break;
default:
ret = 2;
break;
}
} else {
/* decoding failed */
ret = 4;
}
_decode_state = ST24_DECODE_STATE_UNSYNCED;
break;
}
return ret;
}
src/lib/rc/st24.h
+163
View File
@@ -0,0 +1,163 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file st24.h
*
* RC protocol definition for Yuneec ST24 transmitter
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#pragma once
#include <stdint.h>
__BEGIN_DECLS
#define ST24_DATA_LEN_MAX 64
#define ST24_STX1 0x55
#define ST24_STX2 0x55
enum ST24_PACKET_TYPE {
ST24_PACKET_TYPE_CHANNELDATA12 = 0,
ST24_PACKET_TYPE_CHANNELDATA24,
ST24_PACKET_TYPE_TRANSMITTERGPSDATA
};
#pragma pack(push, 1)
typedef struct {
uint8_t header1; ///< 0x55 for a valid packet
uint8_t header2; ///< 0x55 for a valid packet
uint8_t length; ///< length includes type, data, and crc = sizeof(type)+sizeof(data[payload_len])+sizeof(crc8)
uint8_t type; ///< from enum ST24_PACKET_TYPE
uint8_t st24_data[ST24_DATA_LEN_MAX];
uint8_t crc8; ///< crc8 checksum, calculated by st24_common_crc8 and including fields length, type and st24_data
} ReceiverFcPacket;
/**
* RC Channel data (12 channels).
*
* This is incoming from the ST24
*/
typedef struct {
uint16_t t; ///< packet counter or clock
uint8_t rssi; ///< signal strength
uint8_t packet_count; ///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate)
uint8_t channel[18]; ///< channel data, 12 channels (12 bit numbers)
} ChannelData12;
/**
* RC Channel data (12 channels).
*
*/
typedef struct {
uint16_t t; ///< packet counter or clock
uint8_t rssi; ///< signal strength
uint8_t packet_count; ///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate)
uint8_t channel[36]; ///< channel data, 24 channels (12 bit numbers)
} ChannelData24;
/**
* Telemetry packet
*
* This is outgoing to the ST24
*
* imuStatus:
* 8 bit total
* bits 0-2 for status
* - value 0 is FAILED
* - value 1 is INITIALIZING
* - value 2 is RUNNING
* - values 3 through 7 are reserved
* bits 3-7 are status for sensors (0 or 1)
* - mpu6050
* - accelerometer
* - primary gyro x
* - primary gyro y
* - primary gyro z
*
* pressCompassStatus
* 8 bit total
* bits 0-3 for compass status
* - value 0 is FAILED
* - value 1 is INITIALIZING
* - value 2 is RUNNING
* - value 3 - 15 are reserved
* bits 4-7 for pressure status
* - value 0 is FAILED
* - value 1 is INITIALIZING
* - value 2 is RUNNING
* - value 3 - 15 are reserved
*
*/
typedef struct {
uint16_t t; ///< packet counter or clock
int32_t lat; ///< lattitude (degrees) +/- 90 deg
int32_t lon; ///< longitude (degrees) +/- 180 deg
int32_t alt; ///< 0.01m resolution, altitude (meters)
int16_t vx, vy, vz; ///< velocity 0.01m res, +/-320.00 North-East- Down
uint8_t nsat; ///<number of satellites
uint8_t voltage; ///< 25.4V voltage = 5 + 255*0.1 = 30.5V, min=5V
uint8_t current; ///< 0.5A resolution
int16_t roll, pitch, yaw; ///< 0.01 degree resolution
uint8_t motorStatus; ///< 1 bit per motor for status 1=good, 0= fail
uint8_t imuStatus; ///< inertial measurement unit status
uint8_t pressCompassStatus; ///< baro / compass status
} TelemetryData;
#pragma pack(pop)
/**
* CRC8 implementation for ST24 protocol
*
* @param prt Pointer to the data to CRC
* @param len number of bytes to accumulate in the checksum
* @return the checksum of these bytes over len
*/
uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len);
/**
* Decoder for ST24 protocol
*
* @param byte current char to read
* @param rssi pointer to a byte where the RSSI value is written back to
* @param rx_count pointer to a byte where the receive count of packets signce last wireless frame is written back to
* @param channels pointer to a datastructure of size max_chan_count where channel values (12 bit) are written back to
* @param max_chan_count maximum channels to decode - if more channels are decoded, the last n are skipped and success (0) is returned
* @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 2 for unknown packet, 3 for out of sync, 4 for checksum error
*/
__EXPORT int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count,
uint16_t *channels, uint16_t max_chan_count);
__END_DECLS
src/modules/bottle_drop/bottle_drop.cpp
+907
View File
@@ -0,0 +1,907 @@
/****************************************************************************
*
* Copyright (c) 2013, 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 bottle_drop.cpp
*
* Bottle drop module for Outback Challenge 2014, Team Swiss Fang
*
* @author Dominik Juchli <juchlid@ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <sys/ioctl.h>
#include <drivers/device/device.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/parameter_update.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <dataman/dataman.h>
#include <mathlib/mathlib.h>
#include <mavlink/mavlink_log.h>
/**
* bottle_drop app start / stop handling function
*
* @ingroup apps
*/
extern "C" __EXPORT int bottle_drop_main(int argc, char *argv[]);
class BottleDrop
{
public:
/**
* Constructor
*/
BottleDrop();
/**
* Destructor, also kills task.
*/
~BottleDrop();
/**
* Start the task.
*
* @return OK on success.
*/
int start();
/**
* Display status.
*/
void status();
void open_bay();
void close_bay();
void drop();
void lock_release();
private:
bool _task_should_exit; /**< if true, task should exit */
int _main_task; /**< handle for task */
int _mavlink_fd;
int _command_sub;
int _wind_estimate_sub;
struct vehicle_command_s _command;
struct vehicle_global_position_s _global_pos;
map_projection_reference_s ref;
orb_advert_t _actuator_pub;
struct actuator_controls_s _actuators;
bool _drop_approval;
hrt_abstime _doors_opened;
hrt_abstime _drop_time;
float _alt_clearance;
struct position_s {
double lat; ///< degrees
double lon; ///< degrees
float alt; ///< m
} _target_position, _drop_position;
enum DROP_STATE {
DROP_STATE_INIT = 0,
DROP_STATE_TARGET_VALID,
DROP_STATE_TARGET_SET,
DROP_STATE_BAY_OPEN,
DROP_STATE_DROPPED,
DROP_STATE_BAY_CLOSED
} _drop_state;
struct mission_s _onboard_mission;
orb_advert_t _onboard_mission_pub;
void task_main();
void handle_command(struct vehicle_command_s *cmd);
void answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESULT result);
/**
* Set the actuators
*/
int actuators_publish();
/**
* Shim for calling task_main from task_create.
*/
static void task_main_trampoline(int argc, char *argv[]);
};
namespace bottle_drop
{
BottleDrop *g_bottle_drop;
}
BottleDrop::BottleDrop() :
_task_should_exit(false),
_main_task(-1),
_mavlink_fd(-1),
_command_sub(-1),
_wind_estimate_sub(-1),
_command {},
_global_pos {},
ref {},
_actuator_pub(-1),
_actuators {},
_drop_approval(false),
_doors_opened(0),
_drop_time(0),
_alt_clearance(70.0f),
_target_position {},
_drop_position {},
_drop_state(DROP_STATE_INIT),
_onboard_mission {},
_onboard_mission_pub(-1)
{
}
BottleDrop::~BottleDrop()
{
if (_main_task != -1) {
/* task wakes up every 100ms or so at the longest */
_task_should_exit = true;
/* wait for a second for the task to quit at our request */
unsigned i = 0;
do {
/* wait 20ms */
usleep(20000);
/* if we have given up, kill it */
if (++i > 50) {
task_delete(_main_task);
break;
}
} while (_main_task != -1);
}
bottle_drop::g_bottle_drop = nullptr;
}
int
BottleDrop::start()
{
ASSERT(_main_task == -1);
/* start the task */
_main_task = task_spawn_cmd("bottle_drop",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT + 15,
2048,
(main_t)&BottleDrop::task_main_trampoline,
nullptr);
if (_main_task < 0) {
warn("task start failed");
return -errno;
}
return OK;
}
void
BottleDrop::status()
{
warnx("drop state: %d", _drop_state);
}
void
BottleDrop::open_bay()
{
_actuators.control[0] = -1.0f;
_actuators.control[1] = 1.0f;
if (_doors_opened == 0) {
_doors_opened = hrt_absolute_time();
}
warnx("open doors");
actuators_publish();
usleep(500 * 1000);
}
void
BottleDrop::close_bay()
{
// closed door and locked survival kit
_actuators.control[0] = 1.0f;
_actuators.control[1] = -1.0f;
_doors_opened = 0;
actuators_publish();
// delay until the bay is closed
usleep(500 * 1000);
}
void
BottleDrop::drop()
{
// update drop actuator, wait 0.5s until the doors are open before dropping
hrt_abstime starttime = hrt_absolute_time();
// force the door open if we have to
if (_doors_opened == 0) {
open_bay();
warnx("bay not ready, forced open");
}
while (hrt_elapsed_time(&_doors_opened) < 500 * 1000 && hrt_elapsed_time(&starttime) < 2000000) {
usleep(50000);
warnx("delayed by door!");
}
_actuators.control[2] = 1.0f;
_drop_time = hrt_absolute_time();
actuators_publish();
warnx("dropping now");
// Give it time to drop
usleep(1000 * 1000);
}
void
BottleDrop::lock_release()
{
_actuators.control[2] = -1.0f;
actuators_publish();
warnx("closing release");
}
int
BottleDrop::actuators_publish()
{
_actuators.timestamp = hrt_absolute_time();
// lazily publish _actuators only once available
if (_actuator_pub > 0) {
return orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &_actuators);
} else {
_actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators);
if (_actuator_pub > 0) {
return OK;
} else {
return -1;
}
}
}
void
BottleDrop::task_main()
{
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(_mavlink_fd, "[bottle_drop] started");
_command_sub = orb_subscribe(ORB_ID(vehicle_command));
_wind_estimate_sub = orb_subscribe(ORB_ID(wind_estimate));
bool updated = false;
float z_0; // ground properties
float turn_radius; // turn radius of the UAV
float precision; // Expected precision of the UAV
float ground_distance = _alt_clearance; // Replace by closer estimate in loop
// constant
float g = CONSTANTS_ONE_G; // constant of gravity [m/s^2]
float m = 0.5f; // mass of bottle [kg]
float rho = 1.2f; // air density [kg/m^3]
float A = ((0.063f * 0.063f) / 4.0f * M_PI_F); // Bottle cross section [m^2]
float dt_freefall_prediction = 0.01f; // step size of the free fall prediction [s]
// Has to be estimated by experiment
float cd = 0.86f; // Drag coefficient for a cylinder with a d/l ratio of 1/3 []
float t_signal =
0.084f; // Time span between sending the signal and the bottle top reaching level height with the bottom of the plane [s]
float t_door =
0.7f; // The time the system needs to open the door + safety, is also the time the palyload needs to safely escape the shaft [s]
// Definition
float h_0; // height over target
float az; // acceleration in z direction[m/s^2]
float vz; // velocity in z direction [m/s]
float z; // fallen distance [m]
float h; // height over target [m]
float ax; // acceleration in x direction [m/s^2]
float vx; // ground speed in x direction [m/s]
float x; // traveled distance in x direction [m]
float vw; // wind speed [m/s]
float vrx; // relative velocity in x direction [m/s]
float v; // relative speed vector [m/s]
float Fd; // Drag force [N]
float Fdx; // Drag force in x direction [N]
float Fdz; // Drag force in z direction [N]
float x_drop, y_drop; // coordinates of the drop point in reference to the target (projection of NED)
float x_t, y_t; // coordinates of the target in reference to the target x_t = 0, y_t = 0 (projection of NED)
float x_l, y_l; // local position in projected coordinates
float x_f, y_f; // to-be position of the UAV after dt_runs seconds in projected coordinates
double x_f_NED, y_f_NED; // to-be position of the UAV after dt_runs seconds in NED
float distance_open_door; // The distance the UAV travels during its doors open [m]
float approach_error = 0.0f; // The error in radians between current ground vector and desired ground vector
float distance_real = 0; // The distance between the UAVs position and the drop point [m]
float future_distance = 0; // The distance between the UAVs to-be position and the drop point [m]
unsigned counter = 0;
param_t param_gproperties = param_find("BD_GPROPERTIES");
param_t param_turn_radius = param_find("BD_TURNRADIUS");
param_t param_precision = param_find("BD_PRECISION");
param_t param_cd = param_find("BD_OBJ_CD");
param_t param_mass = param_find("BD_OBJ_MASS");
param_t param_surface = param_find("BD_OBJ_SURFACE");
param_get(param_precision, &precision);
param_get(param_turn_radius, &turn_radius);
param_get(param_gproperties, &z_0);
param_get(param_cd, &cd);
param_get(param_mass, &m);
param_get(param_surface, &A);
int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
struct parameter_update_s update;
memset(&update, 0, sizeof(update));
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
struct mission_item_s flight_vector_s {};
struct mission_item_s flight_vector_e {};
flight_vector_s.nav_cmd = NAV_CMD_WAYPOINT;
flight_vector_s.acceptance_radius = 50; // TODO: make parameter
flight_vector_s.autocontinue = true;
flight_vector_s.altitude_is_relative = false;
flight_vector_e.nav_cmd = NAV_CMD_WAYPOINT;
flight_vector_e.acceptance_radius = 50; // TODO: make parameter
flight_vector_e.autocontinue = true;
flight_vector_s.altitude_is_relative = false;
struct wind_estimate_s wind;
// wakeup source(s)
struct pollfd fds[1];
// Setup of loop
fds[0].fd = _command_sub;
fds[0].events = POLLIN;
// Whatever state the bay is in, we want it closed on startup
lock_release();
close_bay();
while (!_task_should_exit) {
/* wait for up to 100ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50);
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("poll error %d, %d", pret, errno);
continue;
}
/* vehicle commands updated */
if (fds[0].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_command), _command_sub, &_command);
handle_command(&_command);
}
orb_check(vehicle_global_position_sub, &updated);
if (updated) {
/* copy global position */
orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos);
}
if (_global_pos.timestamp == 0) {
continue;
}
const unsigned sleeptime_us = 9500;
hrt_abstime last_run = hrt_absolute_time();
float dt_runs = sleeptime_us / 1e6f;
// switch to faster updates during the drop
while (_drop_state > DROP_STATE_INIT) {
// Get wind estimate
orb_check(_wind_estimate_sub, &updated);
if (updated) {
orb_copy(ORB_ID(wind_estimate), _wind_estimate_sub, &wind);
}
// Get vehicle position
orb_check(vehicle_global_position_sub, &updated);
if (updated) {
// copy global position
orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos);
}
// Get parameter updates
orb_check(parameter_update_sub, &updated);
if (updated) {
// copy global position
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
// update all parameters
param_get(param_gproperties, &z_0);
param_get(param_turn_radius, &turn_radius);
param_get(param_precision, &precision);
}
orb_check(_command_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_command), _command_sub, &_command);
handle_command(&_command);
}
float windspeed_norm = sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east);
float groundspeed_body = sqrtf(_global_pos.vel_n * _global_pos.vel_n + _global_pos.vel_e * _global_pos.vel_e);
ground_distance = _global_pos.alt - _target_position.alt;
// Distance to drop position and angle error to approach vector
// are relevant in all states greater than target valid (which calculates these positions)
if (_drop_state > DROP_STATE_TARGET_VALID) {
distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, _drop_position.lon));
float ground_direction = atan2f(_global_pos.vel_e, _global_pos.vel_n);
float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon);
approach_error = _wrap_pi(ground_direction - approach_direction);
if (counter % 90 == 0) {
mavlink_log_info(_mavlink_fd, "drop distance %u, heading error %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error));
}
}
switch (_drop_state) {
case DROP_STATE_TARGET_VALID:
{
az = g; // acceleration in z direction[m/s^2]
vz = 0; // velocity in z direction [m/s]
z = 0; // fallen distance [m]
h_0 = _global_pos.alt - _target_position.alt; // height over target at start[m]
h = h_0; // height over target [m]
ax = 0; // acceleration in x direction [m/s^2]
vx = groundspeed_body;// XXX project // ground speed in x direction [m/s]
x = 0; // traveled distance in x direction [m]
vw = 0; // wind speed [m/s]
vrx = 0; // relative velocity in x direction [m/s]
v = groundspeed_body; // relative speed vector [m/s]
Fd = 0; // Drag force [N]
Fdx = 0; // Drag force in x direction [N]
Fdz = 0; // Drag force in z direction [N]
// Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x
while (h > 0.05f) {
// z-direction
vz = vz + az * dt_freefall_prediction;
z = z + vz * dt_freefall_prediction;
h = h_0 - z;
// x-direction
vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0);
vx = vx + ax * dt_freefall_prediction;
x = x + vx * dt_freefall_prediction;
vrx = vx + vw;
// drag force
v = sqrtf(vz * vz + vrx * vrx);
Fd = 0.5f * rho * A * cd * (v * v);
Fdx = Fd * vrx / v;
Fdz = Fd * vz / v;
// acceleration
az = g - Fdz / m;
ax = -Fdx / m;
}
// compute drop vector
x = groundspeed_body * t_signal + x;
x_t = 0.0f;
y_t = 0.0f;
float wind_direction_n, wind_direction_e;
if (windspeed_norm < 0.5f) { // If there is no wind, an arbitrarily direction is chosen
wind_direction_n = 1.0f;
wind_direction_e = 0.0f;
} else {
wind_direction_n = wind.windspeed_north / windspeed_norm;
wind_direction_e = wind.windspeed_east / windspeed_norm;
}
x_drop = x_t + x * wind_direction_n;
y_drop = y_t + x * wind_direction_e;
map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon);
_drop_position.alt = _target_position.alt + _alt_clearance;
// Compute flight vector
map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_e,
&(flight_vector_s.lat), &(flight_vector_s.lon));
flight_vector_s.altitude = _drop_position.alt;
map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_n, y_drop - turn_radius * wind_direction_e,
&flight_vector_e.lat, &flight_vector_e.lon);
flight_vector_e.altitude = _drop_position.alt;
// Save WPs in datamanager
const ssize_t len = sizeof(struct mission_item_s);
if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) {
warnx("ERROR: could not save onboard WP");
}
if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 1, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_e, len) != len) {
warnx("ERROR: could not save onboard WP");
}
_onboard_mission.count = 2;
_onboard_mission.current_seq = 0;
if (_onboard_mission_pub > 0) {
orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
} else {
_onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &_onboard_mission);
}
float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon);
mavlink_log_critical(_mavlink_fd, "position set, approach heading: %u", (unsigned)distance_real, (unsigned)math::degrees(approach_direction + M_PI_F));
_drop_state = DROP_STATE_TARGET_SET;
}
break;
case DROP_STATE_TARGET_SET:
{
float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon);
if (distance_wp2 < distance_real) {
_onboard_mission.current_seq = 0;
orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
} else {
// We're close enough - open the bay
distance_open_door = math::max(10.0f, 3.0f * fabsf(t_door * groundspeed_body));
if (isfinite(distance_real) && distance_real < distance_open_door &&
fabsf(approach_error) < math::radians(20.0f)) {
open_bay();
_drop_state = DROP_STATE_BAY_OPEN;
mavlink_log_info(_mavlink_fd, "#audio: opening bay");
}
}
}
break;
case DROP_STATE_BAY_OPEN:
{
if (_drop_approval) {
map_projection_project(&ref, _global_pos.lat, _global_pos.lon, &x_l, &y_l);
x_f = x_l + _global_pos.vel_n * dt_runs;
y_f = y_l + _global_pos.vel_e * dt_runs;
map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED);
future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon);
if (isfinite(distance_real) &&
(distance_real < precision) && ((distance_real < future_distance))) {
drop();
_drop_state = DROP_STATE_DROPPED;
mavlink_log_info(_mavlink_fd, "#audio: payload dropped");
} else {
float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon);
if (distance_wp2 < distance_real) {
_onboard_mission.current_seq = 0;
orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
}
}
}
}
break;
case DROP_STATE_DROPPED:
/* 2s after drop, reset and close everything again */
if ((hrt_elapsed_time(&_doors_opened) > 2 * 1000 * 1000)) {
_drop_state = DROP_STATE_INIT;
_drop_approval = false;
lock_release();
close_bay();
mavlink_log_info(_mavlink_fd, "#audio: closing bay");
// remove onboard mission
_onboard_mission.current_seq = -1;
_onboard_mission.count = 0;
orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
}
break;
}
counter++;
// update_actuators();
// run at roughly 100 Hz
usleep(sleeptime_us);
dt_runs = hrt_elapsed_time(&last_run) / 1e6f;
last_run = hrt_absolute_time();
}
}
warnx("exiting.");
_main_task = -1;
_exit(0);
}
void
BottleDrop::handle_command(struct vehicle_command_s *cmd)
{
switch (cmd->command) {
case VEHICLE_CMD_CUSTOM_0:
/*
* param1 and param2 set to 1: open and drop
* param1 set to 1: open
* else: close (and don't drop)
*/
if (cmd->param1 > 0.5f && cmd->param2 > 0.5f) {
open_bay();
drop();
mavlink_log_info(_mavlink_fd, "#audio: drop bottle");
} else if (cmd->param1 > 0.5f) {
open_bay();
mavlink_log_info(_mavlink_fd, "#audio: opening bay");
} else {
lock_release();
close_bay();
mavlink_log_info(_mavlink_fd, "#audio: closing bay");
}
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
break;
case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY:
switch ((int)(cmd->param1 + 0.5f)) {
case 0:
_drop_approval = false;
mavlink_log_info(_mavlink_fd, "#audio: got drop position, no approval");
break;
case 1:
_drop_approval = true;
mavlink_log_info(_mavlink_fd, "#audio: got drop position and approval");
break;
default:
_drop_approval = false;
warnx("param1 val unknown");
break;
}
// XXX check all fields (2-3)
_alt_clearance = cmd->param4;
_target_position.lat = cmd->param5;
_target_position.lon = cmd->param6;
_target_position.alt = cmd->param7;
_drop_state = DROP_STATE_TARGET_VALID;
mavlink_log_info(_mavlink_fd, "got target: %8.4f, %8.4f, %8.4f", (double)_target_position.lat,
(double)_target_position.lon, (double)_target_position.alt);
map_projection_init(&ref, _target_position.lat, _target_position.lon);
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
break;
case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY:
if (cmd->param1 < 0) {
// Clear internal states
_drop_approval = false;
_drop_state = DROP_STATE_INIT;
// Abort if mission is present
_onboard_mission.current_seq = -1;
if (_onboard_mission_pub > 0) {
orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
}
} else {
switch ((int)(cmd->param1 + 0.5f)) {
case 0:
_drop_approval = false;
break;
case 1:
_drop_approval = true;
mavlink_log_info(_mavlink_fd, "#audio: got drop approval");
break;
default:
_drop_approval = false;
break;
// XXX handle other values
}
}
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
break;
default:
break;
}
}
void
BottleDrop::answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESULT result)
{
switch (result) {
case VEHICLE_CMD_RESULT_ACCEPTED:
break;
case VEHICLE_CMD_RESULT_DENIED:
mavlink_log_critical(_mavlink_fd, "#audio: command denied: %u", cmd->command);
break;
case VEHICLE_CMD_RESULT_FAILED:
mavlink_log_critical(_mavlink_fd, "#audio: command failed: %u", cmd->command);
break;
case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
mavlink_log_critical(_mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command);
break;
case VEHICLE_CMD_RESULT_UNSUPPORTED:
mavlink_log_critical(_mavlink_fd, "#audio: command unsupported: %u", cmd->command);
break;
default:
break;
}
}
void
BottleDrop::task_main_trampoline(int argc, char *argv[])
{
bottle_drop::g_bottle_drop->task_main();
}
static void usage()
{
errx(1, "usage: bottle_drop {start|stop|status}");
}
int bottle_drop_main(int argc, char *argv[])
{
if (argc < 2) {
usage();
}
if (!strcmp(argv[1], "start")) {
if (bottle_drop::g_bottle_drop != nullptr) {
errx(1, "already running");
}
bottle_drop::g_bottle_drop = new BottleDrop;
if (bottle_drop::g_bottle_drop == nullptr) {
errx(1, "alloc failed");
}
if (OK != bottle_drop::g_bottle_drop->start()) {
delete bottle_drop::g_bottle_drop;
bottle_drop::g_bottle_drop = nullptr;
err(1, "start failed");
}
return 0;
}
if (bottle_drop::g_bottle_drop == nullptr) {
errx(1, "not running");
}
if (!strcmp(argv[1], "stop")) {
delete bottle_drop::g_bottle_drop;
bottle_drop::g_bottle_drop = nullptr;
} else if (!strcmp(argv[1], "status")) {
bottle_drop::g_bottle_drop->status();
} else if (!strcmp(argv[1], "drop")) {
bottle_drop::g_bottle_drop->drop();
} else if (!strcmp(argv[1], "open")) {
bottle_drop::g_bottle_drop->open_bay();
} else if (!strcmp(argv[1], "close")) {
bottle_drop::g_bottle_drop->close_bay();
} else if (!strcmp(argv[1], "lock")) {
bottle_drop::g_bottle_drop->lock_release();
} else {
usage();
}
return 0;
}

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