diff --git a/Tools/AntennaTracker/AntennaTracker.pde b/Tools/AntennaTracker/AntennaTracker.pde index 0820342d6ed5c2192b6b10128be7563d6fb3be92..56590451dd8e7db72373f06b1a3bfefaec099f84 100644 --- a/Tools/AntennaTracker/AntennaTracker.pde +++ b/Tools/AntennaTracker/AntennaTracker.pde @@ -109,6 +109,8 @@ static struct { bool manual_control_yaw:1; bool manual_control_pitch:1; bool need_altitude_calibration:1; + bool scan_reverse_pitch:1; + bool scan_reverse_yaw:1; } nav_status; static uint32_t start_time_ms; diff --git a/Tools/AntennaTracker/GCS_Mavlink.pde b/Tools/AntennaTracker/GCS_Mavlink.pde index fd7781271bd1f3d9b2a54764264889e437a51cb5..f6462a83cb3b7be0b787d623a4d4bcc3c64b26f9 100644 --- a/Tools/AntennaTracker/GCS_Mavlink.pde +++ b/Tools/AntennaTracker/GCS_Mavlink.pde @@ -44,6 +44,7 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan) case STOP: break; + case SCAN: case AUTO: base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; @@ -897,6 +898,7 @@ mission_failed: switch (packet.custom_mode) { case MANUAL: case STOP: + case SCAN: case AUTO: set_mode((enum ControlMode)packet.custom_mode); break; diff --git a/Tools/AntennaTracker/Parameters.h b/Tools/AntennaTracker/Parameters.h index b6e2d49f66b14c643acb25fadbab21d3ff8ee168..933431ab74475bd6b0be77b370eeda4f1f5db15f 100644 --- a/Tools/AntennaTracker/Parameters.h +++ b/Tools/AntennaTracker/Parameters.h @@ -79,6 +79,7 @@ public: k_param_startup_delay, k_param_BoardConfig, k_param_gps, + k_param_scan_speed, k_param_channel_yaw = 200, k_param_channel_pitch, @@ -109,6 +110,7 @@ public: AP_Float yaw_slew_time; AP_Float pitch_slew_time; AP_Float min_reverse_time; + AP_Float scan_speed; AP_Float start_latitude; AP_Float start_longitude; diff --git a/Tools/AntennaTracker/Parameters.pde b/Tools/AntennaTracker/Parameters.pde index 1348fd1850b80eb18e47018a6e2da6597bbfb861..f72d2d996f5cfd37b3f1c58acc5cdfd93c91e0b3 100644 --- a/Tools/AntennaTracker/Parameters.pde +++ b/Tools/AntennaTracker/Parameters.pde @@ -75,6 +75,14 @@ const AP_Param::Info var_info[] PROGMEM = { // @Range: 0 20 GSCALAR(pitch_slew_time, "PITCH_SLEW_TIME", 2), + // @Param: SCAN_SPEED + // @DisplayName: Speed at which to rotate in scan mode + // @Description: This controls how rapidly the tracker will move the servos in SCAN mode + // @Units: degrees/second + // @Increment: 1 + // @Range: 0 100 + GSCALAR(scan_speed, "SCAN_SPEED", 5), + // @Param: MIN_REVERSE_TIME // @DisplayName: Minimum time to apply a yaw reversal // @Description: When the tracker detects it has reached the limit of servo movement in yaw it will reverse and try moving to the other extreme of yaw. This parameter controls the minimum time it should reverse for. It is used to cope with trackers that have a significant lag in movement to ensure they do move all the way around. diff --git a/Tools/AntennaTracker/defines.h b/Tools/AntennaTracker/defines.h index de8c37589e76c6e6e5aab5b15bd1c7b4be9a0e81..c1b23bef7b8ca4e32421133edbc636e65f161108 100644 --- a/Tools/AntennaTracker/defines.h +++ b/Tools/AntennaTracker/defines.h @@ -56,6 +56,7 @@ enum ControlMode { MANUAL=0, STOP=1, + SCAN=2, AUTO=10, INITIALISING=16 }; diff --git a/Tools/AntennaTracker/system.pde b/Tools/AntennaTracker/system.pde index 75ea860f1288dd8faa929803de9f79ba634fdb2f..977e611f4b23b0d49137147583dc154ccb495bc3 100644 --- a/Tools/AntennaTracker/system.pde +++ b/Tools/AntennaTracker/system.pde @@ -228,6 +228,7 @@ static void set_mode(enum ControlMode mode) switch (control_mode) { case AUTO: case MANUAL: + case SCAN: arm_servos(); break; diff --git a/Tools/AntennaTracker/tracking.pde b/Tools/AntennaTracker/tracking.pde index 63e736b59c57eb4e5f6b5da3f035a51493a8e76f..d4ce5199779354ce04ca5f5b47e3026881f1de9c 100644 --- a/Tools/AntennaTracker/tracking.pde +++ b/Tools/AntennaTracker/tracking.pde @@ -207,6 +207,38 @@ static void update_manual(void) channel_pitch.output(); } +/* + control servos for SCAN mode + */ +static void update_scan(void) +{ + if (!nav_status.manual_control_yaw) { + float yaw_delta = g.scan_speed * 0.02f; + nav_status.bearing += yaw_delta * (nav_status.scan_reverse_yaw?-1:1); + if (nav_status.bearing < 0 && nav_status.scan_reverse_yaw) { + nav_status.scan_reverse_yaw = false; + } + if (nav_status.bearing > 360 && !nav_status.scan_reverse_yaw) { + nav_status.scan_reverse_yaw = true; + } + nav_status.bearing = constrain_float(nav_status.bearing, 0, 360); + } + + if (!nav_status.manual_control_pitch) { + float pitch_delta = g.scan_speed * 0.02f; + nav_status.pitch += pitch_delta * (nav_status.scan_reverse_pitch?-1:1); + if (nav_status.pitch < -90 && nav_status.scan_reverse_pitch) { + nav_status.scan_reverse_pitch = false; + } + if (nav_status.pitch > 90 && !nav_status.scan_reverse_pitch) { + nav_status.scan_reverse_pitch = true; + } + nav_status.pitch = constrain_float(nav_status.pitch, -90, 90); + } + + update_auto(); +} + /** main antenna tracking code, called at 50Hz @@ -230,10 +262,10 @@ static void update_tracking(void) float pitch = degrees(atan2f(nav_status.altitude_difference, distance)); // update nav_status for NAV_CONTROLLER_OUTPUT - if (!nav_status.manual_control_yaw) { + if (control_mode != SCAN && !nav_status.manual_control_yaw) { nav_status.bearing = bearing; } - if (!nav_status.manual_control_pitch) { + if (control_mode != SCAN && !nav_status.manual_control_pitch) { nav_status.pitch = pitch; } nav_status.distance = distance; @@ -247,6 +279,10 @@ static void update_tracking(void) update_manual(); break; + case SCAN: + update_scan(); + break; + case STOP: case INITIALISING: break;