aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_Precland: use SITL precland object

Browse Source 
AC_Precland: simplify includes
This commit is contained in:
Pierre Kancir 2018-08-03 12:52:48 +02:00 committed by Peter Barker
parent be5478902c
commit 40e7d22811
10 changed files with 27 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AC_PrecLand/AC_PrecLand.cpp
View File

@ -1,5 +1,6 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_Scheduler/AP_Scheduler.h>
#include <AP_AHRS/AP_AHRS.h>
#include "AC_PrecLand.h"
#include "AC_PrecLand_Backend.h"
#include "AC_PrecLand_Companion.h"

1
libraries/AC_PrecLand/AC_PrecLand.h
View File

@ -1,6 +1,5 @@
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_Math/AP_Math.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <stdint.h>

1
libraries/AC_PrecLand/AC_PrecLand_Backend.h
View File

@ -1,6 +1,5 @@
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_Math/AP_Math.h>
#include <AC_PID/AC_PID.h>
#include "AC_PrecLand.h"

2
libraries/AC_PrecLand/AC_PrecLand_Companion.cpp
View File

@ -1,8 +1,6 @@
#include <AP_HAL/AP_HAL.h>
#include "AC_PrecLand_Companion.h"
extern const AP_HAL::HAL& hal;
// perform any required initialisation of backend
void AC_PrecLand_Companion::init()
{

1
libraries/AC_PrecLand/AC_PrecLand_Companion.h
View File

@ -1,6 +1,5 @@
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_Math/AP_Math.h>
#include "AC_PrecLand_Backend.h"

2
libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp
View File

@ -1,8 +1,6 @@
#include <AP_HAL/AP_HAL.h>
#include "AC_PrecLand_IRLock.h"
extern const AP_HAL::HAL& hal;
// Constructor
AC_PrecLand_IRLock::AC_PrecLand_IRLock(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state)
: AC_PrecLand_Backend(frontend, state),

12
libraries/AC_PrecLand/AC_PrecLand_IRLock.h
View File

@ -1,12 +1,11 @@
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_Math/AP_Math.h>
#include <AC_PrecLand/AC_PrecLand_Backend.h>
#include <AP_IRLock/AP_IRLock.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <AP_IRLock/AP_IRLock_SITL.h>
#include <AP_IRLock/AP_IRLock_SITL.h>
#else
#include <AP_IRLock/AP_IRLock.h>
#endif
/*
@ -38,8 +37,11 @@ public:
bool have_los_meas() override;
private:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
AP_IRLock_SITL irlock;
#else
AP_IRLock_I2C irlock;
#endif
Vector3f _los_meas_body; // unit vector in body frame pointing towards target
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

37
libraries/AC_PrecLand/AC_PrecLand_SITL.cpp
View File

@ -1,41 +1,32 @@
#include <AP_HAL/AP_HAL.h>
#include "AC_PrecLand_SITL.h"
extern const AP_HAL::HAL& hal;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <stdio.h>
#include "AP_AHRS/AP_AHRS.h"
// init - perform initialisation of this backend
void AC_PrecLand_SITL::init()
{
_sitl = AP::sitl();
}
// update - give chance to driver to get updates from sensor
void AC_PrecLand_SITL::update()
{
const uint32_t now = AP_HAL::millis();
if (_los_meas_time_ms + 10 > now) { // 100Hz update
return;
_state.healthy = _sitl->precland_sim.healthy();
if (_state.healthy && _sitl->precland_sim.last_update_ms() != _los_meas_time_ms) {
const Vector3f position = _sitl->precland_sim.get_target_position();
const Matrix3f &body_to_ned = AP::ahrs().get_rotation_body_to_ned();
_los_meas_body = body_to_ned.mul_transpose(-position);
_los_meas_body /= _los_meas_body.length();
_have_los_meas = true;
_los_meas_time_ms = _sitl->precland_sim.last_update_ms();
} else {
_have_los_meas = false;
}
// get new sensor data; we always point home
Vector3f home;
if (! AP::ahrs().get_relative_position_NED_home(home)) {
_state.healthy = false;
return;
}
if (home.length() > 10.0f) { // we can see the target out to 10 metres
return;
}
_state.healthy = true;
const Matrix3f &body_to_ned = AP::ahrs().get_rotation_body_to_ned();
_los_meas_body = body_to_ned.mul_transpose(-home);
_los_meas_body /= _los_meas_body.length();
_los_meas_time_ms = now;
_have_los_meas = _have_los_meas && AP_HAL::millis() - _los_meas_time_ms <= 1000;
}
bool AC_PrecLand_SITL::have_los_meas() {

5
libraries/AC_PrecLand/AC_PrecLand_SITL.h
View File

@ -1,9 +1,9 @@
#pragma once
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <AP_Common/AP_Common.h>
#include <AP_Math/AP_Math.h>
#include <AC_PrecLand/AC_PrecLand_Backend.h>
#include <SITL/SITL.h>
/*
* AC_PrecLand_SITL - supplies vectors to a fake landing target
@ -33,9 +33,10 @@ public:
bool have_los_meas() override;
private:
SITL::SITL *_sitl; // sitl instance pointer
Vector3f _los_meas_body; // unit vector in body frame pointing towards target
uint32_t _los_meas_time_ms; // system time in milliseconds when los was measured
bool _have_los_meas; // true if there is a valid measurement from the camera
};
#endif

5
libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.h
View File

@ -1,10 +1,9 @@
#pragma once
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <AP_Common/AP_Common.h>
#include <AP_Math/AP_Math.h>
#include <AC_PrecLand/AC_PrecLand_Backend.h>
#include <AP_IRLock/AP_IRLock_SITL.h>
#include <AP_IRLock/AP_IRLock_SITL_Gazebo.h>
/*
* AC_PrecLand_SITL_Gazebo - implements precision landing using target
@ -35,7 +34,7 @@ public:
bool have_los_meas() override;
private:
AP_IRLock_SITL irlock;
AP_IRLock_SITL_Gazebo irlock;
Vector3f _los_meas_body; // unit vector in body frame pointing towards target
bool _have_los_meas; // true if there is a valid measurement from the camera