From 42cc9c755aa97067a5a3e0807f1aa692dd01b373 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Tue, 12 Feb 2019 12:08:28 +0100 Subject: [PATCH] AP_IRLock: add SITL IRLock AP_IRLock: simplify include AP_IRLock: reduce scope of gazebo irlock_paquet AP_IRLock: add z pos for IRLock SITL, set to 1.0 for pixycam AP_IRLock: remove unused target size and get_angle_to_target_rad function --- libraries/AP_IRLock/AP_IRLock.h | 1 + libraries/AP_IRLock/AP_IRLock_I2C.cpp | 6 +- libraries/AP_IRLock/AP_IRLock_I2C.h | 1 + libraries/AP_IRLock/AP_IRLock_SITL.cpp | 58 +++-------- libraries/AP_IRLock/AP_IRLock_SITL.h | 21 +--- libraries/AP_IRLock/AP_IRLock_SITL_Gazebo.cpp | 95 +++++++++++++++++++ libraries/AP_IRLock/AP_IRLock_SITL_Gazebo.h | 29 ++++++ libraries/AP_IRLock/IRLock.cpp | 17 +--- libraries/AP_IRLock/IRLock.h | 9 +- 9 files changed, 150 insertions(+), 87 deletions(-) create mode 100644 libraries/AP_IRLock/AP_IRLock_SITL_Gazebo.cpp create mode 100644 libraries/AP_IRLock/AP_IRLock_SITL_Gazebo.h diff --git a/libraries/AP_IRLock/AP_IRLock.h b/libraries/AP_IRLock/AP_IRLock.h index ecfcc5ae8b..34cebcb224 100644 --- a/libraries/AP_IRLock/AP_IRLock.h +++ b/libraries/AP_IRLock/AP_IRLock.h @@ -12,5 +12,6 @@ #include "AP_IRLock_I2C.h" #if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#include "AP_IRLock_SITL_Gazebo.h" #include "AP_IRLock_SITL.h" #endif diff --git a/libraries/AP_IRLock/AP_IRLock_I2C.cpp b/libraries/AP_IRLock/AP_IRLock_I2C.cpp index 56170170ba..4b8da6d0bc 100644 --- a/libraries/AP_IRLock/AP_IRLock_I2C.cpp +++ b/libraries/AP_IRLock/AP_IRLock_I2C.cpp @@ -25,6 +25,7 @@ #include #include #include +#include extern const AP_HAL::HAL& hal; @@ -137,8 +138,7 @@ void AP_IRLock_I2C::read_frames(void) _target_info.timestamp = AP_HAL::millis(); _target_info.pos_x = 0.5f*(corner1_pos_x+corner2_pos_x); _target_info.pos_y = 0.5f*(corner1_pos_y+corner2_pos_y); - _target_info.size_x = corner2_pos_x-corner1_pos_x; - _target_info.size_y = corner2_pos_y-corner1_pos_y; + _target_info.pos_z = 1.0f; } #if 0 @@ -148,7 +148,7 @@ void AP_IRLock_I2C::read_frames(void) lastt = _target_info.timestamp; printf("pos_x:%.5f pos_y:%.5f size_x:%.6f size_y:%.5f\n", _target_info.pos_x, _target_info.pos_y, - _target_info.size_x, _target_info.size_y); + (corner2_pos_x-corner1_pos_x), (corner2_pos_y-corner1_pos_y)); } #endif } diff --git a/libraries/AP_IRLock/AP_IRLock_I2C.h b/libraries/AP_IRLock/AP_IRLock_I2C.h index 29d2ca46f4..55858a259f 100644 --- a/libraries/AP_IRLock/AP_IRLock_I2C.h +++ b/libraries/AP_IRLock/AP_IRLock_I2C.h @@ -5,6 +5,7 @@ #pragma once #include "IRLock.h" +#include class AP_IRLock_I2C : public IRLock { diff --git a/libraries/AP_IRLock/AP_IRLock_SITL.cpp b/libraries/AP_IRLock/AP_IRLock_SITL.cpp index 5352b79df8..f54c483d97 100644 --- a/libraries/AP_IRLock/AP_IRLock_SITL.cpp +++ b/libraries/AP_IRLock/AP_IRLock_SITL.cpp @@ -23,64 +23,36 @@ #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #include "AP_IRLock_SITL.h" -#include -#include -#include - -#include - -extern const AP_HAL::HAL& hal; - -AP_IRLock_SITL::AP_IRLock_SITL() : - _last_timestamp(0), - sock(true) -{} +#include "AP_AHRS/AP_AHRS.h" void AP_IRLock_SITL::init(int8_t bus) { - SITL::SITL *sitl = AP::sitl(); - // try to bind to a specific port so that if we restart ArduPilot - // Gazebo keeps sending us packets. Not strictly necessary but - // useful for debugging - sock.bind("127.0.0.1", sitl->irlock_port); - - sock.reuseaddress(); - sock.set_blocking(false); - - hal.console->printf("AP_IRLock_SITL::init()\n"); - - _flags.healthy = true; + _sitl = AP::sitl(); + _sitl->precland_sim._type.set_and_notify(SITL::SIM_Precland::PreclandType::PRECLAND_TYPE_CONE); } // retrieve latest sensor data - returns true if new data is available bool AP_IRLock_SITL::update() { // return immediately if not healthy + _flags.healthy = _sitl->precland_sim.healthy(); if (!_flags.healthy) { return false; } - // receive packet from Gazebo IRLock plugin - irlock_packet pkt; - const int wait_ms = 0; - ssize_t s = sock.recv(&pkt, sizeof(irlock_packet), wait_ms); - - bool new_data = false; - - if (s == sizeof(irlock_packet) && pkt.timestamp > _last_timestamp) { - // fprintf(stderr, " posx %f posy %f sizex %f sizey %f\n", pkt.pos_x, pkt.pos_y, pkt.size_x, pkt.size_y); - _target_info.timestamp = pkt.timestamp; - _target_info.pos_x = pkt.pos_x; - _target_info.pos_y = pkt.pos_y; - _target_info.size_x = pkt.size_x; - _target_info.size_y = pkt.size_y; - _last_timestamp = pkt.timestamp; + if (_sitl->precland_sim.last_update_ms() != _last_timestamp) { + const Vector3f position = _sitl->precland_sim.get_target_position(); + const Matrix3f &body_to_ned = AP::ahrs().get_rotation_body_to_ned(); + const Vector3f real_position = body_to_ned.mul_transpose(-position); + _last_timestamp = _sitl->precland_sim.last_update_ms(); _last_update_ms = _last_timestamp; - new_data = true; + _target_info.timestamp = _last_timestamp; + _target_info.pos_x = real_position.y; + _target_info.pos_y = -real_position.x; + _target_info.pos_z = real_position.z; + return true; } - - // return true if new data found - return new_data; + return false; } #endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL diff --git a/libraries/AP_IRLock/AP_IRLock_SITL.h b/libraries/AP_IRLock/AP_IRLock_SITL.h index caac3d64bd..1e0624b3e0 100644 --- a/libraries/AP_IRLock/AP_IRLock_SITL.h +++ b/libraries/AP_IRLock/AP_IRLock_SITL.h @@ -6,15 +6,13 @@ */ #pragma once -#include #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #include "IRLock.h" +#include class AP_IRLock_SITL : public IRLock { public: - AP_IRLock_SITL(); - // init - initialize sensor library void init(int8_t bus) override; @@ -22,20 +20,7 @@ public: bool update() override; private: - - /* - reply packet sent from simulator to ArduPilot - */ - struct irlock_packet { - uint64_t timestamp; // in miliseconds - uint16_t num_targets; - float pos_x; - float pos_y; - float size_x; - float size_y; - }; - - uint32_t _last_timestamp; - SocketAPM sock; + SITL::SITL *_sitl; // sitl instance pointer + uint32_t _last_timestamp = 0; }; #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_IRLock/AP_IRLock_SITL_Gazebo.cpp b/libraries/AP_IRLock/AP_IRLock_SITL_Gazebo.cpp new file mode 100644 index 0000000000..efbbc91624 --- /dev/null +++ b/libraries/AP_IRLock/AP_IRLock_SITL_Gazebo.cpp @@ -0,0 +1,95 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * AP_IRLock_SITL.cpp + * + * Created on: June 09, 2016 + * Author: Ian Chen + */ +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#include "AP_IRLock_SITL_Gazebo.h" +#include +#include +#include + +#include + +extern const AP_HAL::HAL& hal; + +AP_IRLock_SITL_Gazebo::AP_IRLock_SITL_Gazebo() : + _last_timestamp(0), + sock(true) +{} + +void AP_IRLock_SITL_Gazebo::init(int8_t bus) +{ + SITL::SITL *sitl = AP::sitl(); + // try to bind to a specific port so that if we restart ArduPilot + // Gazebo keeps sending us packets. Not strictly necessary but + // useful for debugging + sock.bind("127.0.0.1", sitl->irlock_port); + + sock.reuseaddress(); + sock.set_blocking(false); + + hal.console->printf("AP_IRLock_SITL::init()\n"); + + _flags.healthy = true; +} + +// retrieve latest sensor data - returns true if new data is available +bool AP_IRLock_SITL_Gazebo::update() +{ + // return immediately if not healthy + if (!_flags.healthy) { + return false; + } + + // receive packet from Gazebo IRLock plugin + + /* + reply packet sent from simulator to ArduPilot + */ + struct irlock_packet { + uint64_t timestamp; // in miliseconds + uint16_t num_targets; + float pos_x; + float pos_y; + float size_x; + float size_y; + } pkt; + const int wait_ms = 0; + ssize_t s = sock.recv(&pkt, sizeof(irlock_packet), wait_ms); + + bool new_data = false; + + if (s == sizeof(irlock_packet) && pkt.timestamp > _last_timestamp) { + // fprintf(stderr, " posx %f posy %f sizex %f sizey %f\n", pkt.pos_x, pkt.pos_y, pkt.size_x, pkt.size_y); + _target_info.timestamp = pkt.timestamp; + _target_info.pos_x = pkt.pos_x; + _target_info.pos_y = pkt.pos_y; + _last_timestamp = pkt.timestamp; + _last_update_ms = _last_timestamp; + new_data = true; + } + + // return true if new data found + return new_data; +} + +#endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL diff --git a/libraries/AP_IRLock/AP_IRLock_SITL_Gazebo.h b/libraries/AP_IRLock/AP_IRLock_SITL_Gazebo.h new file mode 100644 index 0000000000..b20e9f6241 --- /dev/null +++ b/libraries/AP_IRLock/AP_IRLock_SITL_Gazebo.h @@ -0,0 +1,29 @@ +/* + * AP_IRLock_SITL.h + * + * Created on: June 09, 2016 + * Author: Ian Chen + */ +#pragma once + +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#include "IRLock.h" + +class AP_IRLock_SITL_Gazebo : public IRLock +{ +public: + AP_IRLock_SITL_Gazebo(); + + // init - initialize sensor library + void init(int8_t bus) override; + + // retrieve latest sensor data - returns true if new data is available + bool update() override; + +private: + + uint32_t _last_timestamp; + SocketAPM sock; +}; +#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_IRLock/IRLock.cpp b/libraries/AP_IRLock/IRLock.cpp index 7bdddc15d3..1308e3fac4 100644 --- a/libraries/AP_IRLock/IRLock.cpp +++ b/libraries/AP_IRLock/IRLock.cpp @@ -7,21 +7,6 @@ #include "IRLock.h" -// retrieve body frame x and y angles (in radians) to target -// returns true if data is available -bool IRLock::get_angle_to_target_rad(float &x_angle_rad, float &y_angle_rad) const -{ - // return false if we have no target - if (!_flags.healthy) { - return false; - } - - // use data from first (largest) object - x_angle_rad = atanf(_target_info.pos_x); - y_angle_rad = atanf(_target_info.pos_y); - return true; -} - // retrieve body frame unit vector in direction of target // returns true if data is available bool IRLock::get_unit_vector_body(Vector3f& ret) const @@ -34,7 +19,7 @@ bool IRLock::get_unit_vector_body(Vector3f& ret) const // use data from first (largest) object ret.x = -_target_info.pos_y; ret.y = _target_info.pos_x; - ret.z = 1.0f; + ret.z = _target_info.pos_z; ret /= ret.length(); return true; } diff --git a/libraries/AP_IRLock/IRLock.h b/libraries/AP_IRLock/IRLock.h index 6d888b2568..257310e1d4 100644 --- a/libraries/AP_IRLock/IRLock.h +++ b/libraries/AP_IRLock/IRLock.h @@ -21,7 +21,7 @@ */ #pragma once -#include +#include class IRLock { @@ -42,10 +42,6 @@ public: // retrieve latest sensor data - returns true if new data is available virtual bool update() = 0; - // retrieve body frame x and y angles (in radians) to target - // returns true if data is available - bool get_angle_to_target_rad(float &x_angle_rad, float &y_angle_rad) const; - // retrieve body frame unit vector in direction of target // returns true if data is available bool get_unit_vector_body(Vector3f& ret) const; @@ -64,8 +60,7 @@ protected: uint32_t timestamp; // milliseconds since system start float pos_x; // x-axis distance from center of image to center of target in units of tan(theta) float pos_y; // y-axis distance from center of image to center of target in units of tan(theta) - float size_x; // size of target along x-axis in units of tan(theta) - float size_y; // size of target along y-axis in units of tan(theta) + float pos_z; } irlock_target_info; irlock_target_info _target_info;