dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merged master

This commit is contained in:
Lorenz Meier 2013-07-15 15:02:45 +02:00
parent 88389ea255 17338ca61a
commit bf2ff98856
56 changed files with 1926 additions and 958 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
Documentation/dsm_bind.odt Normal file
View File

Binary file not shown.

BIN
Documentation/dsm_bind.pdf Normal file
View File

Binary file not shown.

3
makefiles/config_px4fmu-v1_default.mk
View File

@ -27,7 +27,8 @@ MODULES += drivers/ms5611
MODULES += drivers/mb12xx
MODULES += drivers/gps
MODULES += drivers/hil
MODULES += drivers/hott_telemetry
MODULES += drivers/hott/hott_telemetry
MODULES += drivers/hott/hott_sensors
MODULES += drivers/blinkm
MODULES += drivers/mkblctrl
MODULES += drivers/md25

6
mavlink/include/mavlink/v1.0/mavlink_conversions.h
View File

@ -1,6 +1,12 @@
#ifndef _MAVLINK_CONVERSIONS_H_
#define _MAVLINK_CONVERSIONS_H_
/* enable math defines on Windows */
#ifdef _MSC_VER
#ifndef _USE_MATH_DEFINES
#define _USE_MATH_DEFINES
#endif
#endif
#include <math.h>
/**

2
src/drivers/ardrone_interface/ardrone_motor_control.c
View File

@ -109,7 +109,7 @@ int ar_multiplexing_init()
{
int fd;
fd = open(GPIO_DEVICE_PATH, 0);
fd = open(PX4FMU_DEVICE_PATH, 0);
if (fd < 0) {
warn("GPIO: open fail");

37
src/drivers/drv_gpio.h
View File

@ -63,43 +63,12 @@
* they also export GPIO-like things. This is always the GPIOs on the
* main board.
*/
# define GPIO_DEVICE_PATH "/dev/px4fmu"
# define PX4FMU_DEVICE_PATH "/dev/px4fmu"
# define PX4IO_DEVICE_PATH "/dev/px4io"
#endif
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
/*
* PX4IO GPIO numbers.
*
* XXX note that these are here for reference/future use; currently
* there is no good way to wire these up without a common STM32 GPIO
* driver, which isn't implemented yet.
*/
/* outputs */
# define GPIO_ACC1_POWER (1<<0) /**< accessory power 1 */
# define GPIO_ACC2_POWER (1<<1) /**< accessory power 2 */
# define GPIO_SERVO_POWER (1<<2) /**< servo power */
# define GPIO_RELAY1 (1<<3) /**< relay 1 */
# define GPIO_RELAY2 (1<<4) /**< relay 2 */
# define GPIO_LED_BLUE (1<<5) /**< blue LED */
# define GPIO_LED_AMBER (1<<6) /**< amber/red LED */
# define GPIO_LED_SAFETY (1<<7) /**< safety LED */
/* inputs */
# define GPIO_ACC_OVERCURRENT (1<<8) /**< accessory 1/2 overcurrent detect */
# define GPIO_SERVO_OVERCURRENT (1<<9) /**< servo overcurrent detect */
# define GPIO_SAFETY_BUTTON (1<<10) /**< safety button pressed */
/**
* Default GPIO device - other devices may also support this protocol if
* they also export GPIO-like things. This is always the GPIOs on the
* main board.
*/
# define GPIO_DEVICE_PATH "/dev/px4io"
#endif
#ifndef GPIO_DEVICE_PATH
#ifndef PX4IO_DEVICE_PATH
# error No GPIO support for this board.
#endif

9
src/drivers/drv_pwm_output.h
View File

@ -115,6 +115,15 @@ ORB_DECLARE(output_pwm);
/** clear the 'ARM ok' bit, which deactivates the safety switch */
#define PWM_SERVO_CLEAR_ARM_OK _IOC(_PWM_SERVO_BASE, 6)
/** start DSM bind */
#define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 7)
/** stop DSM bind */
#define DSM_BIND_STOP _IOC(_PWM_SERVO_BASE, 8)
/** Power up DSM receiver */
#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 9)
/** set a single servo to a specific value */
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)

868
src/drivers/gps/ubx.cpp
View File

File diff suppressed because it is too large Load Diff

61
src/drivers/gps/ubx.h
View File

@ -3,6 +3,7 @@
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -51,23 +52,23 @@
/* 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_SOL 0x06
#define UBX_MESSAGE_NAV_VELNED 0x12
//#define UBX_MESSAGE_RXM_SVSI 0x20
#define UBX_MESSAGE_ACK_ACK 0x01
#define UBX_MESSAGE_NAV_TIMEUTC 0x21
#define UBX_MESSAGE_NAV_SVINFO 0x30
#define UBX_MESSAGE_ACK_NAK 0x00
#define UBX_MESSAGE_ACK_ACK 0x01
#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_MESSAGE_CFG_NAV5 0x24
#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_BAUDRATE 38400 /**< choose 38400 as GPS baudrate */
#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */
#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */
@ -298,44 +299,6 @@ struct ubx_cfg_msg_rate {
// 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,
@ -401,17 +364,17 @@ private:
int _fd;
struct vehicle_gps_position_s *_gps_position;
ubx_config_state_t _config_state;
bool _configured;
bool _waiting_for_ack;
uint8_t _clsID_needed;
uint8_t _msgID_needed;
uint8_t _message_class_needed;
uint8_t _message_id_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;
uint8_t _message_class;
uint8_t _message_id;
unsigned _payload_size;
uint8_t _disable_cmd_counter;
};

92
src/drivers/hott/comms.cpp Normal file
View File

@ -0,0 +1,92 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file comms.c
* @author Simon Wilks <sjwilks@gmail.com>
*
*/
#include "comms.h"
#include <fcntl.h>
#include <stdio.h>
#include <sys/ioctl.h>
#include <systemlib/err.h>
#include <termios.h>
int
open_uart(const char *device)
{
/* baud rate */
static const speed_t speed = B19200;
/* open uart */
const int uart = open(device, O_RDWR | O_NOCTTY);
if (uart < 0) {
err(1, "Error opening port: %s", device);
}
/* Back up the original uart configuration to restore it after exit */
int termios_state;
struct termios uart_config_original;
if ((termios_state = tcgetattr(uart, &uart_config_original)) < 0) {
close(uart);
err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state);
}
/* Fill the struct for the new configuration */
struct termios uart_config;
tcgetattr(uart, &uart_config);
/* Clear ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag &= ~ONLCR;
/* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
close(uart);
err(1, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)",
device, termios_state);
}
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
close(uart);
err(1, "Error setting baudrate / termios config for %s (tcsetattr)", device);
}
/* Activate single wire mode */
ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
return uart;
}

46
src/drivers/hott/comms.h Normal file
View File

@ -0,0 +1,46 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file comms.h
* @author Simon Wilks <sjwilks@gmail.com>
*
*/
#ifndef COMMS_H_
#define COMMS_H
int open_uart(const char *device);
#endif /* COMMS_H_ */

238
src/drivers/hott/hott_sensors/hott_sensors.cpp Normal file
View File

@ -0,0 +1,238 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Author: Simon Wilks <sjwilks@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file hott_sensors.c
* @author Simon Wilks <sjwilks@gmail.com>
*
* Graupner HoTT sensor driver implementation.
*
* Poll any sensors connected to the PX4 via the telemetry wire.
*/
#include <fcntl.h>
#include <nuttx/config.h>
#include <poll.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include "../comms.h"
#include "../messages.h"
#define DEFAULT_UART "/dev/ttyS0"; /**< USART1 */
/* Oddly, ERROR is not defined for C++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
static int thread_should_exit = false; /**< Deamon exit flag */
static int thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
static const char daemon_name[] = "hott_sensors";
static const char commandline_usage[] = "usage: hott_sensors start|status|stop [-d <device>]";
/**
* Deamon management function.
*/
extern "C" __EXPORT int hott_sensors_main(int argc, char *argv[]);
/**
* Mainloop of daemon.
*/
int hott_sensors_thread_main(int argc, char *argv[]);
static int recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id);
static int send_poll(int uart, uint8_t *buffer, size_t size);
int
send_poll(int uart, uint8_t *buffer, size_t size)
{
for (size_t i = 0; i < size; i++) {
write(uart, &buffer[i], sizeof(buffer[i]));
/* Sleep before sending the next byte. */
usleep(POST_WRITE_DELAY_IN_USECS);
}
/* A hack the reads out what was written so the next read from the receiver doesn't get it. */
/* TODO: Fix this!! */
uint8_t dummy[size];
read(uart, &dummy, size);
return OK;
}
int
recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id)
{
static const int timeout_ms = 1000;
struct pollfd fds;
fds.fd = uart;
fds.events = POLLIN;
// XXX should this poll be inside the while loop???
if (poll(&fds, 1, timeout_ms) > 0) {
int i = 0;
bool stop_byte_read = false;
while (true) {
read(uart, &buffer[i], sizeof(buffer[i]));
if (stop_byte_read) {
// XXX process checksum
*size = ++i;
return OK;
}
// XXX can some other field not have the STOP BYTE value?
if (buffer[i] == STOP_BYTE) {
*id = buffer[1];
stop_byte_read = true;
}
i++;
}
}
return ERROR;
}
int
hott_sensors_thread_main(int argc, char *argv[])
{
warnx("starting");
thread_running = true;
const char *device = DEFAULT_UART;
/* read commandline arguments */
for (int i = 0; i < argc && argv[i]; i++) {
if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set
if (argc > i + 1) {
device = argv[i + 1];
} else {
thread_running = false;
errx(1, "missing parameter to -d\n%s", commandline_usage);
}
}
}
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
const int uart = open_uart(device);
if (uart < 0) {
errx(1, "Failed opening HoTT UART, exiting.");
thread_running = false;
}
init_pub_messages();
uint8_t buffer[MAX_MESSAGE_BUFFER_SIZE];
size_t size = 0;
uint8_t id = 0;
while (!thread_should_exit) {
// Currently we only support a General Air Module sensor.
build_gam_request(&buffer[0], &size);
send_poll(uart, buffer, size);
// The sensor will need a little time before it starts sending.
usleep(5000);
recv_data(uart, &buffer[0], &size, &id);
// Determine which moduel sent it and process accordingly.
if (id == GAM_SENSOR_ID) {
publish_gam_message(buffer);
} else {
warnx("Unknown sensor ID: %d", id);
}
}
warnx("exiting");
close(uart);
thread_running = false;
return 0;
}
/**
* Process command line arguments and start the daemon.
*/
int
hott_sensors_main(int argc, char *argv[])
{
if (argc < 1) {
errx(1, "missing command\n%s", commandline_usage);
}
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("deamon already running");
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn_cmd(daemon_name,
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40,
1024,
hott_sensors_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("daemon is running");
} else {
warnx("daemon not started");
}
exit(0);
}
errx(1, "unrecognized command\n%s", commandline_usage);
}

42
src/drivers/hott/hott_sensors/module.mk Normal file
View File

@ -0,0 +1,42 @@
############################################################################
#
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Graupner HoTT Sensors application.
#
MODULE_COMMAND = hott_sensors
SRCS = hott_sensors.cpp \
../messages.cpp \
../comms.cpp

86
src/drivers/hott_telemetry/hott_telemetry_main.c → src/drivers/hott/hott_telemetry/hott_telemetry.cpp
View File

