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

Merge remote-tracking branch 'upstream/master' into obcfailsafe

This commit is contained in:
Thomas Gubler 2014-08-15 18:03:49 +02:00
parent b71778d7e1 e9b0ee7501
commit e14d3b654b
23 changed files with 756 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/px4fmu_common/init.d/rc.usb
View File

@ -23,7 +23,7 @@ mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS_RAW -r 5
usleep 100000
mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20
usleep 100000
mavlink stream -d /dev/ttyACM0 -s GLOBAL_POSITION_SETPOINT_INT -r 20
mavlink stream -d /dev/ttyACM0 -s POSITION_TARGET_GLOBAL_INT -r 10
usleep 100000
# Exit shell to make it available to MAVLink

2
mavlink/include/mavlink/v1.0

@ -1 +1 @@
Subproject commit 4cd384001b5373e1ecaa6c4cd66994855cec4742
Subproject commit 4d7487c2bc5f5ccf87bca82970fb2c08d6d8bd50

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

@ -135,6 +135,15 @@ public:
*/
virtual int init();
/**
* Initialize the PX4IO class.
*
* Retrieve relevant initial system parameters. Initialize PX4IO registers.
*
* @param disable_rc_handling set to true to forbid override / RC handling on IO
*/
int init(bool disable_rc_handling);
/**
* Detect if a PX4IO is connected.
*
@ -579,6 +588,12 @@ PX4IO::detect()
return 0;
}
int
PX4IO::init(bool rc_handling_disabled) {
_rc_handling_disabled = rc_handling_disabled;
return init();
}
int
PX4IO::init()
{
@ -778,6 +793,11 @@ PX4IO::init()
if (_rc_handling_disabled) {
ret = io_disable_rc_handling();
if (ret != OK) {
log("failed disabling RC handling");
return ret;
}
} else {
/* publish RC config to IO */
ret = io_set_rc_config();
@ -1175,6 +1195,7 @@ PX4IO::io_set_arming_state()
int
PX4IO::disable_rc_handling()
{
_rc_handling_disabled = true;
return io_disable_rc_handling();
}
@ -2613,24 +2634,25 @@ start(int argc, char *argv[])
errx(1, "driver alloc failed");
}
if (OK != g_dev->init()) {
delete g_dev;
g_dev = nullptr;
errx(1, "driver init failed");
}
bool rc_handling_disabled = false;
/* disable RC handling on request */
if (argc > 1) {
if (!strcmp(argv[1], "norc")) {
if (g_dev->disable_rc_handling())
warnx("Failed disabling RC handling");
rc_handling_disabled = true;
} else {
warnx("unknown argument: %s", argv[1]);
}
}
if (OK != g_dev->init(rc_handling_disabled)) {
delete g_dev;
g_dev = nullptr;
errx(1, "driver init failed");
}
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
int dsm_vcc_ctl;

