forked from Archive/PX4-Autopilot
ekf2: select estimator instance manually using RC channel
This commit is contained in:
parent
7467c5fad5
commit
0835c432e3
|
@ -754,7 +754,16 @@ void EKF2Selector::Run()
|
|||
}
|
||||
}
|
||||
|
||||
if (!_instance[_selected_instance].healthy.get_state()) {
|
||||
if (_param_ekf2_sel_rc_map.get() > 0) {
|
||||
// Manual instance selection through RC
|
||||
if (isRcOverride()) {
|
||||
SelectInstance(_param_ekf2_sel_rc_inst.get());
|
||||
|
||||
} else {
|
||||
SelectInstance(0);
|
||||
}
|
||||
|
||||
} else if (!_instance[_selected_instance].healthy.get_state()) {
|
||||
// prefer the best healthy instance using a different IMU
|
||||
if (!SelectInstance(best_ekf_different_imu)) {
|
||||
// otherwise switch to the healthy instance with best overall test ratio
|
||||
|
@ -807,6 +816,48 @@ void EKF2Selector::Run()
|
|||
ScheduleDelayed(FILTER_UPDATE_PERIOD);
|
||||
}
|
||||
|
||||
bool EKF2Selector::isRcOverride()
|
||||
{
|
||||
manual_control_setpoint_s manual_control_setpoint{};
|
||||
_manual_control_setpoint_sub.copy(&manual_control_setpoint);
|
||||
|
||||
float channel = 0;
|
||||
|
||||
switch (_param_ekf2_sel_rc_map.get()) {
|
||||
case 0:
|
||||
return false;
|
||||
|
||||
case 1:
|
||||
channel = manual_control_setpoint.aux1;
|
||||
break;
|
||||
|
||||
case 2:
|
||||
channel = manual_control_setpoint.aux2;
|
||||
break;
|
||||
|
||||
case 3:
|
||||
channel = manual_control_setpoint.aux3;
|
||||
break;
|
||||
|
||||
case 4:
|
||||
channel = manual_control_setpoint.aux4;
|
||||
break;
|
||||
|
||||
case 5:
|
||||
channel = manual_control_setpoint.aux5;
|
||||
break;
|
||||
|
||||
case 6:
|
||||
channel = manual_control_setpoint.aux6;
|
||||
break;
|
||||
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
|
||||
return channel > .5f;
|
||||
}
|
||||
|
||||
void EKF2Selector::PublishEstimatorSelectorStatus()
|
||||
{
|
||||
estimator_selector_status_s selector_status{};
|
||||
|
|
|
@ -47,6 +47,7 @@
|
|||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/estimator_selector_status.h>
|
||||
#include <uORB/topics/estimator_status.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/sensor_selection.h>
|
||||
#include <uORB/topics/sensors_status_imu.h>
|
||||
|
@ -82,6 +83,8 @@ private:
|
|||
|
||||
void Run() override;
|
||||
|
||||
bool isRcOverride();
|
||||
|
||||
void PrintInstanceChange(const uint8_t old_instance, uint8_t new_instance);
|
||||
|
||||
void PublishEstimatorSelectorStatus();
|
||||
|
@ -228,6 +231,7 @@ private:
|
|||
uint8_t _global_position_instance_prev{INVALID_INSTANCE};
|
||||
uint8_t _odometry_instance_prev{INVALID_INSTANCE};
|
||||
|
||||
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
uORB::Subscription _sensors_status_imu{ORB_ID(sensors_status_imu)};
|
||||
|
||||
|
@ -245,7 +249,9 @@ private:
|
|||
(ParamFloat<px4::params::EKF2_SEL_IMU_RAT>) _param_ekf2_sel_imu_angle_rate,
|
||||
(ParamFloat<px4::params::EKF2_SEL_IMU_ANG>) _param_ekf2_sel_imu_angle,
|
||||
(ParamFloat<px4::params::EKF2_SEL_IMU_ACC>) _param_ekf2_sel_imu_accel,
|
||||
(ParamFloat<px4::params::EKF2_SEL_IMU_VEL>) _param_ekf2_sel_imu_velocity
|
||||
(ParamFloat<px4::params::EKF2_SEL_IMU_VEL>) _param_ekf2_sel_imu_velocity,
|
||||
(ParamInt<px4::params::EKF2_SEL_RC_MAP>) _param_ekf2_sel_rc_map,
|
||||
(ParamInt<px4::params::EKF2_SEL_RC_INST>) _param_ekf2_sel_rc_inst
|
||||
)
|
||||
};
|
||||
#endif // !EKF2SELECTOR_HPP
|
||||
|
|
|
@ -79,3 +79,39 @@ PARAM_DEFINE_FLOAT(EKF2_SEL_IMU_ACC, 1.0f);
|
|||
* @unit m/s
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF2_SEL_IMU_VEL, 2.0f);
|
||||
|
||||
/**
|
||||
* Manual instance selection channel
|
||||
*
|
||||
* Defines which RC_MAP_AUXn parameter maps the RC channel used to select a desired
|
||||
* EKF2 instance.
|
||||
* Use EKF2_SEL_RC_INST to define which instance is selected when this switch is active.
|
||||
* When manual selection is active and the switch is off, instance 0 is selected.
|
||||
* Manual selection overrides automatic instance switch.
|
||||
*
|
||||
* @value 0 Disable
|
||||
* @value 1 Aux1
|
||||
* @value 2 Aux2
|
||||
* @value 3 Aux3
|
||||
* @value 4 Aux4
|
||||
* @value 5 Aux5
|
||||
* @value 6 Aux6
|
||||
* @min 0
|
||||
* @max 6
|
||||
* @group EKF2
|
||||
*/
|
||||
PARAM_DEFINE_INT32(EKF2_SEL_RC_MAP, 0);
|
||||
|
||||
/**
|
||||
* Manual instance selection
|
||||
*
|
||||
* Defines which estimator instance is used when manual switch is active.
|
||||
* Use EKF2_SEL_RC_MAP to define which aux switch is used to trigger the switch.
|
||||
* When manual selection is active and the switch is off, instance 0 is selected.
|
||||
* Manual selection overrides automatic instance switch.
|
||||
*
|
||||
* @min 0
|
||||
* @max 10
|
||||
* @group EKF2
|
||||
*/
|
||||
PARAM_DEFINE_INT32(EKF2_SEL_RC_INST, 0);
|
||||
|
|
Loading…
Reference in New Issue