@ -41,7 +41,6 @@
* The HoTT receiver polls each device at a regular interval at which point
* a data packet can be returned if necessary.
*
* TODO: Add support for at least the vario and GPS sensor data.
*/
#include <fcntl.h>
@ -50,13 +49,21 @@
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <termios.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include "messages.h"
#include "../comms.h"
#include "../messages.h"
#define DEFAULT_UART "/dev/ttyS0"; /**< USART1 */
/* Oddly, ERROR is not defined for C++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
static int thread_should_exit = false; /**< Deamon exit flag */
static int thread_running = false; /**< Deamon status flag */
@ -67,7 +74,7 @@ static const char commandline_usage[] = "usage: hott_telemetry start|status|stop
/**
* Deamon management function.
*/
__EXPORT int hott_telemetry_main(int argc, char *argv[]);
extern "C" __EXPORT int hott_telemetry_main(int argc, char *argv[]);
/**
* Mainloop of daemon.
@ -77,60 +84,18 @@ int hott_telemetry_thread_main(int argc, char *argv[]);
static int recv_req_id(int uart, uint8_t *id);
static int send_data(int uart, uint8_t *buffer, size_t size);
static int
open_uart(const char *device, struct termios *uart_config_original)
{
/* baud rate */
static const speed_t speed = B19200;
/* open uart */
const int uart = open(device, O_RDWR | O_NOCTTY);
if (uart < 0) {
err(1, "Error opening port: %s", device);
}
/* Back up the original uart configuration to restore it after exit */
int termios_state;
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
close(uart);
err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state);
}
/* Fill the struct for the new configuration */
struct termios uart_config;
tcgetattr(uart, &uart_config);
/* Clear ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag &= ~ONLCR;
/* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
close(uart);
err(1, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)",
device, termios_state);
}
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
close(uart);
err(1, "Error setting baudrate / termios config for %s (tcsetattr)", device);
}
/* Activate single wire mode */
ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
return uart;
}
int
recv_req_id(int uart, uint8_t *id)
{
static const int timeout_ms = 1000; // TODO make it a define
struct pollfd fds[] = { { .fd = uart, .events = POLLIN } };
uint8_t mode;
struct pollfd fds;
fds.fd = uart;
fds.events = POLLIN;
if (poll(fds, 1, timeout_ms) > 0) {
if (poll(&fds, 1, timeout_ms) > 0) {
/* Get the mode: binary or text */
read(uart, &mode, sizeof(mode));
@ -155,7 +120,6 @@ send_data(int uart, uint8_t *buffer, size_t size)
usleep(POST_READ_DELAY_IN_USECS);
uint16_t checksum = 0;
for (size_t i = 0; i < size; i++) {
if (i == size - 1) {
/* Set the checksum: the first uint8_t is taken as the checksum. */
@ -186,7 +150,7 @@ hott_telemetry_thread_main(int argc, char *argv[])
thread_running = true;
const char *device = "/dev/ttyS1"; /**< Default telemetry port: USART2 */
const char *device = DEFAULT_UART;
/* read commandline arguments */
for (int i = 0; i < argc && argv[i]; i++) {
@ -202,22 +166,20 @@ hott_telemetry_thread_main(int argc, char *argv[])
}
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
struct termios uart_config_original;
const int uart = open_uart(device, &uart_config_original);
const int uart = open_uart(device);
if (uart < 0) {
errx(1, "Failed opening HoTT UART, exiting.");
thread_running = false;
}
messages_init();
init_sub_messages();
uint8_t buffer[MESSAGE_BUFFER_SIZE];
uint8_t buffer[MAX_MESSAGE_BUFFER_SIZE];
size_t size = 0;
uint8_t id = 0;
bool connected = true;
while (!thread_should_exit) {
// Listen for and serve poll from the receiver.
if (recv_req_id(uart, &id) == OK) {
if (!connected) {
connected = true;
@ -228,7 +190,9 @@ hott_telemetry_thread_main(int argc, char *argv[])
case EAM_SENSOR_ID:
build_eam_response(buffer, &size);
break;
case GAM_SENSOR_ID:
build_gam_response(buffer, &size);
break;
case GPS_SENSOR_ID:
build_gps_response(buffer, &size);
break;
@ -254,7 +218,7 @@ hott_telemetry_thread_main(int argc, char *argv[])
}
/**
* Process command line arguments and tart the daemon.
* Process command line arguments and start the daemon.
*/
int
hott_telemetry_main(int argc, char *argv[])

7
src/drivers/hott_telemetry/module.mk → src/drivers/hott/hott_telemetry/module.mk
View File

@ -32,10 +32,11 @@
############################################################################
#
# Graupner HoTT Telemetry application.
# Graupner HoTT Telemetry applications.
#
MODULE_COMMAND = hott_telemetry
SRCS = hott_telemetry_main.c \
messages.c
SRCS = hott_telemetry.cpp \
../messages.cpp \
../comms.cpp

152
src/drivers/hott_telemetry/messages.c → src/drivers/hott/messages.cpp
View File

@ -46,6 +46,7 @@
#include <unistd.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_gps_position.h>
@ -53,24 +54,75 @@
/* The board is very roughly 5 deg warmer than the surrounding air */
#define BOARD_TEMP_OFFSET_DEG 5
static int battery_sub = -1;
static int gps_sub = -1;
static int home_sub = -1;
static int sensor_sub = -1;
static int airspeed_sub = -1;
static int _battery_sub = -1;
static int _gps_sub = -1;
static int _home_sub = -1;
static int _sensor_sub = -1;
static int _airspeed_sub = -1;
static int _esc_sub = -1;
static bool home_position_set = false;
static double home_lat = 0.0d;
static double home_lon = 0.0d;
static orb_advert_t _esc_pub;
struct esc_status_s _esc;
static bool _home_position_set = false;
static double _home_lat = 0.0d;
static double _home_lon = 0.0d;
void
messages_init(void)
init_sub_messages(void)
{
battery_sub = orb_subscribe(ORB_ID(battery_status));
gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
home_sub = orb_subscribe(ORB_ID(home_position));
sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_battery_sub = orb_subscribe(ORB_ID(battery_status));
_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_home_sub = orb_subscribe(ORB_ID(home_position));
_sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_esc_sub = orb_subscribe(ORB_ID(esc_status));
}
void
init_pub_messages(void)
{
memset(&_esc, 0, sizeof(_esc));
_esc_pub = orb_advertise(ORB_ID(esc_status), &_esc);
}
void
build_gam_request(uint8_t *buffer, size_t *size)
{
struct gam_module_poll_msg msg;
*size = sizeof(msg);
memset(&msg, 0, *size);
msg.mode = BINARY_MODE_REQUEST_ID;
msg.id = GAM_SENSOR_ID;
memcpy(buffer, &msg, *size);
}
void
publish_gam_message(const uint8_t *buffer)
{
struct gam_module_msg msg;
size_t size = sizeof(msg);
memset(&msg, 0, size);
memcpy(&msg, buffer, size);
/* announce the esc if needed, just publish else */
if (_esc_pub > 0) {
orb_publish(ORB_ID(esc_status), _esc_pub, &_esc);
} else {
_esc_pub = orb_advertise(ORB_ID(esc_status), &_esc);
}
// Publish it.
_esc.esc_count = 1;
_esc.esc_connectiontype = ESC_CONNECTION_TYPE_PPM;
_esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT;
_esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10;
_esc.esc[0].esc_temperature = msg.temperature1 - 20;
_esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff));
_esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff));
}
void
@ -79,12 +131,12 @@ build_eam_response(uint8_t *buffer, size_t *size)
/* get a local copy of the current sensor values */
struct sensor_combined_s raw;
memset(&raw, 0, sizeof(raw));
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw);
/* get a local copy of the battery data */
struct battery_status_s battery;
memset(&battery, 0, sizeof(battery));
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
orb_copy(ORB_ID(battery_status), _battery_sub, &battery);
struct eam_module_msg msg;
*size = sizeof(msg);
@ -92,7 +144,7 @@ build_eam_response(uint8_t *buffer, size_t *size)
msg.start = START_BYTE;
msg.eam_sensor_id = EAM_SENSOR_ID;
msg.sensor_id = EAM_SENSOR_TEXT_ID;
msg.sensor_text_id = EAM_SENSOR_TEXT_ID;
msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20);
msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG;
@ -106,12 +158,46 @@ build_eam_response(uint8_t *buffer, size_t *size)
/* get a local copy of the airspeed data */
struct airspeed_s airspeed;
memset(&airspeed, 0, sizeof(airspeed));
orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed);
uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6f);
msg.speed_L = (uint8_t)speed & 0xff;
msg.speed_H = (uint8_t)(speed >> 8) & 0xff;
msg.stop = STOP_BYTE;
memcpy(buffer, &msg, *size);
}
void
build_gam_response(uint8_t *buffer, size_t *size)
{
/* get a local copy of the ESC Status values */
struct esc_status_s esc;
memset(&esc, 0, sizeof(esc));
orb_copy(ORB_ID(esc_status), _esc_sub, &esc);
struct gam_module_msg msg;
*size = sizeof(msg);
memset(&msg, 0, *size);
msg.start = START_BYTE;
msg.gam_sensor_id = GAM_SENSOR_ID;
msg.sensor_text_id = GAM_SENSOR_TEXT_ID;
msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20);
msg.temperature2 = 20; // 0 deg. C.
uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage);
msg.main_voltage_L = (uint8_t)voltage & 0xff;
msg.main_voltage_H = (uint8_t)(voltage >> 8) & 0xff;
uint16_t current = (uint16_t)(esc.esc[0].esc_current);
msg.current_L = (uint8_t)current & 0xff;
msg.current_H = (uint8_t)(current >> 8) & 0xff;
uint16_t rpm = (uint16_t)(esc.esc[0].esc_rpm * 0.1f);
msg.rpm_L = (uint8_t)rpm & 0xff;
msg.rpm_H = (uint8_t)(rpm >> 8) & 0xff;
msg.stop = STOP_BYTE;
memcpy(buffer, &msg, *size);
@ -123,14 +209,14 @@ build_gps_response(uint8_t *buffer, size_t *size)
/* get a local copy of the current sensor values */
struct sensor_combined_s raw;
memset(&raw, 0, sizeof(raw));
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw);
/* get a local copy of the battery data */
struct vehicle_gps_position_s gps;
memset(&gps, 0, sizeof(gps));
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps);
orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps);
struct gps_module_msg msg = { 0 };
struct gps_module_msg msg;
*size = sizeof(msg);
memset(&msg, 0, *size);
@ -150,7 +236,7 @@ build_gps_response(uint8_t *buffer, size_t *size)
msg.flight_direction = (uint8_t)(gps.cog_rad * M_RAD_TO_DEG_F);
/* GPS speed */
uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6);
uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6f);
msg.gps_speed_L = (uint8_t)speed & 0xff;
msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff;
@ -196,33 +282,33 @@ build_gps_response(uint8_t *buffer, size_t *size)
msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff;
/* Altitude */
uint16_t alt = (uint16_t)(gps.alt*1e-3 + 500.0f);
uint16_t alt = (uint16_t)(gps.alt*1e-3f + 500.0f);
msg.altitude_L = (uint8_t)alt & 0xff;
msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
/* Get any (and probably only ever one) home_sub postion report */
/* Get any (and probably only ever one) _home_sub postion report */
bool updated;
orb_check(home_sub, &updated);
orb_check(_home_sub, &updated);
if (updated) {
/* get a local copy of the home position data */
struct home_position_s home;
memset(&home, 0, sizeof(home));
orb_copy(ORB_ID(home_position), home_sub, &home);
orb_copy(ORB_ID(home_position), _home_sub, &home);
home_lat = ((double)(home.lat))*1e-7d;
home_lon = ((double)(home.lon))*1e-7d;
home_position_set = true;
_home_lat = ((double)(home.lat))*1e-7d;
_home_lon = ((double)(home.lon))*1e-7d;
_home_position_set = true;
}
/* Distance from home */
if (home_position_set) {
uint16_t dist = (uint16_t)get_distance_to_next_waypoint(home_lat, home_lon, lat, lon);
if (_home_position_set) {
uint16_t dist = (uint16_t)get_distance_to_next_waypoint(_home_lat, _home_lon, lat, lon);
msg.distance_L = (uint8_t)dist & 0xff;
msg.distance_H = (uint8_t)(dist >> 8) & 0xff;
/* Direction back to home */
uint16_t bearing = (uint16_t)(get_bearing_to_next_waypoint(home_lat, home_lon, lat, lon) * M_RAD_TO_DEG_F);
uint16_t bearing = (uint16_t)(get_bearing_to_next_waypoint(_home_lat, _home_lon, lat, lon) * M_RAD_TO_DEG_F);
msg.home_direction = (uint8_t)bearing >> 1;
}
}