203
src/lib/geo/geo.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012, 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 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
@ -49,39 +49,124 @@
#include <stdio.h>
#include <math.h>
#include <stdbool.h>
#include <string.h>
#include <float.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
/*
* Azimuthal Equidistant Projection
* formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html
*/
__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
{
ref->lat = lat_0 / 180.0 * M_PI;
ref->lon = lon_0 / 180.0 * M_PI;
static struct map_projection_reference_s mp_ref = {0.0, 0.0, 0.0, 0.0, false, 0};
static struct globallocal_converter_reference_s gl_ref = {0.0f, false};
ref->sin_lat = sin(ref->lat);
ref->cos_lat = cos(ref->lat);
__EXPORT bool map_projection_global_initialized()
{
return map_projection_initialized(&mp_ref);
}
__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
__EXPORT bool map_projection_initialized(const struct map_projection_reference_s *ref)
{
double lat_rad = lat / 180.0 * M_PI;
double lon_rad = lon / 180.0 * M_PI;
return ref->init_done;
}
__EXPORT uint64_t map_projection_global_timestamp()
{
return map_projection_timestamp(&mp_ref);
}
__EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref)
{
return ref->timestamp;
}
__EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
{
if (strcmp("commander", getprogname()) == 0) {
return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp);
} else {
return -1;
}
}
__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
{
ref->lat_rad = lat_0 * M_DEG_TO_RAD;
ref->lon_rad = lon_0 * M_DEG_TO_RAD;
ref->sin_lat = sin(ref->lat_rad);
ref->cos_lat = cos(ref->lat_rad);
ref->timestamp = timestamp;
ref->init_done = true;
return 0;
}
__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
{
return map_projection_init_timestamped(ref, lat_0, lon_0, hrt_absolute_time());
}
__EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad)
{
return map_projection_reference(&mp_ref, ref_lat_rad, ref_lon_rad);
}
__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad)
{
if (!map_projection_initialized(ref)) {
return -1;
}
*ref_lat_rad = ref->lat_rad;
*ref_lon_rad = ref->lon_rad;
return 0;
}
__EXPORT int map_projection_global_project(double lat, double lon, float *x, float *y)
{
return map_projection_project(&mp_ref, lat, lon, x, y);
}
__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
{
if (!map_projection_initialized(ref)) {
return -1;
}
double lat_rad = lat * M_DEG_TO_RAD;
double lon_rad = lon * M_DEG_TO_RAD;
double sin_lat = sin(lat_rad);
double cos_lat = cos(lat_rad);
double cos_d_lon = cos(lon_rad - ref->lon);
double cos_d_lon = cos(lon_rad - ref->lon_rad);
double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon);
double k = (c == 0.0) ? 1.0 : (c / sin(c));
double k = (fabs(c) < DBL_EPSILON) ? 1.0 : (c / sin(c));
*x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH;
*y = k * cos_lat * sin(lon_rad - ref->lon) * CONSTANTS_RADIUS_OF_EARTH;
*y = k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH;
return 0;
}
__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
__EXPORT int map_projection_global_reproject(float x, float y, double *lat, double *lon)
{
return map_projection_reproject(&mp_ref, x, y, lat, lon);
}
__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
{
if (!map_projection_initialized(ref)) {
return -1;
}
double x_rad = x / CONSTANTS_RADIUS_OF_EARTH;
double y_rad = y / CONSTANTS_RADIUS_OF_EARTH;
double c = sqrtf(x_rad * x_rad + y_rad * y_rad);
@ -91,19 +176,101 @@ __EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, f
double lat_rad;
double lon_rad;
if (c != 0.0) {
if (fabs(c) > DBL_EPSILON) {
lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c);
lon_rad = (ref->lon + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));
lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));
} else {
lat_rad = ref->lat;
lon_rad = ref->lon;
lat_rad = ref->lat_rad;
lon_rad = ref->lon_rad;
}
*lat = lat_rad * 180.0 / M_PI;
*lon = lon_rad * 180.0 / M_PI;
return 0;
}
__EXPORT int map_projection_global_getref(double *lat_0, double *lon_0)
{
if (!map_projection_global_initialized()) {
return -1;
}
if (lat_0 != NULL) {
*lat_0 = M_RAD_TO_DEG * mp_ref.lat_rad;
}
if (lon_0 != NULL) {
*lon_0 = M_RAD_TO_DEG * mp_ref.lon_rad;
}
return 0;
}
__EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp)
{
if (strcmp("commander", getprogname()) == 0) {
gl_ref.alt = alt_0;
if (!map_projection_global_init(lat_0, lon_0, timestamp))
{
gl_ref.init_done = true;
return 0;
} else {
gl_ref.init_done = false;
return -1;
}
} else {
return -1;
}
}
__EXPORT bool globallocalconverter_initialized()
{
return gl_ref.init_done && map_projection_global_initialized();
}
__EXPORT int globallocalconverter_tolocal(double lat, double lon, float alt, float *x, float *y, float *z)
{
if (!map_projection_global_initialized()) {
return -1;
}
map_projection_global_project(lat, lon, x, y);
*z = gl_ref.alt - alt;
return 0;
}
__EXPORT int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt)
{
if (!map_projection_global_initialized()) {
return -1;
}
map_projection_global_reproject(x, y, lat, lon);
*alt = gl_ref.alt - z;
return 0;
}
__EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0)
{
if (!map_projection_global_initialized()) {
return -1;
}
if (map_projection_global_getref(lat_0, lon_0))
{
return -1;
}
if (alt_0 != NULL) {
*alt_0 = gl_ref.alt;
}
return 0;
}
__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
{

141
src/lib/geo/geo.h
View File

@ -69,39 +69,162 @@ struct crosstrack_error_s {
/* lat/lon are in radians */
struct map_projection_reference_s {
double lat;
double lon;
double lat_rad;
double lon_rad;
double sin_lat;
double cos_lat;
bool init_done;
uint64_t timestamp;
};
struct globallocal_converter_reference_s {
float alt;
bool init_done;
};
/**
* Initializes the map transformation.
* Checks if global projection was initialized
* @return true if map was initialized before, false else
*/
__EXPORT bool map_projection_global_initialized(void);
/**
* Checks if projection given as argument was initialized
* @return true if map was initialized before, false else
*/
__EXPORT bool map_projection_initialized(const struct map_projection_reference_s *ref);
/**
* Get the timestamp of the global map projection
* @return the timestamp of the map_projection
*/
__EXPORT uint64_t map_projection_global_timestamp(void);
/**
* Get the timestamp of the map projection given by the argument
* @return the timestamp of the map_projection
*/
__EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref);
/**
* Writes the reference values of the global projection to ref_lat and ref_lon
* @return 0 if map_projection_init was called before, -1 else
*/
__EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad);
/**
* Writes the reference values of the projection given by the argument to ref_lat and ref_lon
* @return 0 if map_projection_init was called before, -1 else
*/
__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad);
/**
* Initializes the global map transformation.
*
* Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane
* Initializes the transformation between the geographic coordinate system and
* the azimuthal equidistant plane
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0);
__EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp);
/**
* Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane
* Initializes the map transformation given by the argument.
*
* Initializes the transformation between the geographic coordinate system and
* the azimuthal equidistant plane
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref,
double lat_0, double lon_0, uint64_t timestamp);
/**
* Initializes the map transformation given by the argument and sets the timestamp to now.
*
* Initializes the transformation between the geographic coordinate system and
* the azimuthal equidistant plane
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0);
/**
* Transforms a point in the geographic coordinate system to the local
* azimuthal equidistant plane using the global projection
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
* @return 0 if map_projection_init was called before, -1 else
*/
__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y);
__EXPORT int map_projection_global_project(double lat, double lon, float *x, float *y);
/* Transforms a point in the geographic coordinate system to the local
* azimuthal equidistant plane using the projection given by the argument
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
* @return 0 if map_projection_init was called before, -1 else
*/
__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y);
/**
* Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system
* Transforms a point in the local azimuthal equidistant plane to the
* geographic coordinate system using the global projection
*
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
* @return 0 if map_projection_init was called before, -1 else
*/
__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon);
__EXPORT int map_projection_global_reproject(float x, float y, double *lat, double *lon);
/**
* Transforms a point in the local azimuthal equidistant plane to the
* geographic coordinate system using the projection given by the argument
*
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
* @return 0 if map_projection_init was called before, -1 else
*/
__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon);
/**
* Get reference position of the global map projection
*/
__EXPORT int map_projection_global_getref(double *lat_0, double *lon_0);
/**
* Initialize the global mapping between global position (spherical) and local position (NED).
*/
__EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp);
/**
* Checks if globallocalconverter was initialized
* @return true if map was initialized before, false else
*/
__EXPORT bool globallocalconverter_initialized(void);
/**
* Convert from global position coordinates to local position coordinates using the global reference
*/
__EXPORT int globallocalconverter_tolocal(double lat, double lon, float alt, float *x, float *y, float *z);
/**
* Convert from local position coordinates to global position coordinates using the global reference
*/
__EXPORT int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt);
/**
* Get reference position of the global to local converter
*/
__EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0);
/**
* Returns the distance to the next waypoint in meters.

14
src/modules/commander/commander.cpp
View File

@ -59,6 +59,7 @@
#include <string.h>
#include <math.h>
#include <poll.h>
#include <float.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
@ -92,6 +93,7 @@
#include <systemlib/err.h>
#include <systemlib/cpuload.h>
#include <systemlib/rc_check.h>
#include <geo/geo.h>
#include <systemlib/state_table.h>
#include <dataman/dataman.h>
@ -903,6 +905,8 @@ int commander_thread_main(int argc, char *argv[])
int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
struct vehicle_gps_position_s gps_position;
memset(&gps_position, 0, sizeof(gps_position));
gps_position.eph = FLT_MAX;
gps_position.epv = FLT_MAX;
/* Subscribe to sensor topic */
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
@ -1391,6 +1395,16 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
}
/* Initialize map projection if gps is valid */
if (!map_projection_global_initialized()
&& (gps_position.eph < eph_threshold)
&& (gps_position.epv < epv_threshold)
&& hrt_elapsed_time((hrt_abstime*)&gps_position.timestamp_position) < 1e6) {
/* set reference for global coordinates <--> local coordiantes conversion and map_projection */
globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time());
}
/* start mission result check */
orb_check(mission_result_sub, &updated);
if (updated) {

2
src/modules/gpio_led/module.mk
View File

@ -37,3 +37,5 @@
MODULE_COMMAND = gpio_led
SRCS = gpio_led.c
MAXOPTIMIZATION = -Os

42
src/modules/mavlink/mavlink_ftp.cpp
View File

@ -39,6 +39,9 @@
#include "mavlink_ftp.h"
// Uncomment the line below to get better debug output. Never commit with this left on.
//#define MAVLINK_FTP_DEBUG
MavlinkFTP *MavlinkFTP::_server;
MavlinkFTP *
@ -125,7 +128,9 @@ MavlinkFTP::_worker(Request *req)
warnx("ftp: bad crc");
}
//printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset);
#ifdef MAVLINK_FTP_DEBUG
printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset);
#endif
switch (hdr->opcode) {
case kCmdNone:
@ -172,7 +177,9 @@ out:
// handle success vs. error
if (errorCode == kErrNone) {
hdr->opcode = kRspAck;
//warnx("FTP: ack\n");
#ifdef MAVLINK_FTP_DEBUG
warnx("FTP: ack\n");
#endif
} else {
warnx("FTP: nak %u", errorCode);
hdr->opcode = kRspNak;
@ -217,7 +224,9 @@ MavlinkFTP::_workList(Request *req)
return kErrNotDir;
}
//warnx("FTP: list %s offset %d", dirPath, hdr->offset);
#ifdef MAVLINK_FTP_DEBUG
warnx("FTP: list %s offset %d", dirPath, hdr->offset);
#endif
ErrorCode errorCode = kErrNone;
struct dirent entry, *result = nullptr;
@ -247,11 +256,13 @@ MavlinkFTP::_workList(Request *req)
uint32_t fileSize = 0;
char buf[256];
char direntType;
// store the type marker
// Determine the directory entry type
switch (entry.d_type) {
case DTYPE_FILE:
hdr->data[offset++] = kDirentFile;
// For files we get the file size as well
direntType = kDirentFile;
snprintf(buf, sizeof(buf), "%s/%s", dirPath, entry.d_name);
struct stat st;
if (stat(buf, &st) == 0) {
@ -259,29 +270,34 @@ MavlinkFTP::_workList(Request *req)
}
break;
case DTYPE_DIRECTORY:
hdr->data[offset++] = kDirentDir;
direntType = kDirentDir;
break;
default:
hdr->data[offset++] = kDirentUnknown;
direntType = kDirentUnknown;
break;
}
if (entry.d_type == DTYPE_FILE) {
// Files send filename and file length
snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize);
} else {
// Everything else just sends name
strncpy(buf, entry.d_name, sizeof(buf));
buf[sizeof(buf)-1] = 0;
}
size_t nameLen = strlen(buf);
// name too big to fit?
if ((nameLen + offset + 2) > kMaxDataLength) {
// Do we have room for the name, the one char directory identifier and the null terminator?
if ((offset + nameLen + 2) > kMaxDataLength) {
break;
}
// copy the name, which we know will fit
// Move the data into the buffer
hdr->data[offset++] = direntType;
strcpy((char *)&hdr->data[offset], buf);
//printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]);
#ifdef MAVLINK_FTP_DEBUG
printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]);
#endif
offset += nameLen + 1;
}
@ -342,7 +358,9 @@ MavlinkFTP::_workRead(Request *req)
}
// Seek to the specified position
//warnx("seek %d", hdr->offset);
#ifdef MAVLINK_FTP_DEBUG
warnx("seek %d", hdr->offset);
#endif
if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) {
// Unable to see to the specified location
warnx("seek fail");

20
src/modules/mavlink/mavlink_ftp.h
View File

@ -154,15 +154,17 @@ private:
if (!success) {
warnx("FTP TX ERR");
}
// else {
// warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d",
// _mavlink->get_system_id(),
// _mavlink->get_component_id(),
// _mavlink->get_channel(),
// len,
// msg.checksum);
// }
}
#ifdef MAVLINK_FTP_DEBUG
else {
warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d",
_mavlink->get_system_id(),
_mavlink->get_component_id(),
_mavlink->get_channel(),
len,
msg.checksum);
}
#endif
}
uint8_t *rawData() { return &_message.data[0]; }

