diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index 9c457c90aa..59514bf9a8 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -67,7 +67,7 @@ void AC_PrecLand::init() _backend = new AC_PrecLand_Companion(*this, _backend_state); break; // IR Lock -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN || CONFIG_HAL_BOARD == HAL_BOARD_SITL case PRECLAND_TYPE_IRLOCK: _backend = new AC_PrecLand_IRLock(*this, _backend_state); break; diff --git a/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp b/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp index 47124e10be..f0fac086fb 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp @@ -5,7 +5,7 @@ extern const AP_HAL::HAL& hal; // this only builds for PX4 so far -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN || CONFIG_HAL_BOARD == HAL_BOARD_SITL // Constructor AC_PrecLand_IRLock::AC_PrecLand_IRLock(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state) diff --git a/libraries/AC_PrecLand/AC_PrecLand_IRLock.h b/libraries/AC_PrecLand/AC_PrecLand_IRLock.h index 2d792bf86a..f8e783cc39 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_IRLock.h +++ b/libraries/AC_PrecLand/AC_PrecLand_IRLock.h @@ -6,8 +6,12 @@ #include #include +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#include +#endif + // this only builds for PX4 so far -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN || CONFIG_HAL_BOARD == HAL_BOARD_SITL /* * AC_PrecLand_IRLock - implements precision landing using target vectors provided @@ -41,8 +45,12 @@ public: void handle_msg(mavlink_message_t* msg) {}; private: +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + AP_IRLock_SITL irlock; +#else AP_IRLock_PX4 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 diff --git a/libraries/AP_IRLock/AP_IRLock.h b/libraries/AP_IRLock/AP_IRLock.h index 960227be74..dad8fb7f02 100644 --- a/libraries/AP_IRLock/AP_IRLock.h +++ b/libraries/AP_IRLock/AP_IRLock.h @@ -10,3 +10,7 @@ #include "IRLock.h" #include "AP_IRLock_PX4.h" + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#include "AP_IRLock_SITL.h" +#endif diff --git a/libraries/AP_IRLock/AP_IRLock_SITL.cpp b/libraries/AP_IRLock/AP_IRLock_SITL.cpp new file mode 100644 index 0000000000..021f677493 --- /dev/null +++ b/libraries/AP_IRLock/AP_IRLock_SITL.cpp @@ -0,0 +1,91 @@ +/* + 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.h" + +#include +#include + +#include + +extern const AP_HAL::HAL& hal; + +AP_IRLock_SITL::AP_IRLock_SITL() : + _last_timestamp(0), + sock(true) +{} + +void AP_IRLock_SITL::init() +{ + // 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", 9005); + + 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::update() +{ + // return immediately if not healthy + if (!_flags.healthy) { + return false; + } + + // receive packet from Gazebo IRLock plugin + irlock_packet pkt; + const int wait_ms = 0; + size_t s = sock.recv(&pkt, sizeof(irlock_packet), wait_ms); + + // TODO: need to fix this + // for some reason, pkt.num_targets occasionally gets a bad number. + // hardcode _num_targets to 1 for now. + // _num_targets = pkt.num_targets; + _num_targets = 1; + + for (uint16_t i=0; i<_num_targets; ++i) { + // fprintf(stderr, "sitl %d %d\n", i, _num_targets); + 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[i].timestamp = pkt.timestamp; + _target_info[i].pos_x = pkt.pos_x; + _target_info[i].pos_y = pkt.pos_y; + _target_info[i].size_x = pkt.size_x; + _target_info[i].size_y = pkt.size_y; + _last_timestamp = pkt.timestamp; + _last_update_ms = AP_HAL::millis(); + } + } + + // return true if new data found + return (_num_targets > 0); +} + +#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 new file mode 100644 index 0000000000..f7097d67ce --- /dev/null +++ b/libraries/AP_IRLock/AP_IRLock_SITL.h @@ -0,0 +1,40 @@ +/* + * AP_IRLock_SITL.h + * + * Created on: June 09, 2016 + * Author: Ian Chen + */ +#pragma once + +#include + +#include "IRLock.h" + +class AP_IRLock_SITL : public IRLock +{ +public: + AP_IRLock_SITL(); + + // init - initialize sensor library + virtual void init(); + + // retrieve latest sensor data - returns true if new data is available + virtual bool update(); + +private: + + /* + reply packet sent from simulator to ArduPilot + */ + struct irlock_packet { + uint64_t timestamp; + uint16_t num_targets; + float pos_x; + float pos_y; + float size_x; + float size_y; + }; + + uint64_t _last_timestamp; + SocketAPM sock; +};