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

Merge master into cmake-2

This commit is contained in:
Lorenz Meier 2015-10-03 15:34:30 +02:00
parent d63231a23c 526bb51570
commit fefdcd3fa9
10 changed files with 121 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ROMFS/px4fmu_common/init.d/rcS
View File

@ -365,11 +365,6 @@ then
then
fi
#
# UAVCAN
#
sh /etc/init.d/rc.uavcan
#
# Sensors System (start before Commander so Preflight checks are properly run)
#
@ -576,6 +571,11 @@ then
fi
fi
#
# UAVCAN
#
sh /etc/init.d/rc.uavcan
#
# Logging
#

1
posix-configs/SITL/init/rcS
View File

@ -53,5 +53,6 @@ mavlink stream -r 80 -s ATTITUDE -u 14556
mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
mavlink stream -r 20 -s RC_CHANNELS -u 14556
mavlink stream -r 250 -s HIGHRES_IMU -u 14556
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
mavlink boot_complete
sdlog2 start -r 100 -e -t -a

25
src/lib/ecl/validation/data_validator.cpp
View File

@ -44,9 +44,10 @@
DataValidator::DataValidator(DataValidator *prev_sibling) :
_time_last(0),
_timeout_interval(70000),
_timeout_interval(20000),
_event_count(0),
_error_count(0),
_error_density(0),
_priority(0),
_mean{0.0f},
_lp{0.0f},
@ -68,6 +69,13 @@ void
DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in, int priority_in)
{
_event_count++;
if (error_count_in > _error_count) {
_error_density += (error_count_in - _error_count);
} else if (_error_density > 0) {
_error_density--;
}
_error_count = error_count_in;
_priority = priority_in;
@ -123,7 +131,13 @@ DataValidator::confidence(uint64_t timestamp)
return 0.0f;
}
return 1.0f;
/* cap error density counter at window size */
if (_error_density > ERROR_DENSITY_WINDOW) {
_error_density = ERROR_DENSITY_WINDOW;
}
/* return local error density for last N measurements */
return 1.0f - (_error_density / ERROR_DENSITY_WINDOW);
}
int
@ -136,12 +150,13 @@ void
DataValidator::print()
{
if (_time_last == 0) {
ECL_INFO("\tno data\n");
ECL_INFO("\tno data");
return;
}
for (unsigned i = 0; i < _dimensions; i++) {
ECL_INFO("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f\n",
(double) _value[i], (double)_lp[i], (double)_mean[i], (double)_rms[i]);
ECL_INFO("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f conf: %8.4f",
(double) _value[i], (double)_lp[i], (double)_mean[i],
(double)_rms[i], (double)confidence(hrt_absolute_time()));
}
}

10
src/lib/ecl/validation/data_validator.h
View File

@ -81,6 +81,12 @@ public:
*/
float* value() { return _value; }
/**
* Get the used status of this validator
* @return true if this validator ever saw data
*/
bool used() { return (_time_last > 0); }
/**
* Get the priority of this validator
* @return the stored priority
@ -112,6 +118,7 @@ private:
uint64_t _timeout_interval; /**< interval in which the datastream times out in us */
uint64_t _event_count; /**< total data counter */
uint64_t _error_count; /**< error count */
int _error_density; /**< ratio between successful reads and errors */
int _priority; /**< sensor nominal priority */
float _mean[_dimensions]; /**< mean of value */
float _lp[3]; /**< low pass value */
@ -120,7 +127,8 @@ private:
float _value[3]; /**< last value */
float _value_equal_count; /**< equal values in a row */
DataValidator *_sibling; /**< sibling in the group */
const unsigned NORETURN_ERRCOUNT = 100; /**< if the error count reaches this value, return sensor as invalid */
const unsigned NORETURN_ERRCOUNT = 10000; /**< if the error count reaches this value, return sensor as invalid */
const float ERROR_DENSITY_WINDOW = 100.0f; /**< window in measurement counts for errors */
const unsigned VALUE_EQUAL_COUNT_MAX = 100; /**< if the sensor value is the same (accumulated also between axes) this many times, flag it */
/* we don't want this class to be copied */