107
src/drivers/hott_telemetry/messages.h → src/drivers/hott/messages.h
View File

@ -60,19 +60,25 @@
#define STOP_BYTE 0x7d
#define TEMP_ZERO_CELSIUS 0x14
/* The GAM Module poll message. */
struct gam_module_poll_msg {
uint8_t mode;
uint8_t id;
};
/* Electric Air Module (EAM) constants. */
#define EAM_SENSOR_ID 0x8e
#define EAM_SENSOR_TEXT_ID 0xe0
/* The Electric Air Module message. */
struct eam_module_msg {
uint8_t start; /**< Start byte */
uint8_t start; /**< Start byte */
uint8_t eam_sensor_id; /**< EAM sensor */
uint8_t warning;
uint8_t sensor_id; /**< Sensor ID, why different? */
uint8_t sensor_text_id;
uint8_t alarm_inverse1;
uint8_t alarm_inverse2;
uint8_t cell1_L; /**< Lipo cell voltages. Not supported. */
uint8_t cell1_L; /**< Lipo cell voltages. Not supported. */
uint8_t cell2_L;
uint8_t cell3_L;
uint8_t cell4_L;
@ -92,9 +98,9 @@ struct eam_module_msg {
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_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_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;
@ -103,35 +109,82 @@ struct eam_module_msg {
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_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_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. */
uint8_t stop; /**< Stop byte */
uint8_t checksum; /**< Lower 8-bits of all bytes summed. */
};
/**
* The maximum buffer size required to store a HoTT message.
*/
#define MESSAGE_BUFFER_SIZE sizeof(union { \
struct eam_module_msg eam; \
})
/* General Air Module (GAM) constants. */
#define GAM_SENSOR_ID 0x8d
#define GAM_SENSOR_TEXT_ID 0xd0
struct gam_module_msg {
uint8_t start; // start byte constant value 0x7c
uint8_t gam_sensor_id; // EAM sensort id. constat value 0x8d
uint8_t warning_beeps; // 1=A 2=B ... 0x1a=Z 0 = no alarm
uint8_t sensor_text_id; // constant value 0xd0
uint8_t alarm_invers1; // alarm bitmask. Value is displayed inverted
uint8_t alarm_invers2; // alarm bitmask. Value is displayed inverted
uint8_t cell1; // cell 1 voltage lower value. 0.02V steps, 124=2.48V
uint8_t cell2;
uint8_t cell3;
uint8_t cell4;
uint8_t cell5;
uint8_t cell6;
uint8_t batt1_L; // battery 1 voltage LSB value. 0.1V steps. 50 = 5.5V
uint8_t batt1_H;
uint8_t batt2_L; // battery 2 voltage LSB value. 0.1V steps. 50 = 5.5V
uint8_t batt2_H;
uint8_t temperature1; // temperature 1. offset of 20. a value of 20 = 0°C
uint8_t temperature2; // temperature 2. offset of 20. a value of 20 = 0°C
uint8_t fuel_procent; // Fuel capacity in %. Values 0--100
// graphical display ranges: 0-25% 50% 75% 100%
uint8_t fuel_ml_L; // Fuel in ml scale. Full = 65535!
uint8_t fuel_ml_H; //
uint8_t rpm_L; // RPM in 10 RPM steps. 300 = 3000rpm
uint8_t rpm_H; //
uint8_t altitude_L; // altitude in meters. offset of 500, 500 = 0m
uint8_t altitude_H; //
uint8_t climbrate_L; // climb rate in 0.01m/s. Value of 30000 = 0.00 m/s
uint8_t climbrate_H; //
uint8_t climbrate3s; // climb rate in m/3sec. Value of 120 = 0m/3sec
uint8_t current_L; // current in 0.1A steps
uint8_t current_H; //
uint8_t main_voltage_L; // Main power voltage using 0.1V steps
uint8_t main_voltage_H; //
uint8_t batt_cap_L; // used battery capacity in 10mAh steps
uint8_t batt_cap_H; //
uint8_t speed_L; // (air?) speed in km/h(?) we are using ground speed here per default
uint8_t speed_H; //
uint8_t min_cell_volt; // minimum cell voltage in 2mV steps. 124 = 2,48V
uint8_t min_cell_volt_num; // number of the cell with the lowest voltage
uint8_t rpm2_L; // RPM in 10 RPM steps. 300 = 3000rpm
uint8_t rpm2_H; //
uint8_t general_error_number; // Voice error == 12. TODO: more docu
uint8_t pressure; // Pressure up to 16bar. 0,1bar scale. 20 = 2bar
uint8_t version; // version number TODO: more info?
uint8_t stop; // stop byte
uint8_t checksum; // checksum
};
/* GPS sensor constants. */
#define GPS_SENSOR_ID 0x8A
#define GPS_SENSOR_TEXT_ID 0xA0
#define GPS_SENSOR_ID 0x8a
#define GPS_SENSOR_TEXT_ID 0xa0
/**
* The GPS sensor message
* Struct based on: https://code.google.com/p/diy-hott-gps/downloads
*/
struct gps_module_msg {
uint8_t start; /**< Start byte */
uint8_t sensor_id; /**< GPS sensor ID*/
uint8_t warning; /**< Byte 3: 0…= warning beeps */
uint8_t start; /**< Start byte */
uint8_t sensor_id; /**< GPS sensor ID*/
uint8_t warning; /**< Byte 3: 0…= warning beeps */
uint8_t sensor_text_id; /**< GPS Sensor text mode ID */
uint8_t alarm_inverse1; /**< Byte 5: 01 inverse status */
uint8_t alarm_inverse2; /**< Byte 6: 00 inverse status status 1 = no GPS Signal */
@ -179,15 +232,15 @@ struct gps_module_msg {
uint8_t checksum; /**< Byte 45: Parity Byte */
};
/**
* The maximum buffer size required to store a HoTT message.
*/
#define GPS_MESSAGE_BUFFER_SIZE sizeof(union { \
struct gps_module_msg gps; \
})
// The maximum size of a message.
#define MAX_MESSAGE_BUFFER_SIZE 45
void messages_init(void);
void init_sub_messages(void);
void init_pub_messages(void);
void build_gam_request(uint8_t *buffer, size_t *size);
void publish_gam_message(const uint8_t *buffer);
void build_eam_response(uint8_t *buffer, size_t *size);
void build_gam_response(uint8_t *buffer, size_t *size);
void build_gps_response(uint8_t *buffer, size_t *size);
float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec);

125
src/drivers/mkblctrl/mkblctrl.cpp
View File

@ -75,6 +75,7 @@
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_safety.h>
#include <uORB/topics/esc_status.h>
#include <systemlib/err.h>
@ -88,7 +89,7 @@
#define MOTOR_STATE_PRESENT_MASK 0x80
#define MOTOR_STATE_ERROR_MASK 0x7F
#define MOTOR_SPINUP_COUNTER 2500
#define ESC_UORB_PUBLISH_DELAY 200
class MK : public device::I2C
{
@ -120,6 +121,7 @@ public:
int set_pwm_rate(unsigned rate);
int set_motor_count(unsigned count);
int set_motor_test(bool motortest);
int set_overrideSecurityChecks(bool overrideSecurityChecks);
int set_px4mode(int px4mode);
int set_frametype(int frametype);
unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput);
@ -137,11 +139,15 @@ private:
unsigned int _motor;
int _px4mode;
int _frametype;
orb_advert_t _t_outputs;
orb_advert_t _t_actuators_effective;
orb_advert_t _t_esc_status;
unsigned int _num_outputs;
bool _primary_pwm_device;
bool _motortest;
bool _overrideSecurityChecks;
volatile bool _task_should_exit;
bool _armed;
@ -215,6 +221,7 @@ struct MotorData_t {
unsigned int Version; // the version of the BL (0 = old)
unsigned int SetPoint; // written by attitude controller
unsigned int SetPointLowerBits; // for higher Resolution of new BLs
float SetPoint_PX4; // Values from PX4
unsigned int State; // 7 bit for I2C error counter, highest bit indicates if motor is present
unsigned int ReadMode; // select data to read
unsigned short RawPwmValue; // length of PWM pulse
@ -244,8 +251,10 @@ MK::MK(int bus) :
_t_actuator_safety(-1),
_t_outputs(0),
_t_actuators_effective(0),
_t_esc_status(0),
_num_outputs(0),
_motortest(false),
_overrideSecurityChecks(false),
_motor(-1),
_px4mode(MAPPING_MK),
_frametype(FRAME_PLUS),
@ -465,6 +474,13 @@ MK::set_motor_test(bool motortest)
return OK;
}
int
MK::set_overrideSecurityChecks(bool overrideSecurityChecks)
{
_overrideSecurityChecks = overrideSecurityChecks;
return OK;
}
short
MK::scaling(float val, float inMin, float inMax, float outMin, float outMax)
{
@ -507,8 +523,6 @@ MK::task_main()
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
&outputs);
/* advertise the effective control inputs */
actuator_controls_effective_s controls_effective;
memset(&controls_effective, 0, sizeof(controls_effective));
@ -516,6 +530,12 @@ MK::task_main()
_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
&controls_effective);
/* advertise the blctrl status */
esc_status_s esc;
memset(&esc, 0, sizeof(esc));
_t_esc_status = orb_advertise(ORB_ID(esc_status), &esc);
pollfd fds[2];
fds[0].fd = _t_actuators;
@ -603,9 +623,11 @@ MK::task_main()
}
}
/* don't go under BLCTRL_MIN_VALUE */
if (outputs.output[i] < BLCTRL_MIN_VALUE) {
outputs.output[i] = BLCTRL_MIN_VALUE;
if(!_overrideSecurityChecks) {
/* don't go under BLCTRL_MIN_VALUE */
if (outputs.output[i] < BLCTRL_MIN_VALUE) {
outputs.output[i] = BLCTRL_MIN_VALUE;
}
}
/* output to BLCtrl's */
@ -613,7 +635,10 @@ MK::task_main()
mk_servo_test(i);
} else {
mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024)); // scale the output to 0 - 1024 and sent to output routine
//mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024)); // scale the output to 0 - 1024 and sent to output routine
// 11 Bit
Motor[i].SetPoint_PX4 = outputs.output[i];
mk_servo_set(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 2047)); // scale the output to 0 - 2047 and sent to output routine
}
}
@ -636,8 +661,43 @@ MK::task_main()
}
/*
* Only update esc topic every half second.
*/
if (hrt_absolute_time() - esc.timestamp > 500000) {
esc.counter++;
esc.timestamp = hrt_absolute_time();
esc.esc_count = (uint8_t) _num_outputs;
esc.esc_connectiontype = ESC_CONNECTION_TYPE_I2C;
for (unsigned int i = 0; i < _num_outputs; i++) {
esc.esc[i].esc_address = (uint8_t) BLCTRL_BASE_ADDR + i;
esc.esc[i].esc_vendor = ESC_VENDOR_MIKROKOPTER;
esc.esc[i].esc_version = (uint16_t) Motor[i].Version;
esc.esc[i].esc_voltage = (uint16_t) 0;
esc.esc[i].esc_current = (uint16_t) Motor[i].Current;
esc.esc[i].esc_rpm = (uint16_t) 0;
esc.esc[i].esc_setpoint = (float) Motor[i].SetPoint_PX4;
if (Motor[i].Version == 1) {
// BLCtrl 2.0 (11Bit)
esc.esc[i].esc_setpoint_raw = (uint16_t) (Motor[i].SetPoint<<3) | Motor[i].SetPointLowerBits;
} else {
// BLCtrl < 2.0 (8Bit)
esc.esc[i].esc_setpoint_raw = (uint16_t) Motor[i].SetPoint;
}
esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature;
esc.esc[i].esc_state = (uint16_t) Motor[i].State;
esc.esc[i].esc_errorcount = (uint16_t) 0;
}
orb_publish(ORB_ID(esc_status), _t_esc_status, &esc);
}
}
//::close(_t_esc_status);
::close(_t_actuators);
::close(_t_actuators_effective);
::close(_t_actuator_safety);
@ -716,17 +776,17 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput)
fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i, Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature);
}
if (foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) {
_task_should_exit = true;
if(!_overrideSecurityChecks) {
if (foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) {
_task_should_exit = true;
}
}
}
return foundMotorCount;
}
int
MK::mk_servo_set(unsigned int chan, short val)
{
@ -739,17 +799,15 @@ MK::mk_servo_set(unsigned int chan, short val)
tmpVal = val;
if (tmpVal > 1024) {
tmpVal = 1024;
if (tmpVal > 2047) {
tmpVal = 2047;
} else if (tmpVal < 0) {
tmpVal = 0;
}
Motor[chan].SetPoint = (uint8_t)(tmpVal / 4);
//Motor[chan].SetPointLowerBits = (uint8_t) tmpVal % 4;
Motor[chan].SetPointLowerBits = 0;
Motor[chan].SetPoint = (uint8_t)(tmpVal>>3)&0xff;
Motor[chan].SetPointLowerBits = ((uint8_t)tmpVal%8)&0x07;
if (_armed == false) {
Motor[chan].SetPoint = 0;
@ -1015,8 +1073,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
if (arg < 2150) {
Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue = (unsigned short)arg;
mk_servo_set_value(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 1024));
mk_servo_set(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 2047));
} else {
ret = -EINVAL;
}
@ -1113,25 +1170,19 @@ ssize_t
MK::write(file *filp, const char *buffer, size_t len)
{
unsigned count = len / 2;
// loeschen uint16_t values[4];
uint16_t values[8];
// loeschen if (count > 4) {
// loeschen // we only have 4 PWM outputs on the FMU
// loeschen count = 4;
// loeschen }
if (count > _num_outputs) {
// we only have 8 I2C outputs in the driver
count = _num_outputs;
}
// allow for misaligned values
memcpy(values, buffer, count * 2);
for (uint8_t i = 0; i < count; i++) {
Motor[i].RawPwmValue = (unsigned short)values[i];
mk_servo_set_value(i, scaling(values[i], 1010, 2100, 0, 1024));
mk_servo_set(i, scaling(values[i], 1010, 2100, 0, 2047));
}
return count * 2;
@ -1283,7 +1334,7 @@ enum FrameType {
PortMode g_port_mode;
int
mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype)
mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks)
{
uint32_t gpio_bits;
int shouldStop = 0;
@ -1342,6 +1393,9 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
/* motortest if enabled */
g_mk->set_motor_test(motortest);
/* ovveride security checks if enabled */
g_mk->set_overrideSecurityChecks(overrideSecurityChecks);
/* count used motors */
do {
@ -1407,6 +1461,7 @@ mkblctrl_main(int argc, char *argv[])
int px4mode = MAPPING_PX4;
int frametype = FRAME_PLUS; // + plus is default
bool motortest = false;
bool overrideSecurityChecks = false;
bool showHelp = false;
bool newMode = false;
@ -1462,11 +1517,21 @@ mkblctrl_main(int argc, char *argv[])
showHelp = true;
}
/* look for the optional --override-security-checks parameter */
if (strcmp(argv[i], "--override-security-checks") == 0) {
overrideSecurityChecks = true;
newMode = true;
}
}
if (showHelp) {
fprintf(stderr, "mkblctrl: help:\n");
fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [-h / --help]\n");
fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [--override-security-checks] [-h / --help]\n\n");
fprintf(stderr, "\t -mkmode frame {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n");
fprintf(stderr, "\t -b i2c_bus_number \t\t Set the i2c bus where the ESCs are connected to (default 1).\n");
fprintf(stderr, "\t -t motortest \t\t\t Spin up once every motor in order of motoraddress. (DANGER !!!)\n");
fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n");
exit(1);
}
@ -1484,7 +1549,7 @@ mkblctrl_main(int argc, char *argv[])
/* parameter set ? */
if (newMode) {
/* switch parameter */
return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype);
return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks);
}
/* test, etc. here g*/

