From e63a9bb7e9f58e8c70b9396158fd73ad31e38b04 Mon Sep 17 00:00:00 2001 From: Maheedhar Korimi Date: Thu, 28 Nov 2024 16:45:38 +0000 Subject: [PATCH] Addition of NED frame --- instlscrip.sh | 35 +++++++++++++++++++ libraries/AC_PrecLand/AC_PrecLand.cpp | 16 +++++++-- libraries/AC_PrecLand/AC_PrecLand_Backend.h | 3 ++ .../AC_PrecLand/AC_PrecLand_Companion.cpp | 10 +++++- libraries/AC_PrecLand/AC_PrecLand_Companion.h | 5 +++ 5 files changed, 66 insertions(+), 3 deletions(-) create mode 100755 instlscrip.sh diff --git a/instlscrip.sh b/instlscrip.sh new file mode 100755 index 00000000000000..af52c66a5ad4e3 --- /dev/null +++ b/instlscrip.sh @@ -0,0 +1,35 @@ +#!/bin/bash + +VER=15:10.3-2021.10-9 +URL=https://developer.arm.com/-/media/Files/downloads/gnu-rm/10.3-2021.10/gcc-arm-none-eabi-10.3-2021.10-x86_64-linux.tar.bz2 +echo "Creating gcc-arm-none-eabi debian package version $VER" + +echo "Entering temporary directory..." +cd /tmp + +echo "Downloading..." +curl -fSL -A "Mozilla/4.0" -o gcc-arm-none-eabi.tar "$URL" + +echo "Extracting..." +tar -xf gcc-arm-none-eabi.tar +rm gcc-arm-none-eabi.tar + +echo "Generating debian package..." +mkdir gcc-arm-none-eabi +mkdir gcc-arm-none-eabi/DEBIAN +mkdir gcc-arm-none-eabi/usr +echo "Package: gcc-arm-none-eabi" > gcc-arm-none-eabi/DEBIAN/control +echo "Version: $VER" >> gcc-arm-none-eabi/DEBIAN/control +echo "Architecture: amd64" >> gcc-arm-none-eabi/DEBIAN/control +echo "Maintainer: maintainer" >> gcc-arm-none-eabi/DEBIAN/control +echo "Description: Arm Embedded toolchain" >> gcc-arm-none-eabi/DEBIAN/control +mv gcc-arm-none-eabi-*/* gcc-arm-none-eabi/usr/ +dpkg-deb --build --root-owner-group gcc-arm-none-eabi + +echo "Installing..." +sudo apt install ./gcc-arm-none-eabi.deb -y --allow-downgrades + +echo "Removing temporary files..." +rm -r gcc-arm-none-eabi* + +echo "Done." diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index ece865343688d9..d703bf889134c6 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -650,8 +650,20 @@ bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m, const struct inertial_data_frame_s *inertial_data_delayed = (*_inertial_history)[0]; const bool target_vec_valid = target_vec_unit_body.projected(_approach_vector_body).dot(_approach_vector_body) > 0.0f; - const Vector3f target_vec_unit_ned = inertial_data_delayed->Tbn * target_vec_unit_body; - const Vector3f approach_vector_NED = inertial_data_delayed->Tbn * _approach_vector_body; + + // Check for FRD or NED frame of reference + Vector3f target_vec_unit_ned; + Vector3f approach_vector_NED; + + if (_backend->get_frame() == MAV_FRAME_BODY_FRD) { + target_vec_unit_ned = inertial_data_delayed->Tbn * target_vec_unit_body; + approach_vector_NED = inertial_data_delayed->Tbn * _approach_vector_body; + } + else if (_backend->get_frame() == MAV_FRAME_LOCAL_NED) { + target_vec_unit_ned = target_vec_unit_body; + approach_vector_NED = _approach_vector_body; + } + const bool alt_valid = (rangefinder_alt_valid && rangefinder_alt_m > 0.0f) || (_backend->distance_to_target() > 0.0f); if (target_vec_valid && alt_valid) { // distance to target and distance to target along approach vector diff --git a/libraries/AC_PrecLand/AC_PrecLand_Backend.h b/libraries/AC_PrecLand/AC_PrecLand_Backend.h index 41facd0deb4a01..7e405e9d608d53 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Backend.h +++ b/libraries/AC_PrecLand/AC_PrecLand_Backend.h @@ -33,6 +33,9 @@ class AC_PrecLand_Backend // returns system time in milliseconds of last los measurement virtual uint32_t los_meas_time_ms() = 0; + // return mav frame, FRD by default + virtual uint8_t get_frame() { return MAV_FRAME_BODY_FRD; }; + // return true if there is a valid los measurement available virtual bool have_los_meas() = 0; diff --git a/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp b/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp index 51d46feb777fec..6121166fed6f97 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp @@ -30,6 +30,12 @@ bool AC_PrecLand_Companion::get_los_body(Vector3f& ret) { return false; } +// return true if there is a valid los measurement available +uint8_t AC_PrecLand_Companion::get_frame() +{ + return _frame; +} + // returns system time in milliseconds of last los measurement uint32_t AC_PrecLand_Companion::los_meas_time_ms() { return _los_meas_time_ms; @@ -52,7 +58,9 @@ void AC_PrecLand_Companion::handle_msg(const mavlink_landing_target_t &packet, u _distance_to_target = packet.distance; if (packet.position_valid == 1) { - if (packet.frame == MAV_FRAME_BODY_FRD) { + _frame = packet.frame; + if ( (_frame == MAV_FRAME_BODY_FRD) + || (_frame == MAV_FRAME_LOCAL_NED)) { if (_distance_to_target > 0) { _los_meas_body = Vector3f(packet.x, packet.y, packet.z); _los_meas_body /= _distance_to_target; diff --git a/libraries/AC_PrecLand/AC_PrecLand_Companion.h b/libraries/AC_PrecLand/AC_PrecLand_Companion.h index 892a5aa1f8265b..b2425f9f45511c 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Companion.h +++ b/libraries/AC_PrecLand/AC_PrecLand_Companion.h @@ -30,6 +30,9 @@ class AC_PrecLand_Companion : public AC_PrecLand_Backend // returns same as have_los_meas() bool get_los_body(Vector3f& ret) override; + // returns mavlink frame + uint8_t get_frame() override; + // returns system time in milliseconds of last los measurement uint32_t los_meas_time_ms() override; @@ -42,6 +45,7 @@ class AC_PrecLand_Companion : public AC_PrecLand_Backend // parses a mavlink message from the companion computer void handle_msg(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) override; + private: float _distance_to_target; // distance from the camera to target in meters @@ -49,6 +53,7 @@ class AC_PrecLand_Companion : public AC_PrecLand_Backend 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 bool _wrong_frame_msg_sent; + uint8_t _frame; // frame parameter (FRD or NED) };