From bfcd3bc4257bf53eb029bc9c69c889d8eb2d1de4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 25 Sep 2019 19:55:59 +1000 Subject: [PATCH] Tracker: add ModeGuided --- AntennaTracker/GCS_Mavlink.cpp | 45 ++++++++++++++++++++++++++++++++++ AntennaTracker/GCS_Mavlink.h | 2 ++ AntennaTracker/Tracker.cpp | 3 +++ AntennaTracker/Tracker.h | 2 ++ AntennaTracker/mode.h | 20 +++++++++++++++ AntennaTracker/mode_guided.cpp | 22 +++++++++++++++++ AntennaTracker/system.cpp | 3 +++ 7 files changed, 97 insertions(+) create mode 100644 AntennaTracker/mode_guided.cpp diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index 73e7c3ca0c..1edbdafe31 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -41,6 +41,7 @@ MAV_MODE GCS_MAVLINK_Tracker::base_mode() const case Mode::Number::SCAN: case Mode::Number::SERVOTEST: case Mode::Number::AUTO: + case Mode::Number::GUIDED: _base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what @@ -91,6 +92,46 @@ void GCS_MAVLINK_Tracker::send_nav_controller_output() const 0); } +void GCS_MAVLINK_Tracker::handle_set_attitude_target(const mavlink_message_t &msg) +{ + // decode packet + mavlink_set_attitude_target_t packet; + mavlink_msg_set_attitude_target_decode(&msg, &packet); + + // exit if vehicle is not in Guided mode + if (tracker.mode != &tracker.mode_guided) { + return; + } + + // sanity checks: + if (!is_zero(packet.body_roll_rate)) { + return; + } + if (!(packet.type_mask & (1<<0))) { + // not told to ignore body roll rate + return; + } + if (!(packet.type_mask & (1<<6))) { + // not told to ignore throttle + return; + } + if (packet.type_mask & (1<<7)) { + // told to ignore attitude (we don't allow continuous motion yet) + return; + } + if ((packet.type_mask & (1<<3)) && (packet.type_mask&(1<<4))) { + // told to ignore both pitch and yaw rates - nothing to do?! + return; + } + + const bool use_yaw_rate = !(packet.type_mask & (1<<2)); + + tracker.mode_guided.set_angle( + Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]), + use_yaw_rate, + packet.body_yaw_rate); +} + /* send PID tuning message */ @@ -428,6 +469,10 @@ void GCS_MAVLINK_Tracker::handleMessage(const mavlink_message_t &msg) { switch (msg.msgid) { + case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: + handle_set_attitude_target(msg); + break; + // When mavproxy 'wp sethome' case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: { diff --git a/AntennaTracker/GCS_Mavlink.h b/AntennaTracker/GCS_Mavlink.h index 8d41e3c7b1..23223a8990 100644 --- a/AntennaTracker/GCS_Mavlink.h +++ b/AntennaTracker/GCS_Mavlink.h @@ -38,6 +38,8 @@ private: void handleMessage(const mavlink_message_t &msg) override; bool handle_guided_request(AP_Mission::Mission_Command &cmd) override; void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override; + void handle_set_attitude_target(const mavlink_message_t &msg); + void send_global_position_int() override; MAV_MODE base_mode() const override; diff --git a/AntennaTracker/Tracker.cpp b/AntennaTracker/Tracker.cpp index 61d5baf0f7..9eed1aecea 100644 --- a/AntennaTracker/Tracker.cpp +++ b/AntennaTracker/Tracker.cpp @@ -143,6 +143,9 @@ Mode *Tracker::mode_from_mode_num(const Mode::Number num) case Mode::Number::SCAN: ret = &mode_scan; break; + case Mode::Number::GUIDED: + ret = &mode_guided; + break; case Mode::Number::SERVOTEST: ret = &mode_servotest; break; diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h index 91ad74bfa0..a3aa872031 100644 --- a/AntennaTracker/Tracker.h +++ b/AntennaTracker/Tracker.h @@ -83,6 +83,7 @@ public: friend class GCS_Tracker; friend class Parameters; friend class ModeAuto; + friend class ModeGuided; friend class Mode; Tracker(void); @@ -146,6 +147,7 @@ private: ModeAuto mode_auto; ModeInitialising mode_initialising; ModeManual mode_manual; + ModeGuided mode_guided; ModeScan mode_scan; ModeServoTest mode_servotest; ModeStop mode_stop; diff --git a/AntennaTracker/mode.h b/AntennaTracker/mode.h index cb39b1c7ca..30ec715073 100644 --- a/AntennaTracker/mode.h +++ b/AntennaTracker/mode.h @@ -1,6 +1,7 @@ #pragma once #include +#include class Mode { public: @@ -9,6 +10,7 @@ public: STOP=1, SCAN=2, SERVOTEST=3, + GUIDED=4, AUTO=10, INITIALISING=16 }; @@ -44,6 +46,24 @@ public: void update() override; }; +class ModeGuided : public Mode { +public: + Mode::Number number() const override { return Mode::Number::GUIDED; } + bool requires_armed_servos() const override { return true; } + void update() override; + + void set_angle(const Quaternion &target_att, bool use_yaw_rate, float yaw_rate_rads) { + _target_att = target_att; + _use_yaw_rate = use_yaw_rate; + _yaw_rate_rads = yaw_rate_rads; + } + +private: + Quaternion _target_att; + bool _use_yaw_rate; + float _yaw_rate_rads; +}; + class ModeInitialising : public Mode { public: Mode::Number number() const override { return Mode::Number::INITIALISING; } diff --git a/AntennaTracker/mode_guided.cpp b/AntennaTracker/mode_guided.cpp new file mode 100644 index 0000000000..32b0274dca --- /dev/null +++ b/AntennaTracker/mode_guided.cpp @@ -0,0 +1,22 @@ +#include "mode.h" + +#include "Tracker.h" +#include + +void ModeGuided::update() +{ + static uint32_t last_debug; + const uint32_t now = AP_HAL::millis(); + float target_roll, target_pitch, target_yaw; + _target_att.to_euler(target_roll, target_pitch, target_yaw); + if (now - last_debug > 5000) { + last_debug = now; + gcs().send_text(MAV_SEVERITY_INFO, "target_yaw=%f target_pitch=%f", degrees(target_yaw), degrees(target_pitch)); + } + calc_angle_error(degrees(target_pitch)*100, degrees(target_yaw)*100, false); + float bf_pitch; + float bf_yaw; + convert_ef_to_bf(target_pitch, target_yaw, bf_pitch, bf_yaw); + tracker.update_pitch_servo(bf_pitch); + tracker.update_yaw_servo(bf_yaw); +} diff --git a/AntennaTracker/system.cpp b/AntennaTracker/system.cpp index 11403d684a..cd90e03803 100644 --- a/AntennaTracker/system.cpp +++ b/AntennaTracker/system.cpp @@ -243,6 +243,9 @@ bool Tracker::set_mode(const uint8_t new_mode, const ModeReason reason) case Mode::Number::STOP: fred = &mode_stop; break; + case Mode::Number::GUIDED: + fred = &mode_guided; + break; } if (fred == nullptr) { return false;