30
src/drivers/mpu6000/mpu6000.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* 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
@ -272,6 +272,11 @@ private:
*/
void _set_dlpf_filter(uint16_t frequency_hz);
/*
set sample rate (approximate) - 1kHz to 5Hz
*/
void _set_sample_rate(uint16_t desired_sample_rate_hz);
};
/**
@ -378,7 +383,8 @@ MPU6000::init()
up_udelay(1000);
// SAMPLE RATE
write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz
//write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz
_set_sample_rate(200); // default sample rate = 200Hz
usleep(1000);
// FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter)
@ -493,6 +499,18 @@ MPU6000::probe()
return -EIO;
}
/*
set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro
*/
void
MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz)
{
uint8_t div = 1000 / desired_sample_rate_hz;
if(div>200) div=200;
if(div<1) div=1;
write_reg(MPUREG_SMPLRT_DIV, div-1);
}
/*
set the DLPF filter frequency. This affects both accel and gyro.
*/
@ -644,8 +662,8 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
case ACCELIOCSSAMPLERATE:
case ACCELIOCGSAMPLERATE:
/* XXX not implemented */
return -EINVAL;
_set_sample_rate(arg);
return OK;
case ACCELIOCSLOWPASS:
case ACCELIOCGLOWPASS:
@ -702,8 +720,8 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
case GYROIOCSSAMPLERATE:
case GYROIOCGSAMPLERATE:
/* XXX not implemented */
return -EINVAL;
_set_sample_rate(arg);
return OK;
case GYROIOCSLOWPASS:
case GYROIOCGLOWPASS:

2
src/drivers/px4fmu/fmu.cpp
View File

@ -167,7 +167,7 @@ PX4FMU *g_fmu;
} // namespace
PX4FMU::PX4FMU() :
CDev("fmuservo", "/dev/px4fmu"),
CDev("fmuservo", PX4FMU_DEVICE_PATH),
_mode(MODE_NONE),
_pwm_default_rate(50),
_pwm_alt_rate(50),

138
src/drivers/px4io/px4io.cpp
View File

@ -145,6 +145,16 @@ public:
*/
void print_status();
inline void set_dsm_vcc_ctl(bool enable)
{
_dsm_vcc_ctl = enable;
};
inline bool get_dsm_vcc_ctl()
{
return _dsm_vcc_ctl;
};
private:
// XXX
unsigned _max_actuators;
@ -189,6 +199,12 @@ private:
float _battery_mamphour_total;
uint64_t _battery_last_timestamp;
/**
* Relay1 is dedicated to controlling DSM receiver power
*/
bool _dsm_vcc_ctl;
/**
* Trampoline to the worker task
*/
@ -330,7 +346,7 @@ PX4IO *g_dev;
}
PX4IO::PX4IO() :
I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000),
I2C("px4io", PX4IO_DEVICE_PATH, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000),
_max_actuators(0),
_max_controls(0),
_max_rc_input(0),
@ -356,7 +372,8 @@ PX4IO::PX4IO() :
_battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor
_battery_amp_bias(0),
_battery_mamphour_total(0),
_battery_last_timestamp(0)
_battery_last_timestamp(0),
_dsm_vcc_ctl(false)
{
/* we need this potentially before it could be set in task_main */
g_dev = this;
@ -762,8 +779,6 @@ PX4IO::io_set_control_state()
int
PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len)
{
uint16_t regs[_max_actuators];
if (len > _max_actuators)
/* fail with error */
return E2BIG;
@ -1399,14 +1414,15 @@ PX4IO::print_status()
printf("%u bytes free\n",
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM));
uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n",
printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
flags,
((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""),
((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"),
((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"),
((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""),
((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) ? " DSM" : ""),
(((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_STATUS_FLAGS_RC_DSM11))) ? " DSM10" : ""),
(((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""),
((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""),
((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"),
((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PPM" : ""),
@ -1506,7 +1522,8 @@ PX4IO::print_status()
}
int
PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
/* Make it obvious that file * isn't used here */
{
int ret = OK;
@ -1558,6 +1575,26 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
*(unsigned *)arg = _max_actuators;
break;
case DSM_BIND_START:
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
usleep(500000);
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out);
usleep(1000);
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
usleep(100000);
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4));
break;
case DSM_BIND_STOP:
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart);
usleep(500000);
break;
case DSM_BIND_POWER_UP:
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
break;
case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS - 1): {
/* TODO: we could go lower for e.g. TurboPWM */
@ -1600,18 +1637,31 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
break;
}
case GPIO_RESET:
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0);
case GPIO_RESET: {
uint32_t bits = (1 << _max_relays) - 1;
/* don't touch relay1 if it's controlling RX vcc */
if (_dsm_vcc_ctl)
bits &= ~PX4IO_RELAY1;
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0);
break;
}
case GPIO_SET:
arg &= ((1 << _max_relays) - 1);
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg);
/* don't touch relay1 if it's controlling RX vcc */
if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1))
ret = -EINVAL;
else
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg);
break;
case GPIO_CLEAR:
arg &= ((1 << _max_relays) - 1);
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0);
/* don't touch relay1 if it's controlling RX vcc */
if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1))
ret = -EINVAL;
else
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0);
break;
case GPIO_GET:
@ -1748,9 +1798,64 @@ start(int argc, char *argv[])
errx(1, "driver init failed");
}
int dsm_vcc_ctl;
if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) {
if (dsm_vcc_ctl) {
g_dev->set_dsm_vcc_ctl(true);
g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0);
}
}
exit(0);
}
void
bind(int argc, char *argv[])
{
int pulses;
if (g_dev == nullptr)
errx(1, "px4io must be started first");
if (!g_dev->get_dsm_vcc_ctl())
errx(1, "DSM bind feature not enabled");
if (argc < 3)
errx(0, "needs argument, use dsm2 or dsmx");
if (!strcmp(argv[2], "dsm2"))
pulses = 3;
else if (!strcmp(argv[2], "dsmx"))
pulses = 7;
else
errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]);
/* Open console directly to grab CTRL-C signal */
int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
if (!console)
errx(1, "failed opening console");
warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1.");
warnx("Press CTRL-C or 'c' when done.");
g_dev->ioctl(nullptr, DSM_BIND_START, pulses);
for (;;) {
usleep(500000L);
/* Check if user wants to quit */
char c;
if (read(console, &c, 1) == 1) {
if (c == 0x03 || c == 0x63) {
warnx("Done\n");
g_dev->ioctl(nullptr, DSM_BIND_STOP, 0);
g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0);
close(console);
exit(0);
}
}
}
}
void
test(void)
{
@ -1760,7 +1865,7 @@ test(void)
int direction = 1;
int ret;
fd = open("/dev/px4io", O_WRONLY);
fd = open(PX4IO_DEVICE_PATH, O_WRONLY);
if (fd < 0)
err(1, "failed to open device");
@ -2044,7 +2149,7 @@ px4io_main(int argc, char *argv[])
* We can cheat and call the driver directly, as it
* doesn't reference filp in ioctl()
*/
g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1);
g_dev->ioctl(nullptr, PX4IO_INAIR_RESTART_ENABLE, 1);
} else {
errx(1, "not loaded");
}
@ -2088,7 +2193,7 @@ px4io_main(int argc, char *argv[])
/* we can cheat and call the driver directly, as it
* doesn't reference filp in ioctl()
*/
int ret = g_dev->ioctl(NULL, PX4IO_SET_DEBUG, level);
int ret = g_dev->ioctl(nullptr, PX4IO_SET_DEBUG, level);
if (ret != 0) {
printf("SET_DEBUG failed - %d\n", ret);
exit(1);
@ -2162,6 +2267,9 @@ px4io_main(int argc, char *argv[])
if (!strcmp(argv[1], "monitor"))
monitor();
if (!strcmp(argv[1], "bind"))
bind(argc, argv);
out:
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe', 'min, 'max', 'idle' or 'update'");
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'failsafe', 'min, 'max',\n 'idle', 'bind' or 'update'");
}