49
src/lib/ecl/validation/data_validator_group.cpp
View File

@ -97,24 +97,35 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
// XXX This should eventually also include voting
int pre_check_best = _curr_best;
float pre_check_confidence = 1.0f;
int pre_check_prio = -1;
float max_confidence = -1.0f;
int max_priority = -1000;
int max_index = -1;
uint64_t min_error_count = 30000;
DataValidator *best = nullptr;
unsigned i = 0;
while (next != nullptr) {
float confidence = next->confidence(timestamp);
if (confidence > max_confidence ||
(fabsf(confidence - max_confidence) < 0.01f &&
((next->error_count() < min_error_count) &&
(next->priority() >= max_priority)))) {
if (i == pre_check_best) {
pre_check_prio = next->priority();
pre_check_confidence = confidence;
}
/*
* Switch if:
* 1) the confidence is higher and priority is equal or higher
* 2) the confidence is no less than 1% different and the priority is higher
*/
if (((max_confidence < MIN_REGULAR_CONFIDENCE) && (confidence >= MIN_REGULAR_CONFIDENCE)) ||
(confidence > max_confidence && (next->priority() >= max_priority)) ||
(fabsf(confidence - max_confidence) < 0.01f && (next->priority() > max_priority))
) {
max_index = i;
max_confidence = confidence;
max_priority = next->priority();
min_error_count = next->error_count();
best = next;
}
@ -125,17 +136,29 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
/* the current best sensor is not matching the previous best sensor */
if (max_index != _curr_best) {
bool true_failsafe = true;
/* check wether the switch was a failsafe or preferring a higher priority sensor */
if (pre_check_prio != -1 && pre_check_prio < max_priority &&
fabsf(pre_check_confidence - max_confidence) < 0.1f) {
/* this is not a failover */
true_failsafe = false;
}
/* if we're no initialized, initialize the bookkeeping but do not count a failsafe */
if (_curr_best < 0) {
_prev_best = max_index;
} else {
/* we were initialized before, this is a real failsafe */
_prev_best = pre_check_best;
_toggle_count++;
/* if this is the first time, log when we failed */
if (_first_failover_time == 0) {
_first_failover_time = timestamp;
if (true_failsafe) {
_toggle_count++;
/* if this is the first time, log when we failed */
if (_first_failover_time == 0) {
_first_failover_time = timestamp;
}
}
}
@ -185,8 +208,10 @@ DataValidatorGroup::print()
unsigned i = 0;
while (next != nullptr) {
ECL_INFO("sensor #%u:\n", i);
next->print();
if (next->used()) {
ECL_INFO("sensor #%u, prio: %d", i, next->priority());
next->print();
}
next = next->sibling();
i++;
}

1
src/lib/ecl/validation/data_validator_group.h
View File

@ -100,6 +100,7 @@ private:
int _prev_best; /**< the previous best index */
uint64_t _first_failover_time; /**< timestamp where the first failover occured or zero if none occured */
unsigned _toggle_count; /**< number of back and forth switches between two sensors */
static constexpr float MIN_REGULAR_CONFIDENCE = 0.9f;
/* we don't want this class to be copied */
DataValidatorGroup(const DataValidatorGroup&);

2
src/modules/attitude_estimator_q/attitude_estimator_q_params.c
View File

@ -116,4 +116,4 @@ PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f);
* @min 0.001
* @max 100
*/
PARAM_DEFINE_FLOAT(ATT_VIBE_THRESH, 0.1f);
PARAM_DEFINE_FLOAT(ATT_VIBE_THRESH, 0.2f);

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

@ -1666,6 +1666,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("GPS_RAW_INT", 1.0f);
configure_stream("GLOBAL_POSITION_INT", 3.0f);
configure_stream("LOCAL_POSITION_NED", 3.0f);
configure_stream("NAMED_VALUE_FLOAT", 2.0f);
configure_stream("RC_CHANNELS", 1.0f);
configure_stream("SERVO_OUTPUT_RAW_0", 1.0f);
configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
@ -1682,6 +1683,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("GPS_RAW_INT", 5.0f);
configure_stream("GLOBAL_POSITION_INT", 50.0f);
configure_stream("LOCAL_POSITION_NED", 30.0f);
configure_stream("NAMED_VALUE_FLOAT", 10.0f);
configure_stream("CAMERA_CAPTURE", 2.0f);
configure_stream("HOME_POSITION", 0.5f);
configure_stream("ATTITUDE_TARGET", 10.0f);
@ -1723,7 +1725,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("ATTITUDE_TARGET", 8.0f);
configure_stream("PARAM_VALUE", 300.0f);
configure_stream("MISSION_ITEM", 50.0f);
configure_stream("NAMED_VALUE_FLOAT", 10.0f);
configure_stream("NAMED_VALUE_FLOAT", 50.0f);
configure_stream("OPTICAL_FLOW_RAD", 10.0f);
configure_stream("DISTANCE_SENSOR", 10.0f);
configure_stream("VFR_HUD", 20.0f);

3
src/modules/simulator/simulator.h
View File

@ -51,6 +51,7 @@
#include <drivers/drv_hrt.h>
#include <drivers/drv_rc_input.h>
#include <uORB/uORB.h>
#include <uORB/topics/optical_flow.h>
#include <v1.0/mavlink_types.h>
#include <v1.0/common/mavlink.h>
namespace simulator
@ -259,11 +260,13 @@ private:
orb_advert_t _baro_pub;
orb_advert_t _gyro_pub;
orb_advert_t _mag_pub;
orb_advert_t _flow_pub;
bool _initialized;
// class methods
int publish_sensor_topics(mavlink_hil_sensor_t *imu);
int publish_flow_topic(mavlink_hil_optical_flow_t *flow);
#ifndef __PX4_QURT
// uORB publisher handlers

41
src/modules/simulator/simulator_mavlink.cpp
View File

@ -245,6 +245,12 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
update_sensors(&imu);
break;
case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW:
mavlink_hil_optical_flow_t flow;
mavlink_msg_hil_optical_flow_decode(msg, &flow);
publish_flow_topic(&flow);
break;
case MAVLINK_MSG_ID_HIL_GPS:
mavlink_hil_gps_t gps_sim;
mavlink_msg_hil_gps_decode(msg, &gps_sim);
@ -762,3 +768,38 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu)
return OK;
}
int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t* flow_mavlink)
{
uint64_t timestamp = hrt_absolute_time();
/* flow */
{
struct optical_flow_s flow;
memset(&flow, 0, sizeof(flow));
flow.sensor_id = flow_mavlink->sensor_id;
flow.timestamp = timestamp;
flow.time_since_last_sonar_update = 0;
flow.frame_count_since_last_readout = 0; // ?
flow.integration_timespan = flow_mavlink->integration_time_us;
flow.ground_distance_m = flow_mavlink->distance;
flow.gyro_temperature = flow_mavlink->temperature;
flow.gyro_x_rate_integral = flow_mavlink->integrated_xgyro;
flow.gyro_y_rate_integral = flow_mavlink->integrated_ygyro;
flow.gyro_z_rate_integral = flow_mavlink->integrated_zgyro;
flow.pixel_flow_x_integral = flow_mavlink->integrated_x;
flow.pixel_flow_x_integral = flow_mavlink->integrated_y;
flow.quality = flow_mavlink->quality;
if (_flow_pub == nullptr) {
_flow_pub = orb_advertise(ORB_ID(optical_flow), &flow);
} else {
orb_publish(ORB_ID(optical_flow), _flow_pub, &flow);
}
}
return OK;
}