mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-26 09:26:25 +08:00
Moved the bulk of sensor drivers to the new world
This commit is contained in:
Lorenz Meier
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,40 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# BlinkM I2C LED driver
|
||||
#
|
||||
|
||||
MODULE_COMMAND = blinkm
|
||||
|
||||
SRCS = blinkm.cpp
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,40 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# Makefile to build the BMA180 driver.
|
||||
#
|
||||
|
||||
MODULE_COMMAND = bma180
|
||||
|
||||
SRCS = bma180.cpp
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,92 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* Julian Oes <joes@student.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <termios.h>
|
||||
#include <errno.h>
|
||||
#include <systemlib/err.h>
|
||||
#include "gps_helper.h"
|
||||
|
||||
/* @file gps_helper.cpp */
|
||||
|
||||
int
|
||||
GPS_Helper::set_baudrate(const int &fd, unsigned baud)
|
||||
{
|
||||
/* process baud rate */
|
||||
int speed;
|
||||
|
||||
switch (baud) {
|
||||
case 9600: speed = B9600; break;
|
||||
|
||||
case 19200: speed = B19200; break;
|
||||
|
||||
case 38400: speed = B38400; break;
|
||||
|
||||
case 57600: speed = B57600; break;
|
||||
|
||||
case 115200: speed = B115200; break;
|
||||
|
||||
warnx("try baudrate: %d\n", speed);
|
||||
|
||||
default:
|
||||
warnx("ERROR: Unsupported baudrate: %d\n", baud);
|
||||
return -EINVAL;
|
||||
}
|
||||
struct termios uart_config;
|
||||
int termios_state;
|
||||
|
||||
/* fill the struct for the new configuration */
|
||||
tcgetattr(fd, &uart_config);
|
||||
|
||||
/* clear ONLCR flag (which appends a CR for every LF) */
|
||||
uart_config.c_oflag &= ~ONLCR;
|
||||
/* no parity, one stop bit */
|
||||
uart_config.c_cflag &= ~(CSTOPB | PARENB);
|
||||
|
||||
/* set baud rate */
|
||||
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
|
||||
warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state);
|
||||
return -1;
|
||||
}
|
||||
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
|
||||
warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state);
|
||||
return -1;
|
||||
}
|
||||
if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
|
||||
warnx("ERROR setting baudrate (tcsetattr)\n");
|
||||
return -1;
|
||||
}
|
||||
/* XXX if resetting the parser here, ensure it does exist (check for null pointer) */
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,52 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* Julian Oes <joes@student.ethz.ch>
|
||||
*
|
||||
* 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 gps_helper.h */
|
||||
|
||||
#ifndef GPS_HELPER_H
|
||||
#define GPS_HELPER_H
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
|
||||
class GPS_Helper
|
||||
{
|
||||
public:
|
||||
virtual int configure(unsigned &baud) = 0;
|
||||
virtual int receive(unsigned timeout) = 0;
|
||||
int set_baudrate(const int &fd, unsigned baud);
|
||||
};
|
||||
|
||||
#endif /* GPS_HELPER_H */
|
||||
@@ -0,0 +1,43 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# GPS driver
|
||||
#
|
||||
|
||||
MODULE_COMMAND = gps
|
||||
|
||||
SRCS = gps.cpp \
|
||||
gps_helper.cpp \
|
||||
mtk.cpp \
|
||||
ubx.cpp
|
||||
@@ -0,0 +1,274 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* Julian Oes <joes@student.ethz.ch>
|
||||
*
|
||||
* 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 mkt.cpp */
|
||||
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <poll.h>
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
#include <assert.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include "mtk.h"
|
||||
|
||||
|
||||
MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) :
|
||||
_fd(fd),
|
||||
_gps_position(gps_position),
|
||||
_mtk_revision(0)
|
||||
{
|
||||
decode_init();
|
||||
}
|
||||
|
||||
MTK::~MTK()
|
||||
{
|
||||
}
|
||||
|
||||
int
|
||||
MTK::configure(unsigned &baudrate)
|
||||
{
|
||||
/* set baudrate first */
|
||||
if (GPS_Helper::set_baudrate(_fd, MTK_BAUDRATE) != 0)
|
||||
return -1;
|
||||
|
||||
baudrate = MTK_BAUDRATE;
|
||||
|
||||
/* Write config messages, don't wait for an answer */
|
||||
if (strlen(MTK_OUTPUT_5HZ) != write(_fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) {
|
||||
warnx("mtk: config write failed");
|
||||
return -1;
|
||||
}
|
||||
usleep(10000);
|
||||
|
||||
if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) {
|
||||
warnx("mtk: config write failed");
|
||||
return -1;
|
||||
}
|
||||
usleep(10000);
|
||||
|
||||
if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) {
|
||||
warnx("mtk: config write failed");
|
||||
return -1;
|
||||
}
|
||||
usleep(10000);
|
||||
|
||||
if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) {
|
||||
warnx("mtk: config write failed");
|
||||
return -1;
|
||||
}
|
||||
usleep(10000);
|
||||
|
||||
if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
|
||||
warnx("mtk: config write failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int
|
||||
MTK::receive(unsigned timeout)
|
||||
{
|
||||
/* poll descriptor */
|
||||
pollfd fds[1];
|
||||
fds[0].fd = _fd;
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
uint8_t buf[32];
|
||||
gps_mtk_packet_t packet;
|
||||
|
||||
/* timeout additional to poll */
|
||||
uint64_t time_started = hrt_absolute_time();
|
||||
|
||||
int j = 0;
|
||||
ssize_t count = 0;
|
||||
|
||||
while (true) {
|
||||
|
||||
/* first read whatever is left */
|
||||
if (j < count) {
|
||||
/* pass received bytes to the packet decoder */
|
||||
while (j < count) {
|
||||
if (parse_char(buf[j], packet) > 0) {
|
||||
handle_message(packet);
|
||||
return 1;
|
||||
}
|
||||
/* in case we keep trying but only get crap from GPS */
|
||||
if (time_started + timeout*1000 < hrt_absolute_time() ) {
|
||||
return -1;
|
||||
}
|
||||
j++;
|
||||
}
|
||||
/* everything is read */
|
||||
j = count = 0;
|
||||
}
|
||||
|
||||
/* then poll for new data */
|
||||
int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout);
|
||||
|
||||
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...
|
||||
*/
|
||||
count = ::read(_fd, buf, sizeof(buf));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MTK::decode_init(void)
|
||||
{
|
||||
_rx_ck_a = 0;
|
||||
_rx_ck_b = 0;
|
||||
_rx_count = 0;
|
||||
_decode_state = MTK_DECODE_UNINIT;
|
||||
}
|
||||
int
|
||||
MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
|
||||
{
|
||||
int ret = 0;
|
||||
|
||||
if (_decode_state == MTK_DECODE_UNINIT) {
|
||||
|
||||
if (b == MTK_SYNC1_V16) {
|
||||
_decode_state = MTK_DECODE_GOT_CK_A;
|
||||
_mtk_revision = 16;
|
||||
} else if (b == MTK_SYNC1_V19) {
|
||||
_decode_state = MTK_DECODE_GOT_CK_A;
|
||||
_mtk_revision = 19;
|
||||
}
|
||||
|
||||
} else if (_decode_state == MTK_DECODE_GOT_CK_A) {
|
||||
if (b == MTK_SYNC2) {
|
||||
_decode_state = MTK_DECODE_GOT_CK_B;
|
||||
|
||||
} else {
|
||||
// Second start symbol was wrong, reset state machine
|
||||
decode_init();
|
||||
}
|
||||
|
||||
} else if (_decode_state == MTK_DECODE_GOT_CK_B) {
|
||||
// Add to checksum
|
||||
if (_rx_count < 33)
|
||||
add_byte_to_checksum(b);
|
||||
|
||||
// Fill packet buffer
|
||||
((uint8_t*)(&packet))[_rx_count] = b;
|
||||
_rx_count++;
|
||||
|
||||
/* Packet size minus checksum, XXX ? */
|
||||
if (_rx_count >= sizeof(packet)) {
|
||||
/* Compare checksum */
|
||||
if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) {
|
||||
ret = 1;
|
||||
} else {
|
||||
warnx("MTK Checksum invalid");
|
||||
ret = -1;
|
||||
}
|
||||
// Reset state machine to decode next packet
|
||||
decode_init();
|
||||
}
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
void
|
||||
MTK::handle_message(gps_mtk_packet_t &packet)
|
||||
{
|
||||
if (_mtk_revision == 16) {
|
||||
_gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7
|
||||
_gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7
|
||||
} else if (_mtk_revision == 19) {
|
||||
_gps_position->lat = packet.latitude; // both degrees*1e7
|
||||
_gps_position->lon = packet.longitude; // both degrees*1e7
|
||||
} else {
|
||||
warnx("mtk: unknown revision");
|
||||
_gps_position->lat = 0;
|
||||
_gps_position->lon = 0;
|
||||
}
|
||||
_gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
|
||||
_gps_position->fix_type = packet.fix_type;
|
||||
_gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit
|
||||
_gps_position->epv_m = 0.0; //unknown in mtk custom mode
|
||||
_gps_position->vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s
|
||||
_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
|
||||
_gps_position->satellites_visible = packet.satellites;
|
||||
|
||||
/* convert time and date information to unix timestamp */
|
||||
struct tm timeinfo; //TODO: test this conversion
|
||||
uint32_t timeinfo_conversion_temp;
|
||||
|
||||
timeinfo.tm_mday = packet.date * 1e-4;
|
||||
timeinfo_conversion_temp = packet.date - timeinfo.tm_mday * 1e4;
|
||||
timeinfo.tm_mon = timeinfo_conversion_temp * 1e-2 - 1;
|
||||
timeinfo.tm_year = (timeinfo_conversion_temp - (timeinfo.tm_mon + 1) * 1e2) + 100;
|
||||
|
||||
timeinfo.tm_hour = packet.utc_time * 1e-7;
|
||||
timeinfo_conversion_temp = packet.utc_time - timeinfo.tm_hour * 1e7;
|
||||
timeinfo.tm_min = timeinfo_conversion_temp * 1e-5;
|
||||
timeinfo_conversion_temp -= timeinfo.tm_min * 1e5;
|
||||
timeinfo.tm_sec = timeinfo_conversion_temp * 1e-3;
|
||||
timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3;
|
||||
time_t epoch = mktime(&timeinfo);
|
||||
|
||||
_gps_position->time_gps_usec = epoch * 1e6; //TODO: test this
|
||||
_gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3;
|
||||
_gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time();
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
void
|
||||
MTK::add_byte_to_checksum(uint8_t b)
|
||||
{
|
||||
_rx_ck_a = _rx_ck_a + b;
|
||||
_rx_ck_b = _rx_ck_b + _rx_ck_a;
|
||||
}
|
||||
@@ -0,0 +1,124 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* Julian Oes <joes@student.ethz.ch>
|
||||
*
|
||||
* 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 mtk.h */
|
||||
|
||||
#ifndef MTK_H_
|
||||
#define MTK_H_
|
||||
|
||||
#include "gps_helper.h"
|
||||
|
||||
#define MTK_SYNC1_V16 0xd0
|
||||
#define MTK_SYNC1_V19 0xd1
|
||||
#define MTK_SYNC2 0xdd
|
||||
|
||||
#define MTK_OUTPUT_5HZ "$PMTK220,200*2C\r\n"
|
||||
#define MTK_SET_BINARY "$PGCMD,16,0,0,0,0,0*6A\r\n"
|
||||
#define SBAS_ON "$PMTK313,1*2E\r\n"
|
||||
#define WAAS_ON "$PMTK301,2*2E\r\n"
|
||||
#define MTK_NAVTHRES_OFF "$PMTK397,0*23\r\n"
|
||||
|
||||
#define MTK_TIMEOUT_5HZ 400
|
||||
#define MTK_BAUDRATE 38400
|
||||
|
||||
typedef enum {
|
||||
MTK_DECODE_UNINIT = 0,
|
||||
MTK_DECODE_GOT_CK_A = 1,
|
||||
MTK_DECODE_GOT_CK_B = 2
|
||||
} mtk_decode_state_t;
|
||||
|
||||
/** the structures of the binary packets */
|
||||
#pragma pack(push, 1)
|
||||
|
||||
typedef struct {
|
||||
uint8_t payload; ///< Number of payload bytes
|
||||
int32_t latitude; ///< Latitude in degrees * 10^7
|
||||
int32_t longitude; ///< Longitude in degrees * 10^7
|
||||
uint32_t msl_altitude; ///< MSL altitude in meters * 10^2
|
||||
uint32_t ground_speed; ///< velocity in m/s
|
||||
int32_t heading; ///< heading in degrees * 10^2
|
||||
uint8_t satellites; ///< number of sattelites used
|
||||
uint8_t fix_type; ///< fix type: XXX correct for that
|
||||
uint32_t date;
|
||||
uint32_t utc_time;
|
||||
uint16_t hdop; ///< horizontal dilution of position (without unit)
|
||||
uint8_t ck_a;
|
||||
uint8_t ck_b;
|
||||
} gps_mtk_packet_t;
|
||||
|
||||
#pragma pack(pop)
|
||||
|
||||
#define MTK_RECV_BUFFER_SIZE 40
|
||||
|
||||
class MTK : public GPS_Helper
|
||||
{
|
||||
public:
|
||||
MTK(const int &fd, struct vehicle_gps_position_s *gps_position);
|
||||
~MTK();
|
||||
int receive(unsigned timeout);
|
||||
int configure(unsigned &baudrate);
|
||||
|
||||
private:
|
||||
/**
|
||||
* Parse the binary MTK packet
|
||||
*/
|
||||
int parse_char(uint8_t b, gps_mtk_packet_t &packet);
|
||||
|
||||
/**
|
||||
* Handle the package once it has arrived
|
||||
*/
|
||||
void handle_message(gps_mtk_packet_t &packet);
|
||||
|
||||
/**
|
||||
* Reset the parse state machine for a fresh start
|
||||
*/
|
||||
void decode_init(void);
|
||||
|
||||
/**
|
||||
* While parsing add every byte (except the sync bytes) to the checksum
|
||||
*/
|
||||
void add_byte_to_checksum(uint8_t);
|
||||
|
||||
int _fd;
|
||||
struct vehicle_gps_position_s *_gps_position;
|
||||
mtk_decode_state_t _decode_state;
|
||||
uint8_t _mtk_revision;
|
||||
uint8_t _rx_buffer[MTK_RECV_BUFFER_SIZE];
|
||||
unsigned _rx_count;
|
||||
uint8_t _rx_ck_a;
|
||||
uint8_t _rx_ck_b;
|
||||
};
|
||||
|
||||
#endif /* MTK_H_ */
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,395 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* Julian Oes <joes@student.ethz.ch>
|
||||
*
|
||||
* 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 U-Blox protocol definitions */
|
||||
|
||||
#ifndef UBX_H_
|
||||
#define UBX_H_
|
||||
|
||||
#include "gps_helper.h"
|
||||
|
||||
#define UBX_SYNC1 0xB5
|
||||
#define UBX_SYNC2 0x62
|
||||
|
||||
/* ClassIDs (the ones that are used) */
|
||||
#define UBX_CLASS_NAV 0x01
|
||||
//#define UBX_CLASS_RXM 0x02
|
||||
#define UBX_CLASS_ACK 0x05
|
||||
#define UBX_CLASS_CFG 0x06
|
||||
|
||||
/* MessageIDs (the ones that are used) */
|
||||
#define UBX_MESSAGE_NAV_POSLLH 0x02
|
||||
#define UBX_MESSAGE_NAV_SOL 0x06
|
||||
#define UBX_MESSAGE_NAV_TIMEUTC 0x21
|
||||
//#define UBX_MESSAGE_NAV_DOP 0x04
|
||||
#define UBX_MESSAGE_NAV_SVINFO 0x30
|
||||
#define UBX_MESSAGE_NAV_VELNED 0x12
|
||||
//#define UBX_MESSAGE_RXM_SVSI 0x20
|
||||
#define UBX_MESSAGE_ACK_ACK 0x01
|
||||
#define UBX_MESSAGE_ACK_NAK 0x00
|
||||
#define UBX_MESSAGE_CFG_PRT 0x00
|
||||
#define UBX_MESSAGE_CFG_NAV5 0x24
|
||||
#define UBX_MESSAGE_CFG_MSG 0x01
|
||||
#define UBX_MESSAGE_CFG_RATE 0x08
|
||||
|
||||
#define UBX_CFG_PRT_LENGTH 20
|
||||
#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */
|
||||
#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */
|
||||
#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */
|
||||
#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */
|
||||
#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */
|
||||
|
||||
#define UBX_CFG_RATE_LENGTH 6
|
||||
#define UBX_CFG_RATE_PAYLOAD_MEASRATE 200 /**< 200ms for 5Hz */
|
||||
#define UBX_CFG_RATE_PAYLOAD_NAVRATE 1 /**< cannot be changed */
|
||||
#define UBX_CFG_RATE_PAYLOAD_TIMEREF 0 /**< 0: UTC, 1: GPS time */
|
||||
|
||||
|
||||
#define UBX_CFG_NAV5_LENGTH 36
|
||||
#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0001 /**< only update dynamic model and fix mode */
|
||||
#define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7 /**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */
|
||||
#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */
|
||||
|
||||
#define UBX_CFG_MSG_LENGTH 8
|
||||
#define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
|
||||
#define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
|
||||
|
||||
#define UBX_MAX_PAYLOAD_LENGTH 500
|
||||
|
||||
// ************
|
||||
/** the structures of the binary packets */
|
||||
#pragma pack(push, 1)
|
||||
|
||||
typedef struct {
|
||||
uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */
|
||||
int32_t lon; /**< Longitude * 1e-7, deg */
|
||||
int32_t lat; /**< Latitude * 1e-7, deg */
|
||||
int32_t height; /**< Height above Ellipsoid, mm */
|
||||
int32_t height_msl; /**< Height above mean sea level, mm */
|
||||
uint32_t hAcc; /**< Horizontal Accuracy Estimate, mm */
|
||||
uint32_t vAcc; /**< Vertical Accuracy Estimate, mm */
|
||||
uint8_t ck_a;
|
||||
uint8_t ck_b;
|
||||
} gps_bin_nav_posllh_packet_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */
|
||||
int32_t time_nanoseconds; /**< Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000 */
|
||||
int16_t week; /**< GPS week (GPS time) */
|
||||
uint8_t gpsFix; /**< GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */
|
||||
uint8_t flags;
|
||||
int32_t ecefX;
|
||||
int32_t ecefY;
|
||||
int32_t ecefZ;
|
||||
uint32_t pAcc;
|
||||
int32_t ecefVX;
|
||||
int32_t ecefVY;
|
||||
int32_t ecefVZ;
|
||||
uint32_t sAcc;
|
||||
uint16_t pDOP;
|
||||
uint8_t reserved1;
|
||||
uint8_t numSV;
|
||||
uint32_t reserved2;
|
||||
uint8_t ck_a;
|
||||
uint8_t ck_b;
|
||||
} gps_bin_nav_sol_packet_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */
|
||||
uint32_t time_accuracy; /**< Time Accuracy Estimate, ns */
|
||||
int32_t time_nanoseconds; /**< Nanoseconds of second, range -1e9 .. 1e9 (UTC) */
|
||||
uint16_t year; /**< Year, range 1999..2099 (UTC) */
|
||||
uint8_t month; /**< Month, range 1..12 (UTC) */
|
||||
uint8_t day; /**< Day of Month, range 1..31 (UTC) */
|
||||
uint8_t hour; /**< Hour of Day, range 0..23 (UTC) */
|
||||
uint8_t min; /**< Minute of Hour, range 0..59 (UTC) */
|
||||
uint8_t sec; /**< Seconds of Minute, range 0..59 (UTC) */
|
||||
uint8_t valid_flag; /**< Validity Flags (see ubx documentation) */
|
||||
uint8_t ck_a;
|
||||
uint8_t ck_b;
|
||||
} gps_bin_nav_timeutc_packet_t;
|
||||
|
||||
//typedef struct {
|
||||
// uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */
|
||||
// uint16_t gDOP; /**< Geometric DOP (scaling 0.01) */
|
||||
// uint16_t pDOP; /**< Position DOP (scaling 0.01) */
|
||||
// uint16_t tDOP; /**< Time DOP (scaling 0.01) */
|
||||
// uint16_t vDOP; /**< Vertical DOP (scaling 0.01) */
|
||||
// uint16_t hDOP; /**< Horizontal DOP (scaling 0.01) */
|
||||
// uint16_t nDOP; /**< Northing DOP (scaling 0.01) */
|
||||
// uint16_t eDOP; /**< Easting DOP (scaling 0.01) */
|
||||
// uint8_t ck_a;
|
||||
// uint8_t ck_b;
|
||||
//} gps_bin_nav_dop_packet_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */
|
||||
uint8_t numCh; /**< Number of channels */
|
||||
uint8_t globalFlags;
|
||||
uint16_t reserved2;
|
||||
|
||||
} gps_bin_nav_svinfo_part1_packet_t;
|
||||
|
||||
typedef struct {
|
||||
uint8_t chn; /**< Channel number, 255 for SVs not assigned to a channel */
|
||||
uint8_t svid; /**< Satellite ID */
|
||||
uint8_t flags;
|
||||
uint8_t quality;
|
||||
uint8_t cno; /**< Carrier to Noise Ratio (Signal Strength), dbHz */
|
||||
int8_t elev; /**< Elevation in integer degrees */
|
||||
int16_t azim; /**< Azimuth in integer degrees */
|
||||
int32_t prRes; /**< Pseudo range residual in centimetres */
|
||||
|
||||
} gps_bin_nav_svinfo_part2_packet_t;
|
||||
|
||||
typedef struct {
|
||||
uint8_t ck_a;
|
||||
uint8_t ck_b;
|
||||
} gps_bin_nav_svinfo_part3_packet_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t time_milliseconds; // GPS Millisecond Time of Week
|
||||
int32_t velN; //NED north velocity, cm/s
|
||||
int32_t velE; //NED east velocity, cm/s
|
||||
int32_t velD; //NED down velocity, cm/s
|
||||
uint32_t speed; //Speed (3-D), cm/s
|
||||
uint32_t gSpeed; //Ground Speed (2-D), cm/s
|
||||
int32_t heading; //Heading of motion 2-D, deg, scaling: 1e-5
|
||||
uint32_t sAcc; //Speed Accuracy Estimate, cm/s
|
||||
uint32_t cAcc; //Course / Heading Accuracy Estimate, scaling: 1e-5
|
||||
uint8_t ck_a;
|
||||
uint8_t ck_b;
|
||||
} gps_bin_nav_velned_packet_t;
|
||||
|
||||
//typedef struct {
|
||||
// int32_t time_milliseconds; /**< Measurement integer millisecond GPS time of week */
|
||||
// int16_t week; /**< Measurement GPS week number */
|
||||
// uint8_t numVis; /**< Number of visible satellites */
|
||||
//
|
||||
// //... rest of package is not used in this implementation
|
||||
//
|
||||
//} gps_bin_rxm_svsi_packet_t;
|
||||
|
||||
typedef struct {
|
||||
uint8_t clsID;
|
||||
uint8_t msgID;
|
||||
uint8_t ck_a;
|
||||
uint8_t ck_b;
|
||||
} gps_bin_ack_ack_packet_t;
|
||||
|
||||
typedef struct {
|
||||
uint8_t clsID;
|
||||
uint8_t msgID;
|
||||
uint8_t ck_a;
|
||||
uint8_t ck_b;
|
||||
} gps_bin_ack_nak_packet_t;
|
||||
|
||||
typedef struct {
|
||||
uint8_t clsID;
|
||||
uint8_t msgID;
|
||||
uint16_t length;
|
||||
uint8_t portID;
|
||||
uint8_t res0;
|
||||
uint16_t res1;
|
||||
uint32_t mode;
|
||||
uint32_t baudRate;
|
||||
uint16_t inProtoMask;
|
||||
uint16_t outProtoMask;
|
||||
uint16_t flags;
|
||||
uint16_t pad;
|
||||
uint8_t ck_a;
|
||||
uint8_t ck_b;
|
||||
} type_gps_bin_cfg_prt_packet_t;
|
||||
|
||||
typedef struct {
|
||||
uint8_t clsID;
|
||||
uint8_t msgID;
|
||||
uint16_t length;
|
||||
uint16_t measRate;
|
||||
uint16_t navRate;
|
||||
uint16_t timeRef;
|
||||
uint8_t ck_a;
|
||||
uint8_t ck_b;
|
||||
} type_gps_bin_cfg_rate_packet_t;
|
||||
|
||||
typedef struct {
|
||||
uint8_t clsID;
|
||||
uint8_t msgID;
|
||||
uint16_t length;
|
||||
uint16_t mask;
|
||||
uint8_t dynModel;
|
||||
uint8_t fixMode;
|
||||
int32_t fixedAlt;
|
||||
uint32_t fixedAltVar;
|
||||
int8_t minElev;
|
||||
uint8_t drLimit;
|
||||
uint16_t pDop;
|
||||
uint16_t tDop;
|
||||
uint16_t pAcc;
|
||||
uint16_t tAcc;
|
||||
uint8_t staticHoldThresh;
|
||||
uint8_t dgpsTimeOut;
|
||||
uint32_t reserved2;
|
||||
uint32_t reserved3;
|
||||
uint32_t reserved4;
|
||||
uint8_t ck_a;
|
||||
uint8_t ck_b;
|
||||
} type_gps_bin_cfg_nav5_packet_t;
|
||||
|
||||
typedef struct {
|
||||
uint8_t clsID;
|
||||
uint8_t msgID;
|
||||
uint16_t length;
|
||||
uint8_t msgClass_payload;
|
||||
uint8_t msgID_payload;
|
||||
uint8_t rate[6];
|
||||
uint8_t ck_a;
|
||||
uint8_t ck_b;
|
||||
} type_gps_bin_cfg_msg_packet_t;
|
||||
|
||||
|
||||
// END the structures of the binary packets
|
||||
// ************
|
||||
|
||||
typedef enum {
|
||||
UBX_CONFIG_STATE_PRT = 0,
|
||||
UBX_CONFIG_STATE_PRT_NEW_BAUDRATE,
|
||||
UBX_CONFIG_STATE_RATE,
|
||||
UBX_CONFIG_STATE_NAV5,
|
||||
UBX_CONFIG_STATE_MSG_NAV_POSLLH,
|
||||
UBX_CONFIG_STATE_MSG_NAV_TIMEUTC,
|
||||
UBX_CONFIG_STATE_MSG_NAV_DOP,
|
||||
UBX_CONFIG_STATE_MSG_NAV_SVINFO,
|
||||
UBX_CONFIG_STATE_MSG_NAV_SOL,
|
||||
UBX_CONFIG_STATE_MSG_NAV_VELNED,
|
||||
// UBX_CONFIG_STATE_MSG_RXM_SVSI,
|
||||
UBX_CONFIG_STATE_CONFIGURED
|
||||
} ubx_config_state_t;
|
||||
|
||||
typedef enum {
|
||||
CLASS_UNKNOWN = 0,
|
||||
NAV = 1,
|
||||
RXM = 2,
|
||||
ACK = 3,
|
||||
CFG = 4
|
||||
} ubx_message_class_t;
|
||||
|
||||
typedef enum {
|
||||
//these numbers do NOT correspond to the message id numbers of the ubx protocol
|
||||
ID_UNKNOWN = 0,
|
||||
NAV_POSLLH,
|
||||
NAV_SOL,
|
||||
NAV_TIMEUTC,
|
||||
// NAV_DOP,
|
||||
NAV_SVINFO,
|
||||
NAV_VELNED,
|
||||
// RXM_SVSI,
|
||||
CFG_NAV5,
|
||||
ACK_ACK,
|
||||
ACK_NAK,
|
||||
} ubx_message_id_t;
|
||||
|
||||
typedef enum {
|
||||
UBX_DECODE_UNINIT = 0,
|
||||
UBX_DECODE_GOT_SYNC1,
|
||||
UBX_DECODE_GOT_SYNC2,
|
||||
UBX_DECODE_GOT_CLASS,
|
||||
UBX_DECODE_GOT_MESSAGEID,
|
||||
UBX_DECODE_GOT_LENGTH1,
|
||||
UBX_DECODE_GOT_LENGTH2
|
||||
} ubx_decode_state_t;
|
||||
|
||||
//typedef type_gps_bin_ubx_state gps_bin_ubx_state_t;
|
||||
#pragma pack(pop)
|
||||
|
||||
#define RECV_BUFFER_SIZE 500 //The NAV-SOL messages really need such a big buffer
|
||||
|
||||
class UBX : public GPS_Helper
|
||||
{
|
||||
public:
|
||||
UBX(const int &fd, struct vehicle_gps_position_s *gps_position);
|
||||
~UBX();
|
||||
int receive(unsigned timeout);
|
||||
int configure(unsigned &baudrate);
|
||||
|
||||
private:
|
||||
|
||||
/**
|
||||
* Parse the binary MTK packet
|
||||
*/
|
||||
int parse_char(uint8_t b);
|
||||
|
||||
/**
|
||||
* Handle the package once it has arrived
|
||||
*/
|
||||
int handle_message(void);
|
||||
|
||||
/**
|
||||
* Reset the parse state machine for a fresh start
|
||||
*/
|
||||
void decode_init(void);
|
||||
|
||||
/**
|
||||
* While parsing add every byte (except the sync bytes) to the checksum
|
||||
*/
|
||||
void add_byte_to_checksum(uint8_t);
|
||||
|
||||
/**
|
||||
* Add the two checksum bytes to an outgoing message
|
||||
*/
|
||||
void add_checksum_to_message(uint8_t* message, const unsigned length);
|
||||
|
||||
/**
|
||||
* Helper to send a config packet
|
||||
*/
|
||||
void send_config_packet(const int &fd, uint8_t *packet, const unsigned length);
|
||||
|
||||
int _fd;
|
||||
struct vehicle_gps_position_s *_gps_position;
|
||||
ubx_config_state_t _config_state;
|
||||
bool _waiting_for_ack;
|
||||
uint8_t _clsID_needed;
|
||||
uint8_t _msgID_needed;
|
||||
ubx_decode_state_t _decode_state;
|
||||
uint8_t _rx_buffer[RECV_BUFFER_SIZE];
|
||||
unsigned _rx_count;
|
||||
uint8_t _rx_ck_a;
|
||||
uint8_t _rx_ck_b;
|
||||
ubx_message_class_t _message_class;
|
||||
ubx_message_id_t _message_id;
|
||||
unsigned _payload_size;
|
||||
};
|
||||
|
||||
#endif /* UBX_H_ */
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,40 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# Hardware in the Loop (HIL) simulation actuator output bank
|
||||
#
|
||||
|
||||
MODULE_COMMAND = hil
|
||||
|
||||
SRCS = hil.cpp
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,43 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# HMC5883 driver
|
||||
#
|
||||
|
||||
MODULE_COMMAND = hmc5883
|
||||
|
||||
# XXX seems excessive, check if 2048 is sufficient
|
||||
MODULE_STACKSIZE = 4096
|
||||
|
||||
SRCS = hmc5883.cpp
|
||||
@@ -0,0 +1,306 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Simon Wilks <sjwilks@gmail.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file hott_telemetry_main.c
|
||||
* @author Simon Wilks <sjwilks@gmail.com>
|
||||
*
|
||||
* Graupner HoTT Telemetry implementation.
|
||||
*
|
||||
* The HoTT receiver polls each device at a regular interval at which point
|
||||
* a data packet can be returned if necessary.
|
||||
*
|
||||
* TODO: Add support for at least the vario and GPS sensor data.
|
||||
*/
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <nuttx/config.h>
|
||||
#include <poll.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <termios.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <unistd.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
#include "messages.h"
|
||||
|
||||
static int thread_should_exit = false; /**< Deamon exit flag */
|
||||
static int thread_running = false; /**< Deamon status flag */
|
||||
static int deamon_task; /**< Handle of deamon task / thread */
|
||||
static char *daemon_name = "hott_telemetry";
|
||||
static char *commandline_usage = "usage: hott_telemetry start|status|stop [-d <device>]";
|
||||
|
||||
|
||||
/* A little console messaging experiment - console helper macro */
|
||||
#define FATAL_MSG(_msg) fprintf(stderr, "[%s] %s\n", daemon_name, _msg); exit(1);
|
||||
#define ERROR_MSG(_msg) fprintf(stderr, "[%s] %s\n", daemon_name, _msg);
|
||||
#define INFO_MSG(_msg) printf("[%s] %s\n", daemon_name, _msg);
|
||||
/**
|
||||
* Deamon management function.
|
||||
*/
|
||||
__EXPORT int hott_telemetry_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Mainloop of deamon.
|
||||
*/
|
||||
int hott_telemetry_thread_main(int argc, char *argv[]);
|
||||
|
||||
static int read_data(int uart, int *id);
|
||||
static int send_data(int uart, uint8_t *buffer, int size);
|
||||
|
||||
static int open_uart(const char *device, struct termios *uart_config_original)
|
||||
{
|
||||
/* baud rate */
|
||||
int speed = B19200;
|
||||
int uart;
|
||||
|
||||
/* open uart */
|
||||
uart = open(device, O_RDWR | O_NOCTTY);
|
||||
|
||||
if (uart < 0) {
|
||||
char msg[80];
|
||||
sprintf(msg, "Error opening port: %s\n", device);
|
||||
FATAL_MSG(msg);
|
||||
}
|
||||
|
||||
/* Try to set baud rate */
|
||||
struct termios uart_config;
|
||||
int termios_state;
|
||||
|
||||
/* Back up the original uart configuration to restore it after exit */
|
||||
char msg[80];
|
||||
|
||||
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
|
||||
sprintf(msg, "Error getting baudrate / termios config for %s: %d\n", device, termios_state);
|
||||
close(uart);
|
||||
FATAL_MSG(msg);
|
||||
}
|
||||
|
||||
/* Fill the struct for the new configuration */
|
||||
tcgetattr(uart, &uart_config);
|
||||
|
||||
/* Clear ONLCR flag (which appends a CR for every LF) */
|
||||
uart_config.c_oflag &= ~ONLCR;
|
||||
|
||||
/* Set baud rate */
|
||||
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
|
||||
sprintf(msg, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n",
|
||||
device, termios_state);
|
||||
close(uart);
|
||||
FATAL_MSG(msg);
|
||||
}
|
||||
|
||||
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
|
||||
sprintf(msg, "Error setting baudrate / termios config for %s (tcsetattr)\n", device);
|
||||
close(uart);
|
||||
FATAL_MSG(msg);
|
||||
}
|
||||
|
||||
/* Activate single wire mode */
|
||||
ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
|
||||
|
||||
return uart;
|
||||
}
|
||||
|
||||
int read_data(int uart, int *id)
|
||||
{
|
||||
const int timeout = 1000;
|
||||
struct pollfd fds[] = { { .fd = uart, .events = POLLIN } };
|
||||
|
||||
char mode;
|
||||
|
||||
if (poll(fds, 1, timeout) > 0) {
|
||||
/* Get the mode: binary or text */
|
||||
read(uart, &mode, 1);
|
||||
/* Read the device ID being polled */
|
||||
read(uart, id, 1);
|
||||
|
||||
/* if we have a binary mode request */
|
||||
if (mode != BINARY_MODE_REQUEST_ID) {
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
} else {
|
||||
ERROR_MSG("UART timeout on TX/RX port");
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int send_data(int uart, uint8_t *buffer, int size)
|
||||
{
|
||||
usleep(POST_READ_DELAY_IN_USECS);
|
||||
|
||||
uint16_t checksum = 0;
|
||||
|
||||
for (int i = 0; i < size; i++) {
|
||||
if (i == size - 1) {
|
||||
/* Set the checksum: the first uint8_t is taken as the checksum. */
|
||||
buffer[i] = checksum & 0xff;
|
||||
|
||||
} else {
|
||||
checksum += buffer[i];
|
||||
}
|
||||
|
||||
write(uart, &buffer[i], 1);
|
||||
|
||||
/* Sleep before sending the next byte. */
|
||||
usleep(POST_WRITE_DELAY_IN_USECS);
|
||||
}
|
||||
|
||||
/* A hack the reads out what was written so the next read from the receiver doesn't get it. */
|
||||
/* TODO: Fix this!! */
|
||||
uint8_t dummy[size];
|
||||
read(uart, &dummy, size);
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int hott_telemetry_thread_main(int argc, char *argv[])
|
||||
{
|
||||
INFO_MSG("starting");
|
||||
|
||||
thread_running = true;
|
||||
|
||||
char *device = "/dev/ttyS1"; /**< Default telemetry port: USART2 */
|
||||
|
||||
/* read commandline arguments */
|
||||
for (int i = 0; i < argc && argv[i]; i++) {
|
||||
if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set
|
||||
if (argc > i + 1) {
|
||||
device = argv[i + 1];
|
||||
|
||||
} else {
|
||||
thread_running = false;
|
||||
ERROR_MSG("missing parameter to -d");
|
||||
ERROR_MSG(commandline_usage);
|
||||
exit(1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
|
||||
struct termios uart_config_original;
|
||||
int uart = open_uart(device, &uart_config_original);
|
||||
|
||||
if (uart < 0) {
|
||||
ERROR_MSG("Failed opening HoTT UART, exiting.");
|
||||
thread_running = false;
|
||||
exit(ERROR);
|
||||
}
|
||||
|
||||
messages_init();
|
||||
|
||||
uint8_t buffer[MESSAGE_BUFFER_SIZE];
|
||||
int size = 0;
|
||||
int id = 0;
|
||||
|
||||
while (!thread_should_exit) {
|
||||
if (read_data(uart, &id) == OK) {
|
||||
switch (id) {
|
||||
case ELECTRIC_AIR_MODULE:
|
||||
build_eam_response(buffer, &size);
|
||||
break;
|
||||
|
||||
default:
|
||||
continue; // Not a module we support.
|
||||
}
|
||||
|
||||
send_data(uart, buffer, size);
|
||||
}
|
||||
}
|
||||
|
||||
INFO_MSG("exiting");
|
||||
|
||||
close(uart);
|
||||
|
||||
thread_running = false;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Process command line arguments and tart the daemon.
|
||||
*/
|
||||
int hott_telemetry_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 1) {
|
||||
ERROR_MSG("missing command");
|
||||
ERROR_MSG(commandline_usage);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (thread_running) {
|
||||
INFO_MSG("deamon already running");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
deamon_task = task_spawn("hott_telemetry",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 40,
|
||||
2048,
|
||||
hott_telemetry_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
thread_should_exit = true;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (thread_running) {
|
||||
INFO_MSG("daemon is running");
|
||||
|
||||
} else {
|
||||
INFO_MSG("daemon not started");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
ERROR_MSG("unrecognized command");
|
||||
ERROR_MSG(commandline_usage);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,87 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Simon Wilks <sjwilks@gmail.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file messages.c
|
||||
* @author Simon Wilks <sjwilks@gmail.com>
|
||||
*/
|
||||
|
||||
#include "messages.h"
|
||||
|
||||
#include <string.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <unistd.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
|
||||
static int battery_sub = -1;
|
||||
static int sensor_sub = -1;
|
||||
|
||||
void messages_init(void)
|
||||
{
|
||||
battery_sub = orb_subscribe(ORB_ID(battery_status));
|
||||
sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
|
||||
}
|
||||
|
||||
void build_eam_response(uint8_t *buffer, int *size)
|
||||
{
|
||||
/* get a local copy of the current sensor values */
|
||||
struct sensor_combined_s raw;
|
||||
memset(&raw, 0, sizeof(raw));
|
||||
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
|
||||
|
||||
/* get a local copy of the battery data */
|
||||
struct battery_status_s battery;
|
||||
memset(&battery, 0, sizeof(battery));
|
||||
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
|
||||
|
||||
struct eam_module_msg msg;
|
||||
*size = sizeof(msg);
|
||||
memset(&msg, 0, *size);
|
||||
|
||||
msg.start = START_BYTE;
|
||||
msg.eam_sensor_id = ELECTRIC_AIR_MODULE;
|
||||
msg.sensor_id = EAM_SENSOR_ID;
|
||||
msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20);
|
||||
msg.temperature2 = TEMP_ZERO_CELSIUS;
|
||||
msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10);
|
||||
|
||||
uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500);
|
||||
msg.altitude_L = (uint8_t)alt & 0xff;
|
||||
msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
|
||||
|
||||
msg.stop = STOP_BYTE;
|
||||
|
||||
memcpy(buffer, &msg, *size);
|
||||
}
|
||||
@@ -0,0 +1,124 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Simon Wilks <sjwilks@gmail.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file messages.h
|
||||
* @author Simon Wilks <sjwilks@gmail.com>
|
||||
*
|
||||
* Graupner HoTT Telemetry message generation.
|
||||
*
|
||||
*/
|
||||
#ifndef MESSAGES_H_
|
||||
#define MESSAGES_H
|
||||
|
||||
#include <stdlib.h>
|
||||
|
||||
/* The buffer size used to store messages. This must be at least as big as the number of
|
||||
* fields in the largest message struct.
|
||||
*/
|
||||
#define MESSAGE_BUFFER_SIZE 50
|
||||
|
||||
/* The HoTT receiver demands a minimum 5ms period of silence after delivering its request.
|
||||
* Note that the value specified here is lower than 5000 (5ms) as time is lost constucting
|
||||
* the message after the read which takes some milliseconds.
|
||||
*/
|
||||
#define POST_READ_DELAY_IN_USECS 4000
|
||||
/* A pause of 3ms is required between each uint8_t sent back to the HoTT receiver. Much lower
|
||||
* values can be used in practise though.
|
||||
*/
|
||||
#define POST_WRITE_DELAY_IN_USECS 2000
|
||||
|
||||
// Protocol constants.
|
||||
#define BINARY_MODE_REQUEST_ID 0x80 // Binary mode request.
|
||||
#define START_BYTE 0x7c
|
||||
#define STOP_BYTE 0x7d
|
||||
#define TEMP_ZERO_CELSIUS 0x14
|
||||
|
||||
/* Electric Air Module (EAM) constants. */
|
||||
#define ELECTRIC_AIR_MODULE 0x8e
|
||||
#define EAM_SENSOR_ID 0xe0
|
||||
|
||||
/* The Electric Air Module message. */
|
||||
struct eam_module_msg {
|
||||
uint8_t start; /**< Start byte */
|
||||
uint8_t eam_sensor_id; /**< EAM sensor ID */
|
||||
uint8_t warning;
|
||||
uint8_t sensor_id; /**< Sensor ID, why different? */
|
||||
uint8_t alarm_inverse1;
|
||||
uint8_t alarm_inverse2;
|
||||
uint8_t cell1_L; /**< Lipo cell voltages. Not supported. */
|
||||
uint8_t cell2_L;
|
||||
uint8_t cell3_L;
|
||||
uint8_t cell4_L;
|
||||
uint8_t cell5_L;
|
||||
uint8_t cell6_L;
|
||||
uint8_t cell7_L;
|
||||
uint8_t cell1_H;
|
||||
uint8_t cell2_H;
|
||||
uint8_t cell3_H;
|
||||
uint8_t cell4_H;
|
||||
uint8_t cell5_H;
|
||||
uint8_t cell6_H;
|
||||
uint8_t cell7_H;
|
||||
uint8_t batt1_voltage_L; /**< Battery 1 voltage, lower 8-bits in steps of 0.02V */
|
||||
uint8_t batt1_voltage_H;
|
||||
uint8_t batt2_voltage_L; /**< Battery 2 voltage, lower 8-bits in steps of 0.02V */
|
||||
uint8_t batt2_voltage_H;
|
||||
uint8_t temperature1; /**< Temperature sensor 1. 20 = 0 degrees */
|
||||
uint8_t temperature2;
|
||||
uint8_t altitude_L; /**< Attitude (meters) lower 8-bits. 500 = 0 meters */
|
||||
uint8_t altitude_H;
|
||||
uint8_t current_L; /**< Current (mAh) lower 8-bits in steps of 0.1V */
|
||||
uint8_t current_H;
|
||||
uint8_t main_voltage_L; /**< Main power voltage lower 8-bits in steps of 0.1V */
|
||||
uint8_t main_voltage_H;
|
||||
uint8_t battery_capacity_L; /**< Used battery capacity in steps of 10mAh */
|
||||
uint8_t battery_capacity_H;
|
||||
uint8_t climbrate_L; /**< Climb rate in 0.01m/s. 0m/s = 30000 */
|
||||
uint8_t climbrate_H;
|
||||
uint8_t climbrate_3s; /**< Climb rate in m/3sec. 0m/3sec = 120 */
|
||||
uint8_t rpm_L; /**< RPM Lower 8-bits In steps of 10 U/min */
|
||||
uint8_t rpm_H;
|
||||
uint8_t electric_min; /**< Flight time in minutes. */
|
||||
uint8_t electric_sec; /**< Flight time in seconds. */
|
||||
uint8_t speed_L; /**< Airspeed in km/h in steps of 1 km/h */
|
||||
uint8_t speed_H;
|
||||
uint8_t stop; /**< Stop byte */
|
||||
uint8_t checksum; /**< Lower 8-bits of all bytes summed. */
|
||||
};
|
||||
|
||||
void messages_init(void);
|
||||
void build_eam_response(uint8_t *buffer, int *size);
|
||||
|
||||
#endif /* MESSAGES_H_ */
|
||||
@@ -0,0 +1,41 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# Graupner HoTT Telemetry application.
|
||||
#
|
||||
|
||||
MODULE_COMMAND = hott_telemetry
|
||||
|
||||
SRCS = hott_telemetry_main.c \
|
||||
messages.c
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,40 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# Makefile to build the Maxbotix Sonar driver.
|
||||
#
|
||||
|
||||
MODULE_COMMAND = mb12xx
|
||||
|
||||
SRCS = mb12xx.cpp
|
||||
@@ -0,0 +1,43 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# Makefile to build the MPU6000 driver.
|
||||
#
|
||||
|
||||
MODULE_COMMAND = mpu6000
|
||||
|
||||
# XXX seems excessive, check if 2048 is not sufficient
|
||||
MODULE_STACKSIZE = 4096
|
||||
|
||||
SRCS = mpu6000.cpp
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,40 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# MS5611 driver
|
||||
#
|
||||
|
||||
MODULE_COMMAND = ms5611
|
||||
|
||||
SRCS = ms5611.cpp
|
||||
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user