2
src/examples/fixedwing_control/main.c
View File

@ -515,7 +515,7 @@ usage(const char *reason)
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
* to task_spawn_cmd().
*/
int ex_fixedwing_control_main(int argc, char *argv[])
{

4
src/examples/flow_position_control/flow_position_control_main.c
View File

@ -102,7 +102,7 @@ usage(const char *reason)
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_spawn().
* to task_spawn_cmd().
*/
int flow_position_control_main(int argc, char *argv[])
{
@ -119,7 +119,7 @@ int flow_position_control_main(int argc, char *argv[])
}
thread_should_exit = false;
deamon_task = task_spawn("flow_position_control",
deamon_task = task_spawn_cmd("flow_position_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 6,
4096,

4
src/examples/flow_position_estimator/flow_position_estimator_main.c
View File

@ -91,7 +91,7 @@ static void usage(const char *reason)
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
* to task_spawn_cmd().
*/
int flow_position_estimator_main(int argc, char *argv[])
{
@ -108,7 +108,7 @@ int flow_position_estimator_main(int argc, char *argv[])
}
thread_should_exit = false;
daemon_task = task_spawn("flow_position_estimator",
daemon_task = task_spawn_cmd("flow_position_estimator",
SCHED_RR,
SCHED_PRIORITY_MAX - 5,
4096,

4
src/examples/flow_speed_control/flow_speed_control_main.c
View File

@ -100,7 +100,7 @@ usage(const char *reason)
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_spawn().
* to task_spawn_cmd().
*/
int flow_speed_control_main(int argc, char *argv[])
{
@ -117,7 +117,7 @@ int flow_speed_control_main(int argc, char *argv[])
}
thread_should_exit = false;
deamon_task = task_spawn("flow_speed_control",
deamon_task = task_spawn_cmd("flow_speed_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 6,
4096,

12
src/modules/att_pos_estimator_ekf/KalmanNav.cpp
View File

@ -661,10 +661,10 @@ int KalmanNav::correctPos()
Vector y(6);
y(0) = _gps.vel_n_m_s - vN;
y(1) = _gps.vel_e_m_s - vE;
y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG;
y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG;
y(4) = double(_gps.alt) / 1.0e3 - alt;
y(5) = double(_sensors.baro_alt_meter) - alt;
y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG;
y(3) = double(_gps.lon) - double(lon) * 1.0e7 * M_RAD_TO_DEG;
y(4) = _gps.alt / 1.0e3f - alt;
y(5) = _sensors.baro_alt_meter - alt;
// compute correction
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
@ -698,7 +698,7 @@ int KalmanNav::correctPos()
vD += xCorrect(VD);
lat += double(xCorrect(LAT));
lon += double(xCorrect(LON));
alt += double(xCorrect(ALT));
alt += xCorrect(ALT);
// update state covariance
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
@ -710,7 +710,7 @@ int KalmanNav::correctPos()
static int counter = 0;
if (beta > _faultPos.get() && (counter % 10 == 0)) {
warnx("fault in gps: beta = %8.4f", (double)beta);
warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f",
warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f, baro: %8.4f",
double(y(0) / sqrtf(RPos(0, 0))),
double(y(1) / sqrtf(RPos(1, 1))),
double(y(2) / sqrtf(RPos(2, 2))),

18
src/modules/commander/commander.c
View File

@ -917,6 +917,7 @@ int commander_thread_main(int argc, char *argv[])
/* XXX use this! */
//uint64_t failsave_ll_start_time = 0;
enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode;
bool state_changed = true;
bool param_init_forced = true;
@ -1567,17 +1568,23 @@ int commander_thread_main(int argc, char *argv[])
*/
// printf("checking\n");
if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) {
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
(current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
(current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
) {
arming_state_transition(status_pub, &current_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd);
tune_positive();
if (current_status.mode_switch != MODE_SWITCH_MANUAL) {
mavlink_log_critical(mavlink_fd, "DISARM DENY, go manual mode first");
tune_negative();
} else {
arming_state_transition(status_pub, &current_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd);
tune_positive();
}
} else {
mavlink_log_critical(mavlink_fd, "STICK DISARM not allowed");
mavlink_log_critical(mavlink_fd, "DISARM not allowed");
tune_negative();
}
stick_off_counter = 0;
@ -1589,7 +1596,10 @@ int commander_thread_main(int argc, char *argv[])
}
/* check if left stick is in lower right position --> arm */
if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) {
if (current_status.flag_control_manual_enabled &&
current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS &&
sp_man.yaw > STICK_ON_OFF_LIMIT &&
sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
arming_state_transition(status_pub, &current_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd);
stick_on_counter = 0;

107
src/modules/gpio_led/gpio_led.c
View File

@ -54,10 +54,12 @@
#include <uORB/topics/actuator_safety.h>
#include <poll.h>
#include <drivers/drv_gpio.h>
#include <modules/px4iofirmware/protocol.h>
struct gpio_led_s {
struct work_s work;
int gpio_fd;
bool use_io;
int pin;
struct vehicle_status_s status;
int vehicle_status_sub;
@ -78,51 +80,97 @@ void gpio_led_cycle(FAR void *arg);
int gpio_led_main(int argc, char *argv[])
{
int pin = GPIO_EXT_1;
if (argc < 2) {
errx(1, "no argument provided. Try 'start' or 'stop' [-p 1/2]");
errx(1, "usage: gpio_led {start|stop} [-p <1|2|a1|a2|r1|r2>]\n"
"\t-p\tUse pin:\n"
"\t\t1\tPX4FMU GPIO_EXT1 (default)\n"
"\t\t2\tPX4FMU GPIO_EXT2\n"
"\t\ta1\tPX4IO ACC1\n"
"\t\ta2\tPX4IO ACC2\n"
"\t\tr1\tPX4IO RELAY1\n"
"\t\tr2\tPX4IO RELAY2");
} else {
/* START COMMAND HANDLING */
if (!strcmp(argv[1], "start")) {
if (gpio_led_started) {
errx(1, "already running");
}
bool use_io = false;
int pin = GPIO_EXT_1;
if (argc > 2) {
if (!strcmp(argv[1], "-p")) {
if (!strcmp(argv[2], "1")) {
if (!strcmp(argv[2], "-p")) {
if (!strcmp(argv[3], "1")) {
use_io = false;
pin = GPIO_EXT_1;
} else if (!strcmp(argv[2], "2")) {
} else if (!strcmp(argv[3], "2")) {
use_io = false;
pin = GPIO_EXT_2;
} else if (!strcmp(argv[3], "a1")) {
use_io = true;
pin = PX4IO_ACC1;
} else if (!strcmp(argv[3], "a2")) {
use_io = true;
pin = PX4IO_ACC2;
} else if (!strcmp(argv[3], "r1")) {
use_io = true;
pin = PX4IO_RELAY1;
} else if (!strcmp(argv[3], "r2")) {
use_io = true;
pin = PX4IO_RELAY2;
} else {
warnx("[gpio_led] Unsupported pin: %s\n", argv[2]);
exit(1);
errx(1, "unsupported pin: %s", argv[3]);
}
}
}
memset(&gpio_led_data, 0, sizeof(gpio_led_data));
gpio_led_data.use_io = use_io;
gpio_led_data.pin = pin;
int ret = work_queue(LPWORK, &gpio_led_data.work, gpio_led_start, &gpio_led_data, 0);
if (ret != 0) {
warnx("[gpio_led] Failed to queue work: %d\n", ret);
exit(1);
errx(1, "failed to queue work: %d", ret);
} else {
gpio_led_started = true;
char pin_name[24];
if (use_io) {
if (pin & (PX4IO_ACC1 | PX4IO_ACC2)) {
sprintf(pin_name, "PX4IO ACC%i", (pin >> 3));
} else {
sprintf(pin_name, "PX4IO RELAY%i", pin);
}
} else {
sprintf(pin_name, "PX4FMU GPIO_EXT%i", pin);
}
warnx("start, using pin: %s", pin_name);
}
exit(0);
/* STOP COMMAND HANDLING */
} else if (!strcmp(argv[1], "stop")) {
gpio_led_started = false;
if (gpio_led_started) {
gpio_led_started = false;
warnx("stop");
/* INVALID COMMAND */
} else {
errx(1, "not running");
}
} else {
errx(1, "unrecognized command '%s', only supporting 'start' or 'stop'", argv[1]);
@ -134,15 +182,26 @@ void gpio_led_start(FAR void *arg)
{
FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg;
char *gpio_dev;
if (priv->use_io) {
gpio_dev = PX4IO_DEVICE_PATH;
} else {
gpio_dev = PX4FMU_DEVICE_PATH;
}
/* open GPIO device */
priv->gpio_fd = open(GPIO_DEVICE_PATH, 0);
priv->gpio_fd = open(gpio_dev, 0);
if (priv->gpio_fd < 0) {
warnx("[gpio_led] GPIO: open fail\n");
// TODO find way to print errors
//printf("gpio_led: GPIO device \"%s\" open fail\n", gpio_dev);
gpio_led_started = false;
return;
}
/* configure GPIO pin */
/* px4fmu only, px4io doesn't support GPIO_SET_OUTPUT and will ignore */
ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, priv->pin);
/* subscribe to vehicle status topic */
@ -153,11 +212,11 @@ void gpio_led_start(FAR void *arg)
int ret = work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, 0);
if (ret != 0) {
warnx("[gpio_led] Failed to queue work: %d\n", ret);
// TODO find way to print errors
//printf("gpio_led: failed to queue work: %d\n", ret);
gpio_led_started = false;
return;
}
warnx("[gpio_led] Started, using pin GPIO_EXT%i\n", priv->pin);
}
void gpio_led_cycle(FAR void *arg)
@ -207,7 +266,6 @@ void gpio_led_cycle(FAR void *arg)
if (led_state_new) {
ioctl(priv->gpio_fd, GPIO_SET, priv->pin);
} else {
ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin);
}
@ -218,7 +276,12 @@ void gpio_led_cycle(FAR void *arg)
if (priv->counter > 5)
priv->counter = 0;
/* repeat cycle at 5 Hz*/
if (gpio_led_started)
/* repeat cycle at 5 Hz */
if (gpio_led_started) {
work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, USEC2TICK(200000));
} else {
/* switch off LED on stop */
ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin);
}
}

7
src/modules/mavlink/mavlink.c
View File

@ -49,7 +49,6 @@
#include <mqueue.h>
#include <string.h>
#include "mavlink_bridge_header.h"
#include <v1.0/common/mavlink.h>
#include <drivers/drv_hrt.h>
#include <time.h>
#include <float.h>
@ -464,7 +463,7 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf
}
void
mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length)
mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
{
write(uart, ch, (size_t)(sizeof(uint8_t) * length));
}
@ -472,7 +471,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length)
/*
* Internal function to give access to the channel status for each channel
*/
mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
extern mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
{
static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
return &m_mavlink_status[channel];
@ -481,7 +480,7 @@ mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
/*
* Internal function to give access to the channel buffer for each channel
*/
mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel)
extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel)
{
static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
return &m_mavlink_buffer[channel];

8
src/modules/mavlink/mavlink_bridge_header.h
View File

@ -73,9 +73,11 @@ extern mavlink_system_t mavlink_system;
* @param chan MAVLink channel to use, usually MAVLINK_COMM_0 = UART0
* @param ch Character to send
*/
extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int length);
extern void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length);
mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
#include <v1.0/common/mavlink.h>
#endif /* MAVLINK_BRIDGE_HEADER_H */

1
src/modules/mavlink/mavlink_parameters.c
View File

@ -40,7 +40,6 @@
*/
#include "mavlink_bridge_header.h"
#include <v1.0/common/mavlink.h>
#include "mavlink_parameters.h"
#include <uORB/uORB.h>
#include "math.h" /* isinf / isnan checks */

2
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -49,7 +49,6 @@
#include <fcntl.h>
#include <mqueue.h>
#include <string.h>
#include <v1.0/common/mavlink.h>
#include <drivers/drv_hrt.h>
#include <time.h>
#include <float.h>
@ -503,7 +502,6 @@ handle_message(mavlink_message_t *msg)
} else {
orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
}
warnx("IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s);
hil_global_pos.lat = hil_state.lat;
hil_global_pos.lon = hil_state.lon;

1
src/modules/mavlink/missionlib.c
View File

@ -48,7 +48,6 @@
#include <mqueue.h>
#include <string.h>
#include "mavlink_bridge_header.h"
#include <v1.0/common/mavlink.h>
#include <drivers/drv_hrt.h>
#include <time.h>
#include <float.h>

2
src/modules/mavlink/missionlib.h
View File

@ -39,7 +39,7 @@
#pragma once
#include <v1.0/common/mavlink.h>
#include "mavlink_bridge_header.h"
//extern void mavlink_wpm_send_message(mavlink_message_t *msg);
//extern void mavlink_wpm_send_gcs_string(const char *string);

1
src/modules/mavlink/orb_listener.c
View File

@ -47,7 +47,6 @@
#include <fcntl.h>
#include <string.h>
#include "mavlink_bridge_header.h"
#include <v1.0/common/mavlink.h>
#include <drivers/drv_hrt.h>
#include <time.h>
#include <float.h>

3
src/modules/mavlink/waypoints.c
View File

@ -45,6 +45,7 @@
#include <unistd.h>
#include <stdio.h>
#include "mavlink_bridge_header.h"
#include "missionlib.h"
#include "waypoints.h"
#include "util.h"
@ -372,7 +373,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt);
} else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, global_pos->lat, global_pos->lon, global_pos->relative_alt);
dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt);
} else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) {
dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z);

