mirror of https://github.com/ArduPilot/ardupilot
AC_PrecLand: configuration of Precision Landing for custom build server
This commit is contained in:
parent
1bc9c490a1
commit
a91178d0e1
|
@ -1,7 +1,12 @@
|
||||||
|
#include "AC_PrecLand_config.h"
|
||||||
|
|
||||||
|
#if AC_PRECLAND_ENABLED
|
||||||
|
|
||||||
|
#include "AC_PrecLand.h"
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
#include <AP_Scheduler/AP_Scheduler.h>
|
#include <AP_Scheduler/AP_Scheduler.h>
|
||||||
#include <AP_AHRS/AP_AHRS.h>
|
#include <AP_AHRS/AP_AHRS.h>
|
||||||
#include "AC_PrecLand.h"
|
|
||||||
#include "AC_PrecLand_Backend.h"
|
#include "AC_PrecLand_Backend.h"
|
||||||
#include "AC_PrecLand_Companion.h"
|
#include "AC_PrecLand_Companion.h"
|
||||||
#include "AC_PrecLand_IRLock.h"
|
#include "AC_PrecLand_IRLock.h"
|
||||||
|
@ -774,3 +779,5 @@ AC_PrecLand *ac_precland()
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif // AC_PRECLAND_ENABLED
|
||||||
|
|
|
@ -1,7 +1,11 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <AP_Math/AP_Math.h>
|
#include "AC_PrecLand_config.h"
|
||||||
|
|
||||||
|
#if AC_PRECLAND_ENABLED
|
||||||
|
|
||||||
#include <GCS_MAVLink/GCS_MAVLink.h>
|
#include <GCS_MAVLink/GCS_MAVLink.h>
|
||||||
|
#include <AP_Math/AP_Math.h>
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
#include "PosVelEKF.h"
|
#include "PosVelEKF.h"
|
||||||
#include <AP_HAL/utility/RingBuffer.h>
|
#include <AP_HAL/utility/RingBuffer.h>
|
||||||
|
@ -231,3 +235,5 @@ private:
|
||||||
namespace AP {
|
namespace AP {
|
||||||
AC_PrecLand *ac_precland();
|
AC_PrecLand *ac_precland();
|
||||||
};
|
};
|
||||||
|
|
||||||
|
#endif // AC_PRECLAND_ENABLED
|
||||||
|
|
|
@ -1,8 +1,13 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include "AC_PrecLand_config.h"
|
||||||
|
|
||||||
|
#if AC_PRECLAND_ENABLED
|
||||||
|
|
||||||
|
#include "AC_PrecLand.h"
|
||||||
#include <AP_Math/AP_Math.h>
|
#include <AP_Math/AP_Math.h>
|
||||||
#include <AC_PID/AC_PID.h>
|
#include <AC_PID/AC_PID.h>
|
||||||
#include "AC_PrecLand.h"
|
|
||||||
|
|
||||||
class AC_PrecLand_Backend
|
class AC_PrecLand_Backend
|
||||||
{
|
{
|
||||||
|
@ -44,3 +49,5 @@ protected:
|
||||||
const AC_PrecLand& _frontend; // reference to precision landing front end
|
const AC_PrecLand& _frontend; // reference to precision landing front end
|
||||||
AC_PrecLand::precland_state &_state; // reference to this instances state
|
AC_PrecLand::precland_state &_state; // reference to this instances state
|
||||||
};
|
};
|
||||||
|
|
||||||
|
#endif // AC_PRECLAND_ENABLED
|
||||||
|
|
|
@ -1,6 +1,10 @@
|
||||||
|
#include "AC_PrecLand_config.h"
|
||||||
|
|
||||||
|
#if AC_PRECLAND_COMPANION_ENABLED
|
||||||
|
|
||||||
|
#include "AC_PrecLand_Companion.h"
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
#include <GCS_MAVLink/GCS.h>
|
#include <GCS_MAVLink/GCS.h>
|
||||||
#include "AC_PrecLand_Companion.h"
|
|
||||||
|
|
||||||
// perform any required initialisation of backend
|
// perform any required initialisation of backend
|
||||||
void AC_PrecLand_Companion::init()
|
void AC_PrecLand_Companion::init()
|
||||||
|
@ -73,3 +77,5 @@ void AC_PrecLand_Companion::handle_msg(const mavlink_landing_target_t &packet, u
|
||||||
_los_meas_time_ms = timestamp_ms;
|
_los_meas_time_ms = timestamp_ms;
|
||||||
_have_los_meas = true;
|
_have_los_meas = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif // AC_PRECLAND_COMPANION_ENABLED
|
||||||
|
|
|
@ -1,7 +1,11 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <AP_Math/AP_Math.h>
|
#include "AC_PrecLand_config.h"
|
||||||
|
|
||||||
|
#if AC_PRECLAND_COMPANION_ENABLED
|
||||||
|
|
||||||
#include "AC_PrecLand_Backend.h"
|
#include "AC_PrecLand_Backend.h"
|
||||||
|
#include <AP_Math/AP_Math.h>
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* AC_PrecLand_Companion - implements precision landing using target vectors provided
|
* AC_PrecLand_Companion - implements precision landing using target vectors provided
|
||||||
|
@ -46,3 +50,6 @@ private:
|
||||||
uint32_t _los_meas_time_ms; // system time in milliseconds when los was measured
|
uint32_t _los_meas_time_ms; // system time in milliseconds when los was measured
|
||||||
bool _wrong_frame_msg_sent;
|
bool _wrong_frame_msg_sent;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
#endif // AC_PRECLAND_COMPANION_ENABLED
|
||||||
|
|
|
@ -1,5 +1,9 @@
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include "AC_PrecLand_config.h"
|
||||||
|
|
||||||
|
#if AC_PRECLAND_IRLOCK_ENABLED
|
||||||
|
|
||||||
#include "AC_PrecLand_IRLock.h"
|
#include "AC_PrecLand_IRLock.h"
|
||||||
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
|
||||||
// Constructor
|
// Constructor
|
||||||
AC_PrecLand_IRLock::AC_PrecLand_IRLock(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state)
|
AC_PrecLand_IRLock::AC_PrecLand_IRLock(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state)
|
||||||
|
@ -50,3 +54,5 @@ uint32_t AC_PrecLand_IRLock::los_meas_time_ms() {
|
||||||
bool AC_PrecLand_IRLock::have_los_meas() {
|
bool AC_PrecLand_IRLock::have_los_meas() {
|
||||||
return _have_los_meas;
|
return _have_los_meas;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif // AC_PRECLAND_IRLOCK_ENABLED
|
||||||
|
|
|
@ -1,7 +1,12 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <AP_Math/AP_Math.h>
|
#include "AC_PrecLand_config.h"
|
||||||
|
|
||||||
|
#if AC_PRECLAND_IRLOCK_ENABLED
|
||||||
|
|
||||||
#include <AC_PrecLand/AC_PrecLand_Backend.h>
|
#include <AC_PrecLand/AC_PrecLand_Backend.h>
|
||||||
|
#include <AP_Math/AP_Math.h>
|
||||||
|
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||||
#include <AP_IRLock/AP_IRLock_SITL.h>
|
#include <AP_IRLock/AP_IRLock_SITL.h>
|
||||||
#else
|
#else
|
||||||
|
@ -46,3 +51,5 @@ private:
|
||||||
bool _have_los_meas; // true if there is a valid measurement from the camera
|
bool _have_los_meas; // true if there is a valid measurement from the camera
|
||||||
uint32_t _los_meas_time_ms; // system time in milliseconds when los was measured
|
uint32_t _los_meas_time_ms; // system time in milliseconds when los was measured
|
||||||
};
|
};
|
||||||
|
|
||||||
|
#endif // AC_PRECLAND_IRLOCK_ENABLED
|
||||||
|
|
|
@ -1,7 +1,7 @@
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
#include "AC_PrecLand_SITL.h"
|
#include "AC_PrecLand_SITL.h"
|
||||||
|
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
#if AC_PRECLAND_SITL_ENABLED
|
||||||
|
|
||||||
#include "AP_AHRS/AP_AHRS.h"
|
#include "AP_AHRS/AP_AHRS.h"
|
||||||
// init - perform initialisation of this backend
|
// init - perform initialisation of this backend
|
||||||
|
@ -54,4 +54,4 @@ bool AC_PrecLand_SITL::get_los_body(Vector3f& ret) {
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif // AC_PRECLAND_SITL_ENABLED
|
||||||
|
|
|
@ -1,8 +1,11 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
#include "AC_PrecLand_config.h"
|
||||||
#include <AP_Math/AP_Math.h>
|
|
||||||
|
#if AC_PRECLAND_SITL_ENABLED
|
||||||
|
|
||||||
#include <AC_PrecLand/AC_PrecLand_Backend.h>
|
#include <AC_PrecLand/AC_PrecLand_Backend.h>
|
||||||
|
#include <AP_Math/AP_Math.h>
|
||||||
#include <SITL/SITL.h>
|
#include <SITL/SITL.h>
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
@ -43,4 +46,4 @@ private:
|
||||||
float _distance_to_target; // distance to target in meters
|
float _distance_to_target; // distance to target in meters
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif
|
#endif // AC_PRECLAND_SITL_ENABLED
|
||||||
|
|
|
@ -3,7 +3,7 @@
|
||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
#if AC_PRECLAND_SITL_GAZEBO_ENABLED
|
||||||
|
|
||||||
// Constructor
|
// Constructor
|
||||||
AC_PrecLand_SITL_Gazebo::AC_PrecLand_SITL_Gazebo(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state)
|
AC_PrecLand_SITL_Gazebo::AC_PrecLand_SITL_Gazebo(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state)
|
||||||
|
@ -55,4 +55,4 @@ bool AC_PrecLand_SITL_Gazebo::have_los_meas() {
|
||||||
return _have_los_meas;
|
return _have_los_meas;
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif // AC_PRECLAND_SITL_GAZEBO_ENABLED
|
||||||
|
|
|
@ -1,8 +1,11 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
#include "AC_PrecLand_config.h"
|
||||||
#include <AP_Math/AP_Math.h>
|
|
||||||
|
#if AC_PRECLAND_SITL_GAZEBO_ENABLED
|
||||||
|
|
||||||
#include <AC_PrecLand/AC_PrecLand_Backend.h>
|
#include <AC_PrecLand/AC_PrecLand_Backend.h>
|
||||||
|
#include <AP_Math/AP_Math.h>
|
||||||
#include <AP_IRLock/AP_IRLock_SITL_Gazebo.h>
|
#include <AP_IRLock/AP_IRLock_SITL_Gazebo.h>
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
@ -41,4 +44,4 @@ private:
|
||||||
uint32_t _los_meas_time_ms; // system time in milliseconds when los was measured
|
uint32_t _los_meas_time_ms; // system time in milliseconds when los was measured
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif
|
#endif // AC_PRECLAND_SITL_GAZEBO_ENABLED
|
||||||
|
|
|
@ -1,5 +1,9 @@
|
||||||
#include "AC_PrecLand_StateMachine.h"
|
#include "AC_PrecLand_config.h"
|
||||||
|
|
||||||
|
#if AC_PRECLAND_ENABLED
|
||||||
|
|
||||||
#include <AC_PrecLand/AC_PrecLand.h>
|
#include <AC_PrecLand/AC_PrecLand.h>
|
||||||
|
#include "AC_PrecLand_StateMachine.h"
|
||||||
#include <AP_AHRS/AP_AHRS.h>
|
#include <AP_AHRS/AP_AHRS.h>
|
||||||
#include <GCS_MAVLink/GCS.h>
|
#include <GCS_MAVLink/GCS.h>
|
||||||
|
|
||||||
|
@ -274,3 +278,5 @@ AC_PrecLand_StateMachine::FailSafeAction AC_PrecLand_StateMachine::get_failsafe_
|
||||||
// should never reach here
|
// should never reach here
|
||||||
return FailSafeAction::DESCEND;
|
return FailSafeAction::DESCEND;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif // AC_PRECLAND_ENABLED
|
||||||
|
|
|
@ -1,5 +1,10 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include "AC_PrecLand_config.h"
|
||||||
|
|
||||||
|
#if AC_PRECLAND_ENABLED
|
||||||
|
|
||||||
|
#include <AC_PrecLand/AC_PrecLand.h>
|
||||||
#include <AP_Math/AP_Math.h>
|
#include <AP_Math/AP_Math.h>
|
||||||
|
|
||||||
// This class constantly monitors what the status of the landing target is
|
// This class constantly monitors what the status of the landing target is
|
||||||
|
@ -101,3 +106,5 @@ private:
|
||||||
uint32_t failsafe_start_ms; // timestamp of when failsafe was triggered
|
uint32_t failsafe_start_ms; // timestamp of when failsafe was triggered
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
#endif // AC_PRECLAND_ENABLED
|
||||||
|
|
|
@ -0,0 +1,27 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <AP_HAL/AP_HAL_Boards.h>
|
||||||
|
|
||||||
|
#ifndef AC_PRECLAND_ENABLED
|
||||||
|
#define AC_PRECLAND_ENABLED 1
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef AC_PRECLAND_BACKEND_DEFAULT_ENABLED
|
||||||
|
#define AC_PRECLAND_BACKEND_DEFAULT_ENABLED AC_PRECLAND_ENABLED
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef AC_PRECLAND_COMPANION_ENABLED
|
||||||
|
#define AC_PRECLAND_COMPANION_ENABLED AC_PRECLAND_BACKEND_DEFAULT_ENABLED
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef AC_PRECLAND_IRLOCK_ENABLED
|
||||||
|
#define AC_PRECLAND_IRLOCK_ENABLED AC_PRECLAND_BACKEND_DEFAULT_ENABLED
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef AC_PRECLAND_SITL_ENABLED
|
||||||
|
#define AC_PRECLAND_SITL_ENABLED (AC_PRECLAND_BACKEND_DEFAULT_ENABLED && CONFIG_HAL_BOARD == HAL_BOARD_SITL)
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef AC_PRECLAND_SITL_GAZEBO_ENABLED
|
||||||
|
#define AC_PRECLAND_SITL_GAZEBO_ENABLED (AC_PRECLAND_BACKEND_DEFAULT_ENABLED && CONFIG_HAL_BOARD == HAL_BOARD_SITL)
|
||||||
|
#endif
|
Loading…
Reference in New Issue