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

sensors: add SENS_MAG_MODE to optionally publish all magnetometers

This commit is contained in:
Daniel Agar 2020-09-01 14:57:31 -04:00
parent a04412fc1f
commit dc507ee544
3 changed files with 119 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/sensors/sensor_params_mag.c
View File

@ -93,3 +93,15 @@ PARAM_DEFINE_INT32(CAL_MAG_ROT_AUTO, 1);
*
*/
PARAM_DEFINE_FLOAT(SENS_MAG_RATE, 50.0f);
/**
* Sensors hub mag mode
*
* @value 0 Publish all magnetometers
* @value 1 Publish primary magnetometer
*
* @category system
* @reboot_required true
* @group Sensors
*/
PARAM_DEFINE_INT32(SENS_MAG_MODE, 1);

97
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
View File

@ -212,6 +212,13 @@ void VehicleMagnetometer::Run()
_advertised[uorb_index] = true;
// advertise outputs in order if publishing all
if (!_param_sens_mag_mode.get()) {
for (int instance = 0; instance < uorb_index; instance++) {
_vehicle_magnetometer_multi_pub[instance].advertise();
}
}
} else {
_last_data[uorb_index].timestamp = hrt_absolute_time();
}
@ -229,16 +236,20 @@ void VehicleMagnetometer::Run()
}
if (_calibration[uorb_index].enabled()) {
Vector3f vect = _calibration[uorb_index].Correct(Vector3f{report.x, report.y, report.z});
const Vector3f vect = _calibration[uorb_index].Correct(Vector3f{report.x, report.y, report.z});
float mag_array[3] {vect(0), vect(1), vect(2)};
_voter.put(uorb_index, report.timestamp, mag_array, report.error_count, _priority[uorb_index]);
_timestamp_sample_sum[uorb_index] += report.timestamp_sample;
_mag_sum[uorb_index] += vect;
_mag_sum_count[uorb_index]++;
_last_data[uorb_index].timestamp_sample = report.timestamp_sample;
_last_data[uorb_index].device_id = report.device_id;
_last_data[uorb_index].x = vect(0);
_last_data[uorb_index].y = vect(1);
_last_data[uorb_index].z = vect(2);
float mag_array[3] {vect(0), vect(1), vect(2)};
_voter.put(uorb_index, report.timestamp, mag_array, report.error_count, _priority[uorb_index]);
}
}
}
@ -255,50 +266,40 @@ void VehicleMagnetometer::Run()
sub.unregisterCallback();
}
if (_param_sens_mag_mode.get()) {
if (_selected_sensor_sub_index >= 0) {
PX4_INFO("%s switch from #%u -> #%d", "MAG", _selected_sensor_sub_index, best_index);
}
}
_selected_sensor_sub_index = best_index;
_sensor_sub[_selected_sensor_sub_index].registerCallback();
}
}
// Publish
if (_param_sens_mag_mode.get()) {
// publish only best mag
if ((_selected_sensor_sub_index >= 0)
&& (_voter.get_sensor_state(_selected_sensor_sub_index) == DataValidator::ERROR_FLAG_NO_ERROR)
&& updated[_selected_sensor_sub_index]) {
const sensor_mag_s &mag = _last_data[_selected_sensor_sub_index];
Publish(_selected_sensor_sub_index);
}
_mag_timestamp_sum += mag.timestamp_sample;
_mag_sum += Vector3f{mag.x, mag.y, mag.z};
_mag_sum_count++;
if ((_param_sens_mag_rate.get() > 0)
&& hrt_elapsed_time(&_last_publication_timestamp) >= (1e6f / _param_sens_mag_rate.get())) {
const Vector3f magnetometer_data = _mag_sum / _mag_sum_count;
const hrt_abstime timestamp_sample = _mag_timestamp_sum / _mag_sum_count;
// reset
_mag_timestamp_sum = 0;
_mag_sum.zero();
_mag_sum_count = 0;
// populate vehicle_magnetometer with primary mag and publish
vehicle_magnetometer_s out{};
out.timestamp_sample = timestamp_sample;
out.device_id = mag.device_id;
magnetometer_data.copyTo(out.magnetometer_ga);
out.timestamp = hrt_absolute_time();
_vehicle_magnetometer_pub.publish(out);
_last_publication_timestamp = out.timestamp;
} else {
// publish all
for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) {
// publish all magnetometers as separate instances
if (updated[uorb_index] && (_calibration[uorb_index].device_id() != 0)) {
Publish(uorb_index, true);
}
}
}
// check failover and report
if (_param_sens_mag_mode.get()) {
if (_last_failover_count != _voter.failover_count()) {
uint32_t flags = _voter.failover_state();
int failover_index = _voter.failover_index();
@ -326,17 +327,51 @@ void VehicleMagnetometer::Run()
_last_failover_count = _voter.failover_count();
}
}
if (!_armed) {
calcMagInconsistency();
}
// reschedule timeout
ScheduleDelayed(100_ms);
ScheduleDelayed(20_ms);
perf_end(_cycle_perf);
}
void VehicleMagnetometer::Publish(uint8_t instance, bool multi)
{
if ((_param_sens_mag_rate.get() > 0)
&& hrt_elapsed_time(&_last_publication_timestamp[instance]) >= (1e6f / _param_sens_mag_rate.get())) {
const Vector3f magnetometer_data = _mag_sum[instance] / _mag_sum_count[instance];
const hrt_abstime timestamp_sample = _timestamp_sample_sum[instance] / _mag_sum_count[instance];
// reset
_timestamp_sample_sum[instance] = 0;
_mag_sum[instance].zero();
_mag_sum_count[instance] = 0;
// populate vehicle_magnetometer with primary mag and publish
vehicle_magnetometer_s out{};
out.timestamp_sample = timestamp_sample;
out.device_id = _calibration[instance].device_id();
magnetometer_data.copyTo(out.magnetometer_ga);
out.timestamp = hrt_absolute_time();
if (multi) {
_vehicle_magnetometer_multi_pub[instance].publish(out);
} else {
// otherwise only ever publish the first instance
_vehicle_magnetometer_pub.publish(out);
}
_last_publication_timestamp[instance] = out.timestamp;
}
}
void VehicleMagnetometer::calcMagInconsistency()
{
sensor_preflight_mag_s preflt{};

19
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp
View File

@ -46,6 +46,7 @@
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_controls.h>
@ -75,6 +76,8 @@ private:
void ParametersUpdate(bool force = false);
void Publish(uint8_t instance, bool multi = false);
/**
* Calculates the magnitude in Gauss of the largest difference between the primary and any other magnetometers
*/
@ -83,7 +86,14 @@ private:
static constexpr int MAX_SENSOR_COUNT = 4;
uORB::Publication<sensor_preflight_mag_s> _sensor_preflight_mag_pub{ORB_ID(sensor_preflight_mag)};
uORB::Publication<vehicle_magnetometer_s> _vehicle_magnetometer_pub{ORB_ID(vehicle_magnetometer)};
uORB::PublicationMulti<vehicle_magnetometer_s> _vehicle_magnetometer_multi_pub[MAX_SENSOR_COUNT] {
{ORB_ID(vehicle_magnetometer)},
{ORB_ID(vehicle_magnetometer)},
{ORB_ID(vehicle_magnetometer)},
{ORB_ID(vehicle_magnetometer)},
};
uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)};
uORB::Subscription _battery_status_sub{ORB_ID(battery_status), 0};
@ -110,16 +120,16 @@ private:
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
hrt_abstime _last_publication_timestamp{0};
hrt_abstime _last_error_message{0};
orb_advert_t _mavlink_log_pub{nullptr};
DataValidatorGroup _voter{1};
unsigned _last_failover_count{0};
uint64_t _mag_timestamp_sum{0};
matrix::Vector3f _mag_sum{};
int _mag_sum_count{0};
uint64_t _timestamp_sample_sum[MAX_SENSOR_COUNT] {0};
matrix::Vector3f _mag_sum[MAX_SENSOR_COUNT] {};
int _mag_sum_count[MAX_SENSOR_COUNT] {};
hrt_abstime _last_publication_timestamp[MAX_SENSOR_COUNT] {};
sensor_mag_s _last_data[MAX_SENSOR_COUNT] {};
bool _advertised[MAX_SENSOR_COUNT] {};
@ -134,6 +144,7 @@ private:
DEFINE_PARAMETERS(
(ParamInt<px4::params::CAL_MAG_COMP_TYP>) _param_mag_comp_typ,
(ParamBool<px4::params::SENS_MAG_MODE>) _param_sens_mag_mode,
(ParamFloat<px4::params::SENS_MAG_RATE>) _param_sens_mag_rate
)
};