10
src/modules/mavlink/waypoints.h
View File

@ -47,11 +47,11 @@
#include <v1.0/mavlink_types.h>
#ifndef MAVLINK_SEND_UART_BYTES
#define MAVLINK_SEND_UART_BYTES(chan, buffer, len) mavlink_send_uart_bytes(chan, buffer, len)
#endif
extern mavlink_system_t mavlink_system;
#include <v1.0/common/mavlink.h>
// #ifndef MAVLINK_SEND_UART_BYTES
// #define MAVLINK_SEND_UART_BYTES(chan, buffer, len) mavlink_send_uart_bytes(chan, buffer, len)
// #endif
//extern mavlink_system_t mavlink_system;
#include "mavlink_bridge_header.h"
#include <stdbool.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>

1
src/modules/mavlink_onboard/mavlink.c
View File

@ -49,7 +49,6 @@
#include <mqueue.h>
#include <string.h>
#include "mavlink_bridge_header.h"
#include <v1.0/common/mavlink.h>
#include <drivers/drv_hrt.h>
#include <time.h>
#include <float.h>

2
src/modules/mavlink_onboard/mavlink_bridge_header.h
View File

@ -78,4 +78,6 @@ extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int len
mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan);
#include <v1.0/common/mavlink.h>
#endif /* MAVLINK_BRIDGE_HEADER_H */

1
src/modules/mavlink_onboard/mavlink_receiver.c
View File

@ -50,7 +50,6 @@
#include <mqueue.h>
#include <string.h>
#include "mavlink_bridge_header.h"
#include <v1.0/common/mavlink.h>
#include <drivers/drv_hrt.h>
#include <time.h>
#include <float.h>

11
src/modules/px4iofirmware/controls.c
View File

@ -95,9 +95,16 @@ controls_tick() {
*/
perf_begin(c_gather_dsm);
bool dsm_updated = dsm_input(r_raw_rc_values, &r_raw_rc_count);
if (dsm_updated)
uint16_t temp_count = r_raw_rc_count;
bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
if (dsm_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
r_raw_rc_count = temp_count & 0x7fff;
if (temp_count & 0x8000)
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11;
else
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11;
}
perf_end(c_gather_dsm);
perf_begin(c_gather_sbus);

41
src/modules/px4iofirmware/dsm.c
View File

@ -40,6 +40,7 @@
*/
#include <nuttx/config.h>
#include <nuttx/arch.h>
#include <fcntl.h>
#include <unistd.h>
@ -101,6 +102,41 @@ dsm_init(const char *device)
return dsm_fd;
}
void
dsm_bind(uint16_t cmd, int pulses)
{
const uint32_t usart1RxAsOutp = GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10;
if (dsm_fd < 0)
return;
switch (cmd) {
case dsm_bind_power_down:
// power down DSM satellite
POWER_RELAY1(0);
break;
case dsm_bind_power_up:
POWER_RELAY1(1);
dsm_guess_format(true);
break;
case dsm_bind_set_rx_out:
stm32_configgpio(usart1RxAsOutp);
break;
case dsm_bind_send_pulses:
for (int i = 0; i < pulses; i++) {
stm32_gpiowrite(usart1RxAsOutp, false);
up_udelay(50);
stm32_gpiowrite(usart1RxAsOutp, true);
up_udelay(50);
}
break;
case dsm_bind_reinit_uart:
// Restore USART rx pin
stm32_configgpio(GPIO_USART1_RX);
break;
}
}
bool
dsm_input(uint16_t *values, uint16_t *num_values)
{
@ -218,7 +254,7 @@ dsm_guess_format(bool reset)
/*
* Iterate the set of sensible sniffed channel sets and see whether
* decoding in 10 or 11-bit mode has yielded anything we recognise.
* decoding in 10 or 11-bit mode has yielded anything we recognize.
*
* XXX Note that due to what seem to be bugs in the DSM2 high-resolution
* stream, we may want to sniff for longer in some cases when we think we
@ -349,6 +385,9 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
values[channel] = value;
}
if (channel_shift == 11)
*num_values |= 0x8000;
/*
* XXX Note that we may be in failsafe here; we need to work out how to detect that.
*/

16
src/modules/px4iofirmware/protocol.h
View File

@ -109,6 +109,7 @@
#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */
#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */
#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */
#define PX4IO_P_STATUS_FLAGS_RC_DSM11 (1 << 13) /* DSM input is 11 bit data */
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */
@ -161,7 +162,22 @@
#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */
#define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */
#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */
/* px4io relay bit definitions */
#define PX4IO_RELAY1 (1<<0)
#define PX4IO_RELAY2 (1<<1)
#define PX4IO_ACC1 (1<<2)
#define PX4IO_ACC2 (1<<3)
#define PX4IO_P_SETUP_VBATT_SCALE 6 /* battery voltage correction factor (float) */
#define PX4IO_P_SETUP_DSM 7 /* DSM bind state */
enum { /* DSM bind states */
dsm_bind_power_down = 0,
dsm_bind_power_up,
dsm_bind_set_rx_out,
dsm_bind_send_pulses,
dsm_bind_reinit_uart
};
#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
/* autopilot control values, -10000..10000 */

1
src/modules/px4iofirmware/px4io.h
View File

@ -187,6 +187,7 @@ extern void controls_init(void);
extern void controls_tick(void);
extern int dsm_init(const char *device);
extern bool dsm_input(uint16_t *values, uint16_t *num_values);
extern void dsm_bind(uint16_t cmd, int pulses);
extern int sbus_init(const char *device);
extern bool sbus_input(uint16_t *values, uint16_t *num_values);

12
src/modules/px4iofirmware/registers.c
View File

@ -446,10 +446,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
case PX4IO_P_SETUP_RELAYS:
value &= PX4IO_P_SETUP_RELAYS_VALID;
r_setup_relays = value;
POWER_RELAY1(value & (1 << 0) ? 1 : 0);
POWER_RELAY2(value & (1 << 1) ? 1 : 0);
POWER_ACC1(value & (1 << 2) ? 1 : 0);
POWER_ACC2(value & (1 << 3) ? 1 : 0);
POWER_RELAY1(value & PX4IO_RELAY1 ? 1 : 0);
POWER_RELAY2(value & PX4IO_RELAY2 ? 1 : 0);
POWER_ACC1(value & PX4IO_ACC1 ? 1 : 0);
POWER_ACC2(value & PX4IO_ACC2 ? 1 : 0);
break;
case PX4IO_P_SETUP_SET_DEBUG:
@ -457,6 +457,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]);
break;
case PX4IO_P_SETUP_DSM:
dsm_bind(value & 0x0f, (value >> 4) & 7);
break;
default:
return -1;
}

67
src/modules/sdlog2/sdlog2.c
View File

@ -73,6 +73,7 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_control_debug.h>
@ -81,6 +82,7 @@
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/esc_status.h>
#include <systemlib/systemlib.h>
@ -614,9 +616,9 @@ int sdlog2_thread_main(int argc, char *argv[])
errx(1, "can't allocate log buffer, exiting.");
}
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* number of messages */
const ssize_t fdsc = 20;
/* --- IMPORTANT: DEFINE MAX NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* max number of messages */
const ssize_t fdsc = 21;
/* Sanity check variable and index */
ssize_t fdsc_count = 0;
@ -642,6 +644,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vehicle_local_position_s local_pos;
struct vehicle_local_position_setpoint_s local_pos_sp;
struct vehicle_global_position_s global_pos;
struct vehicle_global_position_setpoint_s global_pos_sp;
struct vehicle_gps_position_s gps_pos;
struct vehicle_vicon_position_s vicon_pos;
struct vehicle_control_debug_s control_debug;
@ -649,6 +652,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct rc_channels_s rc;
struct differential_pressure_s diff_pres;
struct airspeed_s airspeed;
struct esc_status_s esc;
} buf;
memset(&buf, 0, sizeof(buf));
@ -666,12 +670,14 @@ int sdlog2_thread_main(int argc, char *argv[])
int local_pos_sub;
int local_pos_sp_sub;
int global_pos_sub;
int global_pos_sp_sub;
int gps_pos_sub;
int vicon_pos_sub;
int control_debug_sub;
int flow_sub;
int rc_sub;
int airspeed_sub;
int esc_sub;
} subs;
/* log message buffer: header + body */
@ -696,6 +702,8 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_ARSP_s log_ARSP;
struct log_FLOW_s log_FLOW;
struct log_GPOS_s log_GPOS;
struct log_GPSP_s log_GPSP;
struct log_ESC_s log_ESC;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@ -787,6 +795,12 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- GLOBAL POSITION SETPOINT--- */
subs.global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
fds[fdsc_count].fd = subs.global_pos_sp_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- VICON POSITION --- */
subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
fds[fdsc_count].fd = subs.vicon_pos_sub;
@ -817,6 +831,12 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- ESCs --- */
subs.esc_sub = orb_subscribe(ORB_ID(esc_status));
fds[fdsc_count].fd = subs.esc_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* WARNING: If you get the error message below,
* then the number of registered messages (fdsc)
* differs from the number of messages in the above list.
@ -1108,6 +1128,25 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(GPOS);
}
/* --- GLOBAL POSITION SETPOINT --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_global_position_setpoint), subs.global_pos_sp_sub, &buf.global_pos_sp);
log_msg.msg_type = LOG_GPSP_MSG;
log_msg.body.log_GPSP.altitude_is_relative = buf.global_pos_sp.altitude_is_relative;
log_msg.body.log_GPSP.lat = buf.global_pos_sp.lat;
log_msg.body.log_GPSP.lon = buf.global_pos_sp.lon;
log_msg.body.log_GPSP.altitude = buf.global_pos_sp.altitude;
log_msg.body.log_GPSP.yaw = buf.global_pos_sp.yaw;
log_msg.body.log_GPSP.loiter_radius = buf.global_pos_sp.loiter_radius;
log_msg.body.log_GPSP.loiter_direction = buf.global_pos_sp.loiter_direction;
log_msg.body.log_GPSP.nav_cmd = buf.global_pos_sp.nav_cmd;
log_msg.body.log_GPSP.param1 = buf.global_pos_sp.param1;
log_msg.body.log_GPSP.param2 = buf.global_pos_sp.param2;
log_msg.body.log_GPSP.param3 = buf.global_pos_sp.param3;
log_msg.body.log_GPSP.param4 = buf.global_pos_sp.param4;
LOGBUFFER_WRITE_AND_COUNT(GPSP);
}
/* --- VICON POSITION --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos);
@ -1167,6 +1206,28 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(AIRS);
}
/* --- ESCs --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(esc_status), subs.esc_sub, &buf.esc);
for (uint8_t i=0; i<buf.esc.esc_count; i++)
{
log_msg.msg_type = LOG_ESC_MSG;
log_msg.body.log_ESC.counter = buf.esc.counter;
log_msg.body.log_ESC.esc_count = buf.esc.esc_count;
log_msg.body.log_ESC.esc_connectiontype = buf.esc.esc_connectiontype;
log_msg.body.log_ESC.esc_num = i;
log_msg.body.log_ESC.esc_address = buf.esc.esc[i].esc_address;
log_msg.body.log_ESC.esc_version = buf.esc.esc[i].esc_version;
log_msg.body.log_ESC.esc_voltage = buf.esc.esc[i].esc_voltage;
log_msg.body.log_ESC.esc_current = buf.esc.esc[i].esc_current;
log_msg.body.log_ESC.esc_rpm = buf.esc.esc[i].esc_rpm;
log_msg.body.log_ESC.esc_temperature = buf.esc.esc[i].esc_temperature;
log_msg.body.log_ESC.esc_setpoint = buf.esc.esc[i].esc_setpoint;
log_msg.body.log_ESC.esc_setpoint_raw = buf.esc.esc[i].esc_setpoint_raw;
LOGBUFFER_WRITE_AND_COUNT(ESC);
}
}
#ifdef SDLOG2_DEBUG
printf("fill rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb));
#endif

49
src/modules/sdlog2/sdlog2_messages.h
View File

@ -152,15 +152,15 @@ struct log_ATTC_s {
/* --- STAT - VEHICLE STATE --- */
#define LOG_STAT_MSG 10
struct log_STAT_s {
unsigned char state;
unsigned char flight_mode;
unsigned char manual_control_mode;
unsigned char manual_sas_mode;
unsigned char armed;
uint8_t state;
uint8_t flight_mode;
uint8_t manual_control_mode;
uint8_t manual_sas_mode;
uint8_t armed;
float battery_voltage;
float battery_current;
float battery_remaining;
unsigned char battery_warning;
uint8_t battery_warning;
};
/* --- CTRL - CONTROL DEBUG --- */
@ -229,6 +229,41 @@ struct log_GPOS_s {
float vel_e;
float vel_d;
};
/* --- GPSP - GLOBAL POSITION SETPOINT --- */
#define LOG_GPSP_MSG 17
struct log_GPSP_s {
uint8_t altitude_is_relative;
int32_t lat;
int32_t lon;
float altitude;
float yaw;
float loiter_radius;
int8_t loiter_direction;
uint8_t nav_cmd;
float param1;
float param2;
float param3;
float param4;
};
/* --- ESC - ESC STATE --- */
#define LOG_ESC_MSG 18
struct log_ESC_s {
uint16_t counter;
uint8_t esc_count;
uint8_t esc_connectiontype;
uint8_t esc_num;
uint16_t esc_address;
uint16_t esc_version;
uint16_t esc_voltage;
uint16_t esc_current;
uint16_t esc_rpm;
uint16_t esc_temperature;
float esc_setpoint;
uint16_t esc_setpoint_raw;
};
#pragma pack(pop)
/* construct list of all message formats */
@ -251,6 +286,8 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,No,Version,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
};
static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);

