mirror of https://github.com/ArduPilot/ardupilot
Rover: move handling of visual odometry messages up
This commit is contained in:
parent
ed5140307a
commit
3c58eca0ab
|
@ -1359,10 +1359,6 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
||||||
rover.g2.proximity.handle_msg(msg);
|
rover.g2.proximity.handle_msg(msg);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_VISION_POSITION_DELTA:
|
|
||||||
rover.g2.visual_odom.handle_msg(msg);
|
|
||||||
break;
|
|
||||||
|
|
||||||
default:
|
default:
|
||||||
handle_common_message(msg);
|
handle_common_message(msg);
|
||||||
break;
|
break;
|
||||||
|
@ -1469,6 +1465,15 @@ AP_AdvancedFailsafe *GCS_MAVLINK_Rover::get_advanced_failsafe() const
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
AP_VisualOdom *GCS_MAVLINK_Rover::get_visual_odom() const
|
||||||
|
{
|
||||||
|
#if VISUAL_ODOMETRY_ENABLED == ENABLED
|
||||||
|
return &rover.g2.visual_odom;
|
||||||
|
#else
|
||||||
|
return nullptr;
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
Compass *GCS_MAVLINK_Rover::get_compass() const
|
Compass *GCS_MAVLINK_Rover::get_compass() const
|
||||||
{
|
{
|
||||||
return &rover.compass;
|
return &rover.compass;
|
||||||
|
|
|
@ -22,6 +22,7 @@ protected:
|
||||||
AP_Camera *get_camera() const override;
|
AP_Camera *get_camera() const override;
|
||||||
AP_ServoRelayEvents *get_servorelayevents() const override;
|
AP_ServoRelayEvents *get_servorelayevents() const override;
|
||||||
AP_AdvancedFailsafe *get_advanced_failsafe() const override;
|
AP_AdvancedFailsafe *get_advanced_failsafe() const override;
|
||||||
|
AP_VisualOdom *get_visual_odom() const override;
|
||||||
const AP_FWVersion &get_fwver() const override;
|
const AP_FWVersion &get_fwver() const override;
|
||||||
void set_ekf_origin(const Location& loc) override;
|
void set_ekf_origin(const Location& loc) override;
|
||||||
|
|
||||||
|
|
|
@ -96,6 +96,13 @@
|
||||||
#error XXX
|
#error XXX
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
|
// VISUAL ODOMETRY
|
||||||
|
#ifndef VISUAL_ODOMETRY_ENABLED
|
||||||
|
# define VISUAL_ODOMETRY_ENABLED ENABLED
|
||||||
|
#endif
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
// STARTUP BEHAVIOUR
|
// STARTUP BEHAVIOUR
|
||||||
|
|
|
@ -42,7 +42,6 @@ LIBRARIES += AP_RPM
|
||||||
LIBRARIES += AP_Arming
|
LIBRARIES += AP_Arming
|
||||||
LIBRARIES += AP_Stats
|
LIBRARIES += AP_Stats
|
||||||
LIBRARIES += AP_Beacon
|
LIBRARIES += AP_Beacon
|
||||||
LIBRARIES += AP_VisualOdom
|
|
||||||
LIBRARIES += AP_WheelEncoder
|
LIBRARIES += AP_WheelEncoder
|
||||||
LIBRARIES += AP_AdvancedFailsafe
|
LIBRARIES += AP_AdvancedFailsafe
|
||||||
LIBRARIES += AR_AttitudeControl
|
LIBRARIES += AR_AttitudeControl
|
||||||
|
|
|
@ -18,7 +18,6 @@ def build(bld):
|
||||||
'AC_PID',
|
'AC_PID',
|
||||||
'AP_Stats',
|
'AP_Stats',
|
||||||
'AP_Beacon',
|
'AP_Beacon',
|
||||||
'AP_VisualOdom',
|
|
||||||
'AP_AdvancedFailsafe',
|
'AP_AdvancedFailsafe',
|
||||||
'AP_WheelEncoder',
|
'AP_WheelEncoder',
|
||||||
'AP_SmartRTL',
|
'AP_SmartRTL',
|
||||||
|
|
Loading…
Reference in New Issue