From 6d7b196212882a82b7097b5df49deba7521b6576 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Wed, 16 Oct 2019 20:49:06 -0700 Subject: [PATCH] Tracker: Support new AP_Vehicle::set_mode --- AntennaTracker/GCS_Mavlink.cpp | 19 +------------------ AntennaTracker/GCS_Mavlink.h | 2 -- AntennaTracker/Log.cpp | 2 +- AntennaTracker/Tracker.h | 3 ++- AntennaTracker/control_servo_test.cpp | 2 +- AntennaTracker/defines.h | 7 ------- AntennaTracker/system.cpp | 24 +++++++++++++++++++----- 7 files changed, 24 insertions(+), 35 deletions(-) diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index a16d9b669e..059059b659 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -406,7 +406,7 @@ MAV_RESULT GCS_MAVLINK_Tracker::handle_command_long_packet(const mavlink_command // mavproxy/mavutil sends this when auto command is entered case MAV_CMD_MISSION_START: - tracker.set_mode(AUTO, MODE_REASON_GCS_COMMAND); + tracker.set_mode(AUTO, ModeReason::GCS_COMMAND); return MAV_RESULT_ACCEPTED; default: @@ -607,23 +607,6 @@ void Tracker::mavlink_delay_cb() logger.EnableWrites(true); } -/* - set_mode() wrapper for MAVLink SET_MODE - */ -bool GCS_MAVLINK_Tracker::set_mode(uint8_t mode) -{ - switch (mode) { - case AUTO: - case MANUAL: - case SCAN: - case SERVO_TEST: - case STOP: - tracker.set_mode((enum ControlMode)mode, MODE_REASON_GCS_COMMAND); - return true; - } - return false; -} - // send position tracker is using void GCS_MAVLINK_Tracker::send_global_position_int() { diff --git a/AntennaTracker/GCS_Mavlink.h b/AntennaTracker/GCS_Mavlink.h index b4be60801a..4c3c0baf0b 100644 --- a/AntennaTracker/GCS_Mavlink.h +++ b/AntennaTracker/GCS_Mavlink.h @@ -18,8 +18,6 @@ protected: uint8_t sysid_my_gcs() const override; - bool set_mode(uint8_t mode) override; - MAV_RESULT _handle_command_preflight_calibration_baro() override; MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override; diff --git a/AntennaTracker/Log.cpp b/AntennaTracker/Log.cpp index 2549edadd6..fdbf4361c9 100644 --- a/AntennaTracker/Log.cpp +++ b/AntennaTracker/Log.cpp @@ -78,7 +78,7 @@ const struct LogStructure Tracker::log_structure[] = { void Tracker::Log_Write_Vehicle_Startup_Messages() { - logger.Write_Mode(control_mode, MODE_REASON_INITIALISED); + logger.Write_Mode(control_mode, ModeReason::INITIALISED); gps.Write_AP_Logger_Log_Startup_messages(); } diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h index 1fb6d64ada..1cf1a20f5a 100644 --- a/AntennaTracker/Tracker.h +++ b/AntennaTracker/Tracker.h @@ -246,7 +246,8 @@ private: void arm_servos(); void disarm_servos(); void prepare_servos(); - void set_mode(enum ControlMode mode, mode_reason_t reason); + void set_mode(enum ControlMode mode, ModeReason reason); + bool set_mode(const uint8_t new_mode, const ModeReason reason) override; bool should_log(uint32_t mask); bool start_command_callback(const AP_Mission::Mission_Command& cmd) { return false; } void exit_mission_callback() { return; } diff --git a/AntennaTracker/control_servo_test.cpp b/AntennaTracker/control_servo_test.cpp index 1238e30f7b..3b635feaa3 100644 --- a/AntennaTracker/control_servo_test.cpp +++ b/AntennaTracker/control_servo_test.cpp @@ -20,7 +20,7 @@ bool Tracker::servo_test_set_servo(uint8_t servo_num, uint16_t pwm) // ensure we are in servo test mode if (control_mode != SERVO_TEST) { - set_mode(SERVO_TEST, MODE_REASON_SERVOTEST); + set_mode(SERVO_TEST, ModeReason::SERVOTEST); } // set yaw servo pwm and send output to servo diff --git a/AntennaTracker/defines.h b/AntennaTracker/defines.h index 5e3d05f1d8..2403176a58 100644 --- a/AntennaTracker/defines.h +++ b/AntennaTracker/defines.h @@ -24,13 +24,6 @@ enum AltSource { ALT_SOURCE_GPS_VEH_ONLY=2 }; -enum mode_reason_t { - MODE_REASON_INITIALISED = 0, - MODE_REASON_STARTUP, - MODE_REASON_SERVOTEST, - MODE_REASON_GCS_COMMAND, -}; - enum class PWMDisarmed { ZERO = 0, TRIM, diff --git a/AntennaTracker/system.cpp b/AntennaTracker/system.cpp index bd702dbb5d..e3ffefef73 100644 --- a/AntennaTracker/system.cpp +++ b/AntennaTracker/system.cpp @@ -110,20 +110,20 @@ void Tracker::init_tracker() switch (g.initial_mode) { case MANUAL: - set_mode(MANUAL, MODE_REASON_STARTUP); + set_mode(MANUAL, ModeReason::STARTUP); break; case SCAN: - set_mode(SCAN, MODE_REASON_STARTUP); + set_mode(SCAN, ModeReason::STARTUP); break; case STOP: - set_mode(STOP, MODE_REASON_STARTUP); + set_mode(STOP, ModeReason::STARTUP); break; case AUTO: default: - set_mode(AUTO, MODE_REASON_STARTUP); + set_mode(AUTO, ModeReason::STARTUP); break; } @@ -214,7 +214,7 @@ void Tracker::prepare_servos() SRV_Channels::output_ch_all(); } -void Tracker::set_mode(enum ControlMode mode, mode_reason_t reason) +void Tracker::set_mode(enum ControlMode mode, ModeReason reason) { if (control_mode == mode) { // don't switch modes if we are already in the correct mode. @@ -243,6 +243,20 @@ void Tracker::set_mode(enum ControlMode mode, mode_reason_t reason) nav_status.bearing = ahrs.yaw_sensor * 0.01f; } +bool Tracker::set_mode(const uint8_t new_mode, const ModeReason reason) +{ + switch (new_mode) { + case AUTO: + case MANUAL: + case SCAN: + case SERVO_TEST: + case STOP: + set_mode((enum ControlMode)new_mode, reason); + return true; + } + return false; +} + /* should we log a message type now? */