1
src/modules/sensors/sensor_params.c
View File

@ -155,6 +155,7 @@ PARAM_DEFINE_FLOAT(RC14_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f);
PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */
PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f));

12
src/modules/sensors/sensors.cpp
View File

@ -228,6 +228,8 @@ private:
float rc_scale_flaps;
float battery_voltage_scaling;
int rc_rl1_DSM_VCC_control;
} _parameters; /**< local copies of interesting parameters */
struct {
@ -274,6 +276,8 @@ private:
param_t rc_scale_flaps;
param_t battery_voltage_scaling;
param_t rc_rl1_DSM_VCC_control;
} _parameter_handles; /**< handles for interesting parameters */
@ -509,6 +513,9 @@ Sensors::Sensors() :
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
/* DSM VCC relay control */
_parameter_handles.rc_rl1_DSM_VCC_control = param_find("RC_RL1_DSM_VCC");
/* fetch initial parameter values */
parameters_update();
}
@ -715,6 +722,11 @@ Sensors::parameters_update()
warnx("Failed updating voltage scaling param");
}
/* relay 1 DSM VCC control */
if (param_get(_parameter_handles.rc_rl1_DSM_VCC_control, &(_parameters.rc_rl1_DSM_VCC_control)) != OK) {
warnx("Failed updating relay 1 DSM VCC control");
}
return OK;
}

4
src/modules/systemlib/airspeed.c
View File

