diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 8e7df42743..f14dd3015d 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -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 # diff --git a/posix-configs/SITL/init/rcS b/posix-configs/SITL/init/rcS index 35bf7abd5b..878eda3a6e 100644 --- a/posix-configs/SITL/init/rcS +++ b/posix-configs/SITL/init/rcS @@ -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 diff --git a/src/lib/ecl/validation/data_validator.cpp b/src/lib/ecl/validation/data_validator.cpp index 037cb88645..737d9ea1a3 100644 --- a/src/lib/ecl/validation/data_validator.cpp +++ b/src/lib/ecl/validation/data_validator.cpp @@ -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())); } } diff --git a/src/lib/ecl/validation/data_validator.h b/src/lib/ecl/validation/data_validator.h index 686a810c4e..dde9cb51aa 100644 --- a/src/lib/ecl/validation/data_validator.h +++ b/src/lib/ecl/validation/data_validator.h @@ -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 */ diff --git a/src/lib/ecl/validation/data_validator_group.cpp b/src/lib/ecl/validation/data_validator_group.cpp index cdd1f6a677..e1ba53333e 100644 --- a/src/lib/ecl/validation/data_validator_group.cpp +++ b/src/lib/ecl/validation/data_validator_group.cpp @@ -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++; } diff --git a/src/lib/ecl/validation/data_validator_group.h b/src/lib/ecl/validation/data_validator_group.h index f35e0d71c1..3756be2638 100644 --- a/src/lib/ecl/validation/data_validator_group.h +++ b/src/lib/ecl/validation/data_validator_group.h @@ -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&); diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c index d4d9d10eb4..d72f62dc61 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c @@ -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); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5c17f3dd9c..3547341a71 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -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); diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index da2a1ddb48..6f9c35bb95 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -51,6 +51,7 @@ #include #include #include +#include #include #include 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 diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index bf42d346b9..a930d78c80 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -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; +}