From 9cdc072cc3e0c7c8311e0f4e5bbccc00a2533bec Mon Sep 17 00:00:00 2001 From: Anthony Good Date: Fri, 24 Jul 2020 21:24:34 -0400 Subject: [PATCH] 2020.07.24.01 After pulling my hair out for two days, I rewrote the satellite tracking to use the P13 library from Mark VandeWettering \^ command to activate and deactive satellite tracking \~ command to view satellite tracking status --- .../k3ng_rotator_controller.ino | 1057 ++++------------- k3ng_rotator_controller/rotator.h | 7 +- .../rotator_debug_log_activation.h | 1 + k3ng_rotator_controller/rotator_pins.h | 6 + .../rotator_pins_ea4tx_ars_usb.h | 5 + .../rotator_pins_k3ng_g1000.h | 7 +- k3ng_rotator_controller/rotator_pins_m0upu.h | 8 +- k3ng_rotator_controller/rotator_pins_test.h | 5 + k3ng_rotator_controller/rotator_pins_wb6kcn.h | 5 + .../rotator_pins_wb6kcn_az_test_setup.h | 4 + k3ng_rotator_controller/rotator_settings.h | 8 + .../rotator_settings_ea4tx_ars_usb.h | 8 + .../rotator_settings_m0upu.h | 10 +- .../rotator_settings_test.h | 8 + .../rotator_settings_wb6kcn.h | 8 + libraries/P13/P13.cpp | 456 +++++++ libraries/P13/P13.h | 195 +++ libraries/P13/README | 108 ++ 18 files changed, 1097 insertions(+), 809 deletions(-) create mode 100755 libraries/P13/P13.cpp create mode 100755 libraries/P13/P13.h create mode 100755 libraries/P13/README diff --git a/k3ng_rotator_controller/k3ng_rotator_controller.ino b/k3ng_rotator_controller/k3ng_rotator_controller.ino index 1c59f9d..03700e7 100644 --- a/k3ng_rotator_controller/k3ng_rotator_controller.ino +++ b/k3ng_rotator_controller/k3ng_rotator_controller.ino @@ -68,21 +68,11 @@ - All copyrights are the property of their respective owners - - Satellite tracking algorithm originally created by James Miller, G3RUH - https://www.amsat.org/amsat/articles/g3ruh/111.html - - Later ported to the Arduino by VE9QRP http://ve9qrp.blogspot.com - in the QRPtracker Project https://sites.google.com/site/qrptracker/, - code found at https://github.com/BackupGGCode/qrptracker. - - The satellite tracking in this code is based on both works. - Full documentation is currently located here: https://github.com/k3ng/k3ng_rotator_controller/wiki + Full documentation is currently located here: https://github.com/k3ng/k3ng_rotator_controller/wiki Rules for using this code: @@ -605,6 +595,11 @@ Lots more work on FEATURE_SATELLITE_TRACKING. It's only outputting AO-7 in debug logs right now. Pushing to Github in case I get hit by a bus before I finish this. + 2020.07.24.01 + After pulling my hair out for two days, I rewrote the satellite tracking to use the P13 library from Mark VandeWettering + \^ command to activate and deactive satellite tracking + \~ command to view satellite tracking status + All library files should be placed in directories likes \sketchbook\libraries\library1\ , \sketchbook\libraries\library2\ , etc. Anything rotator_*.* should be in the ino directory! @@ -616,7 +611,7 @@ */ -#define CODE_VERSION "2020.07.23.01" +#define CODE_VERSION "2020.07.24.01" #include #include @@ -1248,6 +1243,9 @@ DebugClass debug; #if defined(FEATURE_SATELLITE_TRACKING) + #include + +/* const double YM = 365.25; // Days in a year const double YT = 365.2421970; // Tropical year, days const double WW = 2.0 * M_PI / 365.2421970; // Earth's rotation rate, rads/whole day @@ -1281,35 +1279,24 @@ DebugClass debug; //const double RE = 6378.140; //const double FL = 1.0 / 298.257; + unsigned long current_satellite_rx_freq; + unsigned long current_satellite_tx_freq; + +*/ + double current_satellite_elevation; double current_satellite_azimuth; double current_satellite_longitude; double current_satellite_latitude; - unsigned long current_satellite_rx_freq; - unsigned long current_satellite_tx_freq; + byte satellite_tracking_active = 0; + byte satellite_visible = 0; //zzzzzz - struct kep{ - char satellite[16]; - long satellite_number; - double YE; - double TE; - double IN; - double RA; - double EC; - double WP; - double MA; - double MM; - double M2; - double RV; - double ALON; - double ALAT; - unsigned long rxFrequency; - unsigned long txFrequency; - }; + Satellite sat; + Observer obs("my_location", DEFAULT_LATITUDE, DEFAULT_LONGITUDE, DEFAULT_ALTITUDE_M); + SatDateTime sat_datetime; - kep current_satellite; #endif //FEATURE_SATELLITE_TRACKING @@ -3886,8 +3873,11 @@ void check_buttons(){ perform_screen_redraw = 1; #endif #ifdef FEATURE_SUN_TRACKING - sun_tracking_active = 0; - #endif // FEATURE_SUN_TRACKING + sun_tracking_active = 0; + #endif // FEATURE_SUN_TRACKING + #ifdef FEATURE_SATELLITE_TRACKING + satellite_tracking_active = 0; + #endif // FEATURE_SATELLITE_TRACKING } else { #ifdef DEBUG_BUTTONS debug.println("check_buttons: moon tracking off"); @@ -3909,7 +3899,7 @@ void check_buttons(){ static byte sun_tracking_button_pushed = 0; static unsigned long last_time_sun_tracking_button_pushed = 0; if (sun_tracking_button) { - if ((digitalReadEnhanced(sun_tracking_button) == BUTTON_ACTIVE_STATE)) { + if (digitalReadEnhanced(sun_tracking_button) == BUTTON_ACTIVE_STATE) { sun_tracking_button_pushed = 1; last_time_sun_tracking_button_pushed = millis(); #ifdef DEBUG_BUTTONS @@ -3927,7 +3917,10 @@ void check_buttons(){ #endif #ifdef FEATURE_MOON_TRACKING moon_tracking_active = 0; - #endif // FEATURE_MOON_TRACKING + #endif // FEATURE_MOON_TRACKING + #ifdef FEATURE_SATELLITE_TRACKING + satellite_tracking_active = 0; + #endif // FEATURE_SATELLITE_TRACKING } else { #ifdef DEBUG_BUTTONS debug.print("check_buttons: sun tracking off"); @@ -3944,6 +3937,50 @@ void check_buttons(){ } #endif // FEATURE_SUN_TRACKING + + #ifdef FEATURE_SATELLITE_TRACKING + static byte satellite_tracking_button_pushed = 0; + static unsigned long last_time_satellite_tracking_button_pushed = 0; + if (satellite_tracking_button) { + if (digitalReadEnhanced(satellite_tracking_button) == BUTTON_ACTIVE_STATE) { + satellite_tracking_button_pushed = 1; + last_time_satellite_tracking_button_pushed = millis(); + #ifdef DEBUG_BUTTONS + debug.println("check_buttons: satellite_tracking_button pushed"); + #endif // DEBUG_BUTTONS + } else { + if ((satellite_tracking_button_pushed) && ((millis() - last_time_satellite_tracking_button_pushed) >= 250)) { + if (!satellite_tracking_active) { + #ifdef DEBUG_BUTTONS + debug.println("check_buttons: sun tracking on"); + #endif // DEBUG_BUTTONS + satellite_tracking_active = 1; + #if defined(FEATURE_LCD_DISPLAY) + perform_screen_redraw = 1; + #endif + #ifdef FEATURE_MOON_TRACKING + moon_tracking_active = 0; + #endif // FEATURE_MOON_TRACKING + #ifdef FEATURE_SUN_TRACKING + sun_tracking_active = 0; + #endif // FEATURE_SUN_TRACKING + } else { + #ifdef DEBUG_BUTTONS + debug.print("check_buttons: sun tracking off"); + #endif // DEBUG_BUTTONS + satellite_tracking_active = 0; + submit_request(AZ, REQUEST_STOP, 0, DBG_CHECK_BUTTONS_SATELLITE); + #ifdef FEATURE_ELEVATION_CONTROL + submit_request(EL, REQUEST_STOP, 0, DBG_CHECK_BUTTONS_SATELLITE); + #endif + } + satellite_tracking_button_pushed = 0; + } + } + } + #endif // FEATURE_SATELLITE_TRACKING + + #ifdef DEBUG_PROCESSES service_process_debug(DEBUG_PROCESSES_PROCESS_EXIT,PROCESS_CHECK_BUTTONS); #endif @@ -7647,8 +7684,8 @@ void output_debug(){ #endif //DEBUG_AUTOCORRECT #if defined(FEATURE_SATELLITE_TRACKING) - debug.print("\tSat "); - debug.print(current_satellite.satellite); + debug.print("\tSat:"); + debug.print(sat.name); debug.print(" AZ:"); debug.print(current_satellite_azimuth); debug.print(" EL:"); @@ -7656,7 +7693,17 @@ void output_debug(){ debug.print(" Lat:"); debug.print(current_satellite_latitude); debug.print(" Lon:"); - debug.println(current_satellite_longitude); + debug.print(current_satellite_longitude); + if (satellite_visible) { + debug.print(" AOS"); + } else { + debug.print(" LOS"); + } + debug.print(" TRACKING_"); + if (!satellite_tracking_active) { + debug.print("IN"); + } + debug.println("ACTIVE "); #endif debug.print("\tCONFIG_"); @@ -13560,8 +13607,8 @@ byte process_backslash_command(byte input_buffer[], int input_buffer_index, byte switch (input_buffer[2]) { case '0': moon_tracking_active = 0; - submit_request(AZ, REQUEST_STOP, 0, 17); - submit_request(EL, REQUEST_STOP, 0, 18); + submit_request(AZ, REQUEST_STOP, 0, DBG_SERVICE_MOON_CLI_CMD); + submit_request(EL, REQUEST_STOP, 0, DBG_SERVICE_MOON_CLI_CMD); strcpy(return_string, "Moon tracking deactivated."); break; case '1': @@ -13569,6 +13616,9 @@ byte process_backslash_command(byte input_buffer[], int input_buffer_index, byte #ifdef FEATURE_SUN_TRACKING sun_tracking_active = 0; #endif // FEATURE_SUN_TRACKING + #ifdef FEATURE_SATELLITE_TRACKING + satellite_tracking_active = 0; + #endif // FEATURE_SATELLITE_TRACKING strcpy(return_string, "Moon tracking activated."); break; default: strcpy(return_string, "Error."); break; @@ -13639,16 +13689,19 @@ byte process_backslash_command(byte input_buffer[], int input_buffer_index, byte switch (input_buffer[2]) { case '0': sun_tracking_active = 0; - submit_request(AZ, REQUEST_STOP, 0, 19); - submit_request(EL, REQUEST_STOP, 0, 20); + submit_request(AZ, REQUEST_STOP, 0, DBG_SERVICE_SATELLITE_CLI_CMD); + submit_request(EL, REQUEST_STOP, 0, DBG_SERVICE_SATELLITE_CLI_CMD); strcpy(return_string, "Sun tracking deactivated."); break; case '1': sun_tracking_active = 1; strcpy(return_string, "Sun tracking activated."); #ifdef FEATURE_MOON_TRACKING - moon_tracking_active = 0; + moon_tracking_active = 0; #endif // FEATURE_MOON_TRACKING + #ifdef FEATURE_SATELLITE_TRACKING + satellite_tracking_active = 0; + #endif // FEATURE_SATELLITE_TRACKING break; default: strcpy(return_string, "Error."); break; } @@ -13856,6 +13909,65 @@ byte process_backslash_command(byte input_buffer[], int input_buffer_index, byte configuration_dirty = 1; break; + #if defined(FEATURE_SATELLITE_TRACKING) + case '~': + control_port->print("Satellite:"); + control_port->print(sat.name); + control_port->print(" Location:"); + control_port->print(obs.name); + control_port->print(" ("); + control_port->print(obs.LA); + control_port->print(","); + control_port->print(obs.LO); + control_port->print(","); + control_port->print(obs.HT); + control_port->print(") AZ:"); + control_port->print(current_satellite_azimuth); + control_port->print(" EL:"); + control_port->print(current_satellite_elevation); + control_port->print(" Lat:"); + control_port->print(current_satellite_latitude); + control_port->print(" Long:"); + control_port->print(current_satellite_longitude); + if (satellite_visible) { + control_port->print(" AOS"); + } else { + control_port->print(" LOS"); + } + control_port->print(" TRACKING_"); + if (!satellite_tracking_active) { + control_port->print("IN"); + } + control_port->println("ACTIVE "); + +//zzzzzz + break; + + case '^': + switch (input_buffer[2]) { + case '0': + satellite_tracking_active = 0; + submit_request(AZ, REQUEST_STOP, 0, DBG_SERVICE_SATELLITE_CLI_CMD); + submit_request(EL, REQUEST_STOP, 0, DBG_SERVICE_SATELLITE_CLI_CMD); + strcpy(return_string, "Satellite tracking deactivated."); + break; + case '1': + satellite_tracking_active = 1; + #ifdef FEATURE_SUN_TRACKING + sun_tracking_active = 0; + #endif // FEATURE_SUN_TRACKING + #ifdef FEATURE_MOON_TRACKING + moon_tracking_active = 0; + #endif // FEATURE_SUN_TRACKING + strcpy(return_string, "Satellite tracking activated."); + break; + default: strcpy(return_string, "Error."); break; + } + break; + + + #endif + // TODO : one big status query command #if !defined(OPTION_SAVE_MEMORY_EXCLUDE_EXTENDED_COMMANDS) @@ -13981,7 +14093,8 @@ byte process_backslash_command(byte input_buffer[], int input_buffer_index, byte } if ((input_buffer[2] == 'P') && (input_buffer[3] == 'G')) { // \?PG - Ping strcpy(return_string, "\\!OKPG"); - } + } + if ((input_buffer[2] == 'R') && (input_buffer[3] == 'L')) { // \?RL - rotate left submit_request(AZ, REQUEST_CCW, 0, 121); strcpy(return_string, "\\!OKRL"); @@ -16317,20 +16430,20 @@ void service_moon_tracking(){ } if ((moon_azimuth >= MOON_AOS_AZIMUTH_MIN) && (moon_azimuth <= MOON_AOS_AZIMUTH_MAX) && (moon_elevation >= MOON_AOS_ELEVATION_MIN) && (moon_elevation <= MOON_AOS_ELEVATION_MAX)) { - if (!moon_visible) { - moon_visible = 1; - #ifdef DEBUG_MOON_TRACKING - debug.println("service_moon_tracking: moon AOS"); - #endif // DEBUG_MOON_TRACKING - } - } else { - if (moon_visible) { - moon_visible = 0; - #ifdef DEBUG_MOON_TRACKING - debug.println("service_moon_tracking: moon loss of AOS"); - #endif // DEBUG_MOON_TRACKING - } - } + if (!moon_visible) { + moon_visible = 1; + #ifdef DEBUG_MOON_TRACKING + debug.println("service_moon_tracking: moon AOS"); + #endif // DEBUG_MOON_TRACKING + } + } else { + if (moon_visible) { + moon_visible = 0; + #ifdef DEBUG_MOON_TRACKING + debug.println("service_moon_tracking: moon loss of AOS"); + #endif // DEBUG_MOON_TRACKING + } + } if ((moon_tracking_active) && ((millis() - last_check) > MOON_TRACKING_CHECK_INTERVAL)) { @@ -16358,7 +16471,7 @@ void service_moon_tracking(){ } /* service_moon_tracking */ - #endif // FEATURE_MOON_TRACKING +#endif // FEATURE_MOON_TRACKING // -------------------------------------------------------------- @@ -16822,761 +16935,97 @@ void convert_polar_to_cartesian(byte coordinate_conversion,double azimuth_in,dou //------------------------------------------------------ #if defined(FEATURE_SATELLITE_TRACKING) -void dump_kep(kep * kep_to_dump){ - - control_port->print("Sat:"); - control_port->println(kep_to_dump->satellite); - control_port->print("YE:"); - control_port->println(kep_to_dump->YE,0); - control_port->print("TE:"); - control_port->println(kep_to_dump->TE,8); - control_port->print("IN:"); - control_port->println(kep_to_dump->IN,4); - control_port->print("RA:"); - control_port->println(kep_to_dump->RA,4); - control_port->print("EC:"); - control_port->println(kep_to_dump->EC,8); - control_port->print("WP:"); - control_port->println(kep_to_dump->WP,4); - control_port->print("MA:"); - control_port->println(kep_to_dump->MA,4); - control_port->print("MM:"); - control_port->println(kep_to_dump->MM,8); - control_port->print("M2:"); - control_port->println(kep_to_dump->M2,8); - control_port->print("RV:"); - control_port->println(kep_to_dump->RV,0); - control_port->print("ALON:"); - control_port->println(kep_to_dump->ALON,1); - -} -#endif - -//------------------------------------------------------ -float char_to_float(char * input_buffer){ - - float temp_float = 0; - byte finished = 0; - float hit_decimal = 0; - byte make_negative = 0; - int x = 0; - - - while (!finished){ - if (input_buffer[x] == '.'){ - hit_decimal = 10; - } else { - if ((input_buffer[x] == ' ') || (input_buffer[x] == 0) || (input_buffer[x] == '\r') || (input_buffer[x] == '\n')){ - finished = 1; - } else { - if(input_buffer[x] == '-'){ - make_negative = 1; - } else { - if (hit_decimal > 0){ - temp_float = temp_float + ((float)(input_buffer[x] - 48) / (float)hit_decimal); - hit_decimal = hit_decimal * 10; - } else { - temp_float = (temp_float * 10) + (input_buffer[x] - 48); - } - } - } - } - x++; - if (x > 254){finished = 1;} - } - - - if (make_negative){temp_float = temp_float * -1.0;} - - return temp_float; - -} - - - -//------------------------------------------------------ -#if defined(FEATURE_SATELLITE_TRACKING) -byte parse_tle(char* line1,char* line2,kep * target_kep_struct,char * satellite_name_in){ - - // 1 28375U 04025K 09232.55636497 -.00000001 00000-0 12469-4 0 4653 - // 2 28375 098.0531 238.4104 0083652 290.6047 068.6188 14.40649734270229 - - // 1 AAAAAU 00 0 0 BBBBB.BBBBBBBB .CCCCCCCC 00000-0 00000-0 0 DDDZ - // 2 AAAAA EEE.EEEE FFF.FFFF GGGGGGG HHH.HHHH III.IIII JJ.JJJJJJJJKKKKKZ - - // KEY: A-CATALOGNUM B-EPOCHTIME C-DECAY D-ELSETNUM E-INCLINATION F-RAAN - // G-ECCENTRICITY H-ARGPERIGEE I-MNANOM J-MNMOTION K-ORBITNUM Z-CHECKSUM - - // YE TE IN RA EC WP MA MM M2 RV ALON - //setElements(2009, 232.55636497, 98.0531, 238.4104, 83652*1.0e-7, 290.6047, 68.6188, 14.40649734, -0.00000001, 27022, 180.0); - - - char temp_char1[16]; - char temp_char2[16]; - - strcpy(target_kep_struct->satellite,satellite_name_in); - - // EPOCHTIME -> YE & TE - strcpy(temp_char1,parse_char_string(line1,4,' ',1)); - - // YE - temp_char2[0] = temp_char1[0]; - temp_char2[1] = temp_char1[1]; - temp_char2[2] = 0; - target_kep_struct->YE = char_to_float(temp_char2) + 2000; - - // TE - for (int x = 0;x < 12;x++){ // only get last 12 chars - temp_char2[x] = temp_char1[x+2]; - } - temp_char2[12] = 0; - target_kep_struct->TE = char_to_float(temp_char2); - - // DECAY -> M2 - strcpy(temp_char1,parse_char_string(line1,5,' ',1)); - target_kep_struct->M2 = char_to_float(temp_char1); - - // INCLINATION -> IN - strcpy(temp_char1,parse_char_string(line2,3,' ',1)); - target_kep_struct->IN = char_to_float(temp_char1); - - // RAAN -> RA - strcpy(temp_char1,parse_char_string(line2,4,' ',1)); - target_kep_struct->RA = char_to_float(temp_char1); - - // ECCENTRICITY -> EC - strcpy(temp_char1,parse_char_string(line2,5,' ',1)); - target_kep_struct->EC = char_to_float(temp_char1) * 1.0e-7; - - // ARGPERIGEE -> WP - strcpy(temp_char1,parse_char_string(line2,6,' ',1)); - target_kep_struct->WP = char_to_float(temp_char1); - - // MNANOM -> MA - strcpy(temp_char1,parse_char_string(line2,7,' ',1)); - target_kep_struct->MA = char_to_float(temp_char1); - - // MNMOTION -> MM - strcpy(temp_char1,parse_char_string(line2,8,' ',1)); - for (int x = 0;x < 11;x++){ // only get first 11 chars (Arduino double & float only has about 6 decimal places of accuracy) - temp_char2[x] = temp_char1[x]; - } - temp_char2[11] = 0; - target_kep_struct->MM = char_to_float(temp_char2); - - // ORBITNUM -> RV - for (int x = 11;x < 16;x++){ // get chars 11 to 15 (starts with 0) - temp_char2[x-11] = temp_char1[x]; - } - temp_char2[5] = 0; - target_kep_struct->RV = char_to_float(temp_char2); - - target_kep_struct->ALON = 180.0; - - - -} - -#endif - - -//------------------------------------------------------ -#if defined(FEATURE_SATELLITE_TRACKING) - char* parse_char_string(char* char_string,byte token_number,char delimiter,byte ignore_consecutive_delimiters){ - - #define PARSE_TEMP_STRING_SIZE 32 - - byte current_token = 1; - byte current_position = 0; - byte temp_char_current_position = 0; - byte last_character_was_delimiter = 0; - byte hit_non_whitespace = 0; - byte finished = 0; - static char temp_char[PARSE_TEMP_STRING_SIZE]; - - while (!finished){ - if (char_string[current_position] == delimiter){ - if (current_token == token_number){ - if ((hit_non_whitespace) || (!ignore_consecutive_delimiters)){ - finished = 1; - } - } else { - if (ignore_consecutive_delimiters){ - if (!last_character_was_delimiter){ - current_token++; - hit_non_whitespace = 0; - } - } else { - current_token++; - hit_non_whitespace = 0; - } - } - last_character_was_delimiter = 1; - } else { - if ((char_string[current_position] == '\r') || (char_string[current_position] == '\n') || (char_string[current_position] == 0)){ - finished = 1; - } else { - if (current_token == token_number){ - temp_char[temp_char_current_position] = char_string[current_position]; - temp_char_current_position++; - if (temp_char_current_position == 20){ - finished = 1; - } - } - last_character_was_delimiter = 0; - hit_non_whitespace = 1; - } - } - current_position++; - if (current_position == 255){ - finished = 1; - } - } // while (!finished){ - - if (temp_char_current_position < PARSE_TEMP_STRING_SIZE){ - temp_char[temp_char_current_position] = 0; - } - - return temp_char; - - } -#endif //FEATURE_SATELLITE_TRACKING - - -//------------------------------------------------------ -#if defined(FEATURE_SATELLITE_TRACKING) - void service_satellite_tracking(){ - #define SATELLITE_TRACKING_SERVICE_INTERVAL_MS 5000 + static unsigned long last_tracking_check = 0; + static unsigned long last_update_satellite_position = 0; + static byte satellite_tracking_activated_by_activate_line = 0; + static byte satellite_tracking_pin_state = 0; - static unsigned long last_sat_update = 0; + if (satellite_tracking_active_pin) { + if ((satellite_tracking_active) && (!satellite_tracking_pin_state)) { + digitalWriteEnhanced(satellite_tracking_active_pin, HIGH); + satellite_tracking_pin_state = 1; + } + if ((!satellite_tracking_active) && (satellite_tracking_pin_state)) { + digitalWriteEnhanced(satellite_tracking_active_pin, LOW); + satellite_tracking_pin_state = 0; + } + } - if ((millis() - last_sat_update) > SATELLITE_TRACKING_SERVICE_INTERVAL_MS){ + if (satellite_tracking_activate_line) { + if ((!satellite_tracking_active) && (!digitalReadEnhanced(satellite_tracking_activate_line))) { + satellite_tracking_active = 1; + satellite_tracking_activated_by_activate_line = 1; + } + if ((satellite_tracking_active) && (digitalReadEnhanced(satellite_tracking_activate_line)) && (satellite_tracking_activated_by_activate_line)) { + satellite_tracking_active = 0; + satellite_tracking_activated_by_activate_line = 0; + submit_request(AZ, REQUEST_STOP, 0, DBG_SERVICE_SATELLITE_TRACKING); + #ifdef FEATURE_ELEVATION_CONTROL + submit_request(EL, REQUEST_STOP, 0, DBG_SERVICE_SATELLITE_TRACKING); + #endif + } + } - // AO-07 July 16, 2020 - //1 07530U 74089B 20198.38798810 -.00000037 00000-0 51953-4 0 9995 - //2 07530 101.8012 167.9693 0011953 310.2360 204.6161 12.53644215089612 - - // 1 AAAAAU 00 0 0 BBBBB.BBBBBBBB .CCCCCCCC 00000-0 00000-0 0 DDDZ - // 2 AAAAA EEE.EEEE FFF.FFFF GGGGGGG HHH.HHHH III.IIII JJ.JJJJJJJJKKKKKZ - // KEY: A-CATALOGNUM B-EPOCHTIME C-DECAY D-ELSETNUM E-INCLINATION F-RAAN - // G-ECCENTRICITY H-ARGPERIGEE I-MNANOM J-MNMOTION K-ORBITNUM Z-CHECKSUM - - // char tle_line_1[70]; - // char tle_line_2[70]; - - // strcpy(tle_line_1,"1 07530U 74089B 20198.38798810 -.00000037 00000-0 51953-4 0 9995"); - // strcpy(tle_line_2,"2 07530 101.8012 167.9693 0011953 310.2360 204.6161 12.53644215089612"); - - - // parse_tle(tle_line_1,tle_line_2,¤t_satellite,"AO-07"); - - strcpy(current_satellite.satellite,"AO-7"); - current_satellite.YE = 2020; - current_satellite.TE = 198.38798810; - current_satellite.IN = 101.8012; - current_satellite.RA = 167.9693; - current_satellite.EC = 11953*1.0e-7; - current_satellite.WP = 310.2360; - current_satellite.MA = 204.6161; - current_satellite.MM = 12.53644215; - current_satellite.M2 = -0.00000037; - current_satellite.RV = 8961; - current_satellite.ALON = 180.0; - current_satellite.ALAT = 0; - current_satellite.rxFrequency = 0; - current_satellite.txFrequency = 0; - - //zzzzzz - - - //printElements(); - satellite_calculate(¤t_satellite_azimuth,¤t_satellite_elevation,¤t_satellite_latitude,¤t_satellite_longitude, - clock_years,clock_months,clock_days,clock_hours,clock_minutes,clock_seconds, - latitude, longitude, altitude_m, - ¤t_satellite_rx_freq, ¤t_satellite_tx_freq, - ¤t_satellite); - - - - // Serial.println(); - // printdata(); - // Serial.println(); - - last_sat_update = millis(); + if ((millis() - last_update_satellite_position) > SATELLITE_UPDATE_POSITION_INTERVAL_MS){ + sat.tle("AO-7","1 07530U 74089B 20205.48277872 -.00000043 00000-0 13494-4 0 9990", + "2 07530 101.8018 175.0260 0011861 296.2265 184.6086 12.53644244090508"); + sat_datetime.settime(clock_years, clock_months, clock_days, clock_hours, clock_minutes, clock_seconds); + obs.LA = latitude; + obs.LO = longitude; + obs.HT = altitude_m; + sat.predict(sat_datetime); + sat.LL(current_satellite_latitude,current_satellite_longitude); + sat.altaz(obs, current_satellite_elevation, current_satellite_azimuth); + last_update_satellite_position = millis(); } + + if ((current_satellite_azimuth >= SATELLITE_AOS_AZIMUTH_MIN) && (current_satellite_azimuth <= SATELLITE_AOS_AZIMUTH_MAX) && (current_satellite_elevation >= SATELLITE_AOS_ELEVATION_MIN) && (current_satellite_elevation <= SATELLITE_AOS_ELEVATION_MAX)) { + if (!satellite_visible) { + satellite_visible = 1; + #ifdef DEBUG_SATELLITE_TRACKING + debug.println("service_satellite_tracking: sat AOS"); + #endif // DEBUG_SATELLITE_TRACKING + } + } else { + if (satellite_visible) { + satellite_visible = 0; + #ifdef DEBUG_SATELLITE_TRACKING + debug.println("service_satellite_tracking: sat LOS"); + #endif // DEBUG_SATELLITE_TRACKING + } + } + + if ((satellite_tracking_active) && ((millis() - last_tracking_check) > SATELLITE_TRACKING_UPDATE_INTERVAL)) { + + #ifdef DEBUG_SATELLITE_TRACKING + debug.print(F("service_satellite_tracking: AZ: ")); + debug.print(current_satellite_azimuth); + debug.print(" EL: "); + debug.print(current_satellite_elevation); + debug.print(" lat: "); + debug.print(latitude); + debug.print(" long: "); + debug.println(longitude); + #endif // DEBUG_SATELLITE_TRACKING + + if (satellite_visible) { + submit_request(AZ, REQUEST_AZIMUTH, current_satellite_azimuth, DBG_SERVICE_SATELLITE_TRACKING); + submit_request(EL, REQUEST_ELEVATION, current_satellite_elevation, DBG_SERVICE_SATELLITE_TRACKING); + } + + last_tracking_check = millis(); + } + + + } #endif //FEATURE_SATELLITE_TRACKING -//------------------------------------------------------ -#if defined(FEATURE_SATELLITE_TRACKING) - double rad(double deg){ - - return (M_PI / 180.0 * deg); - } -#endif //FEATURE_SATELLITE_TRACKING -//------------------------------------------------------ -#if defined(FEATURE_SATELLITE_TRACKING) - double deg(double rad){ - - return (rad * 180.0 / M_PI); - - } -#endif //FEATURE_SATELLITE_TRACKING -//------------------------------------------------------ - -#if defined(FEATURE_SATELLITE_TRACKING) - int sgn(double v) { - - if (v < 0) return -1; - if (v > 0) return 1; - return 0; - } -#endif //FEATURE_SATELLITE_TRACKING - -//------------------------------------------------------ -#if defined(FEATURE_SATELLITE_TRACKING) -double FNatn(double y, double x){ - - - float a; - - if (x != 0){ - a = atan(y / x); - } else { - //a = M_PI / 2.0 * sin(y); - a = M_PI / 2.0 * (float)sgn(y); - } - if (x < 0){ - a = a + M_PI; - } - if (a < 0){ - a = a + 2.0 * M_PI; - } - - return (a); -} -#endif //FEATURE_SATELLITE_TRACKING -//------------------------------------------------------ -#if defined(FEATURE_SATELLITE_TRACKING) -double FNday(int year, int month, int day){ - -// Convert date to day number - //* -// * Function returns a general day number from year, month, and day. -// * Value is (JulianDay - 1721409.5) or (AmsatDay + 722100) -// * - - - - double JulianDate; - - #if defined(DEBUG_SATELLITE_TRACKING) - Serial.print("## FNDay: "); - Serial.print("Year: "); - Serial.print(year); - Serial.print(" Month: "); - Serial.print(month); - Serial.print(" Day: "); - Serial.print(day); - #endif - - if (month <= 2){ - year -= 1; - month += 12; - } - - JulianDate = (long)(year * YM) + (int)((month + 1) * 30.6) + (day - 428); - - #if defined(DEBUG_SATELLITE_TRACKING) - Serial.print(" JD: "); - Serial.println(JulianDate); - #endif - - return (JulianDate); -} -#endif //FEATURE_SATELLITE_TRACKING -//------------------------------------------------------ -#if defined(FEATURE_SATELLITE_TRACKING) -void satellite_calculate(double* satellite_az,double* satellite_el,double* satellite_lat,double* satellite_long, - double year_, double month_, double day_, double hour_, double minutes_, double seconds_, - double observer_lon, double observer_lat, double observer_height, - unsigned long* rxOutLong, unsigned long* txOutLong, - kep* kep_in){ - - #if defined(DEBUG_SATELLITE_TRACKING) - Serial.println("satellite_calculate: start"); - #endif - -//zzzzzz - - double YE = kep_in->YE; - double TE = kep_in->TE; - double IN = kep_in->IN; - double RA = kep_in->RA; - double EC = kep_in->EC; - double WP = kep_in->WP; - double MA = kep_in->MA; - double MM = kep_in->MM; - double M2 = kep_in->M2; - double RV = kep_in->RV; - double ALAT = kep_in->ALAT; - double ALON = kep_in->ALON; - unsigned long rxFrequencyLong = kep_in->rxFrequency; - unsigned long txFrequencyLong = kep_in->txFrequency; - double C, S, DNOM; - float dopplerFactor; - - double DN = FNday(year_, month_, day_); - double TN = ((float)hour_ + ((float)minutes_ + ((float)seconds_/60.0)) /60.0)/24.0; - DN = (long)DN; - - // Observer's location - double LA = rad(observer_lat); - double LO = rad(observer_lon); - double HT = ((float) observer_height)/1000.0; // this needs to be in km - double CL = cos(LA); - double SL = sin(LA); - double CO = cos(LO); - double SO = sin(LO); - - double RP = RE * (1.0 - FL); - double XX = RE * RE; - double ZZ = RP * RP; - - double D = sqrt( XX * CL * CL + ZZ * SL * SL ); - - double Rx = XX / D + HT; - double Rz = ZZ / D + HT; - - // Observer's unit vectors Up EAST and NORTH in geocentric coordinates - double Ux = CL * CO; - double Ex = -SO; - double Nx = -SL * CO; - - double Uy = CL * SO; - double Ey = CO; - double Ny = -SL * SO; - - double Uz = SL; - double Ez = 0; - double Nz = CL; - - // Observer's XYZ coordinates at earth's surface - double Ox = Rx * Ux; - double Oy = Rx * Uy; - double Oz = Rz * Uz; - - // Convert angles to radians, etc. - RA = rad(RA); - IN = rad(IN); - WP = rad(WP); - MA = rad(MA); - MM = MM * 2.0 * M_PI; - M2 = M2 * 2.0 * M_PI; - - // Observer's velocity, geocentric coordinates - double VOx = -Oy * W0; - double VOy = Ox * W0; - - // Convert satellite epoch to day number and fraction of a day - double DE = FNday(YE, 1, 0) + (int)TE; - - TE = TE - (int)TE; - - #if defined(DEBUG_SATELLITE_TRACKING) - Serial.print("DE: "); - Serial.println(DE); - Serial.print("TE: "); - Serial.println(TE); - #endif - - // Average Precession rates - - double N0 = MM / 86400.0; // Mean motion rads/s - double Azero = pow(GM / N0 / N0, 1.0 / 3.0); // Semi major axis km - double b0 = Azero * sqrt(1.0 - EC * EC); // Semi minor axis km - double SI = sin(IN); - double CI = cos(IN); - double PC = RE * Azero / (b0 * b0); - PC = 1.5 * J2 * PC * PC * MM; // Precession const, rad/day - double QD = -PC * CI; // Node Precession rate, rad/day - double WD = PC *(5.0 * CI*CI - 1.0) / 2.0; // Perigee Precession rate, rad/day - double DC = -2.0 * M2 / MM / 3.0; // Drag coeff - - // Sun's inclination - //INS = rad(Sun_inclination); - //CNS = cos(INS); - //SNS = sin(INS); - // Sun's equation of center terms - // EQC1 = 0.03341; // unused ? - // EQC2 = 0.00035; // unused ? - - // Bring Sun data to satellite epoch - double TEG = (DE - FNday(YG, 1, 0)) + TE; // Elapsed Time: Epoch - YG - double GHAE = rad(G0) + TEG * WE; // GHA Aries, epoch - //MRSE = rad(G0) + (TEG * WW) + M_PI; // Mean RA Sun at Sat Epoch - //MASE = rad(MAS0 + MASD * TEG); // Mean MA Sun Unused? - - // Antenna unit vector in orbit plane coordinates - CO = cos(rad(ALON)); - SO = sin(rad(ALON)); - CL = cos(rad(ALAT)); - SL = sin(rad(ALAT)); - double ax = -CL * CO; - double ay = -CL * SO; - double az = -SL; - - // Calculate satellite position at DN, TN - double T = (DN - DE) + (TN - TE); // Elapsed T since epoch - - #if defined(DEBUG_SATELLITE_TRACKING) - Serial.print("T: "); - Serial.println(T); - #endif - - double DT = DC * T / 2.0; // Linear drag terms - double KD = 1.0 + 4.0 * DT; - double KDP = 1.0 - 7.0 * DT; - double M = MA + MM * T * (1.0 - 3.0 * DT); // Mean anomaly at YR,/ TN - int DR = (int)(M / (2.0 * M_PI)); // Strip out whole no of revs - M = M - DR * 2.0 * M_PI; // M now in range 0 - 2PI - long RN = RV + DR + 1; // Current orbit number - - // Solve M = EA - EC * sin(EA) for EA given M, by Newton's method - double EA = M; //Initial solution - do{ - C = cos(EA); - S = sin(EA); - DNOM = 1.0 - EC * C; - D = (EA - EC * S - M) / DNOM; // Change EA to better resolution - EA = EA - D; // by this amount until converged - } - while (fabs(D) > 1.0E-5); - - // Distances - double A = Azero * KD; - double B = b0 * KD; - double RS = A * DNOM; - - // Calculate satellite position and velocity in plane of ellipse - double Sx = A * (C - EC); - double Vx = -A * S / DNOM * N0; - double Sy = B * S; - double Vy = B * C / DNOM * N0; - - double AP = WP + WD * T * KDP; - double CWw = cos(AP); - double SW = sin(AP); - double RAAN = RA + QD * T * KDP; - double CQ = cos(RAAN); - double SQ = sin(RAAN); - - // Plane -> celestial coordinate transformation, [C] = [RAAN]*[IN]*[AP] - double CXx = CWw * CQ - SW * CI * SQ; - double CXy = -SW * CQ - CWw * CI * SQ; - double CXz = SI * SQ; - double CYx = CWw * SQ + SW * CI * CQ; - double CYy = -SW * SQ + CWw * CI * CQ; - double CYz = -SI * CQ; - double CZx = SW * SI; - double CZy = CWw * SI; - double CZz = CI; - - // Compute satellite's position vector, ANTenna axis unit vector - // and velocity in celestial coordinates. (Note: Sz = 0, Vz = 0) - double SATx = Sx * CXx + Sy * CXy; - double ANTx = ax * CXx + ay * CXy + az * CXz; - double VELx = Vx * CXx + Vy * CXy; - double SATy = Sx * CYx + Sy * CYy; - double ANTy = ax * CYx + ay * CYy + az * CYz; - double VELy = Vx * CYx + Vy * CYy; - double SATz = Sx * CZx + Sy * CZy; - double ANTz = ax * CZx + ay * CZy + az * CZz; - double VELz = Vx * CZx + Vy * CZy; - - // Also express SAT, ANT, and VEL in geocentric coordinates - double GHAA = GHAE + WE * T; // GHA Aries at elaprsed time T - C = cos(-GHAA); - S = sin(-GHAA); - Sx = SATx * C - SATy * S; - double Ax = ANTx * C - ANTy * S; - Vx = VELx * C - VELy * S; - Sy = SATx * S + SATy * C; - double Ay = ANTx * S + ANTy * C; - Vy = VELx * S + VELy * C; - double Sz = SATz; - double Az = ANTz; - double Vz = VELz; - - // Compute and manipulate range/velocity/antenna vectors - - // Range vector = sat vector - observer vector - Rx = Sx - Ox; - double Ry = Sy - Oy; - Rz = Sz - Oz; - - double R = sqrt(Rx * Rx + Ry * Ry + Rz * Rz); // Range Magnitute - - // Normalize range vector - Rx = Rx / R; - Ry = Ry / R; - Rz = Rz / R; - double U = Rx * Ux + Ry * Uy + Rz * Uz; - double E = Rx * Ex + Ry * Ey; - double N = Rx * Nx + Ry * Ny + Rz * Nz; - - *satellite_az = deg(FNatn(E, N)); - *satellite_el = deg(asin(U)); - - // Solve antenna vector along unit range vector, -r.a = cos(SQ) - // SQ = deg(acos(-(Ax * Rx + Ay * Ry + Az * Rz))); - // Calculate sub-satellite Lat/Lon - *satellite_long = deg(FNatn(Sy, Sx)); // Lon, + East - if (*satellite_long > 180){*satellite_long = *satellite_long - 360.0;} // kludge to fix bug Goody 2020-07-23 - *satellite_lat = deg(asin(Sz/RS)); // Lat, + North - - // Resolve Sat-Obs velocity vector along unit range vector. (VOz = 0) - double RR = (Vx - VOx) * Rx + (Vy - VOy) * Ry + Vz * Rz; // Range rate, km/sec - //FR = rxFrequency * (1 - RR / 299792); - - dopplerFactor = RR / 299792.0; - int rxDoppler = getDoppler(rxFrequencyLong,dopplerFactor); - int txDoppler = getDoppler(txFrequencyLong,dopplerFactor); - *rxOutLong = rxFrequencyLong - rxDoppler; - *txOutLong = txFrequencyLong + txDoppler; - - - #if defined(DEBUG_SATELLITE_TRACKING) - Serial.println("satellite_calculate: end"); - #endif -} - -#endif //FEATURE_SATELLITE_TRACKING -//------------------------------------------------------ -#if defined(FEATURE_SATELLITE_TRACKING) -int getDoppler(unsigned long freq,float dopplerFactor_) { - - freq = (freq + 50000L) / 100000L; - long factor = dopplerFactor_ * 1E11; - int digit; - float tally = 0; - for (int x = 4; x > -1; x--) { - digit = freq/pow(10,x); - long bare = digit * pow(10,x); - freq = freq - bare; - float inBetween = factor * (float(bare) / 1E6); - tally += inBetween; - } - - return int( tally + .5); //round - -} -#endif //FEATURE_SATELLITE_TRACKING -//------------------------------------------------------ -// #if defined(FEATURE_SATELLITE_TRACKING) -// int getDoppler64(unsigned long freq,float dopplerFactor_){ - -// long factor = dopplerFactor_ * 1E11; -// uint64_t doppler_sixfour = freq * dopplerFactor_; -// return (int) doppler_sixfour/1E11; -// } -// #endif //FEATURE_SATELLITE_TRACKING -//------------------------------------------------------ - - -// void footprintOctagon(float *points, float current_satellite_latitudein, float current_satellite_longitudein, float REin, float RSin) { -// // static float points[16]; -// Serial.print("current_satellite_latitude: "); -// Serial.print(current_satellite_latitudein); -// Serial.print(", current_satellite_longitude: "); -// Serial.print(current_satellite_longitudein); -// Serial.print(", RE: "); -// Serial.print(REin); -// Serial.print(", RS: "); -// Serial.println(RSin); -// float srad = acos(REin/RSin); // Beta in Davidoff diag. 13.2, this is in rad -// Serial.print("srad: "); -// Serial.println(srad); -// float cla= cos(rad(current_satellite_latitudein)); -// float sla = sin(rad(current_satellite_latitudein)); -// float clo = cos(rad(current_satellite_longitudein)); -// float slo = sin(rad(current_satellite_longitudein)); -// float sra = sin(srad); -// float cra = cos(srad); -// for (int i = 0; i < 16; i = i +2) { -// float a = 2 * M_PI * i / 16; -// Serial.print("\ta: "); -// Serial.println(a); -// float X = cra; -// Serial.print("\t first X: "); -// Serial.println(X); -// Serial.print("\t first Y: "); -// float Y = sra*sin(a); -// Serial.println(Y); -// float Z = sra*cos(a); -// Serial.print("\t first Z: "); -// Serial.println(Z); -// float x = X*cla - Z*sla; -// float y = Y; -// float z = X*sla + Z*cla; -// X = x*clo - y*slo; -// Serial.print("\tX: "); -// Serial.println(X); -// Y = x*slo + y*clo; -// Serial.print("\tY: "); -// Serial.println(Y); -// Z = z; -// Serial.print("\tZ: "); -// Serial.println(Z); -// points[i] = deg(FNatn(Y,X)); -// Serial.print("\t Long: "); -// Serial.print(points[i]); -// points[i+1] = deg(asin(Z)); -// Serial.print("\t Lat: "); -// Serial.println(points[i+1]); -// } -// } - -//------------------------------------------------------ - -#if defined(FEATURE_SATELLITE_TRACKING) -void printElements(kep* kep_in){ - - Serial.print("YE:"); - Serial.println(kep_in->YE,0); - Serial.print("TE:"); - Serial.println(kep_in->TE,8); - Serial.print("IN:"); - Serial.println(kep_in->IN,4); - Serial.print("RA:"); - Serial.println(kep_in->RA,4); - Serial.print("EC:"); - Serial.println(kep_in->EC,6); - Serial.print("WP:"); - Serial.println(kep_in->WP,4); - Serial.print("MA:"); - Serial.println(kep_in->MA,4); - Serial.print("MM:"); - Serial.println(kep_in->MM,8); - Serial.print("M2:"); - Serial.println(kep_in->M2,8); - Serial.print("RV:"); - Serial.println(kep_in->RV); - Serial.print("ALON:"); - Serial.println(kep_in->ALON,1); - Serial.print("ALON:"); - Serial.println(kep_in->ALON,1); - - -} -#endif //FEATURE_SATELLITE_TRACKING - - //------------------------------------------------------ diff --git a/k3ng_rotator_controller/rotator.h b/k3ng_rotator_controller/rotator.h index 71ed0b7..e839693 100755 --- a/k3ng_rotator_controller/rotator.h +++ b/k3ng_rotator_controller/rotator.h @@ -193,10 +193,13 @@ #define DBG_BACKSLASH_GT_CMD 240 #define DBG_BACKSLASH_GC_CMD 241 - +#define DBG_CHECK_BUTTONS_SATELLITE 244 +#define DBG_SERVICE_MOON_CLI_CMD 245 +#define DBG_SERVICE_SUN_CLI_CMD 246 +#define DBG_SERVICE_SATELLITE_CLI_CMD 247 +#define DBG_SERVICE_SATELLITE_TRACKING 248 #define DBG_NEXTION_DATA_ENT_ENTER_PUSH_CALLBK 249 #define DBG_NEXTION_BUTTON 250 - #define DBG_CHECK_BUTTONS_SUN 251 #define DBG_CHECK_BUTTONS_MOON 252 #define DBG_SERVICE_SUN_TRACKING 253 diff --git a/k3ng_rotator_controller/rotator_debug_log_activation.h b/k3ng_rotator_controller/rotator_debug_log_activation.h index 2f16cc7..c325371 100644 --- a/k3ng_rotator_controller/rotator_debug_log_activation.h +++ b/k3ng_rotator_controller/rotator_debug_log_activation.h @@ -66,4 +66,5 @@ // #define DEBUG_NEXTION_DISPLAY_SERIAL_RECV // #define DEBUG_TEST_POLAR_TO_CARTESIAN // #define DEBUG_SATELLITE_TRACKING +#define DEBUG_SATELLITE_TRACKING_CALC diff --git a/k3ng_rotator_controller/rotator_pins.h b/k3ng_rotator_controller/rotator_pins.h index 09e4b13..b8934cc 100755 --- a/k3ng_rotator_controller/rotator_pins.h +++ b/k3ng_rotator_controller/rotator_pins.h @@ -231,3 +231,9 @@ #endif //#define pin_status_led 0 // Status LED - blinks when there is rotation in progress + +// Added 2020.07.24.01 +#define satellite_tracking_active_pin 0 +#define satellite_tracking_activate_line 0 +#define satellite_tracking_button 0 // use with a normally open momentary switch to ground + diff --git a/k3ng_rotator_controller/rotator_pins_ea4tx_ars_usb.h b/k3ng_rotator_controller/rotator_pins_ea4tx_ars_usb.h index cfa71ea..b873814 100644 --- a/k3ng_rotator_controller/rotator_pins_ea4tx_ars_usb.h +++ b/k3ng_rotator_controller/rotator_pins_ea4tx_ars_usb.h @@ -101,3 +101,8 @@ #endif //#define pin_status_led 0 // Status LED - blinks when there is rotation in progress + +// Added 2020.07.24.01 +#define satellite_tracking_active_pin 0 +#define satellite_tracking_activate_line 0 +#define satellite_tracking_button 0 // use with a normally open momentary switch to ground \ No newline at end of file diff --git a/k3ng_rotator_controller/rotator_pins_k3ng_g1000.h b/k3ng_rotator_controller/rotator_pins_k3ng_g1000.h index 2a22b93..a825328 100755 --- a/k3ng_rotator_controller/rotator_pins_k3ng_g1000.h +++ b/k3ng_rotator_controller/rotator_pins_k3ng_g1000.h @@ -182,4 +182,9 @@ #define pin_audible_alert 0 #endif -//#define pin_status_led 0 // Status LED - blinks when there is rotation in progress \ No newline at end of file +//#define pin_status_led 0 // Status LED - blinks when there is rotation in progress + +// Added 2020.07.24.01 +#define satellite_tracking_active_pin 0 +#define satellite_tracking_activate_line 0 +#define satellite_tracking_button 0 // use with a normally open momentary switch to ground \ No newline at end of file diff --git a/k3ng_rotator_controller/rotator_pins_m0upu.h b/k3ng_rotator_controller/rotator_pins_m0upu.h index 4df28e0..c5dfee7 100644 --- a/k3ng_rotator_controller/rotator_pins_m0upu.h +++ b/k3ng_rotator_controller/rotator_pins_m0upu.h @@ -196,4 +196,10 @@ #define pin_audible_alert 0 #endif -//#define pin_status_led 0 // Status LED - blinks when there is rotation in progress \ No newline at end of file +//#define pin_status_led 0 // Status LED - blinks when there is rotation in progress + +// Added 2020.07.24.01 +#define satellite_tracking_active_pin 0 +#define satellite_tracking_activate_line 0 +#define satellite_tracking_button 0 // use with a normally open momentary switch to ground + diff --git a/k3ng_rotator_controller/rotator_pins_test.h b/k3ng_rotator_controller/rotator_pins_test.h index 2ad556a..da4bef5 100644 --- a/k3ng_rotator_controller/rotator_pins_test.h +++ b/k3ng_rotator_controller/rotator_pins_test.h @@ -242,3 +242,8 @@ #endif #define pin_status_led A8 // Status LED - blinks when there is rotation in progress + +// Added 2020.07.24.01 +#define satellite_tracking_active_pin 0 +#define satellite_tracking_activate_line 0 +#define satellite_tracking_button 0 // use with a normally open momentary switch to ground \ No newline at end of file diff --git a/k3ng_rotator_controller/rotator_pins_wb6kcn.h b/k3ng_rotator_controller/rotator_pins_wb6kcn.h index 9c9ddef..007f43d 100755 --- a/k3ng_rotator_controller/rotator_pins_wb6kcn.h +++ b/k3ng_rotator_controller/rotator_pins_wb6kcn.h @@ -214,3 +214,8 @@ #endif //#define pin_status_led 0 // Status LED - blinks when there is rotation in progress + +// Added 2020.07.24.01 +#define satellite_tracking_active_pin 0 +#define satellite_tracking_activate_line 0 +#define satellite_tracking_button 0 // use with a normally open momentary switch to ground diff --git a/k3ng_rotator_controller/rotator_pins_wb6kcn_az_test_setup.h b/k3ng_rotator_controller/rotator_pins_wb6kcn_az_test_setup.h index 17effb0..dd0523c 100755 --- a/k3ng_rotator_controller/rotator_pins_wb6kcn_az_test_setup.h +++ b/k3ng_rotator_controller/rotator_pins_wb6kcn_az_test_setup.h @@ -207,3 +207,7 @@ //#define pin_status_led 0 // Status LED - blinks when there is rotation in progress +// Added 2020.07.24.01 +#define satellite_tracking_active_pin 0 +#define satellite_tracking_activate_line 0 +#define satellite_tracking_button 0 // use with a normally open momentary switch to ground diff --git a/k3ng_rotator_controller/rotator_settings.h b/k3ng_rotator_controller/rotator_settings.h index fb71137..931d522 100755 --- a/k3ng_rotator_controller/rotator_settings.h +++ b/k3ng_rotator_controller/rotator_settings.h @@ -354,3 +354,11 @@ You can tweak these, but read the online documentation! // Added in 2020.07.22.02 #define DEFAULT_ALTITUDE_M 50 + +// Added in 2020.07.24.01 +#define SATELLITE_UPDATE_POSITION_INTERVAL_MS 5000 +#define SATELLITE_TRACKING_UPDATE_INTERVAL 5000 +#define SATELLITE_AOS_AZIMUTH_MIN 0 +#define SATELLITE_AOS_AZIMUTH_MAX 360 +#define SATELLITE_AOS_ELEVATION_MIN 0 +#define SATELLITE_AOS_ELEVATION_MAX 180 \ No newline at end of file diff --git a/k3ng_rotator_controller/rotator_settings_ea4tx_ars_usb.h b/k3ng_rotator_controller/rotator_settings_ea4tx_ars_usb.h index dabf512..520f40f 100755 --- a/k3ng_rotator_controller/rotator_settings_ea4tx_ars_usb.h +++ b/k3ng_rotator_controller/rotator_settings_ea4tx_ars_usb.h @@ -350,3 +350,11 @@ You can tweak these, but read the online documentation! // Added in 2020.07.22.02 #define DEFAULT_ALTITUDE_M 50 + +// Added in 2020.07.24.01 +#define SATELLITE_UPDATE_POSITION_INTERVAL_MS 5000 +#define SATELLITE_TRACKING_UPDATE_INTERVAL 5000 +#define SATELLITE_AOS_AZIMUTH_MIN 0 +#define SATELLITE_AOS_AZIMUTH_MAX 360 +#define SATELLITE_AOS_ELEVATION_MIN 0 +#define SATELLITE_AOS_ELEVATION_MAX 180 diff --git a/k3ng_rotator_controller/rotator_settings_m0upu.h b/k3ng_rotator_controller/rotator_settings_m0upu.h index e0b22e5..7dda9f9 100755 --- a/k3ng_rotator_controller/rotator_settings_m0upu.h +++ b/k3ng_rotator_controller/rotator_settings_m0upu.h @@ -346,4 +346,12 @@ You can tweak these, but read the online documentation! #define MOON_UPDATE_POSITION_INTERVAL_MS 5000 // Added in 2020.07.22.02 -#define DEFAULT_ALTITUDE_M 50 \ No newline at end of file +#define DEFAULT_ALTITUDE_M 50 + +// Added in 2020.07.24.01 +#define SATELLITE_UPDATE_POSITION_INTERVAL_MS 5000 +#define SATELLITE_TRACKING_UPDATE_INTERVAL 5000 +#define SATELLITE_AOS_AZIMUTH_MIN 0 +#define SATELLITE_AOS_AZIMUTH_MAX 360 +#define SATELLITE_AOS_ELEVATION_MIN 0 +#define SATELLITE_AOS_ELEVATION_MAX 180 \ No newline at end of file diff --git a/k3ng_rotator_controller/rotator_settings_test.h b/k3ng_rotator_controller/rotator_settings_test.h index 45cbf18..6e0b436 100755 --- a/k3ng_rotator_controller/rotator_settings_test.h +++ b/k3ng_rotator_controller/rotator_settings_test.h @@ -366,6 +366,14 @@ You can tweak these, but read the online documentation! // Added in 2020.07.22.02 #define DEFAULT_ALTITUDE_M 500 +// Added in 2020.07.24.01 +#define SATELLITE_UPDATE_POSITION_INTERVAL_MS 5000 +#define SATELLITE_TRACKING_UPDATE_INTERVAL 5000 +#define SATELLITE_AOS_AZIMUTH_MIN 0 +#define SATELLITE_AOS_AZIMUTH_MAX 360 +#define SATELLITE_AOS_ELEVATION_MIN 0 +#define SATELLITE_AOS_ELEVATION_MAX 180 + // ######## ######## ###### ######## diff --git a/k3ng_rotator_controller/rotator_settings_wb6kcn.h b/k3ng_rotator_controller/rotator_settings_wb6kcn.h index f70157f..4d7958d 100755 --- a/k3ng_rotator_controller/rotator_settings_wb6kcn.h +++ b/k3ng_rotator_controller/rotator_settings_wb6kcn.h @@ -349,4 +349,12 @@ You can tweak these, but read the online documentation! // Added in 2020.07.22.02 #define DEFAULT_ALTITUDE_M 50 + +// Added in 2020.07.24.01 +#define SATELLITE_UPDATE_POSITION_INTERVAL_MS 5000 +#define SATELLITE_TRACKING_UPDATE_INTERVAL 5000 +#define SATELLITE_AOS_AZIMUTH_MIN 0 +#define SATELLITE_AOS_AZIMUTH_MAX 360 +#define SATELLITE_AOS_ELEVATION_MIN 0 +#define SATELLITE_AOS_ELEVATION_MAX 180 \ No newline at end of file diff --git a/libraries/P13/P13.cpp b/libraries/P13/P13.cpp new file mode 100755 index 0000000..71a2233 --- /dev/null +++ b/libraries/P13/P13.cpp @@ -0,0 +1,456 @@ +// +// P13.cpp +// +// An implementation of Plan13 in C++ by Mark VandeWettering +// +// Plan13 is an algorithm for satellite orbit prediction first formulated +// by James Miller G3RUH. I learned about it when I saw it was the basis +// of the PIC based antenna rotator project designed by G6LVB. +// +// http://www.g6lvb.com/Articles/LVBTracker2/index.htm +// +// I ported the algorithm to Python, and it was my primary means of orbit +// prediction for a couple of years while I operated the "Easy Sats" with +// a dual band hand held and an Arrow antenna. +// +// I've long wanted to redo the work in C++ so that I could port the code +// to smaller processors including the Atmel AVR chips. Bruce Robertson, +// VE9QRP started the qrpTracker project to fufill many of the same goals, +// but I thought that the code could be made more compact and more modular, +// and could serve not just the embedded targets but could be of more +// use for more general applications. And, I like the BSD License a bit +// better too. +// +// So, here it is! +// + +#include "P13.h" + +double +RADIANS(double deg) +{ + return deg * M_PI / 180. ; +} + +double +DEGREES(double rad) +{ + return rad * 180. / M_PI ; +} + + +//---------------------------------------------------------------------- +// _ ___ _ _____ _ +// __| |__ _ ______ | \ __ _| |_ __|_ _(_)_ __ ___ +// / _| / _` (_-<_-< | |) / _` | _/ -_)| | | | ' \/ -_) +// \__|_\__,_/__/__/ |___/\__,_|\__\___||_| |_|_|_|_\___| +// +//---------------------------------------------------------------------- + +static long +fnday(int y, int m, int d) +{ + if (m < 3) { + m += 12 ; + y -- ; + } + return (long) (y * YM) + (long) ((m+1)*30.6f) + (long)d - 428L ; +} + +static void +fndate(int &y, int &m, int &d, long dt) +{ + dt += 428L ; + y = (int) ((dt-122.1)/365.25) ; + dt -= (long) (y*365.25) ; + m = (int) (dt / 30.61) ; + dt -= (long) (m*30.6) ; + m -- ; + if (m > 12) { + m -= 12 ; + y++ ; + } + d = dt ; +} + +SatDateTime::SatDateTime(int year, int month, int day, int h, int m, int s) +{ + settime(year, month, day, h, m, s) ; +} + + +SatDateTime::SatDateTime(const SatDateTime &dt) +{ + DN = dt.DN ; + TN = dt.TN ; +} + +SatDateTime::SatDateTime() +{ + DN = 0L ; + TN = 0. ; +} + +void +SatDateTime::gettime(int &year, int &month, int &day, int &h, int &m, int &s) +{ + fndate(year, month, day, DN) ; + double t = TN ; + t *= 24. ; + h = (int) t ; + t -= h ; + t *= 60 ; + m = (int) t ; + t -= m ; + t *= 60 ; + s = (int) t ; +} + +void +SatDateTime::settime(int year, int month, int day, int h, int m, int s) +{ + DN = fnday(year, month, day) ; + TN = ((double) h + m / 60. + s / 3600.) / 24. ; +} + +void +SatDateTime::ascii(char *buf) +{ + int year, mon, day ; + int h, m, s ; + gettime(year, mon, day, h, m, s) ; + sprintf(buf, "%02d/%02d/%4d %02d:%02d:%02d", mon, day, year, h, m, s) ; +} + + +void +SatDateTime::add(double days) +{ + TN += days ; + DN += (long) TN ; + TN -= (long) TN ; +} + +void +SatDateTime::roundup(double t) +{ + double inc = t-fmod(TN, t) ; + TN += inc ; + DN += (long) TN ; + TN -= (long) TN ; +} + +//---------------------------------------------------------------------- +// _ ___ _ +// __| |__ _ ______ / _ \| |__ ___ ___ _ ___ _____ _ _ +// / _| / _` (_-<_-< | (_) | '_ (_-name = nm ; + LA = RADIANS(lat) ; + LO = RADIANS(lng) ; + HT = hgt / 1000 ; + + U[0] = cos(LA)*cos(LO) ; + U[1] = cos(LA)*sin(LO) ; + U[2] = sin(LA) ; + + E[0] = -sin(LO) ; + E[1] = cos(LO) ; + E[2] = 0. ; + + N[0] = -sin(LA)*cos(LO) ; + N[1] = -sin(LA)*sin(LO) ; + N[2] = cos(LA) ; + + double RP = RE * (1 - FL) ; + double XX = RE * RE ; + double ZZ = RP * RP ; + double D = sqrt(XX*cos(LA)*cos(LA) + + ZZ*sin(LA)*sin(LA)) ; + double Rx = XX / D + HT ; + double Rz = ZZ / D + HT ; + + O[0] = Rx * U[0] ; + O[1] = Rx * U[1] ; + O[2] = Rz * U[2] ; + + V[0] = -O[1] * W0 ; + V[1] = O[0] * W0 ; + V[2] = 0 ; +} + +//---------------------------------------------------------------------- +// _ ___ _ _ _ _ _ +// __| |__ _ ______ / __| __ _| |_ ___| | (_) |_ ___ +// / _| / _` (_-<_-< \__ \/ _` | _/ -_) | | | _/ -_) +// \__|_\__,_/__/__/ |___/\__,_|\__\___|_|_|_|\__\___| +// +//---------------------------------------------------------------------- + +static double +getdouble(const char *c, int i0, int i1) +{ + char buf[20] ; + int i ; + for (i=0; i0+i OR +CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, +EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +The views and conclusions contained in the software and documentation +are those of the authors and should not be interpreted as representing +official policies, either expressed or implied, of Mark VandeWettering + +*/ + +// Modification by Anthony Good, K3NG 2020-07-24: Change DateTime to SatDateTime to avoid a conflict with an RTC library I'm using + +//---------------------------------------------------------------------- + +#if defined(ARDUINO) && ARDUINO >= 100 + #include "Arduino.h" +#else + #include "WProgram.h" +#endif + +// here are a bunch of constants that will be used throughout the +// code, but which will probably not be helpful outside. + +static const double RE = 6378.137f ; +static const double FL = 1.f/298.257224f ; +static const double GM = 3.986E5f ; +static const double J2 = 1.08263E-3f ; +static const double YM = 365.25f ; +static const double YT = 365.2421874f ; +static const double WW = 2.f*M_PI/YT ; +static const double WE = 2.f*M_PI+ WW ; +static const double W0 = WE/86400.f ; + + +static const double YG = 2014.f; +static const double G0 = 99.5828f; +static const double MAS0 = 356.4105f; +static const double MASD = 0.98560028f; +static const double EQC1 = 0.03340; +static const double EQC2 = 0.00035; +static const double INS = radians(23.4375f); + +// static const double YG = 2000.f ; +// static const double G0 = 98.9821f ; +// static const double MAS0 = 356.0507f ; +// static const double MASD = 0.98560028f ; +// static const double EQC1 = 0.03342 ; +// static const double EQC2 = 0.00035 ; +// static const double INS = radians(23.4393f) ; + +static const double CNS = cos(INS) ; +static const double SNS = sin(INS) ; + +//---------------------------------------------------------------------- + +// the original BASIC code used three variables (e.g. Ox, Oy, Oz) to +// represent a vector quantity. I think that makes for slightly more +// obtuse code, so I going to collapse them into a single variable +// which is an array of three elements + +typedef double Vec3[3] ; + +//---------------------------------------------------------------------- + +class SatDateTime { +public: + long DN ; + double TN ; + SatDateTime(int year, int month, int day, int h, int m, int s) ; + SatDateTime(const SatDateTime &) ; + SatDateTime() ; + ~SatDateTime() { } + void add(double) ; + void settime(int year, int month, int day, int h, int m, int s) ; + void gettime(int& year, int& mon, int& day, int& h, int& m, int& s) ; + void ascii(char *) ; + void roundup(double) ; +} ; + +//---------------------------------------------------------------------- + +class Observer { +public: + const char *name ; + double LA ; + double LO ; + double HT ; + Vec3 U, E, N, O, V ; + + Observer(const char *, double, double, double) ; + ~Observer() { } ; +} ; + +//---------------------------------------------------------------------- + + +class Satellite { + long N ; + long YE ; + long DE ; + double TE ; + double IN ; + double RA ; + double EC ; + double WP ; + double MA ; + double MM ; + double M2 ; + double RV ; + double ALON ; + double ALAT ; + + + // these values are stored, but could be calculated on the fly + // during calls to predict() + // classic space/time tradeoff + + double N0, A_0, B_0 ; + double PC ; + double QD, WD, DC ; + double RS ; + +public: + const char *name ; + Vec3 SAT, VEL ; // celestial coordinates + Vec3 S, V ; // geocentric coordinates + + Satellite() { } ; + Satellite(const char *name, const char *l1, const char *l2) ; + ~Satellite() ; + void tle(const char *name, const char *l1, const char *l2) ; + void predict(const SatDateTime &dt) ; + void LL(double &lat, double &lng) ; + void altaz(const Observer &obs, double &alt, double &az) ; +} ; + +class Sun { +public: + Vec3 SUN, H ; + Sun() ; + ~Sun() { } ; + void predict(const SatDateTime &dt) ; + void LL(double &lat, double &lng) ; + void altaz(const Observer &obs, double &alt, double &az) ; +} ; diff --git a/libraries/P13/README b/libraries/P13/README new file mode 100755 index 0000000..9fe14f7 --- /dev/null +++ b/libraries/P13/README @@ -0,0 +1,108 @@ + _ The Arduino n' Gameduino + __ _ _ _ __ _ __| |_ Satellite Tracker +/ _` | ' \/ _` (_-< _| +\__,_|_||_\__, /__/\__| Written by Mark VandeWettering, K6HX, 2011 + |___/ brainwagon@gmail.com + + +Angst is an application that I wrote for the Arduino and Gameduino. It +is being distributed under the so-called "2-class BSD License", which I +think grants potential users the greatest possible freedom to integrate +this code into their own projects. I would, however, consider it a great +courtesy if you could email me and tell me about your project and how +this code was used, just for my own continued personal gratification. + +Angst includes P13, a port of the Plan 13 algorithm, originally written +by James Miller, G3RUH and documented here: + + http://www.amsat.org/amsat/articles/g3ruh/111.html + +Other implementations exist, even for embedded platforms, such as +the qrpTracker library of Bruce Robertson, VE9QRP and G6LVB's PIC +implementation that is part of his embedded satellite tracker. My own +code was ported from a quick and dirty Python implementation of my own, +and retains a bit of the object orientation that I imposed in that code. + +While the P13 library is reasonably independent and can be used in +other applications, the remainder of the code is fairly specific to the +particular hardware I had on hand: + +1. The Gameduino + + James Bowman developed the Gameduino as an Arduino shield to + generate sound and graphics which might have been typical of + 8 bit computers of the 1980s. It consists of a Xilinx FPGA, + programmed with a version of his J1 Forth processor, and + generates video and sound. It is programmed from the Arduino + using a simple set of commands sent over the I2C bus. + + You can find more information here: + + http://excamera.com/sphinx/gameduino/ + +2. A DS1307 RTC + + Sparkfun has a Dallas Semiconductor DS1307 chip mounted on a + handly little breakout board. I used this chip to keep track + of time. It's not particularly great: it drifts by a couple + of minutes a week in my prototype: a better and more accurate + timepiece would enhance the usability of this prototype. I've + considered using a GPS to keep track of time, that extension is + left as an exercise for the motivated reader. + +3. An AT24C1024 Serial EEPROM + + The ATMEGA328 used in the Arduino contains a mere 512 bytes of + EEPROM. This is enough space to store a few satellites, but I + wanted to have greater capacitity. So, I got some 128K byte + EEPROMS for about $4 from digikey. Each one of these can store + the data for about 750 satellites. My prototype just uses one, + but it's possible to chain up to four of them together. + +4. A Rotary Encoder + + The entire interface is driven from a single rotary encoder w/ + a built in switch, similar to this one: + + http://www.sparkfun.com/products/9117 + + The code turns on the necessary internal pullup resistors to + allow it to be directly wired to the Arduino input pins. + + Currently the encoder is read in the polling main event loop. + This means that quick motion of the encoder is not adequately + tracked: consider this another exercise for the motivated + reader. + + +I hope you enjoy the code, and find it useful and/or informative! + +Copyright 2011, Mark VandeWettering. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are +met: + + 1. Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer + in the documentation and/or other materials provided with the + distribution. + +THIS SOFTWARE IS PROVIDED BY MARK VANDEWETTERING ''AS IS'' AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL OR +CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, +EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +The views and conclusions contained in the software and documentation +are those of the authors and should not be interpreted as representing +official policies, either expressed or implied, of Mark VandeWettering