@ -62,7 +62,7 @@ float calc_indicated_airspeed(float differential_pressure)
if (differential_pressure > 0) {
return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
} else {
return -sqrtf((2.0f*fabs(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
return -sqrtf((2.0f*fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
}
}
@ -106,6 +106,6 @@ float calc_true_airspeed(float total_pressure, float static_pressure, float temp
return sqrtf((2.0f*(pressure_difference)) / density);
} else
{
return -sqrtf((2.0f*fabs(pressure_difference)) / density);
return -sqrtf((2.0f*fabsf(pressure_difference)) / density);
}
}

40
src/modules/systemlib/cpuload.c
View File

@ -71,8 +71,6 @@ extern FAR struct _TCB *sched_gettcb(pid_t pid);
void cpuload_initialize_once()
{
// if (!system_load.initialized)
// {
system_load.start_time = hrt_absolute_time();
int i;
@ -80,27 +78,29 @@ void cpuload_initialize_once()
system_load.tasks[i].valid = false;
}
system_load.total_count = 0;
uint64_t now = hrt_absolute_time();
/* initialize idle thread statically */
system_load.tasks[0].start_time = now;
system_load.tasks[0].total_runtime = 0;
system_load.tasks[0].curr_start_time = 0;
system_load.tasks[0].tcb = sched_gettcb(0);
system_load.tasks[0].valid = true;
system_load.total_count++;
int static_tasks_count = 2; // there are at least 2 threads that should be initialized statically - "idle" and "init"
/* initialize init thread statically */
system_load.tasks[1].start_time = now;
system_load.tasks[1].total_runtime = 0;
system_load.tasks[1].curr_start_time = 0;
system_load.tasks[1].tcb = sched_gettcb(1);
system_load.tasks[1].valid = true;
/* count init thread */
system_load.total_count++;
// }
#ifdef CONFIG_PAGING
static_tasks_count++; // include paging thread in initialization
#endif /* CONFIG_PAGING */
#if CONFIG_SCHED_WORKQUEUE
static_tasks_count++; // include high priority work0 thread in initialization
#endif /* CONFIG_SCHED_WORKQUEUE */
#if CONFIG_SCHED_LPWORK
static_tasks_count++; // include low priority work1 thread in initialization
#endif /* CONFIG_SCHED_WORKQUEUE */
// perform static initialization of "system" threads
for (system_load.total_count = 0; system_load.total_count < static_tasks_count; system_load.total_count++)
{
system_load.tasks[system_load.total_count].start_time = now;
system_load.tasks[system_load.total_count].total_runtime = 0;
system_load.tasks[system_load.total_count].curr_start_time = 0;
system_load.tasks[system_load.total_count].tcb = sched_gettcb(system_load.total_count); // it is assumed that these static threads have consecutive PIDs
system_load.tasks[system_load.total_count].valid = true;
}
}
void sched_note_start(FAR struct tcb_s *tcb)

3
src/modules/uORB/objects_common.cpp
View File

@ -177,3 +177,6 @@ ORB_DEFINE(debug_key_value, struct debug_key_value_s);
#include "topics/navigation_capabilities.h"
ORB_DEFINE(navigation_capabilities, struct navigation_capabilities_s);
#include "topics/esc_status.h"
ORB_DEFINE(esc_status, struct esc_status_s);

115
src/modules/uORB/topics/esc_status.h Normal file
View File

@ -0,0 +1,115 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: @author Marco Bauer <marco@wtns.de>
*
* 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 esc_status.h
* Definition of the esc_status uORB topic.
*
* Published the state machine and the system status bitfields
* (see SYS_STATUS mavlink message), published only by commander app.
*
* All apps should write to subsystem_info:
*
* (any app) --> subsystem_info (published) --> (commander app state machine) --> esc_status --> (mavlink app)
*/
#ifndef ESC_STATUS_H_
#define ESC_STATUS_H_
#include <stdint.h>
#include <stdbool.h>
#include "../uORB.h"
/**
* @addtogroup topics @{
*/
/**
* The number of ESCs supported.
* Current (Q2/2013) we support 8 ESCs,
*/
#define CONNECTED_ESC_MAX 8
enum ESC_VENDOR {
ESC_VENDOR_GENERIC = 0, /**< generic ESC */
ESC_VENDOR_MIKROKOPTER, /**< Mikrokopter */
ESC_VENDOR_GRAUPNER_HOTT /**< Graupner HoTT ESC */
};
enum ESC_CONNECTION_TYPE {
ESC_CONNECTION_TYPE_PPM = 0, /**< Traditional PPM ESC */
ESC_CONNECTION_TYPE_SERIAL, /**< Serial Bus connected ESC */
ESC_CONNECTION_TYPE_ONESHOOT, /**< One Shoot PPM */
ESC_CONNECTION_TYPE_I2C, /**< I2C */
ESC_CONNECTION_TYPE_CAN /**< CAN-Bus */
};
/**
*
*/
struct esc_status_s
{
/* use of a counter and timestamp recommended (but not necessary) */
uint16_t counter; /**< incremented by the writing thread everytime new data is stored */
uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
uint8_t esc_count; /**< number of connected ESCs */
enum ESC_CONNECTION_TYPE esc_connectiontype; /**< how ESCs connected to the system */
struct {
uint16_t esc_address; /**< Address of current ESC (in most cases 1-8 / must be set by driver) */
enum ESC_VENDOR esc_vendor; /**< Vendor of current ESC */
uint16_t esc_version; /**< Version of current ESC - if supported */
uint16_t esc_voltage; /**< Voltage measured from current ESC - if supported */
uint16_t esc_current; /**< Current measured from current ESC (100mA steps) - if supported */
uint16_t esc_rpm; /**< RPM measured from current ESC - if supported */
uint16_t esc_temperature; /**< Temperature measured from current ESC - if supported */
float esc_setpoint; /**< setpoint of current ESC */
uint16_t esc_setpoint_raw; /**< setpoint of current ESC (Value sent to ESC) */
uint16_t esc_state; /**< State of ESC - depend on Vendor */
uint16_t esc_errorcount; /**< Number of reported errors by ESC - if supported */
} esc[CONNECTED_ESC_MAX];
};
/**
* @}
*/
/* register this as object request broker structure */
//ORB_DECLARE(esc_status);
ORB_DECLARE_OPTIONAL(esc_status);
#endif

2
src/modules/uORB/topics/vehicle_global_position_setpoint.h
View File

@ -66,7 +66,7 @@ struct vehicle_global_position_setpoint_s
float altitude; /**< altitude in meters */
float yaw; /**< in radians NED -PI..+PI */
float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */
uint8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */
int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */
enum NAV_CMD nav_cmd; /**< true if loitering is enabled */
float param1;
float param2;

2
src/systemcmds/tests/test_gpio.c
View File

@ -92,7 +92,7 @@ int test_gpio(int argc, char *argv[])
int fd;
int ret = 0;
fd = open(GPIO_DEVICE_PATH, 0);
fd = open(PX4IO_DEVICE_PATH, 0);
if (fd < 0) {
printf("GPIO: open fail\n");

267
src/systemcmds/top/top.c
View File

@ -51,19 +51,46 @@
#include <systemlib/cpuload.h>
#include <drivers/drv_hrt.h>
#define CL "\033[K" // clear line
/**
* Start the top application.
*/
__EXPORT int top_main(int argc, char *argv[]);
__EXPORT int top_main(void);
extern struct system_load_s system_load;
bool top_sigusr1_rcvd = false;
int top_main(int argc, char *argv[])
static const char *
tstate_name(const tstate_t s)
{
int t;
switch (s) {
case TSTATE_TASK_INVALID: return "init";
case TSTATE_TASK_PENDING: return "PEND";
case TSTATE_TASK_READYTORUN: return "READY";
case TSTATE_TASK_RUNNING: return "RUN";
case TSTATE_TASK_INACTIVE: return "inact";
case TSTATE_WAIT_SEM: return "w:sem";
#ifndef CONFIG_DISABLE_SIGNALS
case TSTATE_WAIT_SIG: return "w:sig";
#endif
#ifndef CONFIG_DISABLE_MQUEUE
case TSTATE_WAIT_MQNOTEMPTY: return "w:mqe";
case TSTATE_WAIT_MQNOTFULL: return "w:mqf";
#endif
#ifdef CONFIG_PAGING
case TSTATE_WAIT_PAGEFILL: return "w:pgf";
#endif
default:
return "ERROR";
}
}
int
top_main(void)
{
uint64_t total_user_time = 0;
int running_count = 0;
@ -75,7 +102,7 @@ int top_main(int argc, char *argv[])
uint64_t last_times[CONFIG_MAX_TASKS];
float curr_loads[CONFIG_MAX_TASKS];
for (t = 0; t < CONFIG_MAX_TASKS; t++)
for (int t = 0; t < CONFIG_MAX_TASKS; t++)
last_times[t] = 0;
float interval_time_ms_inv = 0.f;
@ -83,16 +110,16 @@ int top_main(int argc, char *argv[])
/* Open console directly to grab CTRL-C signal */
int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
while (true)
// for (t = 0; t < 10; t++)
{
int i;
/* clear screen */
printf("\033[2J");
uint64_t curr_time_ms = (hrt_absolute_time() / 1000LLU);
unsigned int curr_time_s = curr_time_ms / 1000LLU;
for (;;) {
int i;
uint64_t curr_time_us;
uint64_t idle_time_us;
uint64_t idle_time_total_ms = (system_load.tasks[0].total_runtime / 1000LLU);
unsigned int idle_time_total_s = idle_time_total_ms / 1000LLU;
curr_time_us = hrt_absolute_time();
idle_time_us = system_load.tasks[0].total_runtime;
if (new_time > interval_start_time)
interval_time_ms_inv = 1.f / ((float)((new_time - interval_start_time) / 1000));
@ -102,7 +129,38 @@ int top_main(int argc, char *argv[])
total_user_time = 0;
for (i = 0; i < CONFIG_MAX_TASKS; i++) {
uint64_t interval_runtime = (system_load.tasks[i].valid && last_times[i] > 0 && system_load.tasks[i].total_runtime > last_times[i]) ? (system_load.tasks[i].total_runtime - last_times[i]) / 1000 : 0;
uint64_t interval_runtime;
if (system_load.tasks[i].valid) {
switch (system_load.tasks[i].tcb->task_state) {
case TSTATE_TASK_PENDING:
case TSTATE_TASK_READYTORUN:
case TSTATE_TASK_RUNNING:
running_count++;
break;
case TSTATE_TASK_INVALID:
case TSTATE_TASK_INACTIVE:
case TSTATE_WAIT_SEM:
#ifndef CONFIG_DISABLE_SIGNALS
case TSTATE_WAIT_SIG:
#endif
#ifndef CONFIG_DISABLE_MQUEUE
case TSTATE_WAIT_MQNOTEMPTY:
case TSTATE_WAIT_MQNOTFULL:
#endif
#ifdef CONFIG_PAGING
case TSTATE_WAIT_PAGEFILL:
#endif
blocked_count++;
break;
}
}
interval_runtime = (system_load.tasks[i].valid && last_times[i] > 0 &&
system_load.tasks[i].total_runtime > last_times[i])
? (system_load.tasks[i].total_runtime - last_times[i]) / 1000
: 0;
last_times[i] = system_load.tasks[i].total_runtime;
@ -111,7 +169,6 @@ int top_main(int argc, char *argv[])
if (i > 0)
total_user_time += interval_runtime;
} else
curr_loads[i] = 0;
}
@ -119,127 +176,99 @@ int top_main(int argc, char *argv[])
for (i = 0; i < CONFIG_MAX_TASKS; i++) {
if (system_load.tasks[i].valid && (new_time > interval_start_time)) {
if (system_load.tasks[i].tcb->pid == 0) {
float idle = curr_loads[0];
float task_load = (float)(total_user_time) * interval_time_ms_inv;
float idle;
float task_load;
float sched_load;
if (task_load > (1.f - idle)) task_load = (1.f - idle); /* this can happen if one tasks total runtime was not computed correctly by the scheduler instrumentation TODO */
idle = curr_loads[0];
task_load = (float)(total_user_time) * interval_time_ms_inv;
float sched_load = 1.f - idle - task_load;
/* this can happen if one tasks total runtime was not computed
correctly by the scheduler instrumentation TODO */
if (task_load > (1.f - idle))
task_load = (1.f - idle);
sched_load = 1.f - idle - task_load;
/* print system information */
printf("\033[H"); /* cursor home */
printf("\033[KProcesses: %d total, %d running, %d sleeping\n", system_load.total_count, running_count, blocked_count);
printf("\033[KCPU usage: %d.%02d%% tasks, %d.%02d%% sched, %d.%02d%% idle\n", (int)(task_load * 100), (int)((task_load * 10000.0f) - (int)(task_load * 100.0f) * 100), (int)(sched_load * 100), (int)((sched_load * 10000.0f) - (int)(sched_load * 100.0f) * 100), (int)(idle * 100), (int)((idle * 10000.0f) - ((int)(idle * 100)) * 100));
printf("\033[KUptime: %u.%03u s total, %d.%03d s idle\n\033[K\n", curr_time_s, (unsigned int)(curr_time_ms - curr_time_s * 1000LLU), idle_time_total_s, (int)(idle_time_total_ms - idle_time_total_s * 1000));
printf("\033[H"); /* move cursor home and clear screen */
printf(CL "Processes: %d total, %d running, %d sleeping\n",
system_load.total_count,
running_count,
blocked_count);
printf(CL "CPU usage: %.2f%% tasks, %.2f%% sched, %.2f%% idle\n",
(double)(task_load * 100.f),
(double)(sched_load * 100.f),
(double)(idle * 100.f));
printf(CL "Uptime: %.3fs total, %.3fs idle\n\n",
(double)curr_time_us / 1000000.d,
(double)idle_time_us / 1000000.d);
/* 34 chars command name length (32 chars plus two spaces) */
char header_spaces[CONFIG_TASK_NAME_SIZE + 1];
memset(header_spaces, ' ', CONFIG_TASK_NAME_SIZE);
header_spaces[CONFIG_TASK_NAME_SIZE] = '\0';
/* header for task list */
printf(CL "%4s %*-s %8s %6s %11s %10s %-6s\n",
"PID",
CONFIG_TASK_NAME_SIZE, "COMMAND",
"CPU(ms)",
"CPU(%)",
"USED/STACK",
"PRIO(BASE)",
#if CONFIG_RR_INTERVAL > 0
printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tSTACK USE\tCURR (BASE) PRIO\tRR SLICE\n", header_spaces);
"TSLICE"
#else
printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tSTACK USE\tCURR (BASE) PRIO\n", header_spaces);
#endif
} else {
enum tstate_e task_state = (enum tstate_e)system_load.tasks[i].tcb->task_state;
if (task_state == TSTATE_TASK_PENDING ||
task_state == TSTATE_TASK_READYTORUN ||
task_state == TSTATE_TASK_RUNNING) {
running_count++;
}
if (task_state == TSTATE_TASK_INACTIVE || /* BLOCKED - Initialized but not yet activated */
task_state == TSTATE_WAIT_SEM /* BLOCKED - Waiting for a semaphore */
#ifndef CONFIG_DISABLE_SIGNALS
|| task_state == TSTATE_WAIT_SIG /* BLOCKED - Waiting for a signal */
#endif
#ifndef CONFIG_DISABLE_MQUEUE
|| task_state == TSTATE_WAIT_MQNOTEMPTY /* BLOCKED - Waiting for a MQ to become not empty. */
|| task_state == TSTATE_WAIT_MQNOTFULL /* BLOCKED - Waiting for a MQ to become not full. */
#endif
#ifdef CONFIG_PAGING
|| task_state == TSTATE_WAIT_PAGEFILL /* BLOCKED - Waiting for page fill */
#endif
) {
blocked_count++;
}
char spaces[CONFIG_TASK_NAME_SIZE + 2];
/* count name len */
int namelen = 0;
while (namelen < CONFIG_TASK_NAME_SIZE) {
if (system_load.tasks[i].tcb->name[namelen] == '\0') break;
namelen++;
}
int s = 0;
for (s = 0; s < CONFIG_TASK_NAME_SIZE + 2 - namelen; s++) {
spaces[s] = ' ';
}
spaces[s] = '\0';
char *runtime_spaces = " ";
if ((system_load.tasks[i].total_runtime / 1000) < 99) {
runtime_spaces = "";
}
unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr -
(uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr;
unsigned stack_free = 0;
uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr;
while (stack_free < stack_size) {
if (*stack_sweeper++ != 0xff)
break;
stack_free++;
}
printf("\033[K % 2d\t%s%s % 8lld ms%s \t % 2d.%03d \t % 4u / % 4u",
(int)system_load.tasks[i].tcb->pid,
system_load.tasks[i].tcb->name,
spaces,
(system_load.tasks[i].total_runtime / 1000),
runtime_spaces,
(int)(curr_loads[i] * 100),
(int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100),
stack_size - stack_free,
stack_size);
/* Print scheduling info with RR time slice */
#if CONFIG_RR_INTERVAL > 0
printf("\t%d\t(%d)\t\t%d\n", (int)system_load.tasks[i].tcb->sched_priority, (int)system_load.tasks[i].tcb->base_priority, (int)system_load.tasks[i].tcb->timeslice);
#else
/* Print scheduling info without time slice*/
printf("\t%d (%d)\n", (int)system_load.tasks[i].tcb->sched_priority, (int)system_load.tasks[i].tcb->base_priority);
"STATE"
#endif
);
}
unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr -
(uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr;
unsigned stack_free = 0;
uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr;
while (stack_free < stack_size) {
if (*stack_sweeper++ != 0xff)
break;
stack_free++;
}
printf(CL "%4d %*-s %8lld %2d.%03d %5u/%5u %3u (%3u) ",
system_load.tasks[i].tcb->pid,
CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name,
(system_load.tasks[i].total_runtime / 1000),
(int)(curr_loads[i] * 100),
(int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100),
stack_size - stack_free,
stack_size,
system_load.tasks[i].tcb->sched_priority,
system_load.tasks[i].tcb->base_priority);
#if CONFIG_RR_INTERVAL > 0
/* print scheduling info with RR time slice */
printf(" %6d\n", system_load.tasks[i].tcb->timeslice);
#else
// print task state instead
printf(" %-6s\n", tstate_name(system_load.tasks[i].tcb->task_state));
#endif
}
}
printf("\033[K[ Hit Ctrl-C to quit. ]\n\033[J");
fflush(stdout);
interval_start_time = new_time;
char c;
/* Sleep 200 ms waiting for user input four times */
/* Sleep 200 ms waiting for user input five times ~ 1s */
/* XXX use poll ... */
for (int k = 0; k < 4; k++) {
for (int k = 0; k < 5; k++) {
char c;
if (read(console, &c, 1) == 1) {
if (c == 0x03 || c == 0x63) {
printf("Abort\n");
switch (c) {
case 0x03: // ctrl-c
case 0x1b: // esc
case 'c':
case 'q':
close(console);
return OK;
/* not reached */
}
}