mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-24 15:40:31 +08:00
Merged move of additional apps out of NuttX folders
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
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,180 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 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 KalmanNav.hpp
|
||||
*
|
||||
* kalman filter navigation code
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
//#define MATRIX_ASSERT
|
||||
//#define VECTOR_ASSERT
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <controllib/blocks.hpp>
|
||||
#include <controllib/block/BlockParam.hpp>
|
||||
#include <controllib/block/UOrbSubscription.hpp>
|
||||
#include <controllib/block/UOrbPublication.hpp>
|
||||
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <poll.h>
|
||||
#include <unistd.h>
|
||||
|
||||
/**
|
||||
* Kalman filter navigation class
|
||||
* http://en.wikipedia.org/wiki/Extended_Kalman_filter
|
||||
* Discrete-time extended Kalman filter
|
||||
*/
|
||||
class KalmanNav : public control::SuperBlock
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* Constructor
|
||||
*/
|
||||
KalmanNav(SuperBlock *parent, const char *name);
|
||||
|
||||
/**
|
||||
* Deconstuctor
|
||||
*/
|
||||
|
||||
virtual ~KalmanNav() {};
|
||||
/**
|
||||
* The main callback function for the class
|
||||
*/
|
||||
void update();
|
||||
|
||||
|
||||
/**
|
||||
* Publication update
|
||||
*/
|
||||
virtual void updatePublications();
|
||||
|
||||
/**
|
||||
* State prediction
|
||||
* Continuous, non-linear
|
||||
*/
|
||||
int predictState(float dt);
|
||||
|
||||
/**
|
||||
* State covariance prediction
|
||||
* Continuous, linear
|
||||
*/
|
||||
int predictStateCovariance(float dt);
|
||||
|
||||
/**
|
||||
* Attitude correction
|
||||
*/
|
||||
int correctAtt();
|
||||
|
||||
/**
|
||||
* Position correction
|
||||
*/
|
||||
int correctPos();
|
||||
|
||||
/**
|
||||
* Overloaded update parameters
|
||||
*/
|
||||
virtual void updateParams();
|
||||
protected:
|
||||
// kalman filter
|
||||
math::Matrix F; /**< Jacobian(f,x), where dx/dt = f(x,u) */
|
||||
math::Matrix G; /**< noise shaping matrix for gyro/accel */
|
||||
math::Matrix P; /**< state covariance matrix */
|
||||
math::Matrix P0; /**< initial state covariance matrix */
|
||||
math::Matrix V; /**< gyro/ accel noise matrix */
|
||||
math::Matrix HAtt; /**< attitude measurement matrix */
|
||||
math::Matrix RAtt; /**< attitude measurement noise matrix */
|
||||
math::Matrix HPos; /**< position measurement jacobian matrix */
|
||||
math::Matrix RPos; /**< position measurement noise matrix */
|
||||
// attitude
|
||||
math::Dcm C_nb; /**< direction cosine matrix from body to nav frame */
|
||||
math::Quaternion q; /**< quaternion from body to nav frame */
|
||||
// subscriptions
|
||||
control::UOrbSubscription<sensor_combined_s> _sensors; /**< sensors sub. */
|
||||
control::UOrbSubscription<vehicle_gps_position_s> _gps; /**< gps sub. */
|
||||
control::UOrbSubscription<parameter_update_s> _param_update; /**< parameter update sub. */
|
||||
// publications
|
||||
control::UOrbPublication<vehicle_global_position_s> _pos; /**< position pub. */
|
||||
control::UOrbPublication<vehicle_attitude_s> _att; /**< attitude pub. */
|
||||
// time stamps
|
||||
uint64_t _pubTimeStamp; /**< output data publication time stamp */
|
||||
uint64_t _predictTimeStamp; /**< prediction time stamp */
|
||||
uint64_t _attTimeStamp; /**< attitude correction time stamp */
|
||||
uint64_t _outTimeStamp; /**< output time stamp */
|
||||
// frame count
|
||||
uint16_t _navFrames; /**< navigation frames completed in output cycle */
|
||||
// miss counts
|
||||
uint16_t _miss; /**< number of times fast prediction loop missed */
|
||||
// accelerations
|
||||
float fN, fE, fD; /**< navigation frame acceleration */
|
||||
// states
|
||||
enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT}; /**< state enumeration */
|
||||
float phi, theta, psi; /**< 3-2-1 euler angles */
|
||||
float vN, vE, vD; /**< navigation velocity, m/s */
|
||||
double lat, lon, alt; /**< lat, lon, alt, radians */
|
||||
// parameters
|
||||
control::BlockParam<float> _vGyro; /**< gyro process noise */
|
||||
control::BlockParam<float> _vAccel; /**< accelerometer process noise */
|
||||
control::BlockParam<float> _rMag; /**< magnetometer measurement noise */
|
||||
control::BlockParam<float> _rGpsVel; /**< gps velocity measurement noise */
|
||||
control::BlockParam<float> _rGpsPos; /**< gps position measurement noise */
|
||||
control::BlockParam<float> _rGpsAlt; /**< gps altitude measurement noise */
|
||||
control::BlockParam<float> _rPressAlt; /**< press altitude measurement noise */
|
||||
control::BlockParam<float> _rAccel; /**< accelerometer measurement noise */
|
||||
control::BlockParam<float> _magDip; /**< magnetic inclination with level */
|
||||
control::BlockParam<float> _magDec; /**< magnetic declination, clockwise rotation */
|
||||
control::BlockParam<float> _g; /**< gravitational constant */
|
||||
control::BlockParam<float> _faultPos; /**< fault detection threshold for position */
|
||||
control::BlockParam<float> _faultAtt; /**< fault detection threshold for attitude */
|
||||
// status
|
||||
bool _attitudeInitialized;
|
||||
bool _positionInitialized;
|
||||
uint16_t _attitudeInitCounter;
|
||||
// accessors
|
||||
int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); }
|
||||
void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; }
|
||||
int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); }
|
||||
void setLonDegE7(int32_t val) { lon = val / 1.0e7 / M_RAD_TO_DEG; }
|
||||
int32_t getAltE3() { return int32_t(alt * 1.0e3); }
|
||||
void setAltE3(int32_t val) { alt = double(val) / 1.0e3; }
|
||||
};
|
||||
@@ -0,0 +1,152 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Example User <mail@example.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 kalman_demo.cpp
|
||||
* Demonstration of control library
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <math.h>
|
||||
#include "KalmanNav.hpp"
|
||||
|
||||
static bool thread_should_exit = false; /**< Deamon exit flag */
|
||||
static bool thread_running = false; /**< Deamon status flag */
|
||||
static int deamon_task; /**< Handle of deamon task / thread */
|
||||
|
||||
/**
|
||||
* Deamon management function.
|
||||
*/
|
||||
extern "C" __EXPORT int att_pos_estimator_ekf_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Mainloop of deamon.
|
||||
*/
|
||||
int kalman_demo_thread_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Print the correct usage.
|
||||
*/
|
||||
static void usage(const char *reason);
|
||||
|
||||
static void
|
||||
usage(const char *reason)
|
||||
{
|
||||
if (reason)
|
||||
fprintf(stderr, "%s\n", reason);
|
||||
|
||||
fprintf(stderr, "usage: kalman_demo {start|stop|status} [-p <additional params>]\n\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
/**
|
||||
* The deamon app only briefly exists to start
|
||||
* the background job. The stack size assigned in the
|
||||
* Makefile does only apply to this management task.
|
||||
*
|
||||
* The actual stack size should be set in the call
|
||||
* to task_create().
|
||||
*/
|
||||
int att_pos_estimator_ekf_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
if (argc < 1)
|
||||
usage("missing command");
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (thread_running) {
|
||||
printf("kalman_demo already running\n");
|
||||
/* this is not an error */
|
||||
exit(0);
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
deamon_task = task_spawn("kalman_demo",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
4096,
|
||||
kalman_demo_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) {
|
||||
printf("\tkalman_demo app is running\n");
|
||||
|
||||
} else {
|
||||
printf("\tkalman_demo app not started\n");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
usage("unrecognized command");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
int kalman_demo_thread_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
printf("[kalman_demo] starting\n");
|
||||
|
||||
using namespace math;
|
||||
|
||||
thread_running = true;
|
||||
|
||||
KalmanNav nav(NULL, "KF");
|
||||
|
||||
while (!thread_should_exit) {
|
||||
nav.update();
|
||||
}
|
||||
|
||||
printf("[kalman_demo] exiting.\n");
|
||||
|
||||
thread_running = false;
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,45 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# Full attitude / position Extended Kalman Filter
|
||||
#
|
||||
|
||||
MODULE_COMMAND = att_pos_estimator_ekf
|
||||
|
||||
# XXX this might be intended for the spawned deamon, validate
|
||||
MODULE_PRIORITY = "SCHED_PRIORITY_MAX-30"
|
||||
|
||||
SRCS = kalman_main.cpp \
|
||||
KalmanNav.cpp \
|
||||
params.c
|
||||
@@ -0,0 +1,16 @@
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
|
||||
PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.008f);
|
||||
PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 5.0f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 5.0f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_PRESS_ALT, 0.1f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f);
|
||||
PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f);
|
||||
PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f);
|
||||
PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f);
|
||||
PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f);
|
||||
@@ -0,0 +1,169 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Thomas Gubler <thomasgubler@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 fixedwing_att_control_rate.c
|
||||
* Implementation of a fixed wing attitude controller.
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include <math.h>
|
||||
#include <poll.h>
|
||||
#include <time.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <arch/board/board.h>
|
||||
#include <uORB/uORB.h>
|
||||
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/pid/pid.h>
|
||||
#include <systemlib/geo/geo.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
#include "fixedwing_att_control_att.h"
|
||||
|
||||
|
||||
struct fw_att_control_params {
|
||||
float roll_p;
|
||||
float rollrate_lim;
|
||||
float pitch_p;
|
||||
float pitchrate_lim;
|
||||
float yawrate_lim;
|
||||
float pitch_roll_compensation_p;
|
||||
};
|
||||
|
||||
struct fw_pos_control_param_handles {
|
||||
param_t roll_p;
|
||||
param_t rollrate_lim;
|
||||
param_t pitch_p;
|
||||
param_t pitchrate_lim;
|
||||
param_t yawrate_lim;
|
||||
param_t pitch_roll_compensation_p;
|
||||
};
|
||||
|
||||
|
||||
|
||||
/* Internal Prototypes */
|
||||
static int parameters_init(struct fw_pos_control_param_handles *h);
|
||||
static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_att_control_params *p);
|
||||
|
||||
static int parameters_init(struct fw_pos_control_param_handles *h)
|
||||
{
|
||||
/* PID parameters */
|
||||
h->roll_p = param_find("FW_ROLL_P");
|
||||
h->rollrate_lim = param_find("FW_ROLLR_LIM");
|
||||
h->pitch_p = param_find("FW_PITCH_P");
|
||||
h->pitchrate_lim = param_find("FW_PITCHR_LIM");
|
||||
h->yawrate_lim = param_find("FW_YAWR_LIM");
|
||||
h->pitch_roll_compensation_p = param_find("FW_PITCH_RCOMP");
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_att_control_params *p)
|
||||
{
|
||||
param_get(h->roll_p, &(p->roll_p));
|
||||
param_get(h->rollrate_lim, &(p->rollrate_lim));
|
||||
param_get(h->pitch_p, &(p->pitch_p));
|
||||
param_get(h->pitchrate_lim, &(p->pitchrate_lim));
|
||||
param_get(h->yawrate_lim, &(p->yawrate_lim));
|
||||
param_get(h->pitch_roll_compensation_p, &(p->pitch_roll_compensation_p));
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
|
||||
const struct vehicle_attitude_s *att,
|
||||
const float speed_body[],
|
||||
struct vehicle_rates_setpoint_s *rates_sp)
|
||||
{
|
||||
static int counter = 0;
|
||||
static bool initialized = false;
|
||||
|
||||
static struct fw_att_control_params p;
|
||||
static struct fw_pos_control_param_handles h;
|
||||
|
||||
static PID_t roll_controller;
|
||||
static PID_t pitch_controller;
|
||||
|
||||
|
||||
if (!initialized) {
|
||||
parameters_init(&h);
|
||||
parameters_update(&h, &p);
|
||||
pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller
|
||||
pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller
|
||||
initialized = true;
|
||||
}
|
||||
|
||||
/* load new parameters with lower rate */
|
||||
if (counter % 100 == 0) {
|
||||
/* update parameters from storage */
|
||||
parameters_update(&h, &p);
|
||||
pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim);
|
||||
pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim);
|
||||
}
|
||||
|
||||
/* Roll (P) */
|
||||
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0);
|
||||
|
||||
|
||||
/* Pitch (P) */
|
||||
|
||||
/* compensate feedforward for loss of lift due to non-horizontal angle of wing */
|
||||
float pitch_sp_rollcompensation = p.pitch_roll_compensation_p * fabsf(sinf(att_sp->roll_body));
|
||||
/* set pitch plus feedforward roll compensation */
|
||||
rates_sp->pitch = pid_calculate(&pitch_controller,
|
||||
att_sp->pitch_body + pitch_sp_rollcompensation,
|
||||
att->pitch, 0, 0);
|
||||
|
||||
/* Yaw (from coordinated turn constraint or lateral force) */
|
||||
rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll))
|
||||
/ (speed_body[0] * cosf(att->roll) * cosf(att->pitch) + speed_body[2] * sinf(att->pitch));
|
||||
|
||||
// printf("rates_sp->yaw %.4f \n", (double)rates_sp->yaw);
|
||||
|
||||
counter++;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,51 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Thomas Gubler <thomasgubler@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 Fixed Wing Attitude Control */
|
||||
|
||||
#ifndef FIXEDWING_ATT_CONTROL_ATT_H_
|
||||
#define FIXEDWING_ATT_CONTROL_ATT_H_
|
||||
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
|
||||
int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
|
||||
const struct vehicle_attitude_s *att,
|
||||
const float speed_body[],
|
||||
struct vehicle_rates_setpoint_s *rates_sp);
|
||||
|
||||
#endif /* FIXEDWING_ATT_CONTROL_ATT_H_ */
|
||||
@@ -0,0 +1,370 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Doug Weibel <douglas.weibel@colorado.edu>
|
||||
*
|
||||
* 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 fixedwing_att_control.c
|
||||
* Implementation of a fixed wing attitude controller.
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include <math.h>
|
||||
#include <poll.h>
|
||||
#include <time.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <arch/board/board.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_global_position_setpoint.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/debug_key_value.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/pid/pid.h>
|
||||
#include <systemlib/geo/geo.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
#include "fixedwing_att_control_rate.h"
|
||||
#include "fixedwing_att_control_att.h"
|
||||
|
||||
/* Prototypes */
|
||||
/**
|
||||
* Deamon management function.
|
||||
*/
|
||||
__EXPORT int fixedwing_att_control_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Mainloop of deamon.
|
||||
*/
|
||||
int fixedwing_att_control_thread_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Print the correct usage.
|
||||
*/
|
||||
static void usage(const char *reason);
|
||||
|
||||
/* Variables */
|
||||
static bool thread_should_exit = false; /**< Deamon exit flag */
|
||||
static bool thread_running = false; /**< Deamon status flag */
|
||||
static int deamon_task; /**< Handle of deamon task / thread */
|
||||
|
||||
/* Main Thread */
|
||||
int fixedwing_att_control_thread_main(int argc, char *argv[])
|
||||
{
|
||||
/* read arguments */
|
||||
bool verbose = false;
|
||||
|
||||
for (int i = 1; i < argc; i++) {
|
||||
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
|
||||
verbose = true;
|
||||
}
|
||||
}
|
||||
|
||||
/* welcome user */
|
||||
printf("[fixedwing att control] started\n");
|
||||
|
||||
/* declare and safely initialize all structs */
|
||||
struct vehicle_attitude_s att;
|
||||
memset(&att, 0, sizeof(att));
|
||||
struct vehicle_attitude_setpoint_s att_sp;
|
||||
memset(&att_sp, 0, sizeof(att_sp));
|
||||
struct vehicle_rates_setpoint_s rates_sp;
|
||||
memset(&rates_sp, 0, sizeof(rates_sp));
|
||||
struct vehicle_global_position_s global_pos;
|
||||
memset(&global_pos, 0, sizeof(global_pos));
|
||||
struct manual_control_setpoint_s manual_sp;
|
||||
memset(&manual_sp, 0, sizeof(manual_sp));
|
||||
struct vehicle_status_s vstatus;
|
||||
memset(&vstatus, 0, sizeof(vstatus));
|
||||
|
||||
/* output structs */
|
||||
struct actuator_controls_s actuators;
|
||||
memset(&actuators, 0, sizeof(actuators));
|
||||
|
||||
|
||||
/* publish actuator controls */
|
||||
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
|
||||
actuators.control[i] = 0.0f;
|
||||
}
|
||||
|
||||
orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
|
||||
orb_advert_t rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
|
||||
|
||||
/* subscribe */
|
||||
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
|
||||
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
|
||||
/* Setup of loop */
|
||||
float gyro[3] = {0.0f, 0.0f, 0.0f};
|
||||
float speed_body[3] = {0.0f, 0.0f, 0.0f};
|
||||
struct pollfd fds = { .fd = att_sub, .events = POLLIN };
|
||||
|
||||
while (!thread_should_exit) {
|
||||
/* wait for a sensor update, check for exit condition every 500 ms */
|
||||
poll(&fds, 1, 500);
|
||||
|
||||
/* Check if there is a new position measurement or attitude setpoint */
|
||||
bool pos_updated;
|
||||
orb_check(global_pos_sub, &pos_updated);
|
||||
bool att_sp_updated;
|
||||
orb_check(att_sp_sub, &att_sp_updated);
|
||||
|
||||
/* get a local copy of attitude */
|
||||
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
|
||||
|
||||
if (att_sp_updated)
|
||||
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
|
||||
|
||||
if (pos_updated) {
|
||||
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
|
||||
|
||||
if (att.R_valid) {
|
||||
speed_body[0] = att.R[0][0] * global_pos.vx + att.R[0][1] * global_pos.vy + att.R[0][2] * global_pos.vz;
|
||||
speed_body[1] = att.R[1][0] * global_pos.vx + att.R[1][1] * global_pos.vy + att.R[1][2] * global_pos.vz;
|
||||
speed_body[2] = att.R[2][0] * global_pos.vx + att.R[2][1] * global_pos.vy + att.R[2][2] * global_pos.vz;
|
||||
|
||||
} else {
|
||||
speed_body[0] = 0;
|
||||
speed_body[1] = 0;
|
||||
speed_body[2] = 0;
|
||||
|
||||
printf("FW ATT CONTROL: Did not get a valid R\n");
|
||||
}
|
||||
}
|
||||
|
||||
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
|
||||
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
|
||||
|
||||
gyro[0] = att.rollspeed;
|
||||
gyro[1] = att.pitchspeed;
|
||||
gyro[2] = att.yawspeed;
|
||||
|
||||
/* control */
|
||||
|
||||
if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
|
||||
vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
|
||||
/* attitude control */
|
||||
fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
|
||||
|
||||
/* angular rate control */
|
||||
fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
|
||||
|
||||
/* pass through throttle */
|
||||
actuators.control[3] = att_sp.thrust;
|
||||
|
||||
/* set flaps to zero */
|
||||
actuators.control[4] = 0.0f;
|
||||
|
||||
} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
|
||||
if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
|
||||
|
||||
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
|
||||
if (vstatus.rc_signal_lost) {
|
||||
|
||||
/* put plane into loiter */
|
||||
att_sp.roll_body = 0.3f;
|
||||
att_sp.pitch_body = 0.0f;
|
||||
|
||||
/* limit throttle to 60 % of last value if sane */
|
||||
if (isfinite(manual_sp.throttle) &&
|
||||
(manual_sp.throttle >= 0.0f) &&
|
||||
(manual_sp.throttle <= 1.0f)) {
|
||||
att_sp.thrust = 0.6f * manual_sp.throttle;
|
||||
|
||||
} else {
|
||||
att_sp.thrust = 0.0f;
|
||||
}
|
||||
|
||||
att_sp.yaw_body = 0;
|
||||
|
||||
// XXX disable yaw control, loiter
|
||||
|
||||
} else {
|
||||
|
||||
att_sp.roll_body = manual_sp.roll;
|
||||
att_sp.pitch_body = manual_sp.pitch;
|
||||
att_sp.yaw_body = 0;
|
||||
att_sp.thrust = manual_sp.throttle;
|
||||
}
|
||||
|
||||
att_sp.timestamp = hrt_absolute_time();
|
||||
|
||||
/* attitude control */
|
||||
fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
|
||||
|
||||
/* angular rate control */
|
||||
fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
|
||||
|
||||
/* pass through throttle */
|
||||
actuators.control[3] = att_sp.thrust;
|
||||
|
||||
/* pass through flaps */
|
||||
if (isfinite(manual_sp.flaps)) {
|
||||
actuators.control[4] = manual_sp.flaps;
|
||||
|
||||
} else {
|
||||
actuators.control[4] = 0.0f;
|
||||
}
|
||||
|
||||
} else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
|
||||
/* directly pass through values */
|
||||
actuators.control[0] = manual_sp.roll;
|
||||
/* positive pitch means negative actuator -> pull up */
|
||||
actuators.control[1] = manual_sp.pitch;
|
||||
actuators.control[2] = manual_sp.yaw;
|
||||
actuators.control[3] = manual_sp.throttle;
|
||||
|
||||
if (isfinite(manual_sp.flaps)) {
|
||||
actuators.control[4] = manual_sp.flaps;
|
||||
|
||||
} else {
|
||||
actuators.control[4] = 0.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* publish rates */
|
||||
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
|
||||
|
||||
/* sanity check and publish actuator outputs */
|
||||
if (isfinite(actuators.control[0]) &&
|
||||
isfinite(actuators.control[1]) &&
|
||||
isfinite(actuators.control[2]) &&
|
||||
isfinite(actuators.control[3])) {
|
||||
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
|
||||
}
|
||||
}
|
||||
|
||||
printf("[fixedwing_att_control] exiting, stopping all motors.\n");
|
||||
thread_running = false;
|
||||
|
||||
/* kill all outputs */
|
||||
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
|
||||
actuators.control[i] = 0.0f;
|
||||
|
||||
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
|
||||
|
||||
|
||||
|
||||
close(att_sub);
|
||||
close(actuator_pub);
|
||||
close(rates_pub);
|
||||
|
||||
fflush(stdout);
|
||||
exit(0);
|
||||
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
/* Startup Functions */
|
||||
|
||||
static void
|
||||
usage(const char *reason)
|
||||
{
|
||||
if (reason)
|
||||
fprintf(stderr, "%s\n", reason);
|
||||
|
||||
fprintf(stderr, "usage: fixedwing_att_control {start|stop|status}\n\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
/**
|
||||
* The deamon app only briefly exists to start
|
||||
* the background job. The stack size assigned in the
|
||||
* Makefile does only apply to this management task.
|
||||
*
|
||||
* The actual stack size should be set in the call
|
||||
* to task_create().
|
||||
*/
|
||||
int fixedwing_att_control_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 1)
|
||||
usage("missing command");
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (thread_running) {
|
||||
printf("fixedwing_att_control already running\n");
|
||||
/* this is not an error */
|
||||
exit(0);
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
deamon_task = task_spawn("fixedwing_att_control",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 20,
|
||||
2048,
|
||||
fixedwing_att_control_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
thread_running = true;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
thread_should_exit = true;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (thread_running) {
|
||||
printf("\tfixedwing_att_control is running\n");
|
||||
|
||||
} else {
|
||||
printf("\tfixedwing_att_control not started\n");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
usage("unrecognized command");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,211 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Thomas Gubler <thomasgubler@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 fixedwing_att_control_rate.c
|
||||
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
*
|
||||
* Implementation of a fixed wing attitude controller.
|
||||
*
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include <math.h>
|
||||
#include <poll.h>
|
||||
#include <time.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <arch/board/board.h>
|
||||
#include <uORB/uORB.h>
|
||||
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/pid/pid.h>
|
||||
#include <systemlib/geo/geo.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
#include "fixedwing_att_control_rate.h"
|
||||
|
||||
/*
|
||||
* Controller parameters, accessible via MAVLink
|
||||
*
|
||||
*/
|
||||
// Roll control parameters
|
||||
PARAM_DEFINE_FLOAT(FW_ROLLR_P, 0.9f);
|
||||
PARAM_DEFINE_FLOAT(FW_ROLLR_I, 0.2f);
|
||||
PARAM_DEFINE_FLOAT(FW_ROLLR_AWU, 0.9f);
|
||||
PARAM_DEFINE_FLOAT(FW_ROLLR_LIM, 0.7f); // Roll rate limit in radians/sec, applies to the roll controller
|
||||
PARAM_DEFINE_FLOAT(FW_ROLL_P, 4.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_PITCH_RCOMP, 0.1f);
|
||||
|
||||
//Pitch control parameters
|
||||
PARAM_DEFINE_FLOAT(FW_PITCHR_P, 0.8f);
|
||||
PARAM_DEFINE_FLOAT(FW_PITCHR_I, 0.2f);
|
||||
PARAM_DEFINE_FLOAT(FW_PITCHR_AWU, 0.8f);
|
||||
PARAM_DEFINE_FLOAT(FW_PITCHR_LIM, 0.35f); // Pitch rate limit in radians/sec, applies to the pitch controller
|
||||
PARAM_DEFINE_FLOAT(FW_PITCH_P, 8.0f);
|
||||
|
||||
//Yaw control parameters //XXX TODO this is copy paste, asign correct values
|
||||
PARAM_DEFINE_FLOAT(FW_YAWR_P, 0.3f);
|
||||
PARAM_DEFINE_FLOAT(FW_YAWR_I, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_YAWR_AWU, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_YAWR_LIM, 0.35f); // Yaw rate limit in radians/sec
|
||||
|
||||
/* feedforward compensation */
|
||||
PARAM_DEFINE_FLOAT(FW_PITCH_THR_P, 0.1f); /**< throttle to pitch coupling feedforward */
|
||||
|
||||
struct fw_rate_control_params {
|
||||
float rollrate_p;
|
||||
float rollrate_i;
|
||||
float rollrate_awu;
|
||||
float pitchrate_p;
|
||||
float pitchrate_i;
|
||||
float pitchrate_awu;
|
||||
float yawrate_p;
|
||||
float yawrate_i;
|
||||
float yawrate_awu;
|
||||
float pitch_thr_ff;
|
||||
};
|
||||
|
||||
struct fw_rate_control_param_handles {
|
||||
param_t rollrate_p;
|
||||
param_t rollrate_i;
|
||||
param_t rollrate_awu;
|
||||
param_t pitchrate_p;
|
||||
param_t pitchrate_i;
|
||||
param_t pitchrate_awu;
|
||||
param_t yawrate_p;
|
||||
param_t yawrate_i;
|
||||
param_t yawrate_awu;
|
||||
param_t pitch_thr_ff;
|
||||
};
|
||||
|
||||
|
||||
|
||||
/* Internal Prototypes */
|
||||
static int parameters_init(struct fw_rate_control_param_handles *h);
|
||||
static int parameters_update(const struct fw_rate_control_param_handles *h, struct fw_rate_control_params *p);
|
||||
|
||||
static int parameters_init(struct fw_rate_control_param_handles *h)
|
||||
{
|
||||
/* PID parameters */
|
||||
h->rollrate_p = param_find("FW_ROLLR_P"); //TODO define rate params for fixed wing
|
||||
h->rollrate_i = param_find("FW_ROLLR_I");
|
||||
h->rollrate_awu = param_find("FW_ROLLR_AWU");
|
||||
|
||||
h->pitchrate_p = param_find("FW_PITCHR_P");
|
||||
h->pitchrate_i = param_find("FW_PITCHR_I");
|
||||
h->pitchrate_awu = param_find("FW_PITCHR_AWU");
|
||||
|
||||
h->yawrate_p = param_find("FW_YAWR_P");
|
||||
h->yawrate_i = param_find("FW_YAWR_I");
|
||||
h->yawrate_awu = param_find("FW_YAWR_AWU");
|
||||
h->pitch_thr_ff = param_find("FW_PITCH_THR_P");
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
static int parameters_update(const struct fw_rate_control_param_handles *h, struct fw_rate_control_params *p)
|
||||
{
|
||||
param_get(h->rollrate_p, &(p->rollrate_p));
|
||||
param_get(h->rollrate_i, &(p->rollrate_i));
|
||||
param_get(h->rollrate_awu, &(p->rollrate_awu));
|
||||
param_get(h->pitchrate_p, &(p->pitchrate_p));
|
||||
param_get(h->pitchrate_i, &(p->pitchrate_i));
|
||||
param_get(h->pitchrate_awu, &(p->pitchrate_awu));
|
||||
param_get(h->yawrate_p, &(p->yawrate_p));
|
||||
param_get(h->yawrate_i, &(p->yawrate_i));
|
||||
param_get(h->yawrate_awu, &(p->yawrate_awu));
|
||||
param_get(h->pitch_thr_ff, &(p->pitch_thr_ff));
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
||||
const float rates[],
|
||||
struct actuator_controls_s *actuators)
|
||||
{
|
||||
static int counter = 0;
|
||||
static bool initialized = false;
|
||||
|
||||
static struct fw_rate_control_params p;
|
||||
static struct fw_rate_control_param_handles h;
|
||||
|
||||
static PID_t roll_rate_controller;
|
||||
static PID_t pitch_rate_controller;
|
||||
static PID_t yaw_rate_controller;
|
||||
|
||||
static uint64_t last_run = 0;
|
||||
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
|
||||
last_run = hrt_absolute_time();
|
||||
|
||||
if (!initialized) {
|
||||
parameters_init(&h);
|
||||
parameters_update(&h, &p);
|
||||
pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the controller layout is with a PI rate controller
|
||||
pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
|
||||
pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
|
||||
initialized = true;
|
||||
}
|
||||
|
||||
/* load new parameters with lower rate */
|
||||
if (counter % 100 == 0) {
|
||||
/* update parameters from storage */
|
||||
parameters_update(&h, &p);
|
||||
pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1);
|
||||
pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1);
|
||||
pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1);
|
||||
}
|
||||
|
||||
|
||||
/* roll rate (PI) */
|
||||
actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
|
||||
/* pitch rate (PI) */
|
||||
actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
|
||||
/* yaw rate (PI) */
|
||||
actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
|
||||
|
||||
counter++;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,48 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Thomas Gubler <thomasgubler@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 Fixed Wing Attitude Rate Control */
|
||||
|
||||
#ifndef FIXEDWING_ATT_CONTROL_RATE_H_
|
||||
#define FIXEDWING_ATT_CONTROL_RATE_H_
|
||||
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
|
||||
int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
||||
const float rates[],
|
||||
struct actuator_controls_s *actuators);
|
||||
|
||||
#endif /* FIXEDWING_ATT_CONTROL_RATE_H_ */
|
||||
@@ -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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# Fixedwing Attitude Control application
|
||||
#
|
||||
|
||||
MODULE_COMMAND = fixedwing_att_control
|
||||
|
||||
SRCS = fixedwing_att_control_main.c \
|
||||
fixedwing_att_control_att.c \
|
||||
fixedwing_att_control_rate.c
|
||||
@@ -0,0 +1,170 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: James Goppert
|
||||
*
|
||||
* 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 fixedwing_backside_main.cpp
|
||||
* @author James Goppert
|
||||
*
|
||||
* Fixedwing backside controller using control library
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <controllib/fixedwing.hpp>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <math.h>
|
||||
|
||||
static bool thread_should_exit = false; /**< Deamon exit flag */
|
||||
static bool thread_running = false; /**< Deamon status flag */
|
||||
static int deamon_task; /**< Handle of deamon task / thread */
|
||||
|
||||
/**
|
||||
* Deamon management function.
|
||||
*/
|
||||
extern "C" __EXPORT int fixedwing_backside_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Mainloop of deamon.
|
||||
*/
|
||||
int control_demo_thread_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Test function
|
||||
*/
|
||||
void test();
|
||||
|
||||
/**
|
||||
* Print the correct usage.
|
||||
*/
|
||||
static void usage(const char *reason);
|
||||
|
||||
static void
|
||||
usage(const char *reason)
|
||||
{
|
||||
if (reason)
|
||||
fprintf(stderr, "%s\n", reason);
|
||||
|
||||
fprintf(stderr, "usage: control_demo {start|stop|status} [-p <additional params>]\n\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
/**
|
||||
* The deamon app only briefly exists to start
|
||||
* the background job. The stack size assigned in the
|
||||
* Makefile does only apply to this management task.
|
||||
*
|
||||
* The actual stack size should be set in the call
|
||||
* to task_create().
|
||||
*/
|
||||
int fixedwing_backside_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
if (argc < 1)
|
||||
usage("missing command");
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (thread_running) {
|
||||
printf("control_demo already running\n");
|
||||
/* this is not an error */
|
||||
exit(0);
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
deamon_task = task_spawn("control_demo",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 10,
|
||||
5120,
|
||||
control_demo_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "test")) {
|
||||
test();
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
thread_should_exit = true;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (thread_running) {
|
||||
printf("\tcontrol_demo app is running\n");
|
||||
|
||||
} else {
|
||||
printf("\tcontrol_demo app not started\n");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
usage("unrecognized command");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
int control_demo_thread_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
printf("[control_Demo] starting\n");
|
||||
|
||||
using namespace control;
|
||||
|
||||
fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB");
|
||||
|
||||
thread_running = true;
|
||||
|
||||
while (!thread_should_exit) {
|
||||
autopilot.update();
|
||||
}
|
||||
|
||||
printf("[control_demo] exiting.\n");
|
||||
|
||||
thread_running = false;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void test()
|
||||
{
|
||||
printf("beginning control lib test\n");
|
||||
control::basicBlocksTest();
|
||||
}
|
||||
@@ -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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# Fixedwing backside controller
|
||||
#
|
||||
|
||||
MODULE_COMMAND = fixedwing_backside
|
||||
|
||||
SRCS = fixedwing_backside_main.cpp
|
||||
@@ -0,0 +1,71 @@
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
// currently tuned for easystar from arkhangar in HIL
|
||||
//https://github.com/arktools/arkhangar
|
||||
|
||||
// 16 is max name length
|
||||
|
||||
// gyro low pass filter
|
||||
PARAM_DEFINE_FLOAT(FWB_P_LP, 300.0f); // roll rate low pass cut freq
|
||||
PARAM_DEFINE_FLOAT(FWB_Q_LP, 300.0f); // pitch rate low pass cut freq
|
||||
PARAM_DEFINE_FLOAT(FWB_R_LP, 300.0f); // yaw rate low pass cut freq
|
||||
|
||||
// yaw washout
|
||||
PARAM_DEFINE_FLOAT(FWB_R_HP, 1.0f); // yaw rate high pass
|
||||
|
||||
// stabilization mode
|
||||
PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.3f); // roll rate 2 aileron
|
||||
PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.1f); // pitch rate 2 elevator
|
||||
PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.1f); // yaw rate 2 rudder
|
||||
|
||||
// psi -> phi -> p
|
||||
PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 0.5f); // heading 2 roll
|
||||
PARAM_DEFINE_FLOAT(FWB_PHI2P, 1.0f); // roll to roll rate
|
||||
PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.3f); // roll limit, 28 deg
|
||||
|
||||
// velocity -> theta
|
||||
PARAM_DEFINE_FLOAT(FWB_V2THE_P, 1.0f); // velocity to pitch angle PID, prop gain
|
||||
PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f); // integral gain
|
||||
PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f); // derivative gain
|
||||
PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f); // derivative low-pass
|
||||
PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f); // integrator wind up guard
|
||||
PARAM_DEFINE_FLOAT(FWB_THE_MIN, -0.5f); // the max commanded pitch angle
|
||||
PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f); // the min commanded pitch angle
|
||||
|
||||
|
||||
// theta -> q
|
||||
PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f); // pitch angle to pitch-rate PID
|
||||
PARAM_DEFINE_FLOAT(FWB_THE2Q_I, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_THE2Q_D, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_THE2Q_D_LP, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_THE2Q_I_MAX, 0.0f);
|
||||
|
||||
// h -> thr
|
||||
PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.01f); // altitude to throttle PID
|
||||
PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 0.0f);
|
||||
|
||||
// crosstrack
|
||||
PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f); // cross-track to yaw angle limit 90 deg
|
||||
PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.005f); // cross-track to yaw angle gain
|
||||
|
||||
// speed command
|
||||
PARAM_DEFINE_FLOAT(FWB_V_MIN, 10.0f); // minimum commanded velocity
|
||||
PARAM_DEFINE_FLOAT(FWB_V_CMD, 12.0f); // commanded velocity
|
||||
PARAM_DEFINE_FLOAT(FWB_V_MAX, 16.0f); // maximum commanded velocity
|
||||
|
||||
// rate of climb
|
||||
// this is what rate of climb is commanded (in m/s)
|
||||
// when the pitch stick is fully defelcted in simple mode
|
||||
PARAM_DEFINE_FLOAT(FWB_ROC_MAX, 1.0f);
|
||||
|
||||
// rate of climb -> thr
|
||||
PARAM_DEFINE_FLOAT(FWB_ROC2THR_P, 0.01f); // rate of climb to throttle PID
|
||||
PARAM_DEFINE_FLOAT(FWB_ROC2THR_I, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_ROC2THR_D, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_ROC2THR_D_LP, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_ROC2THR_I_MAX, 0.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f); // trim throttle (0,1)
|
||||
@@ -0,0 +1,479 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Doug Weibel <douglas.weibel@colorado.edu>
|
||||
*
|
||||
* 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 fixedwing_pos_control.c
|
||||
* Implementation of a fixed wing attitude controller.
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include <math.h>
|
||||
#include <poll.h>
|
||||
#include <time.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <arch/board/board.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_global_position_setpoint.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/pid/pid.h>
|
||||
#include <systemlib/geo/geo.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
/*
|
||||
* Controller parameters, accessible via MAVLink
|
||||
*
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_HEAD_P, 0.1f);
|
||||
PARAM_DEFINE_FLOAT(FW_HEADR_I, 0.1f);
|
||||
PARAM_DEFINE_FLOAT(FW_HEADR_LIM, 1.5f); //TODO: think about reasonable value
|
||||
PARAM_DEFINE_FLOAT(FW_XTRACK_P, 0.01745f); // Radians per meter off track
|
||||
PARAM_DEFINE_FLOAT(FW_ALT_P, 0.1f);
|
||||
PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians
|
||||
PARAM_DEFINE_FLOAT(FW_HEADR_P, 0.1f);
|
||||
PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); /**< Pitch angle limit in radians per second */
|
||||
|
||||
struct fw_pos_control_params {
|
||||
float heading_p;
|
||||
float headingr_p;
|
||||
float headingr_i;
|
||||
float headingr_lim;
|
||||
float xtrack_p;
|
||||
float altitude_p;
|
||||
float roll_lim;
|
||||
float pitch_lim;
|
||||
};
|
||||
|
||||
struct fw_pos_control_param_handles {
|
||||
param_t heading_p;
|
||||
param_t headingr_p;
|
||||
param_t headingr_i;
|
||||
param_t headingr_lim;
|
||||
param_t xtrack_p;
|
||||
param_t altitude_p;
|
||||
param_t roll_lim;
|
||||
param_t pitch_lim;
|
||||
};
|
||||
|
||||
|
||||
struct planned_path_segments_s {
|
||||
bool segment_type;
|
||||
double start_lat; // Start of line or center of arc
|
||||
double start_lon;
|
||||
double end_lat;
|
||||
double end_lon;
|
||||
float radius; // Radius of arc
|
||||
float arc_start_bearing; // Bearing from center to start of arc
|
||||
float arc_sweep; // Angle (radians) swept out by arc around center.
|
||||
// Positive for clockwise, negative for counter-clockwise
|
||||
};
|
||||
|
||||
|
||||
/* Prototypes */
|
||||
/* Internal Prototypes */
|
||||
static int parameters_init(struct fw_pos_control_param_handles *h);
|
||||
static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p);
|
||||
|
||||
/**
|
||||
* Deamon management function.
|
||||
*/
|
||||
__EXPORT int fixedwing_pos_control_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Mainloop of deamon.
|
||||
*/
|
||||
int fixedwing_pos_control_thread_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Print the correct usage.
|
||||
*/
|
||||
static void usage(const char *reason);
|
||||
|
||||
/* Variables */
|
||||
static bool thread_should_exit = false; /**< Deamon exit flag */
|
||||
static bool thread_running = false; /**< Deamon status flag */
|
||||
static int deamon_task; /**< Handle of deamon task / thread */
|
||||
|
||||
|
||||
/**
|
||||
* Parameter management
|
||||
*/
|
||||
static int parameters_init(struct fw_pos_control_param_handles *h)
|
||||
{
|
||||
/* PID parameters */
|
||||
h->heading_p = param_find("FW_HEAD_P");
|
||||
h->headingr_p = param_find("FW_HEADR_P");
|
||||
h->headingr_i = param_find("FW_HEADR_I");
|
||||
h->headingr_lim = param_find("FW_HEADR_LIM");
|
||||
h->xtrack_p = param_find("FW_XTRACK_P");
|
||||
h->altitude_p = param_find("FW_ALT_P");
|
||||
h->roll_lim = param_find("FW_ROLL_LIM");
|
||||
h->pitch_lim = param_find("FW_PITCH_LIM");
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p)
|
||||
{
|
||||
param_get(h->heading_p, &(p->heading_p));
|
||||
param_get(h->headingr_p, &(p->headingr_p));
|
||||
param_get(h->headingr_i, &(p->headingr_i));
|
||||
param_get(h->headingr_lim, &(p->headingr_lim));
|
||||
param_get(h->xtrack_p, &(p->xtrack_p));
|
||||
param_get(h->altitude_p, &(p->altitude_p));
|
||||
param_get(h->roll_lim, &(p->roll_lim));
|
||||
param_get(h->pitch_lim, &(p->pitch_lim));
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
|
||||
/* Main Thread */
|
||||
int fixedwing_pos_control_thread_main(int argc, char *argv[])
|
||||
{
|
||||
/* read arguments */
|
||||
bool verbose = false;
|
||||
|
||||
for (int i = 1; i < argc; i++) {
|
||||
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
|
||||
verbose = true;
|
||||
}
|
||||
}
|
||||
|
||||
/* welcome user */
|
||||
printf("[fixedwing pos control] started\n");
|
||||
|
||||
/* declare and safely initialize all structs */
|
||||
struct vehicle_global_position_s global_pos;
|
||||
memset(&global_pos, 0, sizeof(global_pos));
|
||||
struct vehicle_global_position_s start_pos; // Temporary variable, replace with
|
||||
memset(&start_pos, 0, sizeof(start_pos)); // previous waypoint when available
|
||||
struct vehicle_global_position_setpoint_s global_setpoint;
|
||||
memset(&global_setpoint, 0, sizeof(global_setpoint));
|
||||
struct vehicle_attitude_s att;
|
||||
memset(&att, 0, sizeof(att));
|
||||
struct crosstrack_error_s xtrack_err;
|
||||
memset(&xtrack_err, 0, sizeof(xtrack_err));
|
||||
struct parameter_update_s param_update;
|
||||
memset(¶m_update, 0, sizeof(param_update));
|
||||
|
||||
/* output structs */
|
||||
struct vehicle_attitude_setpoint_s attitude_setpoint;
|
||||
memset(&attitude_setpoint, 0, sizeof(attitude_setpoint));
|
||||
|
||||
/* publish attitude setpoint */
|
||||
attitude_setpoint.roll_body = 0.0f;
|
||||
attitude_setpoint.pitch_body = 0.0f;
|
||||
attitude_setpoint.yaw_body = 0.0f;
|
||||
attitude_setpoint.thrust = 0.0f;
|
||||
orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint);
|
||||
|
||||
/* subscribe */
|
||||
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
|
||||
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||
int param_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
|
||||
/* Setup of loop */
|
||||
struct pollfd fds[2] = {
|
||||
{ .fd = param_sub, .events = POLLIN },
|
||||
{ .fd = att_sub, .events = POLLIN }
|
||||
};
|
||||
bool global_sp_updated_set_once = false;
|
||||
|
||||
float psi_track = 0.0f;
|
||||
|
||||
int counter = 0;
|
||||
|
||||
struct fw_pos_control_params p;
|
||||
struct fw_pos_control_param_handles h;
|
||||
|
||||
PID_t heading_controller;
|
||||
PID_t heading_rate_controller;
|
||||
PID_t offtrack_controller;
|
||||
PID_t altitude_controller;
|
||||
|
||||
parameters_init(&h);
|
||||
parameters_update(&h, &p);
|
||||
pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE); //arbitrary high limit
|
||||
pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE);
|
||||
pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE);
|
||||
pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE); //TODO: remove hardcoded value
|
||||
|
||||
/* error and performance monitoring */
|
||||
perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval");
|
||||
perf_counter_t fw_err_perf = perf_alloc(PC_COUNT, "fixedwing_pos_control_err");
|
||||
|
||||
while (!thread_should_exit) {
|
||||
/* wait for a sensor update, check for exit condition every 500 ms */
|
||||
int ret = poll(fds, 2, 500);
|
||||
|
||||
if (ret < 0) {
|
||||
/* poll error, count it in perf */
|
||||
perf_count(fw_err_perf);
|
||||
|
||||
} else if (ret == 0) {
|
||||
/* no return value, ignore */
|
||||
} else {
|
||||
|
||||
/* only update parameters if they changed */
|
||||
if (fds[0].revents & POLLIN) {
|
||||
/* read from param to clear updated flag */
|
||||
struct parameter_update_s update;
|
||||
orb_copy(ORB_ID(parameter_update), param_sub, &update);
|
||||
|
||||
/* update parameters from storage */
|
||||
parameters_update(&h, &p);
|
||||
pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f); //arbitrary high limit
|
||||
pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim);
|
||||
pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim);
|
||||
pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD); //TODO: remove hardcoded value
|
||||
}
|
||||
|
||||
/* only run controller if attitude changed */
|
||||
if (fds[1].revents & POLLIN) {
|
||||
|
||||
|
||||
static uint64_t last_run = 0;
|
||||
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
|
||||
last_run = hrt_absolute_time();
|
||||
|
||||
/* check if there is a new position or setpoint */
|
||||
bool pos_updated;
|
||||
orb_check(global_pos_sub, &pos_updated);
|
||||
bool global_sp_updated;
|
||||
orb_check(global_setpoint_sub, &global_sp_updated);
|
||||
|
||||
/* load local copies */
|
||||
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
|
||||
|
||||
if (pos_updated) {
|
||||
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
|
||||
}
|
||||
|
||||
if (global_sp_updated) {
|
||||
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
|
||||
start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint)
|
||||
global_sp_updated_set_once = true;
|
||||
psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
|
||||
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
|
||||
|
||||
printf("next wp direction: %0.4f\n", (double)psi_track);
|
||||
}
|
||||
|
||||
/* Simple Horizontal Control */
|
||||
if (global_sp_updated_set_once) {
|
||||
// if (counter % 100 == 0)
|
||||
// printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon);
|
||||
|
||||
/* calculate crosstrack error */
|
||||
// Only the case of a straight line track following handled so far
|
||||
int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
|
||||
(double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d,
|
||||
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
|
||||
|
||||
// XXX what is xtrack_err.past_end?
|
||||
if (distance_res == OK /*&& !xtrack_err.past_end*/) {
|
||||
|
||||
float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance
|
||||
|
||||
float psi_c = psi_track + delta_psi_c;
|
||||
float psi_e = psi_c - att.yaw;
|
||||
|
||||
/* wrap difference back onto -pi..pi range */
|
||||
psi_e = _wrap_pi(psi_e);
|
||||
|
||||
if (verbose) {
|
||||
printf("xtrack_err.distance %.4f ", (double)xtrack_err.distance);
|
||||
printf("delta_psi_c %.4f ", (double)delta_psi_c);
|
||||
printf("psi_c %.4f ", (double)psi_c);
|
||||
printf("att.yaw %.4f ", (double)att.yaw);
|
||||
printf("psi_e %.4f ", (double)psi_e);
|
||||
}
|
||||
|
||||
/* calculate roll setpoint, do this artificially around zero */
|
||||
float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f);
|
||||
float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following
|
||||
float psi_rate_c = delta_psi_rate_c + psi_rate_track;
|
||||
|
||||
/* limit turn rate */
|
||||
if (psi_rate_c > p.headingr_lim) {
|
||||
psi_rate_c = p.headingr_lim;
|
||||
|
||||
} else if (psi_rate_c < -p.headingr_lim) {
|
||||
psi_rate_c = -p.headingr_lim;
|
||||
}
|
||||
|
||||
float psi_rate_e = psi_rate_c - att.yawspeed;
|
||||
|
||||
// XXX sanity check: Assume 10 m/s stall speed and no stall condition
|
||||
float ground_speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
|
||||
|
||||
if (ground_speed < 10.0f) {
|
||||
ground_speed = 10.0f;
|
||||
}
|
||||
|
||||
float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g
|
||||
|
||||
attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT);
|
||||
|
||||
if (verbose) {
|
||||
printf("psi_rate_c %.4f ", (double)psi_rate_c);
|
||||
printf("psi_rate_e_scaled %.4f ", (double)psi_rate_e_scaled);
|
||||
printf("rollbody %.4f\n", (double)attitude_setpoint.roll_body);
|
||||
}
|
||||
|
||||
if (verbose && counter % 100 == 0)
|
||||
printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n", xtrack_err.distance, delta_psi_c);
|
||||
|
||||
} else {
|
||||
if (verbose && counter % 100 == 0)
|
||||
printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end);
|
||||
}
|
||||
|
||||
/* Very simple Altitude Control */
|
||||
if (pos_updated) {
|
||||
|
||||
//TODO: take care of relative vs. ab. altitude
|
||||
attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
|
||||
|
||||
}
|
||||
|
||||
// XXX need speed control
|
||||
attitude_setpoint.thrust = 0.7f;
|
||||
|
||||
/* publish the attitude setpoint */
|
||||
orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint);
|
||||
|
||||
/* measure in what intervals the controller runs */
|
||||
perf_count(fw_interval_perf);
|
||||
|
||||
counter++;
|
||||
|
||||
} else {
|
||||
// XXX no setpoint, decent default needed (loiter?)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
printf("[fixedwing_pos_control] exiting.\n");
|
||||
thread_running = false;
|
||||
|
||||
|
||||
close(attitude_setpoint_pub);
|
||||
|
||||
fflush(stdout);
|
||||
exit(0);
|
||||
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
/* Startup Functions */
|
||||
|
||||
static void
|
||||
usage(const char *reason)
|
||||
{
|
||||
if (reason)
|
||||
fprintf(stderr, "%s\n", reason);
|
||||
|
||||
fprintf(stderr, "usage: fixedwing_pos_control {start|stop|status}\n\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
/**
|
||||
* The deamon app only briefly exists to start
|
||||
* the background job. The stack size assigned in the
|
||||
* Makefile does only apply to this management task.
|
||||
*
|
||||
* The actual stack size should be set in the call
|
||||
* to task_create().
|
||||
*/
|
||||
int fixedwing_pos_control_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 1)
|
||||
usage("missing command");
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (thread_running) {
|
||||
printf("fixedwing_pos_control already running\n");
|
||||
/* this is not an error */
|
||||
exit(0);
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
deamon_task = task_spawn("fixedwing_pos_control",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 20,
|
||||
2048,
|
||||
fixedwing_pos_control_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
thread_running = true;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
thread_should_exit = true;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (thread_running) {
|
||||
printf("\tfixedwing_pos_control is running\n");
|
||||
|
||||
} else {
|
||||
printf("\tfixedwing_pos_control not started\n");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
usage("unrecognized command");
|
||||
exit(1);
|
||||
}
|
||||
@@ -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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# Fixedwing PositionControl application
|
||||
#
|
||||
|
||||
MODULE_COMMAND = fixedwing_pos_control
|
||||
|
||||
SRCS = fixedwing_pos_control_main.c
|
||||
Executable
+42
@@ -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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# Makefile to build the multirotor attitude controller
|
||||
#
|
||||
|
||||
MODULE_COMMAND = multirotor_att_control
|
||||
|
||||
SRCS = multirotor_att_control_main.c \
|
||||
multirotor_attitude_control.c \
|
||||
multirotor_rate_control.c
|
||||
@@ -0,0 +1,485 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier <lm@inf.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 multirotor_att_control_main.c
|
||||
*
|
||||
* Implementation of multirotor attitude control main loop.
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <stdbool.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include <debug.h>
|
||||
#include <getopt.h>
|
||||
#include <time.h>
|
||||
#include <math.h>
|
||||
#include <poll.h>
|
||||
#include <sys/prctl.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <drivers/drv_gyro.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/offboard_control_setpoint.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
#include "multirotor_attitude_control.h"
|
||||
#include "multirotor_rate_control.h"
|
||||
|
||||
PARAM_DEFINE_FLOAT(MC_RCLOSS_THR, 0.0f); // This defines the throttle when the RC signal is lost.
|
||||
|
||||
__EXPORT int multirotor_att_control_main(int argc, char *argv[]);
|
||||
|
||||
static bool thread_should_exit;
|
||||
static int mc_task;
|
||||
static bool motor_test_mode = false;
|
||||
|
||||
static orb_advert_t actuator_pub;
|
||||
|
||||
static struct vehicle_status_s state;
|
||||
|
||||
static int
|
||||
mc_thread_main(int argc, char *argv[])
|
||||
{
|
||||
/* declare and safely initialize all structs */
|
||||
memset(&state, 0, sizeof(state));
|
||||
struct vehicle_attitude_s att;
|
||||
memset(&att, 0, sizeof(att));
|
||||
struct vehicle_attitude_setpoint_s att_sp;
|
||||
memset(&att_sp, 0, sizeof(att_sp));
|
||||
struct manual_control_setpoint_s manual;
|
||||
memset(&manual, 0, sizeof(manual));
|
||||
struct sensor_combined_s raw;
|
||||
memset(&raw, 0, sizeof(raw));
|
||||
struct offboard_control_setpoint_s offboard_sp;
|
||||
memset(&offboard_sp, 0, sizeof(offboard_sp));
|
||||
struct vehicle_rates_setpoint_s rates_sp;
|
||||
memset(&rates_sp, 0, sizeof(rates_sp));
|
||||
|
||||
struct actuator_controls_s actuators;
|
||||
memset(&actuators, 0, sizeof(actuators));
|
||||
|
||||
/* subscribe to attitude, motor setpoints and system state */
|
||||
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||
int param_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
|
||||
int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
|
||||
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
|
||||
|
||||
/*
|
||||
* Do not rate-limit the loop to prevent aliasing
|
||||
* if rate-limiting would be desired later, the line below would
|
||||
* enable it.
|
||||
*
|
||||
* rate-limit the attitude subscription to 200Hz to pace our loop
|
||||
* orb_set_interval(att_sub, 5);
|
||||
*/
|
||||
struct pollfd fds[2] = {
|
||||
{ .fd = att_sub, .events = POLLIN },
|
||||
{ .fd = param_sub, .events = POLLIN }
|
||||
};
|
||||
|
||||
/* publish actuator controls */
|
||||
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
|
||||
actuators.control[i] = 0.0f;
|
||||
}
|
||||
|
||||
actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
|
||||
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
|
||||
orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
|
||||
int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
|
||||
|
||||
/* register the perf counter */
|
||||
perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control_runtime");
|
||||
perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "multirotor_att_control_interval");
|
||||
perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err");
|
||||
|
||||
/* welcome user */
|
||||
printf("[multirotor_att_control] starting\n");
|
||||
|
||||
/* store last control mode to detect mode switches */
|
||||
bool flag_control_manual_enabled = false;
|
||||
bool flag_control_attitude_enabled = false;
|
||||
bool flag_system_armed = false;
|
||||
|
||||
/* store if yaw position or yaw speed has been changed */
|
||||
bool control_yaw_position = true;
|
||||
|
||||
/* store if we stopped a yaw movement */
|
||||
bool first_time_after_yaw_speed_control = true;
|
||||
|
||||
/* prepare the handle for the failsafe throttle */
|
||||
param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR");
|
||||
float failsafe_throttle = 0.0f;
|
||||
|
||||
|
||||
while (!thread_should_exit) {
|
||||
|
||||
/* wait for a sensor update, check for exit condition every 500 ms */
|
||||
int ret = poll(fds, 2, 500);
|
||||
|
||||
if (ret < 0) {
|
||||
/* poll error, count it in perf */
|
||||
perf_count(mc_err_perf);
|
||||
|
||||
} else if (ret == 0) {
|
||||
/* no return value, ignore */
|
||||
} else {
|
||||
|
||||
/* only update parameters if they changed */
|
||||
if (fds[1].revents & POLLIN) {
|
||||
/* read from param to clear updated flag */
|
||||
struct parameter_update_s update;
|
||||
orb_copy(ORB_ID(parameter_update), param_sub, &update);
|
||||
|
||||
/* update parameters */
|
||||
// XXX no params here yet
|
||||
}
|
||||
|
||||
/* only run controller if attitude changed */
|
||||
if (fds[0].revents & POLLIN) {
|
||||
|
||||
perf_begin(mc_loop_perf);
|
||||
|
||||
/* get a local copy of system state */
|
||||
bool updated;
|
||||
orb_check(state_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
|
||||
}
|
||||
|
||||
/* get a local copy of manual setpoint */
|
||||
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
|
||||
/* get a local copy of attitude */
|
||||
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
|
||||
/* get a local copy of attitude setpoint */
|
||||
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
|
||||
/* get a local copy of rates setpoint */
|
||||
orb_check(setpoint_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp);
|
||||
}
|
||||
|
||||
/* get a local copy of the current sensor values */
|
||||
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
|
||||
|
||||
|
||||
/** STEP 1: Define which input is the dominating control input */
|
||||
if (state.flag_control_offboard_enabled) {
|
||||
/* offboard inputs */
|
||||
if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
|
||||
rates_sp.roll = offboard_sp.p1;
|
||||
rates_sp.pitch = offboard_sp.p2;
|
||||
rates_sp.yaw = offboard_sp.p3;
|
||||
rates_sp.thrust = offboard_sp.p4;
|
||||
// printf("thrust_rate=%8.4f\n",offboard_sp.p4);
|
||||
rates_sp.timestamp = hrt_absolute_time();
|
||||
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
|
||||
|
||||
} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
|
||||
att_sp.roll_body = offboard_sp.p1;
|
||||
att_sp.pitch_body = offboard_sp.p2;
|
||||
att_sp.yaw_body = offboard_sp.p3;
|
||||
att_sp.thrust = offboard_sp.p4;
|
||||
// printf("thrust_att=%8.4f\n",offboard_sp.p4);
|
||||
att_sp.timestamp = hrt_absolute_time();
|
||||
/* STEP 2: publish the result to the vehicle actuators */
|
||||
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
|
||||
}
|
||||
|
||||
|
||||
} else if (state.flag_control_manual_enabled) {
|
||||
|
||||
if (state.flag_control_attitude_enabled) {
|
||||
|
||||
/* initialize to current yaw if switching to manual or att control */
|
||||
if (state.flag_control_attitude_enabled != flag_control_attitude_enabled ||
|
||||
state.flag_control_manual_enabled != flag_control_manual_enabled ||
|
||||
state.flag_system_armed != flag_system_armed) {
|
||||
att_sp.yaw_body = att.yaw;
|
||||
}
|
||||
|
||||
static bool rc_loss_first_time = true;
|
||||
|
||||
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
|
||||
if (state.rc_signal_lost) {
|
||||
/* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */
|
||||
param_get(failsafe_throttle_handle, &failsafe_throttle);
|
||||
att_sp.roll_body = 0.0f;
|
||||
att_sp.pitch_body = 0.0f;
|
||||
|
||||
/*
|
||||
* Only go to failsafe throttle if last known throttle was
|
||||
* high enough to create some lift to make hovering state likely.
|
||||
*
|
||||
* This is to prevent that someone landing, but not disarming his
|
||||
* multicopter (throttle = 0) does not make it jump up in the air
|
||||
* if shutting down his remote.
|
||||
*/
|
||||
if (isfinite(manual.throttle) && manual.throttle > 0.2f) {
|
||||
att_sp.thrust = failsafe_throttle;
|
||||
|
||||
} else {
|
||||
att_sp.thrust = 0.0f;
|
||||
}
|
||||
|
||||
/* keep current yaw, do not attempt to go to north orientation,
|
||||
* since if the pilot regains RC control, he will be lost regarding
|
||||
* the current orientation.
|
||||
*/
|
||||
if (rc_loss_first_time)
|
||||
att_sp.yaw_body = att.yaw;
|
||||
|
||||
rc_loss_first_time = false;
|
||||
|
||||
} else {
|
||||
rc_loss_first_time = true;
|
||||
|
||||
att_sp.roll_body = manual.roll;
|
||||
att_sp.pitch_body = manual.pitch;
|
||||
|
||||
/* set attitude if arming */
|
||||
if (!flag_control_attitude_enabled && state.flag_system_armed) {
|
||||
att_sp.yaw_body = att.yaw;
|
||||
}
|
||||
|
||||
/* act if stabilization is active or if the (nonsense) direct pass through mode is set */
|
||||
if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS ||
|
||||
state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
|
||||
|
||||
if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) {
|
||||
rates_sp.yaw = manual.yaw;
|
||||
control_yaw_position = false;
|
||||
|
||||
} else {
|
||||
/*
|
||||
* This mode SHOULD be the default mode, which is:
|
||||
* VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS
|
||||
*
|
||||
* However, we fall back to this setting for all other (nonsense)
|
||||
* settings as well.
|
||||
*/
|
||||
|
||||
/* only move setpoint if manual input is != 0 */
|
||||
if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
|
||||
rates_sp.yaw = manual.yaw;
|
||||
control_yaw_position = false;
|
||||
first_time_after_yaw_speed_control = true;
|
||||
|
||||
} else {
|
||||
if (first_time_after_yaw_speed_control) {
|
||||
att_sp.yaw_body = att.yaw;
|
||||
first_time_after_yaw_speed_control = false;
|
||||
}
|
||||
|
||||
control_yaw_position = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
att_sp.thrust = manual.throttle;
|
||||
att_sp.timestamp = hrt_absolute_time();
|
||||
}
|
||||
|
||||
/* STEP 2: publish the controller output */
|
||||
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
|
||||
|
||||
if (motor_test_mode) {
|
||||
printf("testmode");
|
||||
att_sp.roll_body = 0.0f;
|
||||
att_sp.pitch_body = 0.0f;
|
||||
att_sp.yaw_body = 0.0f;
|
||||
att_sp.thrust = 0.1f;
|
||||
att_sp.timestamp = hrt_absolute_time();
|
||||
/* STEP 2: publish the result to the vehicle actuators */
|
||||
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
|
||||
}
|
||||
|
||||
} else {
|
||||
/* manual rate inputs, from RC control or joystick */
|
||||
if (state.flag_control_rates_enabled &&
|
||||
state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) {
|
||||
rates_sp.roll = manual.roll;
|
||||
|
||||
rates_sp.pitch = manual.pitch;
|
||||
rates_sp.yaw = manual.yaw;
|
||||
rates_sp.thrust = manual.throttle;
|
||||
rates_sp.timestamp = hrt_absolute_time();
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
|
||||
if (state.flag_control_attitude_enabled) {
|
||||
multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position);
|
||||
|
||||
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
|
||||
}
|
||||
|
||||
/* measure in what intervals the controller runs */
|
||||
perf_count(mc_interval_perf);
|
||||
|
||||
float gyro[3];
|
||||
|
||||
/* get current rate setpoint */
|
||||
bool rates_sp_valid = false;
|
||||
orb_check(rates_sp_sub, &rates_sp_valid);
|
||||
|
||||
if (rates_sp_valid) {
|
||||
orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
|
||||
}
|
||||
|
||||
/* apply controller */
|
||||
gyro[0] = att.rollspeed;
|
||||
gyro[1] = att.pitchspeed;
|
||||
gyro[2] = att.yawspeed;
|
||||
|
||||
multirotor_control_rates(&rates_sp, gyro, &actuators);
|
||||
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
|
||||
|
||||
/* update state */
|
||||
flag_control_attitude_enabled = state.flag_control_attitude_enabled;
|
||||
flag_control_manual_enabled = state.flag_control_manual_enabled;
|
||||
flag_system_armed = state.flag_system_armed;
|
||||
|
||||
perf_end(mc_loop_perf);
|
||||
} /* end of poll call for attitude updates */
|
||||
} /* end of poll return value check */
|
||||
}
|
||||
|
||||
printf("[multirotor att control] stopping, disarming motors.\n");
|
||||
|
||||
/* kill all outputs */
|
||||
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
|
||||
actuators.control[i] = 0.0f;
|
||||
|
||||
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
|
||||
|
||||
|
||||
close(att_sub);
|
||||
close(state_sub);
|
||||
close(manual_sub);
|
||||
close(actuator_pub);
|
||||
close(att_sp_pub);
|
||||
|
||||
perf_print_counter(mc_loop_perf);
|
||||
perf_free(mc_loop_perf);
|
||||
|
||||
fflush(stdout);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
static void
|
||||
usage(const char *reason)
|
||||
{
|
||||
if (reason)
|
||||
fprintf(stderr, "%s\n", reason);
|
||||
|
||||
fprintf(stderr, "usage: multirotor_att_control [-m <mode>] [-t] {start|status|stop}\n");
|
||||
fprintf(stderr, " <mode> is 'rates' or 'attitude'\n");
|
||||
fprintf(stderr, " -t enables motor test mode with 10%% thrust\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
int multirotor_att_control_main(int argc, char *argv[])
|
||||
{
|
||||
int ch;
|
||||
unsigned int optioncount = 0;
|
||||
|
||||
while ((ch = getopt(argc, argv, "tm:")) != EOF) {
|
||||
switch (ch) {
|
||||
case 't':
|
||||
motor_test_mode = true;
|
||||
optioncount += 1;
|
||||
break;
|
||||
|
||||
case ':':
|
||||
usage("missing parameter");
|
||||
break;
|
||||
|
||||
default:
|
||||
fprintf(stderr, "option: -%c\n", ch);
|
||||
usage("unrecognized option");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
argc -= optioncount;
|
||||
//argv += optioncount;
|
||||
|
||||
if (argc < 1)
|
||||
usage("missing command");
|
||||
|
||||
if (!strcmp(argv[1 + optioncount], "start")) {
|
||||
|
||||
thread_should_exit = false;
|
||||
mc_task = task_spawn("multirotor_att_control",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 15,
|
||||
2048,
|
||||
mc_thread_main,
|
||||
NULL);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1 + optioncount], "stop")) {
|
||||
thread_should_exit = true;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
usage("unrecognized command");
|
||||
exit(1);
|
||||
}
|
||||
@@ -0,0 +1,249 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
* @author Laurens Mackay <mackayl@student.ethz.ch>
|
||||
* @author Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
|
||||
* @author Lorenz Meier <lm@inf.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 multirotor_attitude_control.c
|
||||
* Implementation of attitude controller
|
||||
*/
|
||||
|
||||
#include "multirotor_attitude_control.h"
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <float.h>
|
||||
#include <math.h>
|
||||
#include <systemlib/pid/pid.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f);
|
||||
PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f);
|
||||
PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f);
|
||||
//PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f);
|
||||
//PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f);
|
||||
PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f);
|
||||
//PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f);
|
||||
//PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f);
|
||||
|
||||
//PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f);
|
||||
//PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f);
|
||||
|
||||
struct mc_att_control_params {
|
||||
float yaw_p;
|
||||
float yaw_i;
|
||||
float yaw_d;
|
||||
//float yaw_awu;
|
||||
//float yaw_lim;
|
||||
|
||||
float att_p;
|
||||
float att_i;
|
||||
float att_d;
|
||||
//float att_awu;
|
||||
//float att_lim;
|
||||
|
||||
//float att_xoff;
|
||||
//float att_yoff;
|
||||
};
|
||||
|
||||
struct mc_att_control_param_handles {
|
||||
param_t yaw_p;
|
||||
param_t yaw_i;
|
||||
param_t yaw_d;
|
||||
//param_t yaw_awu;
|
||||
//param_t yaw_lim;
|
||||
|
||||
param_t att_p;
|
||||
param_t att_i;
|
||||
param_t att_d;
|
||||
//param_t att_awu;
|
||||
//param_t att_lim;
|
||||
|
||||
//param_t att_xoff;
|
||||
//param_t att_yoff;
|
||||
};
|
||||
|
||||
/**
|
||||
* Initialize all parameter handles and values
|
||||
*
|
||||
*/
|
||||
static int parameters_init(struct mc_att_control_param_handles *h);
|
||||
|
||||
/**
|
||||
* Update all parameters
|
||||
*
|
||||
*/
|
||||
static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p);
|
||||
|
||||
|
||||
static int parameters_init(struct mc_att_control_param_handles *h)
|
||||
{
|
||||
/* PID parameters */
|
||||
h->yaw_p = param_find("MC_YAWPOS_P");
|
||||
h->yaw_i = param_find("MC_YAWPOS_I");
|
||||
h->yaw_d = param_find("MC_YAWPOS_D");
|
||||
//h->yaw_awu = param_find("MC_YAWPOS_AWU");
|
||||
//h->yaw_lim = param_find("MC_YAWPOS_LIM");
|
||||
|
||||
h->att_p = param_find("MC_ATT_P");
|
||||
h->att_i = param_find("MC_ATT_I");
|
||||
h->att_d = param_find("MC_ATT_D");
|
||||
//h->att_awu = param_find("MC_ATT_AWU");
|
||||
//h->att_lim = param_find("MC_ATT_LIM");
|
||||
|
||||
//h->att_xoff = param_find("MC_ATT_XOFF");
|
||||
//h->att_yoff = param_find("MC_ATT_YOFF");
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p)
|
||||
{
|
||||
param_get(h->yaw_p, &(p->yaw_p));
|
||||
param_get(h->yaw_i, &(p->yaw_i));
|
||||
param_get(h->yaw_d, &(p->yaw_d));
|
||||
//param_get(h->yaw_awu, &(p->yaw_awu));
|
||||
//param_get(h->yaw_lim, &(p->yaw_lim));
|
||||
|
||||
param_get(h->att_p, &(p->att_p));
|
||||
param_get(h->att_i, &(p->att_i));
|
||||
param_get(h->att_d, &(p->att_d));
|
||||
//param_get(h->att_awu, &(p->att_awu));
|
||||
//param_get(h->att_lim, &(p->att_lim));
|
||||
|
||||
//param_get(h->att_xoff, &(p->att_xoff));
|
||||
//param_get(h->att_yoff, &(p->att_yoff));
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
|
||||
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position)
|
||||
{
|
||||
static uint64_t last_run = 0;
|
||||
static uint64_t last_input = 0;
|
||||
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
|
||||
float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f;
|
||||
last_run = hrt_absolute_time();
|
||||
|
||||
if (last_input != att_sp->timestamp) {
|
||||
last_input = att_sp->timestamp;
|
||||
}
|
||||
|
||||
static int sensor_delay;
|
||||
sensor_delay = hrt_absolute_time() - att->timestamp;
|
||||
|
||||
static int motor_skip_counter = 0;
|
||||
|
||||
static PID_t pitch_controller;
|
||||
static PID_t roll_controller;
|
||||
|
||||
static struct mc_att_control_params p;
|
||||
static struct mc_att_control_param_handles h;
|
||||
|
||||
static bool initialized = false;
|
||||
|
||||
static float yaw_error;
|
||||
|
||||
/* initialize the pid controllers when the function is called for the first time */
|
||||
if (initialized == false) {
|
||||
parameters_init(&h);
|
||||
parameters_update(&h, &p);
|
||||
|
||||
pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f,
|
||||
1000.0f, PID_MODE_DERIVATIV_SET);
|
||||
pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f,
|
||||
1000.0f, PID_MODE_DERIVATIV_SET);
|
||||
|
||||
initialized = true;
|
||||
}
|
||||
|
||||
/* load new parameters with lower rate */
|
||||
if (motor_skip_counter % 500 == 0) {
|
||||
/* update parameters from storage */
|
||||
parameters_update(&h, &p);
|
||||
|
||||
/* apply parameters */
|
||||
pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
|
||||
pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
|
||||
}
|
||||
|
||||
/* reset integral if on ground */
|
||||
if (att_sp->thrust < 0.1f) {
|
||||
pid_reset_integral(&pitch_controller);
|
||||
pid_reset_integral(&roll_controller);
|
||||
}
|
||||
|
||||
|
||||
/* calculate current control outputs */
|
||||
|
||||
/* control pitch (forward) output */
|
||||
rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body ,
|
||||
att->pitch, att->pitchspeed, deltaT);
|
||||
|
||||
/* control roll (left/right) output */
|
||||
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body ,
|
||||
att->roll, att->rollspeed, deltaT);
|
||||
|
||||
if (control_yaw_position) {
|
||||
/* control yaw rate */
|
||||
|
||||
/* positive error: rotate to right, negative error, rotate to left (NED frame) */
|
||||
// yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw);
|
||||
|
||||
yaw_error = att_sp->yaw_body - att->yaw;
|
||||
|
||||
if (yaw_error > M_PI_F) {
|
||||
yaw_error -= M_TWOPI_F;
|
||||
|
||||
} else if (yaw_error < -M_PI_F) {
|
||||
yaw_error += M_TWOPI_F;
|
||||
}
|
||||
|
||||
rates_sp->yaw = p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed);
|
||||
}
|
||||
|
||||
rates_sp->thrust = att_sp->thrust;
|
||||
|
||||
motor_skip_counter++;
|
||||
}
|
||||
@@ -0,0 +1,57 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
* @author Laurens Mackay <mackayl@student.ethz.ch>
|
||||
* @author Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
|
||||
* @author Lorenz Meier <lm@inf.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 multirotor_attitude_control.h
|
||||
* Attitude control for multi rotors.
|
||||
*/
|
||||
|
||||
#ifndef MULTIROTOR_ATTITUDE_CONTROL_H_
|
||||
#define MULTIROTOR_ATTITUDE_CONTROL_H_
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
|
||||
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
|
||||
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position);
|
||||
|
||||
#endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */
|
||||
@@ -0,0 +1,230 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* Lorenz Meier <lm@inf.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 multirotor_rate_control.c
|
||||
*
|
||||
* Implementation of rate controller
|
||||
*
|
||||
* @author Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
#include "multirotor_rate_control.h"
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <float.h>
|
||||
#include <math.h>
|
||||
#include <systemlib/pid/pid.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); /* same on Flamewheel */
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
|
||||
//PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f);
|
||||
//PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 1.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.0f); /* 0.15 F405 Flamewheel */
|
||||
PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
|
||||
//PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f);
|
||||
//PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 1.0f); /**< roughly < 500 deg/s limit */
|
||||
|
||||
struct mc_rate_control_params {
|
||||
|
||||
float yawrate_p;
|
||||
float yawrate_d;
|
||||
float yawrate_i;
|
||||
//float yawrate_awu;
|
||||
//float yawrate_lim;
|
||||
|
||||
float attrate_p;
|
||||
float attrate_d;
|
||||
float attrate_i;
|
||||
//float attrate_awu;
|
||||
//float attrate_lim;
|
||||
|
||||
float rate_lim;
|
||||
};
|
||||
|
||||
struct mc_rate_control_param_handles {
|
||||
|
||||
param_t yawrate_p;
|
||||
param_t yawrate_i;
|
||||
param_t yawrate_d;
|
||||
//param_t yawrate_awu;
|
||||
//param_t yawrate_lim;
|
||||
|
||||
param_t attrate_p;
|
||||
param_t attrate_i;
|
||||
param_t attrate_d;
|
||||
//param_t attrate_awu;
|
||||
//param_t attrate_lim;
|
||||
};
|
||||
|
||||
/**
|
||||
* Initialize all parameter handles and values
|
||||
*
|
||||
*/
|
||||
static int parameters_init(struct mc_rate_control_param_handles *h);
|
||||
|
||||
/**
|
||||
* Update all parameters
|
||||
*
|
||||
*/
|
||||
static int parameters_update(const struct mc_rate_control_param_handles *h, struct mc_rate_control_params *p);
|
||||
|
||||
|
||||
static int parameters_init(struct mc_rate_control_param_handles *h)
|
||||
{
|
||||
/* PID parameters */
|
||||
h->yawrate_p = param_find("MC_YAWRATE_P");
|
||||
h->yawrate_i = param_find("MC_YAWRATE_I");
|
||||
h->yawrate_d = param_find("MC_YAWRATE_D");
|
||||
//h->yawrate_awu = param_find("MC_YAWRATE_AWU");
|
||||
//h->yawrate_lim = param_find("MC_YAWRATE_LIM");
|
||||
|
||||
h->attrate_p = param_find("MC_ATTRATE_P");
|
||||
h->attrate_i = param_find("MC_ATTRATE_I");
|
||||
h->attrate_d = param_find("MC_ATTRATE_D");
|
||||
//h->attrate_awu = param_find("MC_ATTRATE_AWU");
|
||||
//h->attrate_lim = param_find("MC_ATTRATE_LIM");
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
static int parameters_update(const struct mc_rate_control_param_handles *h, struct mc_rate_control_params *p)
|
||||
{
|
||||
param_get(h->yawrate_p, &(p->yawrate_p));
|
||||
param_get(h->yawrate_i, &(p->yawrate_i));
|
||||
param_get(h->yawrate_d, &(p->yawrate_d));
|
||||
//param_get(h->yawrate_awu, &(p->yawrate_awu));
|
||||
//param_get(h->yawrate_lim, &(p->yawrate_lim));
|
||||
|
||||
param_get(h->attrate_p, &(p->attrate_p));
|
||||
param_get(h->attrate_i, &(p->attrate_i));
|
||||
param_get(h->attrate_d, &(p->attrate_d));
|
||||
//param_get(h->attrate_awu, &(p->attrate_awu));
|
||||
//param_get(h->attrate_lim, &(p->attrate_lim));
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
||||
const float rates[], struct actuator_controls_s *actuators)
|
||||
{
|
||||
static float roll_control_last = 0;
|
||||
static float pitch_control_last = 0;
|
||||
static uint64_t last_run = 0;
|
||||
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
|
||||
static uint64_t last_input = 0;
|
||||
|
||||
float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f;
|
||||
|
||||
if (last_input != rate_sp->timestamp) {
|
||||
last_input = rate_sp->timestamp;
|
||||
}
|
||||
|
||||
last_run = hrt_absolute_time();
|
||||
|
||||
static int motor_skip_counter = 0;
|
||||
|
||||
static struct mc_rate_control_params p;
|
||||
static struct mc_rate_control_param_handles h;
|
||||
|
||||
static bool initialized = false;
|
||||
|
||||
/* initialize the pid controllers when the function is called for the first time */
|
||||
if (initialized == false) {
|
||||
parameters_init(&h);
|
||||
parameters_update(&h, &p);
|
||||
initialized = true;
|
||||
}
|
||||
|
||||
/* load new parameters with lower rate */
|
||||
if (motor_skip_counter % 2500 == 0) {
|
||||
/* update parameters from storage */
|
||||
parameters_update(&h, &p);
|
||||
// warnx("rate ctrl: p.yawrate_p: %8.4f, loop: %d Hz, input: %d Hz",
|
||||
// (double)p.yawrate_p, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
|
||||
}
|
||||
|
||||
/* calculate current control outputs */
|
||||
|
||||
/* control pitch (forward) output */
|
||||
float pitch_control = p.attrate_p * (rate_sp->pitch - rates[1]) - (p.attrate_d * pitch_control_last);
|
||||
|
||||
/* increase resilience to faulty control inputs */
|
||||
if (isfinite(pitch_control)) {
|
||||
pitch_control_last = pitch_control;
|
||||
|
||||
} else {
|
||||
pitch_control = 0.0f;
|
||||
warnx("rej. NaN ctrl pitch");
|
||||
}
|
||||
|
||||
/* control roll (left/right) output */
|
||||
float roll_control = p.attrate_p * (rate_sp->roll - rates[0]) - (p.attrate_d * roll_control_last);
|
||||
|
||||
/* increase resilience to faulty control inputs */
|
||||
if (isfinite(roll_control)) {
|
||||
roll_control_last = roll_control;
|
||||
|
||||
} else {
|
||||
roll_control = 0.0f;
|
||||
warnx("rej. NaN ctrl roll");
|
||||
}
|
||||
|
||||
/* control yaw rate */
|
||||
float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]);
|
||||
|
||||
/* increase resilience to faulty control inputs */
|
||||
if (!isfinite(yaw_rate_control)) {
|
||||
yaw_rate_control = 0.0f;
|
||||
warnx("rej. NaN ctrl yaw");
|
||||
}
|
||||
|
||||
actuators->control[0] = roll_control;
|
||||
actuators->control[1] = pitch_control;
|
||||
actuators->control[2] = yaw_rate_control;
|
||||
actuators->control[3] = rate_sp->thrust;
|
||||
|
||||
motor_skip_counter++;
|
||||
}
|
||||
@@ -0,0 +1,56 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
* @author Laurens Mackay <mackayl@student.ethz.ch>
|
||||
* @author Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
|
||||
* @author Lorenz Meier <lm@inf.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 multirotor_attitude_control.h
|
||||
* Attitude control for multi rotors.
|
||||
*/
|
||||
|
||||
#ifndef MULTIROTOR_RATE_CONTROL_H_
|
||||
#define MULTIROTOR_RATE_CONTROL_H_
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
|
||||
void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
||||
const float rates[], struct actuator_controls_s *actuators);
|
||||
|
||||
#endif /* MULTIROTOR_RATE_CONTROL_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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# Build multirotor position control
|
||||
#
|
||||
|
||||
MODULE_COMMAND = multirotor_pos_control
|
||||
|
||||
SRCS = multirotor_pos_control.c \
|
||||
multirotor_pos_control_params.c
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user