forked from Archive/PX4-Autopilot
Merge branch 'master' into seatbelt_multirotor
This commit is contained in:
Anton Babushkin
commit
c6d184e29d
|
|
@ -27,7 +27,8 @@ MODULES += drivers/ms5611
|
|||
MODULES += drivers/mb12xx
|
||||
MODULES += drivers/gps
|
||||
MODULES += drivers/hil
|
||||
MODULES += drivers/hott_telemetry
|
||||
MODULES += drivers/hott/hott_telemetry
|
||||
MODULES += drivers/hott/hott_sensors
|
||||
MODULES += drivers/blinkm
|
||||
MODULES += drivers/mkblctrl
|
||||
MODULES += drivers/md25
|
||||
|
|
|
|||
|
|
@ -0,0 +1,92 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file comms.c
|
||||
* @author Simon Wilks <sjwilks@gmail.com>
|
||||
*
|
||||
*/
|
||||
|
||||
#include "comms.h"
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <stdio.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <termios.h>
|
||||
|
||||
int
|
||||
open_uart(const char *device)
|
||||
{
|
||||
/* baud rate */
|
||||
static const speed_t speed = B19200;
|
||||
|
||||
/* open uart */
|
||||
const int uart = open(device, O_RDWR | O_NOCTTY);
|
||||
|
||||
if (uart < 0) {
|
||||
err(1, "Error opening port: %s", device);
|
||||
}
|
||||
|
||||
/* Back up the original uart configuration to restore it after exit */
|
||||
int termios_state;
|
||||
struct termios uart_config_original;
|
||||
if ((termios_state = tcgetattr(uart, &uart_config_original)) < 0) {
|
||||
close(uart);
|
||||
err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state);
|
||||
}
|
||||
|
||||
/* Fill the struct for the new configuration */
|
||||
struct termios uart_config;
|
||||
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) {
|
||||
close(uart);
|
||||
err(1, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)",
|
||||
device, termios_state);
|
||||
}
|
||||
|
||||
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
|
||||
close(uart);
|
||||
err(1, "Error setting baudrate / termios config for %s (tcsetattr)", device);
|
||||
}
|
||||
|
||||
/* Activate single wire mode */
|
||||
ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
|
||||
|
||||
return uart;
|
||||
}
|
||||
|
|
@ -0,0 +1,46 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file comms.h
|
||||
* @author Simon Wilks <sjwilks@gmail.com>
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
#ifndef COMMS_H_
|
||||
#define COMMS_H
|
||||
|
||||
int open_uart(const char *device);
|
||||
|
||||
#endif /* COMMS_H_ */
|
||||
|
|
@ -0,0 +1,238 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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_sensors.c
|
||||
* @author Simon Wilks <sjwilks@gmail.com>
|
||||
*
|
||||
* Graupner HoTT sensor driver implementation.
|
||||
*
|
||||
* Poll any sensors connected to the PX4 via the telemetry wire.
|
||||
*/
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <nuttx/config.h>
|
||||
#include <poll.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <unistd.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
#include "../comms.h"
|
||||
#include "../messages.h"
|
||||
|
||||
#define DEFAULT_UART "/dev/ttyS0"; /**< USART1 */
|
||||
|
||||
/* Oddly, ERROR is not defined for C++ */
|
||||
#ifdef ERROR
|
||||
# undef ERROR
|
||||
#endif
|
||||
static const int ERROR = -1;
|
||||
|
||||
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 const char daemon_name[] = "hott_sensors";
|
||||
static const char commandline_usage[] = "usage: hott_sensors start|status|stop [-d <device>]";
|
||||
|
||||
/**
|
||||
* Deamon management function.
|
||||
*/
|
||||
extern "C" __EXPORT int hott_sensors_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Mainloop of daemon.
|
||||
*/
|
||||
int hott_sensors_thread_main(int argc, char *argv[]);
|
||||
|
||||
static int recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id);
|
||||
static int send_poll(int uart, uint8_t *buffer, size_t size);
|
||||
|
||||
int
|
||||
send_poll(int uart, uint8_t *buffer, size_t size)
|
||||
{
|
||||
for (size_t i = 0; i < size; i++) {
|
||||
write(uart, &buffer[i], sizeof(buffer[i]));
|
||||
|
||||
/* 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
|
||||
recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id)
|
||||
{
|
||||
static const int timeout_ms = 1000;
|
||||
|
||||
struct pollfd fds;
|
||||
fds.fd = uart;
|
||||
fds.events = POLLIN;
|
||||
|
||||
// XXX should this poll be inside the while loop???
|
||||
if (poll(&fds, 1, timeout_ms) > 0) {
|
||||
int i = 0;
|
||||
bool stop_byte_read = false;
|
||||
while (true) {
|
||||
read(uart, &buffer[i], sizeof(buffer[i]));
|
||||
|
||||
if (stop_byte_read) {
|
||||
// XXX process checksum
|
||||
*size = ++i;
|
||||
return OK;
|
||||
}
|
||||
// XXX can some other field not have the STOP BYTE value?
|
||||
if (buffer[i] == STOP_BYTE) {
|
||||
*id = buffer[1];
|
||||
stop_byte_read = true;
|
||||
}
|
||||
i++;
|
||||
}
|
||||
}
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
int
|
||||
hott_sensors_thread_main(int argc, char *argv[])
|
||||
{
|
||||
warnx("starting");
|
||||
|
||||
thread_running = true;
|
||||
|
||||
const char *device = DEFAULT_UART;
|
||||
|
||||
/* 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;
|
||||
errx(1, "missing parameter to -d\n%s", commandline_usage);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
|
||||
const int uart = open_uart(device);
|
||||
if (uart < 0) {
|
||||
errx(1, "Failed opening HoTT UART, exiting.");
|
||||
thread_running = false;
|
||||
}
|
||||
|
||||
init_pub_messages();
|
||||
|
||||
uint8_t buffer[MAX_MESSAGE_BUFFER_SIZE];
|
||||
size_t size = 0;
|
||||
uint8_t id = 0;
|
||||
while (!thread_should_exit) {
|
||||
// Currently we only support a General Air Module sensor.
|
||||
build_gam_request(&buffer[0], &size);
|
||||
send_poll(uart, buffer, size);
|
||||
|
||||
// The sensor will need a little time before it starts sending.
|
||||
usleep(5000);
|
||||
|
||||
recv_data(uart, &buffer[0], &size, &id);
|
||||
|
||||
// Determine which moduel sent it and process accordingly.
|
||||
if (id == GAM_SENSOR_ID) {
|
||||
publish_gam_message(buffer);
|
||||
} else {
|
||||
warnx("Unknown sensor ID: %d", id);
|
||||
}
|
||||
}
|
||||
|
||||
warnx("exiting");
|
||||
close(uart);
|
||||
thread_running = false;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Process command line arguments and start the daemon.
|
||||
*/
|
||||
int
|
||||
hott_sensors_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 1) {
|
||||
errx(1, "missing command\n%s", commandline_usage);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (thread_running) {
|
||||
warnx("deamon already running");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
deamon_task = task_spawn_cmd(daemon_name,
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 40,
|
||||
1024,
|
||||
hott_sensors_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) {
|
||||
warnx("daemon is running");
|
||||
|
||||
} else {
|
||||
warnx("daemon not started");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
errx(1, "unrecognized command\n%s", commandline_usage);
|
||||
}
|
||||
|
|
@ -0,0 +1,42 @@
|
|||
############################################################################
|
||||
#
|
||||
# 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 Sensors application.
|
||||
#
|
||||
|
||||
MODULE_COMMAND = hott_sensors
|
||||
|
||||
SRCS = hott_sensors.cpp \
|
||||
../messages.cpp \
|
||||
../comms.cpp
|
||||
|
|
@ -41,7 +41,6 @@
|
|||
* 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>
|
||||
|
|
@ -50,13 +49,21 @@
|
|||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <termios.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <unistd.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
#include "messages.h"
|
||||
#include "../comms.h"
|
||||
#include "../messages.h"
|
||||
|
||||
#define DEFAULT_UART "/dev/ttyS0"; /**< USART1 */
|
||||
|
||||
/* Oddly, ERROR is not defined for C++ */
|
||||
#ifdef ERROR
|
||||
# undef ERROR
|
||||
#endif
|
||||
static const int ERROR = -1;
|
||||
|
||||
static int thread_should_exit = false; /**< Deamon exit flag */
|
||||
static int thread_running = false; /**< Deamon status flag */
|
||||
|
|
@ -67,7 +74,7 @@ static const char commandline_usage[] = "usage: hott_telemetry start|status|stop
|
|||
/**
|
||||
* Deamon management function.
|
||||
*/
|
||||
__EXPORT int hott_telemetry_main(int argc, char *argv[]);
|
||||
extern "C" __EXPORT int hott_telemetry_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Mainloop of daemon.
|
||||
|
|
@ -77,60 +84,18 @@ int hott_telemetry_thread_main(int argc, char *argv[]);
|
|||
static int recv_req_id(int uart, uint8_t *id);
|
||||
static int send_data(int uart, uint8_t *buffer, size_t size);
|
||||
|
||||
static int
|
||||
open_uart(const char *device, struct termios *uart_config_original)
|
||||
{
|
||||
/* baud rate */
|
||||
static const speed_t speed = B19200;
|
||||
|
||||
/* open uart */
|
||||
const int uart = open(device, O_RDWR | O_NOCTTY);
|
||||
|
||||
if (uart < 0) {
|
||||
err(1, "Error opening port: %s", device);
|
||||
}
|
||||
|
||||
/* Back up the original uart configuration to restore it after exit */
|
||||
int termios_state;
|
||||
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
|
||||
close(uart);
|
||||
err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state);
|
||||
}
|
||||
|
||||
/* Fill the struct for the new configuration */
|
||||
struct termios uart_config;
|
||||
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) {
|
||||
close(uart);
|
||||
err(1, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)",
|
||||
device, termios_state);
|
||||
}
|
||||
|
||||
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
|
||||
close(uart);
|
||||
err(1, "Error setting baudrate / termios config for %s (tcsetattr)", device);
|
||||
}
|
||||
|
||||
/* Activate single wire mode */
|
||||
ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
|
||||
|
||||
return uart;
|
||||
}
|
||||
|
||||
int
|
||||
recv_req_id(int uart, uint8_t *id)
|
||||
{
|
||||
static const int timeout_ms = 1000; // TODO make it a define
|
||||
struct pollfd fds[] = { { .fd = uart, .events = POLLIN } };
|
||||
|
||||
uint8_t mode;
|
||||
|
||||
if (poll(fds, 1, timeout_ms) > 0) {
|
||||
struct pollfd fds;
|
||||
fds.fd = uart;
|
||||
fds.events = POLLIN;
|
||||
|
||||
if (poll(&fds, 1, timeout_ms) > 0) {
|
||||
/* Get the mode: binary or text */
|
||||
read(uart, &mode, sizeof(mode));
|
||||
|
||||
|
|
@ -155,7 +120,6 @@ send_data(int uart, uint8_t *buffer, size_t size)
|
|||
usleep(POST_READ_DELAY_IN_USECS);
|
||||
|
||||
uint16_t checksum = 0;
|
||||
|
||||
for (size_t i = 0; i < size; i++) {
|
||||
if (i == size - 1) {
|
||||
/* Set the checksum: the first uint8_t is taken as the checksum. */
|
||||
|
|
@ -186,7 +150,7 @@ hott_telemetry_thread_main(int argc, char *argv[])
|
|||
|
||||
thread_running = true;
|
||||
|
||||
const char *device = "/dev/ttyS1"; /**< Default telemetry port: USART2 */
|
||||
const char *device = DEFAULT_UART;
|
||||
|
||||
/* read commandline arguments */
|
||||
for (int i = 0; i < argc && argv[i]; i++) {
|
||||
|
|
@ -202,22 +166,20 @@ hott_telemetry_thread_main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
|
||||
struct termios uart_config_original;
|
||||
const int uart = open_uart(device, &uart_config_original);
|
||||
|
||||
const int uart = open_uart(device);
|
||||
if (uart < 0) {
|
||||
errx(1, "Failed opening HoTT UART, exiting.");
|
||||
thread_running = false;
|
||||
}
|
||||
|
||||
messages_init();
|
||||
init_sub_messages();
|
||||
|
||||
uint8_t buffer[MESSAGE_BUFFER_SIZE];
|
||||
uint8_t buffer[MAX_MESSAGE_BUFFER_SIZE];
|
||||
size_t size = 0;
|
||||
uint8_t id = 0;
|
||||
bool connected = true;
|
||||
|
||||
while (!thread_should_exit) {
|
||||
// Listen for and serve poll from the receiver.
|
||||
if (recv_req_id(uart, &id) == OK) {
|
||||
if (!connected) {
|
||||
connected = true;
|
||||
|
|
@ -228,7 +190,9 @@ hott_telemetry_thread_main(int argc, char *argv[])
|
|||
case EAM_SENSOR_ID:
|
||||
build_eam_response(buffer, &size);
|
||||
break;
|
||||
|
||||
case GAM_SENSOR_ID:
|
||||
build_gam_response(buffer, &size);
|
||||
break;
|
||||
case GPS_SENSOR_ID:
|
||||
build_gps_response(buffer, &size);
|
||||
break;
|
||||
|
|
@ -254,7 +218,7 @@ hott_telemetry_thread_main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
/**
|
||||
* Process command line arguments and tart the daemon.
|
||||
* Process command line arguments and start the daemon.
|
||||
*/
|
||||
int
|
||||
hott_telemetry_main(int argc, char *argv[])
|
||||
|
|
@ -32,10 +32,11 @@
|
|||
############################################################################
|
||||
|
||||
#
|
||||
# Graupner HoTT Telemetry application.
|
||||
# Graupner HoTT Telemetry applications.
|
||||
#
|
||||
|
||||
MODULE_COMMAND = hott_telemetry
|
||||
|
||||
SRCS = hott_telemetry_main.c \
|
||||
messages.c
|
||||
SRCS = hott_telemetry.cpp \
|
||||
../messages.cpp \
|
||||
../comms.cpp
|
||||
|
|
@ -46,6 +46,7 @@
|
|||
#include <unistd.h>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
|
|
@ -53,24 +54,75 @@
|
|||
/* The board is very roughly 5 deg warmer than the surrounding air */
|
||||
#define BOARD_TEMP_OFFSET_DEG 5
|
||||
|
||||
static int battery_sub = -1;
|
||||
static int gps_sub = -1;
|
||||
static int home_sub = -1;
|
||||
static int sensor_sub = -1;
|
||||
static int airspeed_sub = -1;
|
||||
static int _battery_sub = -1;
|
||||
static int _gps_sub = -1;
|
||||
static int _home_sub = -1;
|
||||
static int _sensor_sub = -1;
|
||||
static int _airspeed_sub = -1;
|
||||
static int _esc_sub = -1;
|
||||
|
||||
static bool home_position_set = false;
|
||||
static double home_lat = 0.0d;
|
||||
static double home_lon = 0.0d;
|
||||
static orb_advert_t _esc_pub;
|
||||
struct esc_status_s _esc;
|
||||
|
||||
static bool _home_position_set = false;
|
||||
static double _home_lat = 0.0d;
|
||||
static double _home_lon = 0.0d;
|
||||
|
||||
void
|
||||
messages_init(void)
|
||||
init_sub_messages(void)
|
||||
{
|
||||
battery_sub = orb_subscribe(ORB_ID(battery_status));
|
||||
gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
|
||||
home_sub = orb_subscribe(ORB_ID(home_position));
|
||||
sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
|
||||
airspeed_sub = orb_subscribe(ORB_ID(airspeed));
|
||||
_battery_sub = orb_subscribe(ORB_ID(battery_status));
|
||||
_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
|
||||
_home_sub = orb_subscribe(ORB_ID(home_position));
|
||||
_sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
|
||||
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
|
||||
_esc_sub = orb_subscribe(ORB_ID(esc_status));
|
||||
}
|
||||
|
||||
void
|
||||
init_pub_messages(void)
|
||||
{
|
||||
memset(&_esc, 0, sizeof(_esc));
|
||||
_esc_pub = orb_advertise(ORB_ID(esc_status), &_esc);
|
||||
}
|
||||
|
||||
void
|
||||
build_gam_request(uint8_t *buffer, size_t *size)
|
||||
{
|
||||
struct gam_module_poll_msg msg;
|
||||
*size = sizeof(msg);
|
||||
memset(&msg, 0, *size);
|
||||
|
||||
msg.mode = BINARY_MODE_REQUEST_ID;
|
||||
msg.id = GAM_SENSOR_ID;
|
||||
|
||||
memcpy(buffer, &msg, *size);
|
||||
}
|
||||
|
||||
void
|
||||
publish_gam_message(const uint8_t *buffer)
|
||||
{
|
||||
struct gam_module_msg msg;
|
||||
size_t size = sizeof(msg);
|
||||
memset(&msg, 0, size);
|
||||
memcpy(&msg, buffer, size);
|
||||
|
||||
/* announce the esc if needed, just publish else */
|
||||
if (_esc_pub > 0) {
|
||||
orb_publish(ORB_ID(esc_status), _esc_pub, &_esc);
|
||||
} else {
|
||||
_esc_pub = orb_advertise(ORB_ID(esc_status), &_esc);
|
||||
}
|
||||
|
||||
// Publish it.
|
||||
_esc.esc_count = 1;
|
||||
_esc.esc_connectiontype = ESC_CONNECTION_TYPE_PPM;
|
||||
|
||||
_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));
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -79,12 +131,12 @@ build_eam_response(uint8_t *buffer, size_t *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);
|
||||
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);
|
||||
orb_copy(ORB_ID(battery_status), _battery_sub, &battery);
|
||||
|
||||
struct eam_module_msg msg;
|
||||
*size = sizeof(msg);
|
||||
|
|
@ -92,7 +144,7 @@ build_eam_response(uint8_t *buffer, size_t *size)
|
|||
|
||||
msg.start = START_BYTE;
|
||||
msg.eam_sensor_id = EAM_SENSOR_ID;
|
||||
msg.sensor_id = EAM_SENSOR_TEXT_ID;
|
||||
msg.sensor_text_id = EAM_SENSOR_TEXT_ID;
|
||||
|
||||
msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20);
|
||||
msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG;
|
||||
|
|
@ -106,12 +158,46 @@ build_eam_response(uint8_t *buffer, size_t *size)
|
|||
/* get a local copy of the airspeed data */
|
||||
struct airspeed_s airspeed;
|
||||
memset(&airspeed, 0, sizeof(airspeed));
|
||||
orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
|
||||
orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed);
|
||||
|
||||
uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6f);
|
||||
msg.speed_L = (uint8_t)speed & 0xff;
|
||||
msg.speed_H = (uint8_t)(speed >> 8) & 0xff;
|
||||
|
||||
msg.stop = STOP_BYTE;
|
||||
memcpy(buffer, &msg, *size);
|
||||
}
|
||||
|
||||
void
|
||||
build_gam_response(uint8_t *buffer, size_t *size)
|
||||
{
|
||||
/* get a local copy of the ESC Status values */
|
||||
struct esc_status_s esc;
|
||||
memset(&esc, 0, sizeof(esc));
|
||||
orb_copy(ORB_ID(esc_status), _esc_sub, &esc);
|
||||
|
||||
struct gam_module_msg msg;
|
||||
*size = sizeof(msg);
|
||||
memset(&msg, 0, *size);
|
||||
|
||||
msg.start = START_BYTE;
|
||||
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.temperature2 = 20; // 0 deg. C.
|
||||
|
||||
uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage);
|
||||
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);
|
||||
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);
|
||||
msg.rpm_L = (uint8_t)rpm & 0xff;
|
||||
msg.rpm_H = (uint8_t)(rpm >> 8) & 0xff;
|
||||
|
||||
msg.stop = STOP_BYTE;
|
||||
memcpy(buffer, &msg, *size);
|
||||
|
|
@ -123,14 +209,14 @@ build_gps_response(uint8_t *buffer, size_t *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);
|
||||
orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw);
|
||||
|
||||
/* get a local copy of the battery data */
|
||||
struct vehicle_gps_position_s gps;
|
||||
memset(&gps, 0, sizeof(gps));
|
||||
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps);
|
||||
orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps);
|
||||
|
||||
struct gps_module_msg msg = { 0 };
|
||||
struct gps_module_msg msg;
|
||||
*size = sizeof(msg);
|
||||
memset(&msg, 0, *size);
|
||||
|
||||
|
|
@ -150,7 +236,7 @@ build_gps_response(uint8_t *buffer, size_t *size)
|
|||
msg.flight_direction = (uint8_t)(gps.cog_rad * M_RAD_TO_DEG_F);
|
||||
|
||||
/* GPS speed */
|
||||
uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6);
|
||||
uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6f);
|
||||
msg.gps_speed_L = (uint8_t)speed & 0xff;
|
||||
msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff;
|
||||
|
||||
|
|
@ -196,33 +282,33 @@ build_gps_response(uint8_t *buffer, size_t *size)
|
|||
msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff;
|
||||
|
||||
/* Altitude */
|
||||
uint16_t alt = (uint16_t)(gps.alt*1e-3 + 500.0f);
|
||||
uint16_t alt = (uint16_t)(gps.alt*1e-3f + 500.0f);
|
||||
msg.altitude_L = (uint8_t)alt & 0xff;
|
||||
msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
|
||||
|
||||
/* Get any (and probably only ever one) home_sub postion report */
|
||||
/* Get any (and probably only ever one) _home_sub postion report */
|
||||
bool updated;
|
||||
orb_check(home_sub, &updated);
|
||||
orb_check(_home_sub, &updated);
|
||||
if (updated) {
|
||||
/* get a local copy of the home position data */
|
||||
struct home_position_s home;
|
||||
memset(&home, 0, sizeof(home));
|
||||
orb_copy(ORB_ID(home_position), home_sub, &home);
|
||||
orb_copy(ORB_ID(home_position), _home_sub, &home);
|
||||
|
||||
home_lat = ((double)(home.lat))*1e-7d;
|
||||
home_lon = ((double)(home.lon))*1e-7d;
|
||||
home_position_set = true;
|
||||
_home_lat = ((double)(home.lat))*1e-7d;
|
||||
_home_lon = ((double)(home.lon))*1e-7d;
|
||||
_home_position_set = true;
|
||||
}
|
||||
|
||||
/* Distance from home */
|
||||
if (home_position_set) {
|
||||
uint16_t dist = (uint16_t)get_distance_to_next_waypoint(home_lat, home_lon, lat, lon);
|
||||
if (_home_position_set) {
|
||||
uint16_t dist = (uint16_t)get_distance_to_next_waypoint(_home_lat, _home_lon, lat, lon);
|
||||
|
||||
msg.distance_L = (uint8_t)dist & 0xff;
|
||||
msg.distance_H = (uint8_t)(dist >> 8) & 0xff;
|
||||
|
||||
/* Direction back to home */
|
||||
uint16_t bearing = (uint16_t)(get_bearing_to_next_waypoint(home_lat, home_lon, lat, lon) * M_RAD_TO_DEG_F);
|
||||
uint16_t bearing = (uint16_t)(get_bearing_to_next_waypoint(_home_lat, _home_lon, lat, lon) * M_RAD_TO_DEG_F);
|
||||
msg.home_direction = (uint8_t)bearing >> 1;
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,249 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 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
|
||||
|
||||
/* The GAM Module poll message. */
|
||||
struct gam_module_poll_msg {
|
||||
uint8_t mode;
|
||||
uint8_t id;
|
||||
};
|
||||
|
||||
/* Electric Air Module (EAM) constants. */
|
||||
#define EAM_SENSOR_ID 0x8e
|
||||
#define EAM_SENSOR_TEXT_ID 0xe0
|
||||
|
||||
/* The Electric Air Module message. */
|
||||
struct eam_module_msg {
|
||||
uint8_t start; /**< Start byte */
|
||||
uint8_t eam_sensor_id; /**< EAM sensor */
|
||||
uint8_t warning;
|
||||
uint8_t sensor_text_id;
|
||||
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. */
|
||||
};
|
||||
|
||||
|
||||
/* General Air Module (GAM) constants. */
|
||||
#define GAM_SENSOR_ID 0x8d
|
||||
#define GAM_SENSOR_TEXT_ID 0xd0
|
||||
|
||||
struct gam_module_msg {
|
||||
uint8_t start; /**< Start byte */
|
||||
uint8_t gam_sensor_id; /**< GAM sensor id */
|
||||
uint8_t warning_beeps;
|
||||
uint8_t sensor_text_id;
|
||||
uint8_t alarm_invers1;
|
||||
uint8_t alarm_invers2;
|
||||
uint8_t cell1; /**< Lipo cell voltages. Not supported. */
|
||||
uint8_t cell2;
|
||||
uint8_t cell3;
|
||||
uint8_t cell4;
|
||||
uint8_t cell5;
|
||||
uint8_t cell6;
|
||||
uint8_t batt1_L; /**< Battery 1 voltage LSB value. 0.1V steps. 50 = 5.5V */
|
||||
uint8_t batt1_H;
|
||||
uint8_t batt2_L; /**< Battery 2 voltage LSB value. 0.1V steps. 50 = 5.5V */
|
||||
uint8_t batt2_H;
|
||||
uint8_t temperature1; /**< Temperature 1. offset of 20. a value of 20 = 0°C */
|
||||
uint8_t temperature2; /**< Temperature 2. offset of 20. a value of 20 = 0°C */
|
||||
uint8_t fuel_procent; /**< Fuel capacity in %. Values 0 - 100 */
|
||||
/**< Graphical display ranges: 0 25% 50% 75% 100% */
|
||||
uint8_t fuel_ml_L; /**< Fuel in ml scale. Full = 65535 */
|
||||
uint8_t fuel_ml_H;
|
||||
uint8_t rpm_L; /**< RPM in 10 RPM steps. 300 = 3000rpm */
|
||||
uint8_t rpm_H;
|
||||
uint8_t altitude_L; /**< Altitude in meters. offset of 500, 500 = 0m */
|
||||
uint8_t altitude_H;
|
||||
uint8_t climbrate_L; /**< Climb rate in 0.01m/s. Value of 30000 = 0.00 m/s */
|
||||
uint8_t climbrate_H;
|
||||
uint8_t climbrate3s; /**< Climb rate in m/3sec. Value of 120 = 0m/3sec */
|
||||
uint8_t current_L; /**< Current in 0.1A steps */
|
||||
uint8_t current_H;
|
||||
uint8_t main_voltage_L; /**< Main power voltage using 0.1V steps */
|
||||
uint8_t main_voltage_H;
|
||||
uint8_t batt_cap_L; /**< Used battery capacity in 10mAh steps */
|
||||
uint8_t batt_cap_H;
|
||||
uint8_t speed_L; /**< Speed in km/h */
|
||||
uint8_t speed_H;
|
||||
uint8_t min_cell_volt; /**< Minimum cell voltage in 2mV steps. 124 = 2,48V */
|
||||
uint8_t min_cell_volt_num; /**< Number of the cell with the lowest voltage */
|
||||
uint8_t rpm2_L; /**< RPM in 10 RPM steps. 300 = 3000rpm */
|
||||
uint8_t rpm2_H;
|
||||
uint8_t general_error_number; /**< Voice error == 12. TODO: more docu */
|
||||
uint8_t pressure; /**< Pressure up to 16bar. 0,1bar scale. 20 = 2bar */
|
||||
uint8_t version;
|
||||
uint8_t stop; /**< Stop byte */
|
||||
uint8_t checksum; /**< Lower 8-bits of all bytes summed */
|
||||
};
|
||||
|
||||
/* GPS sensor constants. */
|
||||
#define GPS_SENSOR_ID 0x8a
|
||||
#define GPS_SENSOR_TEXT_ID 0xa0
|
||||
|
||||
/**
|
||||
* The GPS sensor message
|
||||
* Struct based on: https://code.google.com/p/diy-hott-gps/downloads
|
||||
*/
|
||||
struct gps_module_msg {
|
||||
uint8_t start; /**< Start byte */
|
||||
uint8_t sensor_id; /**< GPS sensor ID*/
|
||||
uint8_t warning; /**< 0…= warning beeps */
|
||||
uint8_t sensor_text_id; /**< GPS Sensor text mode ID */
|
||||
uint8_t alarm_inverse1; /**< 01 inverse status */
|
||||
uint8_t alarm_inverse2; /**< 00 inverse status status 1 = no GPS Signal */
|
||||
uint8_t flight_direction; /**< 119 = Flightdir./dir. 1 = 2°; 0° (North), 9 0° (East), 180° (South), 270° (West) */
|
||||
uint8_t gps_speed_L; /**< 8 = /GPS speed low byte 8km/h */
|
||||
uint8_t gps_speed_H; /**< 0 = /GPS speed high byte */
|
||||
|
||||
uint8_t latitude_ns; /**< 000 = N = 48°39’988 */
|
||||
uint8_t latitude_min_L; /**< 231 0xE7 = 0x12E7 = 4839 */
|
||||
uint8_t latitude_min_H; /**< 018 18 = 0x12 */
|
||||
uint8_t latitude_sec_L; /**< 171 220 = 0xDC = 0x03DC =0988 */
|
||||
uint8_t latitude_sec_H; /**< 016 3 = 0x03 */
|
||||
|
||||
uint8_t longitude_ew; /**< 000 = E= 9° 25’9360 */
|
||||
uint8_t longitude_min_L; /**< 150 157 = 0x9D = 0x039D = 0925 */
|
||||
uint8_t longitude_min_H; /**< 003 3 = 0x03 */
|
||||
uint8_t longitude_sec_L; /**< 056 144 = 0x90 0x2490 = 9360 */
|
||||
uint8_t longitude_sec_H; /**< 004 36 = 0x24 */
|
||||
|
||||
uint8_t distance_L; /**< 027 123 = /distance low byte 6 = 6 m */
|
||||
uint8_t distance_H; /**< 036 35 = /distance high byte */
|
||||
uint8_t altitude_L; /**< 243 244 = /Altitude low byte 500 = 0m */
|
||||
uint8_t altitude_H; /**< 001 1 = /Altitude high byte */
|
||||
uint8_t resolution_L; /**< 48 = Low Byte m/s resolution 0.01m 48 = 30000 = 0.00m/s (1=0.01m/s) */
|
||||
uint8_t resolution_H; /**< 117 = High Byte m/s resolution 0.01m */
|
||||
uint8_t unknown1; /**< 120 = 0m/3s */
|
||||
uint8_t gps_num_sat; /**< GPS.Satellites (number of satelites) (1 byte) */
|
||||
uint8_t gps_fix_char; /**< GPS.FixChar. (GPS fix character. display, if DGPS, 2D oder 3D) (1 byte) */
|
||||
uint8_t home_direction; /**< HomeDirection (direction from starting point to Model position) (1 byte) */
|
||||
uint8_t angle_x_direction; /**< angle x-direction (1 byte) */
|
||||
uint8_t angle_y_direction; /**< angle y-direction (1 byte) */
|
||||
uint8_t angle_z_direction; /**< angle z-direction (1 byte) */
|
||||
uint8_t gyro_x_L; /**< gyro x low byte (2 bytes) */
|
||||
uint8_t gyro_x_H; /**< gyro x high byte */
|
||||
uint8_t gyro_y_L; /**< gyro y low byte (2 bytes) */
|
||||
uint8_t gyro_y_H; /**< gyro y high byte */
|
||||
uint8_t gyro_z_L; /**< gyro z low byte (2 bytes) */
|
||||
uint8_t gyro_z_H; /**< gyro z high byte */
|
||||
uint8_t vibration; /**< vibration (1 bytes) */
|
||||
uint8_t ascii4; /**< 00 ASCII Free Character [4] */
|
||||
uint8_t ascii5; /**< 00 ASCII Free Character [5] */
|
||||
uint8_t gps_fix; /**< 00 ASCII Free Character [6], we use it for GPS FIX */
|
||||
uint8_t version;
|
||||
uint8_t stop; /**< Stop byte */
|
||||
uint8_t checksum; /**< Lower 8-bits of all bytes summed */
|
||||
};
|
||||
|
||||
// The maximum size of a message.
|
||||
#define MAX_MESSAGE_BUFFER_SIZE 45
|
||||
|
||||
void init_sub_messages(void);
|
||||
void init_pub_messages(void);
|
||||
void build_gam_request(uint8_t *buffer, size_t *size);
|
||||
void publish_gam_message(const uint8_t *buffer);
|
||||
void build_eam_response(uint8_t *buffer, size_t *size);
|
||||
void build_gam_response(uint8_t *buffer, size_t *size);
|
||||
void build_gps_response(uint8_t *buffer, size_t *size);
|
||||
float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
|
||||
void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec);
|
||||
|
||||
|
||||
#endif /* MESSAGES_H_ */
|
||||
|
|
@ -1,196 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 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 EAM_SENSOR_ID 0x8e
|
||||
#define EAM_SENSOR_TEXT_ID 0xe0
|
||||
|
||||
/* The Electric Air Module message. */
|
||||
struct eam_module_msg {
|
||||
uint8_t start; /**< Start byte */
|
||||
uint8_t eam_sensor_id; /**< EAM sensor */
|
||||
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. */
|
||||
};
|
||||
|
||||
/**
|
||||
* The maximum buffer size required to store a HoTT message.
|
||||
*/
|
||||
#define MESSAGE_BUFFER_SIZE sizeof(union { \
|
||||
struct eam_module_msg eam; \
|
||||
})
|
||||
|
||||
/* GPS sensor constants. */
|
||||
#define GPS_SENSOR_ID 0x8A
|
||||
#define GPS_SENSOR_TEXT_ID 0xA0
|
||||
|
||||
/**
|
||||
* The GPS sensor message
|
||||
* Struct based on: https://code.google.com/p/diy-hott-gps/downloads
|
||||
*/
|
||||
struct gps_module_msg {
|
||||
uint8_t start; /**< Start byte */
|
||||
uint8_t sensor_id; /**< GPS sensor ID*/
|
||||
uint8_t warning; /**< Byte 3: 0…= warning beeps */
|
||||
uint8_t sensor_text_id; /**< GPS Sensor text mode ID */
|
||||
uint8_t alarm_inverse1; /**< Byte 5: 01 inverse status */
|
||||
uint8_t alarm_inverse2; /**< Byte 6: 00 inverse status status 1 = no GPS Signal */
|
||||
uint8_t flight_direction; /**< Byte 7: 119 = Flightdir./dir. 1 = 2°; 0° (North), 9 0° (East), 180° (South), 270° (West) */
|
||||
uint8_t gps_speed_L; /**< Byte 8: 8 = /GPS speed low byte 8km/h */
|
||||
uint8_t gps_speed_H; /**< Byte 9: 0 = /GPS speed high byte */
|
||||
|
||||
uint8_t latitude_ns; /**< Byte 10: 000 = N = 48°39’988 */
|
||||
uint8_t latitude_min_L; /**< Byte 11: 231 0xE7 = 0x12E7 = 4839 */
|
||||
uint8_t latitude_min_H; /**< Byte 12: 018 18 = 0x12 */
|
||||
uint8_t latitude_sec_L; /**< Byte 13: 171 220 = 0xDC = 0x03DC =0988 */
|
||||
uint8_t latitude_sec_H; /**< Byte 14: 016 3 = 0x03 */
|
||||
|
||||
uint8_t longitude_ew; /**< Byte 15: 000 = E= 9° 25’9360 */
|
||||
uint8_t longitude_min_L; /**< Byte 16: 150 157 = 0x9D = 0x039D = 0925 */
|
||||
uint8_t longitude_min_H; /**< Byte 17: 003 3 = 0x03 */
|
||||
uint8_t longitude_sec_L; /**< Byte 18: 056 144 = 0x90 0x2490 = 9360*/
|
||||
uint8_t longitude_sec_H; /**< Byte 19: 004 36 = 0x24 */
|
||||
|
||||
uint8_t distance_L; /**< Byte 20: 027 123 = /distance low byte 6 = 6 m */
|
||||
uint8_t distance_H; /**< Byte 21: 036 35 = /distance high byte */
|
||||
uint8_t altitude_L; /**< Byte 22: 243 244 = /Altitude low byte 500 = 0m */
|
||||
uint8_t altitude_H; /**< Byte 23: 001 1 = /Altitude high byte */
|
||||
uint8_t resolution_L; /**< Byte 24: 48 = Low Byte m/s resolution 0.01m 48 = 30000 = 0.00m/s (1=0.01m/s) */
|
||||
uint8_t resolution_H; /**< Byte 25: 117 = High Byte m/s resolution 0.01m */
|
||||
uint8_t unknown1; /**< Byte 26: 120 = 0m/3s */
|
||||
uint8_t gps_num_sat; /**< Byte 27: GPS.Satellites (number of satelites) (1 byte) */
|
||||
uint8_t gps_fix_char; /**< Byte 28: GPS.FixChar. (GPS fix character. display, if DGPS, 2D oder 3D) (1 byte) */
|
||||
uint8_t home_direction; /**< Byte 29: HomeDirection (direction from starting point to Model position) (1 byte) */
|
||||
uint8_t angle_x_direction; /**< Byte 30: angle x-direction (1 byte) */
|
||||
uint8_t angle_y_direction; /**< Byte 31: angle y-direction (1 byte) */
|
||||
uint8_t angle_z_direction; /**< Byte 32: angle z-direction (1 byte) */
|
||||
uint8_t gyro_x_L; /**< Byte 33: gyro x low byte (2 bytes) */
|
||||
uint8_t gyro_x_H; /**< Byte 34: gyro x high byte */
|
||||
uint8_t gyro_y_L; /**< Byte 35: gyro y low byte (2 bytes) */
|
||||
uint8_t gyro_y_H; /**< Byte 36: gyro y high byte */
|
||||
uint8_t gyro_z_L; /**< Byte 37: gyro z low byte (2 bytes) */
|
||||
uint8_t gyro_z_H; /**< Byte 38: gyro z high byte */
|
||||
uint8_t vibration; /**< Byte 39: vibration (1 bytes) */
|
||||
uint8_t ascii4; /**< Byte 40: 00 ASCII Free Character [4] */
|
||||
uint8_t ascii5; /**< Byte 41: 00 ASCII Free Character [5] */
|
||||
uint8_t gps_fix; /**< Byte 42: 00 ASCII Free Character [6], we use it for GPS FIX */
|
||||
uint8_t version; /**< Byte 43: 00 version number */
|
||||
uint8_t stop; /**< Byte 44: 0x7D Ende byte */
|
||||
uint8_t checksum; /**< Byte 45: Parity Byte */
|
||||
};
|
||||
|
||||
/**
|
||||
* The maximum buffer size required to store a HoTT message.
|
||||
*/
|
||||
#define GPS_MESSAGE_BUFFER_SIZE sizeof(union { \
|
||||
struct gps_module_msg gps; \
|
||||
})
|
||||
|
||||
void messages_init(void);
|
||||
void build_eam_response(uint8_t *buffer, size_t *size);
|
||||
void build_gps_response(uint8_t *buffer, size_t *size);
|
||||
float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
|
||||
void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec);
|
||||
|
||||
|
||||
#endif /* MESSAGES_H_ */
|
||||
|
|
@ -282,7 +282,7 @@ void tune_error(void)
|
|||
|
||||
void do_rc_calibration(int status_pub, struct vehicle_status_s *status)
|
||||
{
|
||||
if (current_status.offboard_control_signal_lost) {
|
||||
if (current_status.rc_signal_lost) {
|
||||
mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal.");
|
||||
return;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -62,7 +62,7 @@ float calc_indicated_airspeed(float differential_pressure)
|
|||
if (differential_pressure > 0) {
|
||||
return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
|
||||
} else {
|
||||
return -sqrtf((2.0f*fabs(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
|
||||
return -sqrtf((2.0f*fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -106,6 +106,6 @@ float calc_true_airspeed(float total_pressure, float static_pressure, float temp
|
|||
return sqrtf((2.0f*(pressure_difference)) / density);
|
||||
} else
|
||||
{
|
||||
return -sqrtf((2.0f*fabs(pressure_difference)) / density);
|
||||
return -sqrtf((2.0f*fabsf(pressure_difference)) / density);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -63,7 +63,8 @@
|
|||
|
||||
enum ESC_VENDOR {
|
||||
ESC_VENDOR_GENERIC = 0, /**< generic ESC */
|
||||
ESC_VENDOR_MIKROKOPTER /**< Mikrokopter */
|
||||
ESC_VENDOR_MIKROKOPTER, /**< Mikrokopter */
|
||||
ESC_VENDOR_GRAUPNER_HOTT /**< Graupner HoTT ESC */
|
||||
};
|
||||
|
||||
enum ESC_CONNECTION_TYPE {
|
||||
|
|
|
|||
Loading…
Reference in New Issue