4
src/modules/mavlink/mavlink_main.cpp
View File

@ -1388,8 +1388,8 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("GLOBAL_POSITION_INT", 3.0f);
configure_stream("LOCAL_POSITION_NED", 3.0f);
configure_stream("RC_CHANNELS_RAW", 1.0f);
configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f);
configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f);
configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
configure_stream("ATTITUDE_TARGET", 3.0f);
configure_stream("DISTANCE_SENSOR", 0.5f);
break;

29
src/modules/mavlink/mavlink_messages.cpp
View File

@ -1333,7 +1333,7 @@ protected:
}
for (unsigned i = 0; i < 8; i++) {
if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
if (act.output[i] > PWM_LOWEST_MIN / 2) {
if (i < n) {
/* scale PWM out 900..2100 us to 0..1 for rotors */
out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
@ -1344,7 +1344,7 @@ protected:
}
} else {
/* send 0 when disarmed */
/* send 0 when disarmed and for disabled channels */
out[i] = 0.0f;
}
}
@ -1353,15 +1353,20 @@ protected:
/* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
for (unsigned i = 0; i < 8; i++) {
if (i != 3) {
/* scale PWM out 900..2100 us to -1..1 for normal channels */
out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
if (act.output[i] > PWM_LOWEST_MIN / 2) {
if (i != 3) {
/* scale PWM out 900..2100 us to -1..1 for normal channels */
out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
} else {
/* scale PWM out 900..2100 us to 0..1 for throttle */
out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
}
} else {
/* scale PWM out 900..2100 us to 0..1 for throttle */
out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
/* set 0 for disabled channels */
out[i] = 0.0f;
}
}
}
@ -1637,10 +1642,10 @@ protected:
msg.chan2_raw = (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX;
msg.chan3_raw = (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX;
msg.chan4_raw = (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX;
msg.chan4_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX;
msg.chan4_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX;
msg.chan4_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX;
msg.chan4_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX;
msg.chan5_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX;
msg.chan6_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX;
msg.chan7_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX;
msg.chan8_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX;
msg.rssi = rc.rssi;
_mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_RAW, &msg);

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

@ -108,6 +108,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_att_sp_pub(-1),
_rates_sp_pub(-1),
_vicon_position_pub(-1),
_vision_position_pub(-1),
_telemetry_status_pub(-1),
_rc_pub(-1),
_manual_pub(-1),
@ -148,6 +149,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_vicon_position_estimate(msg);
break;
case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
handle_message_vision_position_estimate(msg);
break;
case MAVLINK_MSG_ID_RADIO_STATUS:
handle_message_radio_status(msg);
break;
@ -358,6 +363,45 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
}
}
void
MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
{
mavlink_vision_position_estimate_t pos;
mavlink_msg_vision_position_estimate_decode(msg, &pos);
struct vision_position_estimate vision_position;
memset(&vision_position, 0, sizeof(vision_position));
// Use the component ID to identify the vision sensor
vision_position.id = msg->compid;
vision_position.timestamp_boot = hrt_absolute_time();
vision_position.timestamp_computer = pos.usec;
vision_position.x = pos.x;
vision_position.y = pos.y;
vision_position.z = pos.z;
// XXX fix this
vision_position.vx = 0.0f;
vision_position.vy = 0.0f;
vision_position.vz = 0.0f;
math::Quaternion q;
q.from_euler(pos.roll, pos.pitch, pos.yaw);
vision_position.q[0] = q(0);
vision_position.q[1] = q(1);
vision_position.q[2] = q(2);
vision_position.q[3] = q(3);
if (_vision_position_pub < 0) {
_vision_position_pub = orb_advertise(ORB_ID(vision_position_estimate), &vision_position);
} else {
orb_publish(ORB_ID(vision_position_estimate), _vision_position_pub, &vision_position);
}
}
void
MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
{

3
src/modules/mavlink/mavlink_receiver.h
View File

@ -58,6 +58,7 @@
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vision_position_estimate.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/optical_flow.h>
@ -112,6 +113,7 @@ private:
void handle_message_optical_flow(mavlink_message_t *msg);
void handle_message_set_mode(mavlink_message_t *msg);
void handle_message_vicon_position_estimate(mavlink_message_t *msg);
void handle_message_vision_position_estimate(mavlink_message_t *msg);
void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
void handle_message_radio_status(mavlink_message_t *msg);
void handle_message_manual_control(mavlink_message_t *msg);
@ -145,6 +147,7 @@ private:
orb_advert_t _att_sp_pub;
orb_advert_t _rates_sp_pub;
orb_advert_t _vicon_position_pub;
orb_advert_t _vision_position_pub;
orb_advert_t _telemetry_status_pub;
orb_advert_t _rc_pub;
orb_advert_t _manual_pub;

124
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -59,6 +59,7 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vision_position_estimate.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/optical_flow.h>
#include <mavlink/mavlink_log.h>
@ -80,6 +81,7 @@ static bool thread_running = false; /**< Deamon status flag */
static int position_estimator_inav_task; /**< Handle of deamon task / thread */
static bool verbose_mode = false;
static const hrt_abstime vision_topic_timeout = 500000; // Vision topic timeout = 0.5s
static const hrt_abstime gps_topic_timeout = 500000; // GPS topic timeout = 0.5s
static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s
static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms
@ -233,6 +235,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float eph_flow = 1.0f;
float eph_vision = 0.5f;
float epv_vision = 0.5f;
float x_est_prev[2], y_est_prev[2], z_est_prev[2];
memset(x_est_prev, 0, sizeof(x_est_prev));
memset(y_est_prev, 0, sizeof(y_est_prev));
@ -279,6 +284,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
};
float w_gps_xy = 1.0f;
float w_gps_z = 1.0f;
float corr_vision[3][2] = {
{ 0.0f, 0.0f }, // N (pos, vel)
{ 0.0f, 0.0f }, // E (pos, vel)
{ 0.0f, 0.0f }, // D (pos, vel)
};
float corr_sonar = 0.0f;
float corr_sonar_filtered = 0.0f;
@ -294,6 +306,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
bool sonar_valid = false; // sonar is valid
bool flow_valid = false; // flow is valid
bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false)
bool vision_valid = false;
/* declare and safely initialize all structs */
struct actuator_controls_s actuator;
@ -312,6 +325,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
memset(&local_pos, 0, sizeof(local_pos));
struct optical_flow_s flow;
memset(&flow, 0, sizeof(flow));
struct vision_position_estimate vision;
memset(&vision, 0, sizeof(vision));
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
@ -323,6 +338,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate));
int home_position_sub = orb_subscribe(ORB_ID(home_position));
/* advertise */
@ -616,6 +632,46 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
}
/* check no vision circuit breaker is set */
if (params.no_vision != CBRK_NO_VISION_KEY) {
/* vehicle vision position */
orb_check(vision_position_estimate_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vision_position_estimate), vision_position_estimate_sub, &vision);
/* reset position estimate on first vision update */
if (!vision_valid) {
x_est[0] = vision.x;
x_est[1] = vision.vx;
y_est[0] = vision.y;
y_est[1] = vision.vy;
/* only reset the z estimate if the z weight parameter is not zero */
if (params.w_z_vision_p > MIN_VALID_W)
{
z_est[0] = vision.z;
z_est[1] = vision.vz;
}
vision_valid = true;
warnx("VISION estimate valid");
mavlink_log_info(mavlink_fd, "[inav] VISION estimate valid");
}
/* calculate correction for position */
corr_vision[0][0] = vision.x - x_est[0];
corr_vision[1][0] = vision.y - y_est[0];
corr_vision[2][0] = vision.z - z_est[0];
/* calculate correction for velocity */
corr_vision[0][1] = vision.vx - x_est[1];
corr_vision[1][1] = vision.vy - y_est[1];
corr_vision[2][1] = vision.vz - z_est[1];
}
}
/* vehicle GPS position */
orb_check(vehicle_gps_position_sub, &updated);
@ -732,14 +788,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
/* check for timeout on GPS topic */
if (gps_valid && t > gps.timestamp_position + gps_topic_timeout) {
if (gps_valid && (t > (gps.timestamp_position + gps_topic_timeout))) {
gps_valid = false;
warnx("GPS timeout");
mavlink_log_info(mavlink_fd, "[inav] GPS timeout");
}
/* check for timeout on vision topic */
if (vision_valid && (t > (vision.timestamp_boot + vision_topic_timeout))) {
vision_valid = false;
warnx("VISION timeout");
mavlink_log_info(mavlink_fd, "[inav] VISION timeout");
}
/* check for sonar measurement timeout */
if (sonar_valid && t > sonar_time + sonar_timeout) {
if (sonar_valid && (t > (sonar_time + sonar_timeout))) {
corr_sonar = 0.0f;
sonar_valid = false;
}
@ -759,10 +822,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* use GPS if it's valid and reference position initialized */
bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W;
bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W;
/* use VISION if it's valid and has a valid weight parameter */
bool use_vision_xy = vision_valid && params.w_xy_vision_p > MIN_VALID_W;
bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W;
/* use flow if it's valid and (accurate or no GPS available) */
bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);
bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow;
bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy;
bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout);
@ -781,6 +847,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy;
float w_z_gps_p = params.w_z_gps_p * w_gps_z;
float w_xy_vision_p = params.w_xy_vision_p;
float w_xy_vision_v = params.w_xy_vision_v;
float w_z_vision_p = params.w_z_vision_p;
/* reduce GPS weight if optical flow is good */
if (use_flow && flow_accurate) {
w_xy_gps_p *= params.w_gps_flow;
@ -821,6 +891,35 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
}
/* accelerometer bias correction for VISION (use buffered rotation matrix) */
accel_bias_corr[0] = 0.0f;
accel_bias_corr[1] = 0.0f;
accel_bias_corr[2] = 0.0f;
if (use_vision_xy) {
accel_bias_corr[0] -= corr_vision[0][0] * w_xy_vision_p * w_xy_vision_p;
accel_bias_corr[0] -= corr_vision[0][1] * w_xy_vision_v;
accel_bias_corr[1] -= corr_vision[1][0] * w_xy_vision_p * w_xy_vision_p;
accel_bias_corr[1] -= corr_vision[1][1] * w_xy_vision_v;
}
if (use_vision_z) {
accel_bias_corr[2] -= corr_vision[2][0] * w_z_vision_p * w_z_vision_p;
}
/* transform error vector from NED frame to body frame */
for (int i = 0; i < 3; i++) {
float c = 0.0f;
for (int j = 0; j < 3; j++) {
c += att.R[j][i] * accel_bias_corr[j];
}
if (isfinite(c)) {
acc_bias[i] += c * params.w_acc_bias * dt;
}
}
/* accelerometer bias correction for flow and baro (assume that there is no delay) */
accel_bias_corr[0] = 0.0f;
accel_bias_corr[1] = 0.0f;
@ -863,10 +962,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
}
if (use_vision_z) {
epv = fminf(epv, epv_vision);
inertial_filter_correct(corr_vision[2][0], dt, z_est, 0, w_z_vision_p);
}
if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) {
write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
memcpy(z_est, z_est_prev, sizeof(z_est));
memset(corr_gps, 0, sizeof(corr_gps));
memset(corr_vision, 0, sizeof(corr_vision));
corr_baro = 0;
} else {
@ -904,11 +1009,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
}
if (use_vision_xy) {
eph = fminf(eph, eph_vision);
inertial_filter_correct(corr_vision[0][0], dt, x_est, 0, w_xy_vision_p);
inertial_filter_correct(corr_vision[1][0], dt, y_est, 0, w_xy_vision_p);
if (w_xy_vision_v > MIN_VALID_W) {
inertial_filter_correct(corr_vision[0][1], dt, x_est, 1, w_xy_vision_v);
inertial_filter_correct(corr_vision[1][1], dt, y_est, 1, w_xy_vision_v);
}
}
if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) {
write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
memcpy(x_est, x_est_prev, sizeof(x_est));
memcpy(y_est, y_est_prev, sizeof(y_est));
memset(corr_gps, 0, sizeof(corr_gps));
memset(corr_vision, 0, sizeof(corr_vision));
memset(corr_flow, 0, sizeof(corr_flow));
} else {

57
src/modules/position_estimator_inav/position_estimator_inav_params.c
View File

@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 Anton Babushkin. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
* Copyright (c) 2013, 2014 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
@ -35,6 +34,8 @@
/*
* @file position_estimator_inav_params.c
*
* @author Anton Babushkin <rk3dov@gmail.com>
*
* Parameters for position_estimator_inav
*/
@ -62,6 +63,17 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f);
*/
PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f);
/**
* Z axis weight for vision
*
* Weight (cutoff frequency) for vision altitude measurements. vision altitude data is very noisy and should be used only as slow correction for baro offset.
*
* @min 0.0
* @max 10.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_W_Z_VIS_P, 0.5f);
/**
* Z axis weight for sonar
*
@ -95,6 +107,28 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f);
*/
PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f);
/**
* XY axis weight for vision position
*
* Weight (cutoff frequency) for vision position measurements.
*
* @min 0.0
* @max 10.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 5.0f);
/**
* XY axis weight for vision velocity
*
* Weight (cutoff frequency) for vision velocity measurements.
*
* @min 0.0
* @max 10.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f);
/**
* XY axis weight for optical flow
*
@ -232,13 +266,27 @@ PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f);
*/
PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f);
/**
* Disable vision input
*
* Set to the appropriate key (328754) to disable vision input.
*
* @min 0.0
* @max 1.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_INT32(CBRK_NO_VISION, 0);
int parameters_init(struct position_estimator_inav_param_handles *h)
{
h->w_z_baro = param_find("INAV_W_Z_BARO");
h->w_z_gps_p = param_find("INAV_W_Z_GPS_P");
h->w_z_vision_p = param_find("INAV_W_Z_VIS_P");
h->w_z_sonar = param_find("INAV_W_Z_SONAR");
h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P");
h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V");
h->w_xy_vision_p = param_find("INAV_W_XY_VIS_P");
h->w_xy_vision_v = param_find("INAV_W_XY_VIS_V");
h->w_xy_flow = param_find("INAV_W_XY_FLOW");
h->w_xy_res_v = param_find("INAV_W_XY_RES_V");
h->w_gps_flow = param_find("INAV_W_GPS_FLOW");
@ -250,6 +298,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
h->land_t = param_find("INAV_LAND_T");
h->land_disp = param_find("INAV_LAND_DISP");
h->land_thr = param_find("INAV_LAND_THR");
h->no_vision = param_find("CBRK_NO_VISION");
h->delay_gps = param_find("INAV_DELAY_GPS");
return OK;
@ -259,9 +308,12 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
{
param_get(h->w_z_baro, &(p->w_z_baro));
param_get(h->w_z_gps_p, &(p->w_z_gps_p));
param_get(h->w_z_vision_p, &(p->w_z_vision_p));
param_get(h->w_z_sonar, &(p->w_z_sonar));
param_get(h->w_xy_gps_p, &(p->w_xy_gps_p));
param_get(h->w_xy_gps_v, &(p->w_xy_gps_v));
param_get(h->w_xy_vision_p, &(p->w_xy_vision_p));
param_get(h->w_xy_vision_v, &(p->w_xy_vision_v));
param_get(h->w_xy_flow, &(p->w_xy_flow));
param_get(h->w_xy_res_v, &(p->w_xy_res_v));
param_get(h->w_gps_flow, &(p->w_gps_flow));
@ -273,6 +325,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
param_get(h->land_t, &(p->land_t));
param_get(h->land_disp, &(p->land_disp));
param_get(h->land_thr, &(p->land_thr));
param_get(h->no_vision, &(p->no_vision));
param_get(h->delay_gps, &(p->delay_gps));
return OK;

19
src/modules/position_estimator_inav/position_estimator_inav_params.h
View File

@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 Anton Babushkin. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
* Copyright (c) 2013, 2014 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
@ -33,9 +32,11 @@
****************************************************************************/
/*
* @file position_estimator_inav_params.h
* @file position_estimator_inav_params.c
*
* Parameters for Position Estimator
* @author Anton Babushkin <rk3dov@gmail.com>
*
* Parameters definition for position_estimator_inav
*/
#include <systemlib/param/param.h>
@ -43,9 +44,12 @@
struct position_estimator_inav_params {
float w_z_baro;
float w_z_gps_p;
float w_z_vision_p;
float w_z_sonar;
float w_xy_gps_p;
float w_xy_gps_v;
float w_xy_vision_p;
float w_xy_vision_v;
float w_xy_flow;
float w_xy_res_v;
float w_gps_flow;
@ -57,15 +61,19 @@ struct position_estimator_inav_params {
float land_t;
float land_disp;
float land_thr;
int32_t no_vision;
float delay_gps;
};
struct position_estimator_inav_param_handles {
param_t w_z_baro;
param_t w_z_gps_p;
param_t w_z_vision_p;
param_t w_z_sonar;
param_t w_xy_gps_p;
param_t w_xy_gps_v;
param_t w_xy_vision_p;
param_t w_xy_vision_v;
param_t w_xy_flow;
param_t w_xy_res_v;
param_t w_gps_flow;
@ -77,9 +85,12 @@ struct position_estimator_inav_param_handles {
param_t land_t;
param_t land_disp;
param_t land_thr;
param_t no_vision;
param_t delay_gps;
};
#define CBRK_NO_VISION_KEY 328754
/**
* Initialize all parameter handles and values
*

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

@ -213,6 +213,9 @@ ORB_DEFINE(encoders, struct encoders_s);
#include "topics/estimator_status.h"
ORB_DEFINE(estimator_status, struct estimator_status_report);
#include "topics/vision_position_estimate.h"
ORB_DEFINE(vision_position_estimate, struct vision_position_estimate);
#include "topics/vehicle_force_setpoint.h"
ORB_DEFINE(vehicle_force_setpoint, struct vehicle_force_setpoint_s);

82
src/modules/uORB/topics/vision_position_estimate.h Normal file
View File

@ -0,0 +1,82 @@
/****************************************************************************
*
* Copyright (c) 2014 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 vision_position_estimate.h
* Vision based position estimate
*/
#ifndef TOPIC_VISION_POSITION_ESTIMATE_H_
#define TOPIC_VISION_POSITION_ESTIMATE_H_
#include <stdint.h>
#include <stdbool.h>
#include "../uORB.h"
/**
* @addtogroup topics
* @{
*/
/**
* Vision based position estimate in NED frame
*/
struct vision_position_estimate {
unsigned id; /**< ID of the estimator, commonly the component ID of the incoming message */
uint64_t timestamp_boot; /**< time of this estimate, in microseconds since system start */
uint64_t timestamp_computer; /**< timestamp provided by the companion computer, in us */
float x; /**< X position in meters in NED earth-fixed frame */
float y; /**< Y position in meters in NED earth-fixed frame */
float z; /**< Z position in meters in NED earth-fixed frame (negative altitude) */
float vx; /**< X velocity in meters per second in NED earth-fixed frame */
float vy; /**< Y velocity in meters per second in NED earth-fixed frame */
float vz; /**< Z velocity in meters per second in NED earth-fixed frame */
float q[4]; /**< Estimated attitude as quaternion */
// XXX Add covariances here
};
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(vision_position_estimate);
#endif /* TOPIC_VISION_POSITION_ESTIMATE_H_ */

1
src/modules/unit_test/module.mk
View File

@ -37,3 +37,4 @@
SRCS = unit_test.cpp
MAXOPTIMIZATION = -Os

2
src/systemcmds/esc_calib/module.mk
View File

@ -39,3 +39,5 @@ MODULE_COMMAND = esc_calib
SRCS = esc_calib.c
MODULE_STACKSIZE = 4096
MAXOPTIMIZATION = -Os

2
src/systemcmds/mtd/module.mk
View File

@ -4,3 +4,5 @@
MODULE_COMMAND = mtd
SRCS = mtd.c 24xxxx_mtd.c
MAXOPTIMIZATION = -Os

2
src/systemcmds/nshterm/module.mk
View File

@ -39,3 +39,5 @@ MODULE_COMMAND = nshterm
SRCS = nshterm.c
MODULE_STACKSIZE = 1400
MAXOPTIMIZATION = -Os

2
src/systemcmds/pwm/module.mk
View File

@ -39,3 +39,5 @@ MODULE_COMMAND = pwm
SRCS = pwm.c
MODULE_STACKSIZE = 1800
MAXOPTIMIZATION = -Os