forked from Archive/PX4-Autopilot
drivers/osd/msp_osd: use proper EKF status flags instead of solution status bits
This commit is contained in:
parent
27f9b1b65a
commit
71b9e31005
|
@ -350,16 +350,12 @@ void MspOsd::Run()
|
||||||
home_position_s home_position{};
|
home_position_s home_position{};
|
||||||
_home_position_sub.copy(&home_position);
|
_home_position_sub.copy(&home_position);
|
||||||
|
|
||||||
estimator_status_s estimator_status{};
|
|
||||||
_estimator_status_sub.copy(&estimator_status);
|
|
||||||
|
|
||||||
vehicle_global_position_s vehicle_global_position{};
|
vehicle_global_position_s vehicle_global_position{};
|
||||||
_vehicle_global_position_sub.copy(&vehicle_global_position);
|
_vehicle_global_position_sub.copy(&vehicle_global_position);
|
||||||
|
|
||||||
// construct and send message
|
// construct and send message
|
||||||
const auto msg = msp_osd::construct_COMP_GPS(
|
const auto msg = msp_osd::construct_COMP_GPS(
|
||||||
home_position,
|
home_position,
|
||||||
estimator_status,
|
|
||||||
vehicle_global_position,
|
vehicle_global_position,
|
||||||
_heartbeat);
|
_heartbeat);
|
||||||
this->Send(MSP_COMP_GPS, &msg);
|
this->Send(MSP_COMP_GPS, &msg);
|
||||||
|
@ -379,17 +375,11 @@ void MspOsd::Run()
|
||||||
sensor_gps_s vehicle_gps_position{};
|
sensor_gps_s vehicle_gps_position{};
|
||||||
_vehicle_gps_position_sub.copy(&vehicle_gps_position);
|
_vehicle_gps_position_sub.copy(&vehicle_gps_position);
|
||||||
|
|
||||||
estimator_status_s estimator_status{};
|
|
||||||
_estimator_status_sub.copy(&estimator_status);
|
|
||||||
|
|
||||||
vehicle_local_position_s vehicle_local_position{};
|
vehicle_local_position_s vehicle_local_position{};
|
||||||
_vehicle_local_position_sub.copy(&vehicle_local_position);
|
_vehicle_local_position_sub.copy(&vehicle_local_position);
|
||||||
|
|
||||||
// construct and send message
|
// construct and send message
|
||||||
const auto msg = msp_osd::construct_ALTITUDE(
|
const auto msg = msp_osd::construct_ALTITUDE(vehicle_gps_position, vehicle_local_position);
|
||||||
vehicle_gps_position,
|
|
||||||
estimator_status,
|
|
||||||
vehicle_local_position);
|
|
||||||
this->Send(MSP_ALTITUDE, &msg);
|
this->Send(MSP_ALTITUDE, &msg);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -43,7 +43,6 @@
|
||||||
#include <uORB/Subscription.hpp>
|
#include <uORB/Subscription.hpp>
|
||||||
#include <uORB/topics/airspeed_validated.h>
|
#include <uORB/topics/airspeed_validated.h>
|
||||||
#include <uORB/topics/battery_status.h>
|
#include <uORB/topics/battery_status.h>
|
||||||
#include <uORB/topics/estimator_status.h>
|
|
||||||
#include <uORB/topics/home_position.h>
|
#include <uORB/topics/home_position.h>
|
||||||
#include <uORB/topics/input_rc.h>
|
#include <uORB/topics/input_rc.h>
|
||||||
#include <uORB/topics/log_message.h>
|
#include <uORB/topics/log_message.h>
|
||||||
|
@ -145,7 +144,6 @@ private:
|
||||||
// subscriptions to desired vehicle display information
|
// subscriptions to desired vehicle display information
|
||||||
uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
|
uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
|
||||||
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
|
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
|
||||||
uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)};
|
|
||||||
uORB::Subscription _home_position_sub{ORB_ID(home_position)};
|
uORB::Subscription _home_position_sub{ORB_ID(home_position)};
|
||||||
uORB::Subscription _input_rc_sub{ORB_ID(input_rc)};
|
uORB::Subscription _input_rc_sub{ORB_ID(input_rc)};
|
||||||
uORB::Subscription _log_message_sub{ORB_ID(log_message)};
|
uORB::Subscription _log_message_sub{ORB_ID(log_message)};
|
||||||
|
|
|
@ -365,7 +365,6 @@ msp_raw_gps_t construct_RAW_GPS(const sensor_gps_s &vehicle_gps_position,
|
||||||
}
|
}
|
||||||
|
|
||||||
msp_comp_gps_t construct_COMP_GPS(const home_position_s &home_position,
|
msp_comp_gps_t construct_COMP_GPS(const home_position_s &home_position,
|
||||||
const estimator_status_s &estimator_status,
|
|
||||||
const vehicle_global_position_s &vehicle_global_position,
|
const vehicle_global_position_s &vehicle_global_position,
|
||||||
const bool heartbeat)
|
const bool heartbeat)
|
||||||
{
|
{
|
||||||
|
@ -375,7 +374,8 @@ msp_comp_gps_t construct_COMP_GPS(const home_position_s &home_position,
|
||||||
// Calculate distance and direction to home
|
// Calculate distance and direction to home
|
||||||
if (home_position.valid_hpos
|
if (home_position.valid_hpos
|
||||||
&& home_position.valid_lpos
|
&& home_position.valid_lpos
|
||||||
&& estimator_status.solution_status_flags & (1 << 4)) {
|
&& (hrt_elapsed_time(&vehicle_global_position.timestamp) < 1_s)) {
|
||||||
|
|
||||||
float bearing_to_home = math::degrees(get_bearing_to_next_waypoint(vehicle_global_position.lat,
|
float bearing_to_home = math::degrees(get_bearing_to_next_waypoint(vehicle_global_position.lat,
|
||||||
vehicle_global_position.lon,
|
vehicle_global_position.lon,
|
||||||
home_position.lat, home_position.lon));
|
home_position.lat, home_position.lon));
|
||||||
|
@ -425,7 +425,6 @@ msp_attitude_t construct_ATTITUDE(const vehicle_attitude_s &vehicle_attitude)
|
||||||
}
|
}
|
||||||
|
|
||||||
msp_altitude_t construct_ALTITUDE(const sensor_gps_s &vehicle_gps_position,
|
msp_altitude_t construct_ALTITUDE(const sensor_gps_s &vehicle_gps_position,
|
||||||
const estimator_status_s &estimator_status,
|
|
||||||
const vehicle_local_position_s &vehicle_local_position)
|
const vehicle_local_position_s &vehicle_local_position)
|
||||||
{
|
{
|
||||||
// initialize result
|
// initialize result
|
||||||
|
@ -438,7 +437,7 @@ msp_altitude_t construct_ALTITUDE(const sensor_gps_s &vehicle_gps_position,
|
||||||
altitude.estimatedActualPosition = 0;
|
altitude.estimatedActualPosition = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (estimator_status.solution_status_flags & (1 << 5)) {
|
if (vehicle_local_position.v_z_valid) {
|
||||||
altitude.estimatedActualVelocity = -vehicle_local_position.vz * 100; //m/s to cm/s
|
altitude.estimatedActualVelocity = -vehicle_local_position.vz * 100; //m/s to cm/s
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|
|
@ -54,7 +54,6 @@
|
||||||
#include <uORB/topics/vehicle_global_position.h>
|
#include <uORB/topics/vehicle_global_position.h>
|
||||||
#include <uORB/topics/vehicle_local_position.h>
|
#include <uORB/topics/vehicle_local_position.h>
|
||||||
#include <uORB/topics/vehicle_attitude.h>
|
#include <uORB/topics/vehicle_attitude.h>
|
||||||
#include <uORB/topics/estimator_status.h>
|
|
||||||
#include <uORB/topics/input_rc.h>
|
#include <uORB/topics/input_rc.h>
|
||||||
#include <uORB/topics/log_message.h>
|
#include <uORB/topics/log_message.h>
|
||||||
|
|
||||||
|
@ -94,7 +93,6 @@ msp_raw_gps_t construct_RAW_GPS(const sensor_gps_s &vehicle_gps_position,
|
||||||
|
|
||||||
// construct an MSP_COMP_GPS struct
|
// construct an MSP_COMP_GPS struct
|
||||||
msp_comp_gps_t construct_COMP_GPS(const home_position_s &home_position,
|
msp_comp_gps_t construct_COMP_GPS(const home_position_s &home_position,
|
||||||
const estimator_status_s &estimator_status,
|
|
||||||
const vehicle_global_position_s &vehicle_global_position,
|
const vehicle_global_position_s &vehicle_global_position,
|
||||||
const bool heartbeat);
|
const bool heartbeat);
|
||||||
|
|
||||||
|
@ -103,7 +101,6 @@ msp_attitude_t construct_ATTITUDE(const vehicle_attitude_s &vehicle_attitude);
|
||||||
|
|
||||||
// construct an MSP_ALTITUDE struct
|
// construct an MSP_ALTITUDE struct
|
||||||
msp_altitude_t construct_ALTITUDE(const sensor_gps_s &vehicle_gps_position,
|
msp_altitude_t construct_ALTITUDE(const sensor_gps_s &vehicle_gps_position,
|
||||||
const estimator_status_s &estimator_status,
|
|
||||||
const vehicle_local_position_s &vehicle_local_position);
|
const vehicle_local_position_s &vehicle_local_position);
|
||||||
|
|
||||||
// construct an MSP_ESC_SENSOR_DATA struct
|
// construct an MSP_ESC_SENSOR_DATA struct
|
||||||
|
|
Loading…
Reference in New Issue