fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 12:48:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into fmuv2_bringup

Browse Source
This commit is contained in:
Lorenz Meier
2013-06-10 15:01:44 +02:00
parent 308ec6001a 42ce3112ad
commit 9444def5f8
47 changed files with 4318 additions and 477 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/gps/ubx.cpp
+1 -1
View File
@@ -739,7 +739,7 @@ UBX::configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate)
msg.msg_class = msg_class;
msg.msg_id = msg_id;
msg.rate = rate;
send_message(CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg));
send_message(UBX_CLASS_CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg));
}
void
src/drivers/hmc5883/hmc5883.cpp
+1 -1
View File
@@ -329,7 +329,7 @@ HMC5883::HMC5883(int bus) :
_calibrated(false)
{
// enable debug() calls
_debug_enabled = true;
_debug_enabled = false;
// default scaling
_scale.x_offset = 0;
src/drivers/hott_telemetry/hott_telemetry_main.c
+64 -70
View File
@@ -53,6 +53,7 @@
#include <termios.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include "messages.h"
@@ -60,56 +61,44 @@
static int thread_should_exit = false; /**< Deamon exit flag */
static int thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
static char *daemon_name = "hott_telemetry";
static char *commandline_usage = "usage: hott_telemetry start|status|stop [-d <device>]";
static const char daemon_name[] = "hott_telemetry";
static const char commandline_usage[] = "usage: hott_telemetry start|status|stop [-d <device>]";
/* A little console messaging experiment - console helper macro */
#define FATAL_MSG(_msg) fprintf(stderr, "[%s] %s\n", daemon_name, _msg); exit(1);
#define ERROR_MSG(_msg) fprintf(stderr, "[%s] %s\n", daemon_name, _msg);
#define INFO_MSG(_msg) printf("[%s] %s\n", daemon_name, _msg);
/**
* Deamon management function.
*/
__EXPORT int hott_telemetry_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
* Mainloop of daemon.
*/
int hott_telemetry_thread_main(int argc, char *argv[]);
static int read_data(int uart, int *id);
static int send_data(int uart, uint8_t *buffer, int size);
static int 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)
static int
open_uart(const char *device, struct termios *uart_config_original)
{
/* baud rate */
int speed = B19200;
int uart;
static const speed_t speed = B19200;
/* open uart */
uart = open(device, O_RDWR | O_NOCTTY);
const int uart = open(device, O_RDWR | O_NOCTTY);
if (uart < 0) {
char msg[80];
sprintf(msg, "Error opening port: %s\n", device);
FATAL_MSG(msg);
err(1, "Error opening port: %s", device);
}
/* Try to set baud rate */
struct termios uart_config;
/* Back up the original uart configuration to restore it after exit */
int termios_state;
/* Back up the original uart configuration to restore it after exit */
char msg[80];
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
sprintf(msg, "Error getting baudrate / termios config for %s: %d\n", device, termios_state);
close(uart);
FATAL_MSG(msg);
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) */
@@ -117,16 +106,14 @@ static int open_uart(const char *device, struct termios *uart_config_original)
/* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
sprintf(msg, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n",
device, termios_state);
close(uart);
FATAL_MSG(msg);
err(1, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)",
device, termios_state);
}
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
sprintf(msg, "Error setting baudrate / termios config for %s (tcsetattr)\n", device);
close(uart);
FATAL_MSG(msg);
err(1, "Error setting baudrate / termios config for %s (tcsetattr)", device);
}
/* Activate single wire mode */
@@ -135,39 +122,41 @@ static int open_uart(const char *device, struct termios *uart_config_original)
return uart;
}
int read_data(int uart, int *id)
int
recv_req_id(int uart, uint8_t *id)
{
const int timeout = 1000;
static const int timeout_ms = 1000; // TODO make it a define
struct pollfd fds[] = { { .fd = uart, .events = POLLIN } };
char mode;
uint8_t mode;
if (poll(fds, 1, timeout) > 0) {
if (poll(fds, 1, timeout_ms) > 0) {
/* Get the mode: binary or text */
read(uart, &mode, 1);
/* Read the device ID being polled */
read(uart, id, 1);
read(uart, &mode, sizeof(mode));
/* if we have a binary mode request */
if (mode != BINARY_MODE_REQUEST_ID) {
return ERROR;
}
/* Read the device ID being polled */
read(uart, id, sizeof(*id));
} else {
ERROR_MSG("UART timeout on TX/RX port");
warnx("UART timeout on TX/RX port");
return ERROR;
}
return OK;
}
int send_data(int uart, uint8_t *buffer, int size)
int
send_data(int uart, uint8_t *buffer, size_t size)
{
usleep(POST_READ_DELAY_IN_USECS);
uint16_t checksum = 0;
for (int i = 0; i < size; i++) {
for (size_t i = 0; i < size; i++) {
if (i == size - 1) {
/* Set the checksum: the first uint8_t is taken as the checksum. */
buffer[i] = checksum & 0xff;
@@ -176,7 +165,7 @@ int send_data(int uart, uint8_t *buffer, int size)
checksum += buffer[i];
}
write(uart, &buffer[i], 1);
write(uart, &buffer[i], sizeof(buffer[i]));
/* Sleep before sending the next byte. */
usleep(POST_WRITE_DELAY_IN_USECS);
@@ -190,13 +179,14 @@ int send_data(int uart, uint8_t *buffer, int size)
return OK;
}
int hott_telemetry_thread_main(int argc, char *argv[])
int
hott_telemetry_thread_main(int argc, char *argv[])
{
INFO_MSG("starting");
warnx("starting");
thread_running = true;
char *device = "/dev/ttyS1"; /**< Default telemetry port: USART2 */
const char *device = "/dev/ttyS1"; /**< Default telemetry port: USART2 */
/* read commandline arguments */
for (int i = 0; i < argc && argv[i]; i++) {
@@ -206,45 +196,55 @@ int hott_telemetry_thread_main(int argc, char *argv[])
} else {
thread_running = false;
ERROR_MSG("missing parameter to -d");
ERROR_MSG(commandline_usage);
exit(1);
errx(1, "missing parameter to -d\n%s", commandline_usage);
}
}
}
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
struct termios uart_config_original;
int uart = open_uart(device, &uart_config_original);
const int uart = open_uart(device, &uart_config_original);
if (uart < 0) {
ERROR_MSG("Failed opening HoTT UART, exiting.");
errx(1, "Failed opening HoTT UART, exiting.");
thread_running = false;
exit(ERROR);
}
messages_init();
uint8_t buffer[MESSAGE_BUFFER_SIZE];
int size = 0;
int id = 0;
size_t size = 0;
uint8_t id = 0;
bool connected = true;
while (!thread_should_exit) {
if (read_data(uart, &id) == OK) {
if (recv_req_id(uart, &id) == OK) {
if (!connected) {
connected = true;
warnx("OK");
}
switch (id) {
case ELECTRIC_AIR_MODULE:
case EAM_SENSOR_ID:
build_eam_response(buffer, &size);
break;
case GPS_SENSOR_ID:
build_gps_response(buffer, &size);
break;
default:
continue; // Not a module we support.
}
send_data(uart, buffer, size);
} else {
connected = false;
warnx("syncing");
}
}
INFO_MSG("exiting");
warnx("exiting");
close(uart);
@@ -256,23 +256,22 @@ int hott_telemetry_thread_main(int argc, char *argv[])
/**
* Process command line arguments and tart the daemon.
*/
int hott_telemetry_main(int argc, char *argv[])
int
hott_telemetry_main(int argc, char *argv[])
{
if (argc < 1) {
ERROR_MSG("missing command");
ERROR_MSG(commandline_usage);
exit(1);
errx(1, "missing command\n%s", commandline_usage);
}
if (!strcmp(argv[1], "start")) {
if (thread_running) {
INFO_MSG("deamon already running");
warnx("deamon already running");
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn("hott_telemetry",
deamon_task = task_spawn(daemon_name,
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40,
2048,
@@ -288,19 +287,14 @@ int hott_telemetry_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
INFO_MSG("daemon is running");
warnx("daemon is running");
} else {
INFO_MSG("daemon not started");
warnx("daemon not started");
}
exit(0);
}
ERROR_MSG("unrecognized command");
ERROR_MSG(commandline_usage);
exit(1);
errx(1, "unrecognized command\n%s", commandline_usage);
}
src/drivers/hott_telemetry/messages.c
+153 -21
View File
@@ -1,7 +1,7 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Author: Simon Wilks <sjwilks@gmail.com>
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @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
@@ -34,30 +34,44 @@
/**
* @file messages.c
* @author Simon Wilks <sjwilks@gmail.com>
*
*/
#include "messages.h"
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <systemlib/systemlib.h>
#include <systemlib/geo/geo.h>
#include <unistd.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_gps_position.h>
/* The board is very roughly 5 deg warmer than the surrounding air */
#define BOARD_TEMP_OFFSET_DEG 5
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;
void messages_init(void)
static bool home_position_set = false;
static double home_lat = 0.0d;
static double home_lon = 0.0d;
void
messages_init(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));
}
void build_eam_response(uint8_t *buffer, int *size)
void
build_eam_response(uint8_t *buffer, size_t *size)
{
/* get a local copy of the current sensor values */
struct sensor_combined_s raw;
@@ -74,26 +88,144 @@ void build_eam_response(uint8_t *buffer, int *size)
memset(&msg, 0, *size);
msg.start = START_BYTE;
msg.eam_sensor_id = ELECTRIC_AIR_MODULE;
msg.sensor_id = EAM_SENSOR_ID;
msg.eam_sensor_id = EAM_SENSOR_ID;
msg.sensor_id = EAM_SENSOR_TEXT_ID;
msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20);
msg.temperature2 = TEMP_ZERO_CELSIUS;
msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG;
msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10);
uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500);
msg.altitude_L = (uint8_t)alt & 0xff;
msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
/* get a local copy of the current sensor values */
struct airspeed_s airspeed;
memset(&airspeed, 0, sizeof(airspeed));
orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
msg.stop = STOP_BYTE;
memcpy(buffer, &msg, *size);
}
uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6);
msg.speed_L = (uint8_t)speed & 0xff;
msg.speed_H = (uint8_t)(speed >> 8) & 0xff;
void
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);
/* 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);
struct gps_module_msg msg = { 0 };
*size = sizeof(msg);
memset(&msg, 0, *size);
msg.start = START_BYTE;
msg.sensor_id = GPS_SENSOR_ID;
msg.sensor_text_id = GPS_SENSOR_TEXT_ID;
msg.gps_num_sat = gps.satellites_visible;
/* The GPS fix type: 0 = none, 2 = 2D, 3 = 3D */
msg.gps_fix_char = (uint8_t)(gps.fix_type + 48);
msg.gps_fix = (uint8_t)(gps.fix_type + 48);
/* No point collecting more data if we don't have a 3D fix yet */
if (gps.fix_type > 2) {
/* Current flight direction */
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);
msg.gps_speed_L = (uint8_t)speed & 0xff;
msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff;
/* Get latitude in degrees, minutes and seconds */
double lat = ((double)(gps.lat))*1e-7d;
/* Set the N or S specifier */
msg.latitude_ns = 0;
if (lat < 0) {
msg.latitude_ns = 1;
lat = abs(lat);
}
int deg;
int min;
int sec;
convert_to_degrees_minutes_seconds(lat, &deg, &min, &sec);
uint16_t lat_min = (uint16_t)(deg * 100 + min);
msg.latitude_min_L = (uint8_t)lat_min & 0xff;
msg.latitude_min_H = (uint8_t)(lat_min >> 8) & 0xff;
uint16_t lat_sec = (uint16_t)(sec);
msg.latitude_sec_L = (uint8_t)lat_sec & 0xff;
msg.latitude_sec_H = (uint8_t)(lat_sec >> 8) & 0xff;
/* Get longitude in degrees, minutes and seconds */
double lon = ((double)(gps.lon))*1e-7d;
/* Set the E or W specifier */
msg.longitude_ew = 0;
if (lon < 0) {
msg.longitude_ew = 1;
lon = abs(lon);
}
convert_to_degrees_minutes_seconds(lon, &deg, &min, &sec);
uint16_t lon_min = (uint16_t)(deg * 100 + min);
msg.longitude_min_L = (uint8_t)lon_min & 0xff;
msg.longitude_min_H = (uint8_t)(lon_min >> 8) & 0xff;
uint16_t lon_sec = (uint16_t)(sec);
msg.longitude_sec_L = (uint8_t)lon_sec & 0xff;
msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff;
/* Altitude */
uint16_t alt = (uint16_t)(gps.alt*1e-3 + 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 */
bool 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);
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);
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);
msg.home_direction = (uint8_t)bearing >> 1;
}
}
msg.stop = STOP_BYTE;
memcpy(buffer, &msg, *size);
}
}
void
convert_to_degrees_minutes_seconds(double val, int *deg, int *min, int *sec)
{
*deg = (int)val;
double delta = val - *deg;
const double min_d = delta * 60.0d;
*min = (int)min_d;
delta = min_d - *min;
*sec = (int)(delta * 10000.0d);
}
src/drivers/hott_telemetry/messages.h
+97 -25
View File
@@ -44,11 +44,6 @@
#include <stdlib.h>
/* The buffer size used to store messages. This must be at least as big as the number of
* fields in the largest message struct.
*/
#define MESSAGE_BUFFER_SIZE 50
/* The HoTT receiver demands a minimum 5ms period of silence after delivering its request.
* Note that the value specified here is lower than 5000 (5ms) as time is lost constucting
* the message after the read which takes some milliseconds.
@@ -66,18 +61,18 @@
#define TEMP_ZERO_CELSIUS 0x14
/* Electric Air Module (EAM) constants. */
#define ELECTRIC_AIR_MODULE 0x8e
#define EAM_SENSOR_ID 0xe0
#define EAM_SENSOR_ID 0x8e
#define EAM_SENSOR_TEXT_ID 0xe0
/* The Electric Air Module message. */
struct eam_module_msg {
uint8_t start; /**< Start byte */
uint8_t eam_sensor_id; /**< EAM sensor ID */
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_id; /**< Sensor ID, why different? */
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;
@@ -95,30 +90,107 @@ struct eam_module_msg {
uint8_t batt1_voltage_H;
uint8_t batt2_voltage_L; /**< Battery 2 voltage, lower 8-bits in steps of 0.02V */
uint8_t batt2_voltage_H;
uint8_t temperature1; /**< Temperature sensor 1. 20 = 0 degrees */
uint8_t 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_L; /**< Main power voltage lower 8-bits in steps of 0.1V */
uint8_t main_voltage_H;
uint8_t battery_capacity_L; /**< Used battery capacity in steps of 10mAh */
uint8_t battery_capacity_L; /**< Used battery capacity in steps of 10mAh */
uint8_t battery_capacity_H;
uint8_t climbrate_L; /**< Climb rate in 0.01m/s. 0m/s = 30000 */
uint8_t climbrate_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 climbrate_3s; /**< Climb rate in m/3sec. 0m/3sec = 120 */
uint8_t rpm_L; /**< RPM Lower 8-bits In steps of 10 U/min */
uint8_t rpm_H;
uint8_t electric_min; /**< Flight time in minutes. */
uint8_t electric_sec; /**< Flight time in seconds. */
uint8_t speed_L; /**< Airspeed in km/h in steps of 1 km/h */
uint8_t electric_min; /**< Flight time in minutes. */
uint8_t electric_sec; /**< Flight time in seconds. */
uint8_t speed_L; /**< Airspeed in km/h in steps of 1 km/h */
uint8_t speed_H;
uint8_t stop; /**< Stop byte */
uint8_t checksum; /**< Lower 8-bits of all bytes summed. */
uint8_t stop; /**< Stop byte */
uint8_t checksum; /**< Lower 8-bits of all bytes summed. */
};
/**
* The maximum buffer size required to store a HoTT message.
*/
#define MESSAGE_BUFFER_SIZE sizeof(union { \
struct eam_module_msg eam; \
})
/* GPS sensor constants. */
#define GPS_SENSOR_ID 0x8A
#define GPS_SENSOR_TEXT_ID 0xA0
/**
* The GPS sensor message
* Struct based on: https://code.google.com/p/diy-hott-gps/downloads
*/
struct gps_module_msg {
uint8_t start; /**< Start byte */
uint8_t sensor_id; /**< GPS sensor ID*/
uint8_t warning; /**< Byte 3: 0…= warning beeps */
uint8_t sensor_text_id; /**< GPS Sensor text mode ID */
uint8_t alarm_inverse1; /**< Byte 5: 01 inverse status */
uint8_t alarm_inverse2; /**< Byte 6: 00 inverse status status 1 = no GPS Signal */
uint8_t flight_direction; /**< Byte 7: 119 = Flightdir./dir. 1 = 2°; 0° (North), 9 0° (East), 180° (South), 270° (West) */
uint8_t gps_speed_L; /**< Byte 8: 8 = /GPS speed low byte 8km/h */
uint8_t gps_speed_H; /**< Byte 9: 0 = /GPS speed high byte */
uint8_t latitude_ns; /**< Byte 10: 000 = N = 48°39’988 */
uint8_t latitude_min_L; /**< Byte 11: 231 0xE7 = 0x12E7 = 4839 */
uint8_t latitude_min_H; /**< Byte 12: 018 18 = 0x12 */
uint8_t latitude_sec_L; /**< Byte 13: 171 220 = 0xDC = 0x03DC =0988 */
uint8_t latitude_sec_H; /**< Byte 14: 016 3 = 0x03 */
uint8_t longitude_ew; /**< Byte 15: 000 = E= 9° 25’9360 */
uint8_t longitude_min_L; /**< Byte 16: 150 157 = 0x9D = 0x039D = 0925 */
uint8_t longitude_min_H; /**< Byte 17: 003 3 = 0x03 */
uint8_t longitude_sec_L; /**< Byte 18: 056 144 = 0x90 0x2490 = 9360*/
uint8_t longitude_sec_H; /**< Byte 19: 004 36 = 0x24 */
uint8_t distance_L; /**< Byte 20: 027 123 = /distance low byte 6 = 6 m */
uint8_t distance_H; /**< Byte 21: 036 35 = /distance high byte */
uint8_t altitude_L; /**< Byte 22: 243 244 = /Altitude low byte 500 = 0m */
uint8_t altitude_H; /**< Byte 23: 001 1 = /Altitude high byte */
uint8_t resolution_L; /**< Byte 24: 48 = Low Byte m/s resolution 0.01m 48 = 30000 = 0.00m/s (1=0.01m/s) */
uint8_t resolution_H; /**< Byte 25: 117 = High Byte m/s resolution 0.01m */
uint8_t unknown1; /**< Byte 26: 120 = 0m/3s */
uint8_t gps_num_sat; /**< Byte 27: GPS.Satellites (number of satelites) (1 byte) */
uint8_t gps_fix_char; /**< Byte 28: GPS.FixChar. (GPS fix character. display, if DGPS, 2D oder 3D) (1 byte) */
uint8_t home_direction; /**< Byte 29: HomeDirection (direction from starting point to Model position) (1 byte) */
uint8_t angle_x_direction; /**< Byte 30: angle x-direction (1 byte) */
uint8_t angle_y_direction; /**< Byte 31: angle y-direction (1 byte) */
uint8_t angle_z_direction; /**< Byte 32: angle z-direction (1 byte) */
uint8_t gyro_x_L; /**< Byte 33: gyro x low byte (2 bytes) */
uint8_t gyro_x_H; /**< Byte 34: gyro x high byte */
uint8_t gyro_y_L; /**< Byte 35: gyro y low byte (2 bytes) */
uint8_t gyro_y_H; /**< Byte 36: gyro y high byte */
uint8_t gyro_z_L; /**< Byte 37: gyro z low byte (2 bytes) */
uint8_t gyro_z_H; /**< Byte 38: gyro z high byte */
uint8_t vibration; /**< Byte 39: vibration (1 bytes) */
uint8_t ascii4; /**< Byte 40: 00 ASCII Free Character [4] */
uint8_t ascii5; /**< Byte 41: 00 ASCII Free Character [5] */
uint8_t gps_fix; /**< Byte 42: 00 ASCII Free Character [6], we use it for GPS FIX */
uint8_t version; /**< Byte 43: 00 version number */
uint8_t stop; /**< Byte 44: 0x7D Ende byte */
uint8_t checksum; /**< Byte 45: Parity Byte */
};
/**
* The maximum buffer size required to store a HoTT message.
*/
#define GPS_MESSAGE_BUFFER_SIZE sizeof(union { \
struct gps_module_msg gps; \
})
void messages_init(void);
void build_eam_response(uint8_t *buffer, int *size);
void build_eam_response(uint8_t *buffer, size_t *size);
void build_gps_response(uint8_t *buffer, size_t *size);
float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec);
#endif /* MESSAGES_H_ */
src/drivers/mkblctrl/mkblctrl.cpp
+287 -237
View File
@@ -76,7 +76,6 @@
#include <uORB/topics/actuator_outputs.h>
#include <systemlib/err.h>
#include <systemlib/ppm_decode.h>
#define I2C_BUS_SPEED 400000
#define UPDATE_RATE 400
@@ -114,6 +113,7 @@ public:
virtual int ioctl(file *filp, int cmd, unsigned long arg);
virtual int init(unsigned motors);
virtual ssize_t write(file *filp, const char *buffer, size_t len);
int set_mode(Mode mode);
int set_pwm_rate(unsigned rate);
@@ -177,9 +177,10 @@ private:
int gpio_ioctl(file *filp, int cmd, unsigned long arg);
int mk_servo_arm(bool status);
int mk_servo_set(unsigned int chan, float val);
int mk_servo_set_test(unsigned int chan, float val);
int mk_servo_set(unsigned int chan, short val);
int mk_servo_set_value(unsigned int chan, short val);
int mk_servo_test(unsigned int chan);
short scaling(float val, float inMin, float inMax, float outMin, float outMax);
};
@@ -207,20 +208,20 @@ const int blctrlAddr_octo_x[] = { 1, 4, 0, 1, -4, 1, 1, -4 }; // Addresstranslat
const int blctrlAddr_px4[] = { 0, 0, 0, 0, 0, 0, 0, 0};
int addrTranslator[] = {0,0,0,0,0,0,0,0};
int addrTranslator[] = {0, 0, 0, 0, 0, 0, 0, 0};
struct MotorData_t
{
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
unsigned int State; // 7 bit for I2C error counter, highest bit indicates if motor is present
unsigned int ReadMode; // select data to read
// the following bytes must be exactly in that order!
unsigned int Current; // in 0.1 A steps, read back from BL
unsigned int MaxPWM; // read back from BL is less than 255 if BL is in current limit
unsigned int Temperature; // old BL-Ctrl will return a 255 here, the new version the temp. in
unsigned int RoundCount;
unsigned int SetPoint; // written by attitude controller
unsigned int SetPointLowerBits; // for higher Resolution of new BLs
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
// the following bytes must be exactly in that order!
unsigned int Current; // in 0.1 A steps, read back from BL
unsigned int MaxPWM; // read back from BL is less than 255 if BL is in current limit
unsigned int Temperature; // old BL-Ctrl will return a 255 here, the new version the temp. in
unsigned int RoundCount;
};
MotorData_t Motor[MAX_MOTORS];
@@ -314,7 +315,7 @@ MK::init(unsigned motors)
/* start the IO interface task */
_task = task_spawn("mkblctrl",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX -20,
SCHED_PRIORITY_MAX - 20,
2048,
(main_t)&MK::task_main_trampoline,
nullptr);
@@ -346,27 +347,11 @@ MK::set_mode(Mode mode)
*/
switch (mode) {
case MODE_2PWM:
if(_num_outputs == 4) {
//debug("MODE_QUAD");
} else if(_num_outputs == 6) {
//debug("MODE_HEXA");
} else if(_num_outputs == 8) {
//debug("MODE_OCTO");
}
//up_pwm_servo_init(0x3);
up_pwm_servo_deinit();
_update_rate = UPDATE_RATE; /* default output rate */
break;
case MODE_4PWM:
if(_num_outputs == 4) {
//debug("MODE_QUADRO");
} else if(_num_outputs == 6) {
//debug("MODE_HEXA");
} else if(_num_outputs == 8) {
//debug("MODE_OCTO");
}
//up_pwm_servo_init(0xf);
up_pwm_servo_deinit();
_update_rate = UPDATE_RATE; /* default output rate */
break;
@@ -412,45 +397,55 @@ MK::set_frametype(int frametype)
int
MK::set_motor_count(unsigned count)
{
if(count > 0) {
if (count > 0) {
_num_outputs = count;
if(_px4mode == MAPPING_MK) {
if(_frametype == FRAME_PLUS) {
if (_px4mode == MAPPING_MK) {
if (_frametype == FRAME_PLUS) {
fprintf(stderr, "[mkblctrl] addresstanslator for Mikrokopter addressing used. Frametype: +\n");
} else if(_frametype == FRAME_X) {
} else if (_frametype == FRAME_X) {
fprintf(stderr, "[mkblctrl] addresstanslator for Mikrokopter addressing used. Frametype: X\n");
}
if(_num_outputs == 4) {
if(_frametype == FRAME_PLUS) {
if (_num_outputs == 4) {
if (_frametype == FRAME_PLUS) {
memcpy(&addrTranslator, &blctrlAddr_quad_plus, sizeof(blctrlAddr_quad_plus));
} else if(_frametype == FRAME_X) {
} else if (_frametype == FRAME_X) {
memcpy(&addrTranslator, &blctrlAddr_quad_x, sizeof(blctrlAddr_quad_x));
}
} else if(_num_outputs == 6) {
if(_frametype == FRAME_PLUS) {
} else if (_num_outputs == 6) {
if (_frametype == FRAME_PLUS) {
memcpy(&addrTranslator, &blctrlAddr_hexa_plus, sizeof(blctrlAddr_hexa_plus));
} else if(_frametype == FRAME_X) {
} else if (_frametype == FRAME_X) {
memcpy(&addrTranslator, &blctrlAddr_hexa_x, sizeof(blctrlAddr_hexa_x));
}
} else if(_num_outputs == 8) {
if(_frametype == FRAME_PLUS) {
} else if (_num_outputs == 8) {
if (_frametype == FRAME_PLUS) {
memcpy(&addrTranslator, &blctrlAddr_octo_plus, sizeof(blctrlAddr_octo_plus));
} else if(_frametype == FRAME_X) {
} else if (_frametype == FRAME_X) {
memcpy(&addrTranslator, &blctrlAddr_octo_x, sizeof(blctrlAddr_octo_x));
}
}
} else {
fprintf(stderr, "[mkblctrl] PX4 native addressing used.\n");
memcpy(&addrTranslator, &blctrlAddr_px4, sizeof(blctrlAddr_px4));
}
if(_num_outputs == 4) {
if (_num_outputs == 4) {
fprintf(stderr, "[mkblctrl] Quadrocopter Mode (4)\n");
} else if(_num_outputs == 6) {
} else if (_num_outputs == 6) {
fprintf(stderr, "[mkblctrl] Hexacopter Mode (6)\n");
} else if(_num_outputs == 8) {
} else if (_num_outputs == 8) {
fprintf(stderr, "[mkblctrl] Octocopter Mode (8)\n");
}
@@ -469,16 +464,35 @@ MK::set_motor_test(bool motortest)
return OK;
}
short
MK::scaling(float val, float inMin, float inMax, float outMin, float outMax)
{
short retVal = 0;
retVal = (val - inMin) / (inMax - inMin) * (outMax - outMin) + outMin;
if (retVal < outMin) {
retVal = outMin;
} else if (retVal > outMax) {
retVal = outMax;
}
return retVal;
}
void
MK::task_main()
{
long update_rate_in_us = 0;
float tmpVal = 0;
/*
* Subscribe to the appropriate PWM output topic based on whether we are the
* primary PWM output or not.
*/
_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1));
_t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
/* force a reset of the update rate */
_current_update_rate = 0;
@@ -492,6 +506,8 @@ 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));
@@ -499,21 +515,14 @@ 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);
pollfd fds[2];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
fds[1].fd = _t_armed;
fds[1].events = POLLIN;
// rc input, published to ORB
struct rc_input_values rc_in;
orb_advert_t to_input_rc = 0;
memset(&rc_in, 0, sizeof(rc_in));
rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM;
log("starting");
long update_rate_in_us = 0;
/* loop until killed */
while (!_task_should_exit) {
@@ -528,6 +537,7 @@ MK::task_main()
update_rate_in_ms = 2;
_update_rate = 500;
}
/* reject slower than 50 Hz updates */
if (update_rate_in_ms > 20) {
update_rate_in_ms = 20;
@@ -539,8 +549,8 @@ MK::task_main()
_current_update_rate = _update_rate;
}
/* sleep waiting for data, but no more than a second */
int ret = ::poll(&fds[0], 2, 1000);
/* sleep waiting for data max 100ms */
int ret = ::poll(&fds[0], 2, 100);
/* this would be bad... */
if (ret < 0) {
@@ -553,7 +563,7 @@ MK::task_main()
if (fds[0].revents & POLLIN) {
/* get controls - must always do this to avoid spinning */
orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls);
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
/* can we mix? */
if (_mixers != nullptr) {
@@ -565,53 +575,52 @@ MK::task_main()
// XXX output actual limited values
memcpy(&controls_effective, &_controls, sizeof(controls_effective));
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
/* iterate actuators */
for (unsigned int i = 0; i < _num_outputs; i++) {
/* last resort: catch NaN, INF and out-of-band errors */
if (i < outputs.noutputs &&
isfinite(outputs.output[i]) &&
outputs.output[i] >= -1.0f &&
outputs.output[i] <= 1.0f) {
isfinite(outputs.output[i]) &&
outputs.output[i] >= -1.0f &&
outputs.output[i] <= 1.0f) {
/* scale for PWM output 900 - 2100us */
//outputs.output[i] = 1500 + (600 * outputs.output[i]);
//outputs.output[i] = 127 + (127 * outputs.output[i]);
/* nothing to do here */
} else {
/*
* Value is NaN, INF or out of band - set to the minimum value.
* This will be clearly visible on the servo status and will limit the risk of accidentally
* spinning motors. It would be deadly in flight.
*/
if(outputs.output[i] < -1.0f) {
if (outputs.output[i] < -1.0f) {
outputs.output[i] = -1.0f;
} else if(outputs.output[i] > 1.0f) {
} else if (outputs.output[i] > 1.0f) {
outputs.output[i] = 1.0f;
} else {
outputs.output[i] = -1.0f;
}
}
/* don't go under BLCTRL_MIN_VALUE */
if(outputs.output[i] < BLCTRL_MIN_VALUE) {
if (outputs.output[i] < BLCTRL_MIN_VALUE) {
outputs.output[i] = BLCTRL_MIN_VALUE;
}
//_motortest = true;
/* output to BLCtrl's */
if(_motortest == true) {
mk_servo_test(i);
} else {
//mk_servo_set(i, outputs.output[i]);
mk_servo_set_test(i, outputs.output[i]); // 8Bit
}
/* output to BLCtrl's */
if (_motortest == true) {
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
}
}
/* and publish for anyone that cares to see */
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
}
}
/* how about an arming update? */
@@ -622,29 +631,9 @@ MK::task_main()
orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
/* update PWM servo armed status if armed and not locked down */
////up_pwm_servo_arm(aa.armed && !aa.lockdown);
mk_servo_arm(aa.armed && !aa.lockdown);
}
// see if we have new PPM input data
if (ppm_last_valid_decode != rc_in.timestamp) {
// we have a new PPM frame. Publish it.
rc_in.channel_count = ppm_decoded_channels;
if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) {
rc_in.channel_count = RC_INPUT_MAX_CHANNELS;
}
for (uint8_t i=0; i<rc_in.channel_count; i++) {
rc_in.values[i] = ppm_buffer[i];
}
rc_in.timestamp = ppm_last_valid_decode;
/* lazily advertise on first publication */
if (to_input_rc == 0) {
to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in);
} else {
orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in);
}
}
}
@@ -666,7 +655,7 @@ MK::task_main()
}
int
int
MK::mk_servo_arm(bool status)
{
_armed = status;
@@ -680,12 +669,13 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput)
_retries = 50;
uint8_t foundMotorCount = 0;
for(unsigned i=0; i<MAX_MOTORS; i++) {
for (unsigned i = 0; i < MAX_MOTORS; i++) {
Motor[i].Version = 0;
Motor[i].SetPoint = 0;
Motor[i].SetPointLowerBits = 0;
Motor[i].State = 0;
Motor[i].ReadMode = 0;
Motor[i].RawPwmValue = 0;
Motor[i].Current = 0;
Motor[i].MaxPWM = 0;
Motor[i].Temperature = 0;
@@ -695,34 +685,37 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput)
uint8_t msg = 0;
uint8_t result[3];
for(unsigned i=0; i< count; i++) {
for (unsigned i = 0; i < count; i++) {
result[0] = 0;
result[1] = 0;
result[2] = 0;
set_address( BLCTRL_BASE_ADDR + i );
set_address(BLCTRL_BASE_ADDR + i);
if (OK == transfer(&msg, 1, &result[0], 3)) {
Motor[i].Current = result[0];
Motor[i].MaxPWM = result[1];
Motor[i].Temperature = result[2];
Motor[i].State |= MOTOR_STATE_PRESENT_MASK; // set present bit;
foundMotorCount++;
if(Motor[i].MaxPWM == 250) {
if (Motor[i].MaxPWM == 250) {
Motor[i].Version = BLCTRL_NEW;
} else {
Motor[i].Version = BLCTRL_OLD;
}
}
}
if(showOutput) {
fprintf(stderr, "[mkblctrl] MotorsFound: %i\n",foundMotorCount);
for(unsigned i=0; i< foundMotorCount; i++) {
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 (showOutput) {
fprintf(stderr, "[mkblctrl] MotorsFound: %i\n", foundMotorCount);
for (unsigned i = 0; i < foundMotorCount; i++) {
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) {
if (foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) {
_task_should_exit = true;
}
}
@@ -734,122 +727,136 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput)
int
MK::mk_servo_set(unsigned int chan, float val)
MK::mk_servo_set(unsigned int chan, short val)
{
float tmpVal = 0;
short tmpVal = 0;
_retries = 0;
uint8_t result[3] = { 0,0,0 };
uint8_t msg[2] = { 0,0 };
uint8_t rod=0;
uint8_t result[3] = { 0, 0, 0 };
uint8_t msg[2] = { 0, 0 };
uint8_t rod = 0;
uint8_t bytesToSendBL2 = 2;
tmpVal = val;
tmpVal = (1023 + (1023 * val));
if(tmpVal > 2047) {
tmpVal = 2047;
if (tmpVal > 1024) {
tmpVal = 1024;
} else if (tmpVal < 0) {
tmpVal = 0;
}
Motor[chan].SetPoint = (uint8_t)(tmpVal / 4);
//Motor[chan].SetPointLowerBits = (uint8_t) tmpVal % 4;
Motor[chan].SetPoint = (uint8_t) tmpVal / 3; // divide 8
Motor[chan].SetPointLowerBits = (uint8_t) tmpVal % 8; // rest of divide 8
//rod = (uint8_t) tmpVal % 8;
//Motor[chan].SetPointLowerBits = rod<<1; // rest of divide 8
Motor[chan].SetPointLowerBits = 0;
if(_armed == false) {
if (_armed == false) {
Motor[chan].SetPoint = 0;
Motor[chan].SetPointLowerBits = 0;
}
//if(Motor[chan].State & MOTOR_STATE_PRESENT_MASK) {
set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan]));
set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan]));
if (Motor[chan].Version == BLCTRL_OLD) {
/*
* Old BL-Ctrl 8Bit served. Version < 2.0
*/
msg[0] = Motor[chan].SetPoint;
if (Motor[chan].RoundCount >= 16) {
// on each 16th cyle we read out the status messages from the blctrl
if (OK == transfer(&msg[0], 1, &result[0], 2)) {
Motor[chan].Current = result[0];
Motor[chan].MaxPWM = result[1];
Motor[chan].Temperature = 255;;
if(Motor[chan].Version == BLCTRL_OLD) {
/*
* Old BL-Ctrl 8Bit served. Version < 2.0
*/
msg[0] = Motor[chan].SetPoint;
if(Motor[chan].RoundCount >= 16) {
// on each 16th cyle we read out the status messages from the blctrl
if (OK == transfer(&msg[0], 1, &result[0], 2)) {
Motor[chan].Current = result[0];
Motor[chan].MaxPWM = result[1];
Motor[chan].Temperature = 255;;
} else {
if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error
}
Motor[chan].RoundCount = 0;
} else {
if (OK != transfer(&msg[0], 1, nullptr, 0)) {
if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error
}
if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error
}
Motor[chan].RoundCount = 0;
} else {
/*
* New BL-Ctrl 11Bit served. Version >= 2.0
*/
msg[0] = Motor[chan].SetPoint;
msg[1] = Motor[chan].SetPointLowerBits;
if(Motor[chan].SetPointLowerBits == 0) {
bytesToSendBL2 = 1; // if setpoint lower bits are zero, we send only the higher bits - this saves time
if (OK != transfer(&msg[0], 1, nullptr, 0)) {
if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error
}
if(Motor[chan].RoundCount >= 16) {
// on each 16th cyle we read out the status messages from the blctrl
if (OK == transfer(&msg[0], bytesToSendBL2, &result[0], 3)) {
Motor[chan].Current = result[0];
Motor[chan].MaxPWM = result[1];
Motor[chan].Temperature = result[2];
} else {
if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error
}
Motor[chan].RoundCount = 0;
} else {
if (OK != transfer(&msg[0], bytesToSendBL2, nullptr, 0)) {
if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error
}
}
}
Motor[chan].RoundCount++;
} else {
/*
* New BL-Ctrl 11Bit served. Version >= 2.0
*/
msg[0] = Motor[chan].SetPoint;
msg[1] = Motor[chan].SetPointLowerBits;
if (Motor[chan].SetPointLowerBits == 0) {
bytesToSendBL2 = 1; // if setpoint lower bits are zero, we send only the higher bits - this saves time
}
if (Motor[chan].RoundCount >= 16) {
// on each 16th cyle we read out the status messages from the blctrl
if (OK == transfer(&msg[0], bytesToSendBL2, &result[0], 3)) {
Motor[chan].Current = result[0];
Motor[chan].MaxPWM = result[1];
Motor[chan].Temperature = result[2];
} else {
if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error
}
Motor[chan].RoundCount = 0;
} else {
if (OK != transfer(&msg[0], bytesToSendBL2, nullptr, 0)) {
if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error
}
}
}
Motor[chan].RoundCount++;
//}
if(showDebug == true) {
if (showDebug == true) {
debugCounter++;
if(debugCounter == 2000) {
if (debugCounter == 2000) {
debugCounter = 0;
for(int i=0; i<_num_outputs; i++){
if(Motor[i].State & MOTOR_STATE_PRESENT_MASK) {
for (int i = 0; i < _num_outputs; i++) {
if (Motor[i].State & MOTOR_STATE_PRESENT_MASK) {
fprintf(stderr, "[mkblctrl] #%i:\tVer: %i\tVal: %i\tCurr: %i\tMaxPWM: %i\tTemp: %i\tState: %i\n", i, Motor[i].Version, Motor[i].SetPoint, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature, Motor[i].State);
}
}
fprintf(stderr, "\n");
}
}
return 0;
}
int
MK::mk_servo_set_test(unsigned int chan, float val)
MK::mk_servo_set_value(unsigned int chan, short val)
{
_retries = 0;
int ret;
short tmpVal = 0;
uint8_t msg[2] = { 0, 0 };
float tmpVal = 0;
tmpVal = val;
uint8_t msg[2] = { 0,0 };
if (tmpVal > 1024) {
tmpVal = 1024;
tmpVal = (1023 + (1023 * val));
if(tmpVal > 2048) {
tmpVal = 2048;
} else if (tmpVal < 0) {
tmpVal = 0;
}
Motor[chan].SetPoint = (uint8_t) (tmpVal / 8);
Motor[chan].SetPoint = (uint8_t)(tmpVal / 4);
if(_armed == false) {
if (_armed == false) {
Motor[chan].SetPoint = 0;
Motor[chan].SetPointLowerBits = 0;
}
@@ -860,7 +867,6 @@ MK::mk_servo_set_test(unsigned int chan, float val)
ret = transfer(&msg[0], 1, nullptr, 0);
ret = OK;
return ret;
}
@@ -868,59 +874,61 @@ MK::mk_servo_set_test(unsigned int chan, float val)
int
MK::mk_servo_test(unsigned int chan)
{
int ret=0;
int ret = 0;
float tmpVal = 0;
float val = -1;
_retries = 0;
uint8_t msg[2] = { 0,0 };
uint8_t msg[2] = { 0, 0 };
if(debugCounter >= MOTOR_SPINUP_COUNTER) {
if (debugCounter >= MOTOR_SPINUP_COUNTER) {
debugCounter = 0;
_motor++;
if(_motor < _num_outputs) {
if (_motor < _num_outputs) {
fprintf(stderr, "[mkblctrl] Motortest - #%i:\tspinup\n", _motor);
}
if(_motor >= _num_outputs) {
if (_motor >= _num_outputs) {
_motor = -1;
_motortest = false;
}
}
debugCounter++;
if(_motor == chan) {
if (_motor == chan) {
val = BLCTRL_MIN_VALUE;
} else {
val = -1;
}
tmpVal = (1023 + (1023 * val));
if(tmpVal > 2048) {
tmpVal = 2048;
tmpVal = (511 + (511 * val));
if (tmpVal > 1024) {
tmpVal = 1024;
}
//Motor[chan].SetPoint = (uint8_t) (tmpVal / 8);
//Motor[chan].SetPointLowerBits = (uint8_t) (tmpVal % 8) & 0x07;
Motor[chan].SetPoint = (uint8_t) tmpVal>>3;
Motor[chan].SetPointLowerBits = (uint8_t) tmpVal & 0x07;
Motor[chan].SetPoint = (uint8_t)(tmpVal / 4);
if(_motor != chan) {
if (_motor != chan) {
Motor[chan].SetPoint = 0;
Motor[chan].SetPointLowerBits = 0;
}
if(Motor[chan].Version == BLCTRL_OLD) {
if (Motor[chan].Version == BLCTRL_OLD) {
msg[0] = Motor[chan].SetPoint;
} else {
msg[0] = Motor[chan].SetPoint;
msg[1] = Motor[chan].SetPointLowerBits;
}
set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan]));
if(Motor[chan].Version == BLCTRL_OLD) {
if (Motor[chan].Version == BLCTRL_OLD) {
ret = transfer(&msg[0], 1, nullptr, 0);
} else {
ret = transfer(&msg[0], 2, nullptr, 0);
}
@@ -931,9 +939,9 @@ MK::mk_servo_test(unsigned int chan)
int
MK::control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input)
uint8_t control_group,
uint8_t control_index,
float &input)
{
const actuator_controls_s *controls = (actuator_controls_s *)handle;
@@ -947,7 +955,6 @@ MK::ioctl(file *filp, int cmd, unsigned long arg)
int ret;
// XXX disabled, confusing users
//debug("ioctl 0x%04x 0x%08x", cmd, arg);
/* try it as a GPIO ioctl first */
ret = gpio_ioctl(filp, cmd, arg);
@@ -978,32 +985,37 @@ int
MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
{
int ret = OK;
int channel;
lock();
switch (cmd) {
case PWM_SERVO_ARM:
////up_pwm_servo_arm(true);
mk_servo_arm(true);
break;
case PWM_SERVO_SET_ARM_OK:
case PWM_SERVO_CLEAR_ARM_OK:
// these are no-ops, as no safety switch
break;
case PWM_SERVO_DISARM:
////up_pwm_servo_arm(false);
mk_servo_arm(false);
break;
case PWM_SERVO_SET_UPDATE_RATE:
set_pwm_rate(arg);
ret = OK;
break;
case PWM_SERVO_SELECT_UPDATE_RATE:
ret = OK;
break;
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));
/* fake an update to the selected 'servo' channel */
if ((arg >= 0) && (arg <= 255)) {
channel = cmd - PWM_SERVO_SET(0);
//mk_servo_set(channel, arg);
} else {
ret = -EINVAL;
}
@@ -1012,20 +1024,20 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1):
/* copy the current output value from the channel */
*(servo_position_t *)arg = cmd - PWM_SERVO_GET(0);
*(servo_position_t *)arg = Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue;
break;
case PWM_SERVO_GET_RATEGROUP(0):
case PWM_SERVO_GET_RATEGROUP(1):
case PWM_SERVO_GET_RATEGROUP(2):
case PWM_SERVO_GET_RATEGROUP(3):
//*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
break;
case PWM_SERVO_GET_COUNT:
case MIXERIOCGETOUTPUTCOUNT:
/*
if (_mode == MODE_4PWM) {
*(unsigned *)arg = 4;
} else {
*(unsigned *)arg = 2;
}
*/
*(unsigned *)arg = _num_outputs;
break;
case MIXERIOCRESET:
@@ -1078,6 +1090,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
}
}
break;
}
@@ -1091,6 +1104,38 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
return ret;
}
/*
this implements PWM output via a write() method, for compatibility
with px4io
*/
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));
}
return count * 2;
}
void
MK::gpio_reset(void)
{
@@ -1229,10 +1274,10 @@ enum MappingMode {
MAPPING_PX4,
};
enum FrameType {
FRAME_PLUS = 0,
FRAME_X,
};
enum FrameType {
FRAME_PLUS = 0,
FRAME_X,
};
PortMode g_port_mode;
@@ -1297,18 +1342,17 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
g_mk->set_motor_test(motortest);
/* (re)set count of used motors */
////g_mk->set_motor_count(motorcount);
/* count used motors */
do {
if(g_mk->mk_check_for_blctrl(8, false) != 0) {
if (g_mk->mk_check_for_blctrl(8, false) != 0) {
shouldStop = 4;
} else {
shouldStop++;
}
sleep(1);
} while ( shouldStop < 3);
} while (shouldStop < 3);
g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true));
@@ -1375,7 +1419,8 @@ mkblctrl_main(int argc, char *argv[])
if (argc > i + 1) {
bus = atoi(argv[i + 1]);
newMode = true;
} else {
} else {
errx(1, "missing argument for i2c bus (-b)");
return 1;
}
@@ -1384,17 +1429,21 @@ mkblctrl_main(int argc, char *argv[])
/* look for the optional frame parameter */
if (strcmp(argv[i], "-mkmode") == 0 || strcmp(argv[i], "--mkmode") == 0) {
if (argc > i + 1) {
if(strcmp(argv[i + 1], "+") == 0 || strcmp(argv[i + 1], "x") == 0 || strcmp(argv[i + 1], "X") == 0) {
if (strcmp(argv[i + 1], "+") == 0 || strcmp(argv[i + 1], "x") == 0 || strcmp(argv[i + 1], "X") == 0) {
px4mode = MAPPING_MK;
newMode = true;
if(strcmp(argv[i + 1], "+") == 0) {
if (strcmp(argv[i + 1], "+") == 0) {
frametype = FRAME_PLUS;
} else {
frametype = FRAME_X;
}
} else {
errx(1, "only + or x for frametype supported !");
}
} else {
errx(1, "missing argument for mkmode (-mkmode)");
return 1;
@@ -1409,12 +1458,12 @@ mkblctrl_main(int argc, char *argv[])
/* look for the optional -h --help parameter */
if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) {
showHelp == true;
showHelp = true;
}
}
if(showHelp) {
if (showHelp) {
fprintf(stderr, "mkblctrl: help:\n");
fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [-h / --help]\n");
exit(1);
@@ -1424,6 +1473,7 @@ mkblctrl_main(int argc, char *argv[])
if (g_mk == nullptr) {
if (mk_start(bus, motorcount) != OK) {
errx(1, "failed to start the MK-BLCtrl driver");
} else {
newMode = true;
}
src/drivers/px4io/px4io.cpp
+83 -9
View File
@@ -106,7 +106,7 @@ public:
* @param rate The rate in Hz actuator outpus are sent to IO.
* Min 10 Hz, max 400 Hz
*/
int set_update_rate(int rate);
int set_update_rate(int rate);
/**
* Set the battery current scaling and bias
@@ -114,7 +114,15 @@ public:
* @param amp_per_volt
* @param amp_bias
*/
void set_battery_current_scaling(float amp_per_volt, float amp_bias);
void set_battery_current_scaling(float amp_per_volt, float amp_bias);
/**
* Push failsafe values to IO.
*
* @param vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
* @param len Number of channels, could up to 8
*/
int set_failsafe_values(const uint16_t *vals, unsigned len);
/**
* Print the current status of IO
@@ -326,11 +334,11 @@ PX4IO::PX4IO() :
_to_actuators_effective(0),
_to_outputs(0),
_to_battery(0),
_primary_pwm_device(false),
_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),
_primary_pwm_device(false)
_battery_last_timestamp(0)
{
/* we need this potentially before it could be set in task_main */
g_dev = this;
@@ -689,6 +697,19 @@ PX4IO::io_set_control_state()
return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls);
}
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;
/* copy values to registers in IO */
return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, vals, len);
}
int
PX4IO::io_set_arming_state()
{
@@ -1250,7 +1271,7 @@ 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\n",
printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n",
flags,
((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""),
((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
@@ -1262,7 +1283,8 @@ PX4IO::print_status()
((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PPM" : ""),
((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"),
((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"),
((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL"));
((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL"),
((flags & PX4IO_P_STATUS_FLAGS_FAILSAFE) ? " FAILSAFE" : ""));
uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
printf("alarms 0x%04x%s%s%s%s%s%s%s\n",
alarms,
@@ -1280,7 +1302,7 @@ PX4IO::print_status()
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE));
printf("amp_per_volt %.3f amp_offset %.3f mAhDischarged %.3f\n",
printf("amp_per_volt %.3f amp_offset %.3f mAh discharged %.3f\n",
(double)_battery_amp_per_volt,
(double)_battery_amp_bias,
(double)_battery_mamphour_total);
@@ -1474,7 +1496,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
case MIXERIOCLOADBUF: {
const char *buf = (const char *)arg;
ret = mixer_send(buf, strnlen(buf, 1024));
ret = mixer_send(buf, strnlen(buf, 2048));
break;
}
@@ -1615,6 +1637,13 @@ test(void)
if (ioctl(fd, PWM_SERVO_ARM, 0))
err(1, "failed to arm servos");
/* Open console directly to grab CTRL-C signal */
int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
if (!console)
err(1, "failed opening console");
warnx("Press CTRL-C or 'c' to abort.");
for (;;) {
/* sweep all servos between 1000..2000 */
@@ -1649,6 +1678,16 @@ test(void)
if (value != servos[i])
errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]);
}
/* Check if user wants to quit */
char c;
if (read(console, &c, 1) == 1) {
if (c == 0x03 || c == 0x63) {
warnx("User abort\n");
close(console);
exit(0);
}
}
}
}
@@ -1718,6 +1757,41 @@ px4io_main(int argc, char *argv[])
exit(0);
}
if (!strcmp(argv[1], "failsafe")) {
if (argc < 3) {
errx(1, "failsafe command needs at least one channel value (ppm)");
}
if (g_dev != nullptr) {
/* set values for first 8 channels, fill unassigned channels with 1500. */
uint16_t failsafe[8];
for (int i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++)
{
/* set channel to commanline argument or to 900 for non-provided channels */
if (argc > i + 2) {
failsafe[i] = atoi(argv[i+2]);
if (failsafe[i] < 800 || failsafe[i] > 2200) {
errx(1, "value out of range of 800 < value < 2200. Aborting.");
}
} else {
/* a zero value will result in stopping to output any pulse */
failsafe[i] = 0;
}
}
int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0]));
if (ret != OK)
errx(ret, "failed setting failsafe values");
} else {
errx(1, "not loaded");
}
exit(0);
}
if (!strcmp(argv[1], "recovery")) {
if (g_dev != nullptr) {
@@ -1845,5 +1919,5 @@ px4io_main(int argc, char *argv[])
monitor();
out:
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current' or 'update'");
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe' or 'update'");
}
src/drivers/stm32/drv_hrt.c
+1 -1
View File
@@ -330,7 +330,7 @@ static void hrt_call_invoke(void);
/*
* PPM decoder tuning parameters
*/
# define PPM_MAX_PULSE_WIDTH 500 /* maximum width of a pulse */
# define PPM_MAX_PULSE_WIDTH 550 /* maximum width of a valid pulse */
# define PPM_MIN_CHANNEL_VALUE 800 /* shortest valid channel signal */
# define PPM_MAX_CHANNEL_VALUE 2200 /* longest valid channel signal */
# define PPM_MIN_START 2500 /* shortest valid start gap */