mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-21 17:33:33 +08:00
Moved the bulk of sensor drivers to the new world
This commit is contained in:
Lorenz Meier
@@ -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;
|
||||
}
|
||||
Reference in New Issue
Block a user