From 5a75ef13560672d4963c5b6ceb9ed71eb3a16168 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Mon, 28 Nov 2011 10:25:16 -0800 Subject: [PATCH 01/16] made missions work with index of 1 --- Tools/autotest/arducopter.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 82f281e153..3b15c8aa0e 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -170,8 +170,8 @@ def fly_mission(mavproxy, mav, height_accuracy=-1, target_altitude=None): '''fly a mission from a file''' global homeloc global num_wp - print("test: Fly a mission from 0 to %u" % num_wp) - mavproxy.send('wp set 0\n') + print("test: Fly a mission from 1 to %u" % num_wp) + mavproxy.send('wp set 1\n') mavproxy.send('switch 4\n') # auto mode wait_mode(mav, 'AUTO') #wait_altitude(mav, 30, 40) @@ -254,7 +254,7 @@ def fly_ArduCopter(viewerip=None): # reboot with new parameters util.pexpect_close(mavproxy) util.pexpect_close(sil) - + sil = util.start_SIL('ArduCopter', height=HOME.alt) hquad = pexpect.spawn(hquad_cmd, logfile=sys.stdout, timeout=10) util.pexpect_autoclose(hquad) From d4a9808a6129f0cb9f3a75de1b3dc3d1b240bd0f Mon Sep 17 00:00:00 2001 From: Jason Short Date: Mon, 28 Nov 2011 10:26:01 -0800 Subject: [PATCH 02/16] Erasing Logs causing major havoc. Users getting infinite loop. --- ArduCopter/system.pde | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index abbd049b22..de883542e3 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -164,8 +164,8 @@ static void init_ardupilot() delay(100); // wait for serial send AP_Var::erase_all(); - // clear logs - erase_logs(NULL, NULL); + // clear logs - createing infinate lockup + //erase_logs(NULL, NULL); // save the new format version g.format_version.set_and_save(Parameters::k_format_version); From 8e582b416966b52783b82803464e71d4486c6a6a Mon Sep 17 00:00:00 2001 From: Jason Short Date: Mon, 28 Nov 2011 10:26:36 -0800 Subject: [PATCH 03/16] Logging input to find this freeze bug --- ArduCopter/Log.pde | 42 ++++++++++++++++-------------------------- 1 file changed, 16 insertions(+), 26 deletions(-) diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 31a7a9fd57..b88c1da507 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -724,30 +724,20 @@ static void Log_Write_Control_Tuning() DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG); - // yaw - DataFlash.WriteInt(dcm.yaw_sensor/100); //1 - DataFlash.WriteInt(nav_yaw/100); //2 - DataFlash.WriteInt(yaw_error/100); //3 - - // Alt hold - DataFlash.WriteInt(sonar_alt); //4 - DataFlash.WriteInt(baro_alt); //5 - DataFlash.WriteInt(next_WP.alt); //6 - - DataFlash.WriteInt(nav_throttle); //7 - DataFlash.WriteInt(angle_boost); //8 - DataFlash.WriteInt(manual_boost); //9 - DataFlash.WriteInt(climb_rate); //10 - -#if HIL_MODE == HIL_MODE_ATTITUDE - DataFlash.WriteInt(0); //11 -#else - DataFlash.WriteInt((int)(barometer.RawPress - barometer._offset_press));//11 -#endif - - DataFlash.WriteInt(g.rc_3.servo_out); //12 - DataFlash.WriteInt(g.pi_alt_hold.get_integrator()); //13 - DataFlash.WriteInt(g.pi_throttle.get_integrator()); //14 + DataFlash.WriteInt(g.rc_1.control_in); // 0 + DataFlash.WriteInt(g.rc_2.control_in); // 1 + DataFlash.WriteInt(g.rc_3.control_in); // 2 + DataFlash.WriteInt(g.rc_4.control_in); // 3 + DataFlash.WriteInt(sonar_alt); // 4 + DataFlash.WriteInt(baro_alt); // 5 + DataFlash.WriteInt(next_WP.alt); // 6 + DataFlash.WriteInt(nav_throttle); // 7 + DataFlash.WriteInt(angle_boost); // 8 + DataFlash.WriteInt(manual_boost); // 9 + DataFlash.WriteInt(climb_rate); // 10 + DataFlash.WriteInt(g.rc_3.servo_out); // 11 + DataFlash.WriteInt(g.pi_alt_hold.get_integrator()); // 12 + DataFlash.WriteInt(g.pi_throttle.get_integrator()); // 13 DataFlash.WriteByte(END_BYTE); } @@ -759,11 +749,11 @@ static void Log_Read_Control_Tuning() Serial.printf_P(PSTR("CTUN, ")); - for(int8_t i = 1; i < 14; i++ ){ + for(int8_t i = 0; i < 13; i++ ){ temp = DataFlash.ReadInt(); Serial.printf("%d, ", temp); } - + // read 13 temp = DataFlash.ReadInt(); Serial.printf("%d\n", temp); } From eea0062ea6540a36e87db920513e92ace3a36469 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Mon, 28 Nov 2011 10:30:34 -0800 Subject: [PATCH 04/16] better comments out of some old unused functions to avoid compiler errors --- ArduCopter/commands_logic.pde | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 2abb2a2bf9..de3fb66354 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -103,8 +103,8 @@ static void process_now_command() } } -static void handle_no_commands() -{ +//static void handle_no_commands() +//{ /* switch (control_mode){ default: @@ -113,7 +113,7 @@ static void handle_no_commands() }*/ //return; //Serial.println("Handle No CMDs"); -} +//} /********************************************************************************/ // Verify command Handlers @@ -430,10 +430,10 @@ static bool verify_nav_wp() } } -static bool verify_loiter_unlim() -{ - return false; -} +//static bool verify_loiter_unlim() +//{ +// return false; +//} static bool verify_loiter_time() { From 60237bd01b999d7d6902b11f7fb9f4e5e1b27032 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Mon, 28 Nov 2011 10:31:12 -0800 Subject: [PATCH 05/16] slight refactoring to avoid the increment issue when changing commands --- ArduCopter/commands_process.pde | 52 +++++++++++++++++++-------------- 1 file changed, 30 insertions(+), 22 deletions(-) diff --git a/ArduCopter/commands_process.pde b/ArduCopter/commands_process.pde index 3d3f3a5dec..d799c3063c 100644 --- a/ArduCopter/commands_process.pde +++ b/ArduCopter/commands_process.pde @@ -4,6 +4,7 @@ //---------------------------------------- static void change_command(uint8_t cmd_index) { + //Serial.printf("change_command: %d\n",cmd_index ); // limit range cmd_index = min(g.command_total-1, cmd_index); @@ -17,18 +18,21 @@ static void change_command(uint8_t cmd_index) //gcs_send_text_P(SEVERITY_LOW,PSTR("error: non-Nav cmd")); } else { - //Serial.printf("APM:New cmd Index: %d\n", cmd_index); + // clear out command queue init_commands(); + + // copy command to the queue + command_nav_queue = temp; command_nav_index = cmd_index; - prev_nav_index = command_nav_index; - update_commands(false); + execute_nav_command(); } } // called by 10 Hz loop // -------------------- -static void update_commands(bool increment) +static void update_commands() { + //Serial.printf("update_commands: %d\n",increment ); // A: if we do not have any commands there is nothing to do // B: We have completed the mission, don't redo the mission if (g.command_total <= 1 || g.command_index == 255) @@ -40,27 +44,11 @@ static void update_commands(bool increment) // ------------------------------------------------------------------- if (command_nav_index < (g.command_total -1)) { - // load next index - if (increment) - command_nav_index++; - + command_nav_index++; command_nav_queue = get_cmd_with_index(command_nav_index); if (command_nav_queue.id <= MAV_CMD_NAV_LAST ){ - // This is what we report to MAVLINK - g.command_index = command_nav_index; - - // Save CMD to Log - if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(g.command_index + 1, &command_nav_queue); - - // Act on the new command - process_nav_command(); - - // clear May indexes to force loading of more commands - // existing May commands are tossed. - command_cond_index = NO_COMMAND; - + execute_nav_command(); } else{ // this is a conditional command so we skip it command_nav_queue.id = NO_COMMAND; @@ -119,6 +107,26 @@ static void update_commands(bool increment) } } +static void execute_nav_command(void) +{ + // This is what we report to MAVLINK + g.command_index = command_nav_index; + + // Save CMD to Log + if (g.log_bitmask & MASK_LOG_CMD) + Log_Write_Cmd(g.command_index, &command_nav_queue); + + // Act on the new command + process_nav_command(); + + // clear navigation prameters + reset_nav(); + + // clear May indexes to force loading of more commands + // existing May commands are tossed. + command_cond_index = NO_COMMAND; +} + // called with GPS navigation update - not constantly static void verify_commands(void) { From dddf829c01efe12f26e6d39027ccdc9bebb39f4a Mon Sep 17 00:00:00 2001 From: Jason Short Date: Mon, 28 Nov 2011 10:31:49 -0800 Subject: [PATCH 06/16] removed need to send increment flag to update commands constrained climb_rate value --- ArduCopter/ArduCopter.pde | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index ef399b5f97..1138f5e808 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -753,7 +753,7 @@ static void medium_loop() // -------------------- if(control_mode == AUTO){ if(home_is_set == true && g.command_total > 1){ - update_commands(true); + update_commands(); } } @@ -1374,6 +1374,9 @@ static void update_altitude() current_loc.alt = baro_alt + home.alt; climb_rate = baro_rate; } + + // manage bad data + climb_rate = constrain(climb_rate, -300, 300); } static void From e1ea71efff268fcbb300dddc4cbfd755d47854f2 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Mon, 28 Nov 2011 10:32:58 -0800 Subject: [PATCH 07/16] made the crosstrack use the previous WP instead of the current loc to get a better line in AP. --- ArduCopter/commands.pde | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index 691218493d..6ef53a72ae 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -146,7 +146,17 @@ static void set_next_WP(struct Location *wp) // copy the current WP into the OldWP slot // --------------------------------------- - prev_WP = next_WP; + if (next_WP.lat == 0 || command_nav_index <= 1){ + prev_WP = current_loc; + }else{ + if (get_distance(¤t_loc, &next_WP) < 10) + prev_WP = next_WP; + else + prev_WP = current_loc; + } + + //Serial.printf("set_next_WP #%d, ", command_nav_index); + //print_wp(&prev_WP, command_nav_index -1); // Load the next_WP slot // --------------------- @@ -165,7 +175,7 @@ static void set_next_WP(struct Location *wp) // ----------------------------------- wp_totalDistance = get_distance(¤t_loc, &next_WP); wp_distance = wp_totalDistance; - target_bearing = get_bearing(¤t_loc, &next_WP); + target_bearing = get_bearing(&prev_WP, &next_WP); // to check if we have missed the WP // --------------------------------- From f3b3350d25fc9e1edb2d9158a5abcc22f5ecc506 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Mon, 28 Nov 2011 10:54:20 -0800 Subject: [PATCH 08/16] Git Test --- ArduCopter/ArduCopter.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 1138f5e808..b0b858037a 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -748,7 +748,7 @@ static void medium_loop() //------------------------------------------------- case 3: medium_loopCounter++; - + // test // perform next command // -------------------- if(control_mode == AUTO){ From 104f93cef52be360eed75a648c24bea7f1d959b7 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Mon, 28 Nov 2011 14:22:05 -0800 Subject: [PATCH 09/16] Fixed navigation bug --- ArduCopter/navigation.pde | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 88b94813b1..5399dc456b 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -30,9 +30,7 @@ static bool check_missed_wp() { int32_t temp = target_bearing - original_target_bearing; temp = wrap_180(temp); - //return (abs(temp) > 10000); //we pased the waypoint by 10 ° - // temp testing - return false; + return (abs(temp) > 10000); //we pased the waypoint by 10 ° } // ------------------------------ From aa6c336479bbd254ff5f9e9f43919bac1347d23a Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Mon, 28 Nov 2011 19:42:27 -0700 Subject: [PATCH 10/16] Add DataFlash.Init before doing a forced log erase on eeprom reset. Add progress indication during log erase --- ArduCopter/Log.pde | 1 + ArduCopter/system.pde | 6 ++++++ ArduPlane/Log.pde | 1 + ArduPlane/system.pde | 35 +++++++++++++++-------------------- 4 files changed, 23 insertions(+), 20 deletions(-) diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index b88c1da507..c9adce420f 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -136,6 +136,7 @@ erase_logs(uint8_t argc, const Menu::arg *argv) for(int j = 1; j <= DF_LAST_PAGE; j++) { DataFlash.PageErase(j); DataFlash.StartWrite(j); // We need this step to clean FileNumbers + if(j%128 == 0) Serial.printf_P(PSTR("+")); } g.log_last_filenumber.set_and_save(0); diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index de883542e3..9618148c66 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -163,6 +163,12 @@ static void init_ardupilot() Serial.printf_P(PSTR("Firmware change: erasing EEPROM...\n")); delay(100); // wait for serial send AP_Var::erase_all(); + + // erase DataFlash on format version change + #if LOGGING_ENABLED == ENABLED + DataFlash.Init(); + erase_logs(NULL, NULL); + #endif // clear logs - createing infinate lockup //erase_logs(NULL, NULL); diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index f5d2aa57e2..b6b8d22a4b 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -140,6 +140,7 @@ erase_logs(uint8_t argc, const Menu::arg *argv) for(int j = 1; j <= DF_LAST_PAGE; j++) { DataFlash.PageErase(j); DataFlash.StartWrite(j); // We need this step to clean FileNumbers + if(j%128 == 0) Serial.printf_P(PSTR("+")); } g.log_last_filenumber.set_and_save(0); diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index e01c4f105a..a571914f6f 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -116,31 +116,26 @@ static void init_ardupilot() // // Check the EEPROM format version before loading any parameters from EEPROM. // - if (!g.format_version.load()) { - - Serial.println_P(PSTR("\nEEPROM blank - resetting all parameters to defaults...\n")); - delay(100); // wait for serial msg to flush - - AP_Var::erase_all(); - - // save the current format version - g.format_version.set_and_save(Parameters::k_format_version); - - } else if (g.format_version != Parameters::k_format_version) { - - Serial.printf_P(PSTR("\n\nEEPROM format version %d not compatible with this firmware (requires %d)" - "\n\nForcing complete parameter reset..."), - g.format_version.get(), Parameters::k_format_version); - delay(100); // wait for serial msg to flush + + if (!g.format_version.load() || + g.format_version != Parameters::k_format_version) { // erase all parameters + Serial.printf_P(PSTR("Firmware change: erasing EEPROM...\n")); + delay(100); // wait for serial send AP_Var::erase_all(); - - // save the new format version + + // erase DataFlash on format version change + #if LOGGING_ENABLED == ENABLED + DataFlash.Init(); + erase_logs(NULL, NULL); + #endif + + // save the current format version g.format_version.set_and_save(Parameters::k_format_version); - Serial.println_P(PSTR("done.")); - } else { + + } else { unsigned long before = micros(); // Load all auto-loaded EEPROM variables AP_Var::load_all(); From b3b218fa4093d9c62a35cb42e2b04152fa154bf5 Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Mon, 28 Nov 2011 19:13:18 -0700 Subject: [PATCH 11/16] Fix for intermittent bug of log #1 starting on page 2 and confusing file system --- ArduCopter/Log.pde | 10 +++------- ArduPlane/Log.pde | 10 +++------- 2 files changed, 6 insertions(+), 14 deletions(-) diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index c9adce420f..42d854afc0 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -234,13 +234,9 @@ static byte get_num_logs(void) // This function starts a new log file in the DataFlash static void start_new_log() { - uint16_t last_page; - - if(g.log_last_filenumber < 1) { - last_page = 0; - } else { - last_page = find_last(); - } + uint16_t last_page = find_last(); + if(last_page == 1) last_page = 0; + g.log_last_filenumber.set_and_save(g.log_last_filenumber+1); DataFlash.SetFileNumber(g.log_last_filenumber); DataFlash.StartWrite(last_page + 1); diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index b6b8d22a4b..db61ee5f3f 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -234,13 +234,9 @@ static byte get_num_logs(void) // This function starts a new log file in the DataFlash static void start_new_log() { - uint16_t last_page; - - if(g.log_last_filenumber < 1) { - last_page = 0; - } else { - last_page = find_last(); - } + uint16_t last_page = find_last(); + if(last_page == 1) last_page = 0; + g.log_last_filenumber.set_and_save(g.log_last_filenumber+1); DataFlash.SetFileNumber(g.log_last_filenumber); DataFlash.StartWrite(last_page + 1); From 7e1d41be1434146e09f89b371fd2a9d001bb07f9 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Mon, 28 Nov 2011 21:26:55 -0800 Subject: [PATCH 12/16] Fix error with calc_nav_pitch_roll --- ArduCopter/navigation.pde | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 5399dc456b..01718439e2 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -169,7 +169,6 @@ static void calc_nav_rate(int max_speed) max_speed = min(max_speed, waypoint_speed_gov); } - // XXX target_angle should be the original desired target angle! //float temp = radians((target_bearing - g_gps->ground_course)/100.0); float temp = (target_bearing - g_gps->ground_course) * RADX100; @@ -187,8 +186,6 @@ static void calc_nav_rate(int max_speed) y_rate_error = max_speed - y_actual_speed; // 413 y_rate_error = constrain(y_rate_error, -800, 800); // added a rate error limit to keep pitching down to a minimum nav_lat = constrain(g.pi_nav_lat.get_pi(y_rate_error, dTnav), -3500, 3500); - // 400cm/s * 3 = 1200 or 12 deg pitch - // 800cm/s * 3 = 2400 or 24 deg pitch MAX // nav_lat and nav_lon will be rotated to the angle of the quad in calc_nav_pitch_roll() @@ -227,7 +224,7 @@ static int32_t cross_track_test() // nav_roll, nav_pitch static void calc_nav_pitch_roll() { - float temp = (9000l - (dcm.yaw_sensor - original_target_bearing)) * RADX100; + float temp = (9000l - (dcm.yaw_sensor - target_bearing)) * RADX100; //t: 1.5465, t1: -10.9451, t2: 1.5359, t3: 1.5465 float _cos_yaw_x = cos(temp); From 8a1fcfd0f45f3b288b3f6d485cec23acb0ac72b2 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Mon, 28 Nov 2011 21:40:22 -0800 Subject: [PATCH 13/16] cleanup --- ArduCopter/system.pde | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 9618148c66..6b50b3dfcf 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -163,16 +163,13 @@ static void init_ardupilot() Serial.printf_P(PSTR("Firmware change: erasing EEPROM...\n")); delay(100); // wait for serial send AP_Var::erase_all(); - + // erase DataFlash on format version change #if LOGGING_ENABLED == ENABLED - DataFlash.Init(); + DataFlash.Init(); erase_logs(NULL, NULL); #endif - // clear logs - createing infinate lockup - //erase_logs(NULL, NULL); - // save the new format version g.format_version.set_and_save(Parameters::k_format_version); From a7e0135cd441dbc9813cfe95b243ce6e88f167b6 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Mon, 28 Nov 2011 22:28:51 -0800 Subject: [PATCH 14/16] adding flag for finished mission --- ArduCopter/commands_process.pde | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ArduCopter/commands_process.pde b/ArduCopter/commands_process.pde index d799c3063c..6002d81173 100644 --- a/ArduCopter/commands_process.pde +++ b/ArduCopter/commands_process.pde @@ -53,6 +53,8 @@ static void update_commands() // this is a conditional command so we skip it command_nav_queue.id = NO_COMMAND; } + }else{ + command_nav_index == 255; } } From 9b47e376d54b2a0b21ded768ab7948e06f38edec Mon Sep 17 00:00:00 2001 From: Jason Short Date: Mon, 28 Nov 2011 22:41:12 -0800 Subject: [PATCH 15/16] Enabled Ch7 simple mode switching --- ArduCopter/control_modes.pde | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index c443158bcd..94b6f7e3be 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -14,11 +14,11 @@ static void read_control_switch() set_mode(flight_modes[switchPosition]); - #if CH7_OPTION != CH7_SIMPLE_MODE + if(g.ch7_option == CH7_SIMPLE_MODE){ // setup Simple mode // do we enable simple mode? do_simple = (g.simple_modes & (1 << switchPosition)); - #endif + } }else{ switch_debouncer = true; } From 20bceddeefd05cba11e19bc91fc2f7b4bf2163f7 Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Tue, 29 Nov 2011 21:49:11 +0800 Subject: [PATCH 16/16] APM Planner 1.0.99 trial Scripting - rc based fix zoom to cancel add srtm alt and auto download --- .../ArdupilotMegaPlanner/ArdupilotMega.csproj | 1 + .../GCSViews/FlightData.Designer.cs | 10 + .../GCSViews/FlightData.cs | 96 +++++ .../GCSViews/FlightData.resx | 111 +++-- .../GCSViews/FlightPlanner.cs | 22 +- Tools/ArdupilotMegaPlanner/HUD.cs | 2 +- Tools/ArdupilotMegaPlanner/MainV2.cs | 16 +- .../Properties/AssemblyInfo.cs | 2 +- Tools/ArdupilotMegaPlanner/Script.cs | 89 ++-- .../Release/ArdupilotMegaPlanner.application | 2 +- .../bin/Release/dataflashlog.xml | 14 +- Tools/ArdupilotMegaPlanner/dataflashlog.xml | 14 +- Tools/ArdupilotMegaPlanner/srtm.cs | 402 +++++++++++++----- 13 files changed, 559 insertions(+), 222 deletions(-) diff --git a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj index 761ff0cb1c..c1c1176ee0 100644 --- a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj +++ b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj @@ -581,6 +581,7 @@ Always + Designer diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs index bddd7f9853..a5650efb77 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs @@ -22,6 +22,7 @@ this.bindingSource1 = new System.Windows.Forms.BindingSource(this.components); this.tabControl1 = new System.Windows.Forms.TabControl(); this.tabActions = new System.Windows.Forms.TabPage(); + this.BUT_script = new ArdupilotMega.MyButton(); this.BUT_joystick = new ArdupilotMega.MyButton(); this.BUT_quickmanual = new ArdupilotMega.MyButton(); this.BUT_quickrtl = new ArdupilotMega.MyButton(); @@ -236,6 +237,7 @@ // // tabActions // + this.tabActions.Controls.Add(this.BUT_script); this.tabActions.Controls.Add(this.BUT_joystick); this.tabActions.Controls.Add(this.BUT_quickmanual); this.tabActions.Controls.Add(this.BUT_quickrtl); @@ -254,6 +256,13 @@ this.tabActions.Name = "tabActions"; this.tabActions.UseVisualStyleBackColor = true; // + // BUT_script + // + resources.ApplyResources(this.BUT_script, "BUT_script"); + this.BUT_script.Name = "BUT_script"; + this.BUT_script.UseVisualStyleBackColor = true; + this.BUT_script.Click += new System.EventHandler(this.BUT_script_Click); + // // BUT_joystick // resources.ApplyResources(this.BUT_joystick, "BUT_joystick"); @@ -1331,5 +1340,6 @@ private MyLabel lbl_logpercent; private System.Windows.Forms.ToolStripMenuItem pointCameraHereToolStripMenuItem; private System.Windows.Forms.SplitContainer splitContainer1; + private MyButton BUT_script; } } \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs index 23e320a9e3..34b0c68756 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs @@ -1539,5 +1539,101 @@ namespace ArdupilotMega.GCSViews MainV2.comPort.setMountControl(gotolocation.Lat, gotolocation.Lng, (int)(intalt / MainV2.cs.multiplierdist), true); } + + private void BUT_script_Click(object sender, EventArgs e) + { + + System.Threading.Thread t11 = new System.Threading.Thread(new System.Threading.ThreadStart(ScriptStart)) + { + IsBackground = true, + Name = "Script Thread" + }; + t11.Start(); + } + + void ScriptStart() + { + string myscript = @" +print 'Start Script' +for chan in range(1,9): + Script.SendRC(chan,1500,False) +Script.SendRC(3,Script.GetParam('RC3_MIN'),True) + +Script.Sleep(5000) +while cs.lat == 0: + print 'Waiting for GPS' + Script.Sleep(1000) +print 'Got GPS' +jo = 10 * 13 +print jo +Script.SendRC(3,1000,False) +Script.SendRC(4,2000,True) +cs.messages.Clear() +Script.WaitFor('ARMING MOTORS',30000) +Script.SendRC(4,1500,True) +print 'Motors Armed!' + +Script.SendRC(3,1700,True) +while cs.alt < 50: + Script.Sleep(50) + +Script.SendRC(5,2000,True) # acro + +Script.SendRC(1,2000,False) # roll +Script.SendRC(3,1370,True) # throttle +while cs.roll > -45: # top hald 0 - 180 + Script.Sleep(5) +while cs.roll < -45: # -180 - -45 + Script.Sleep(5) + +Script.SendRC(5,1500,False) # stabalise +Script.SendRC(1,1500,True) # level roll +Script.Sleep(2000) # 2 sec to stabalise +Script.SendRC(3,1300,True) # throttle back to land + +thro = 1350 # will decend + +while cs.alt > 0.1: + Script.Sleep(300) + +Script.SendRC(3,1000,False) +Script.SendRC(4,1000,True) +Script.WaitFor('DISARMING MOTORS',30000) +Script.SendRC(4,1500,True) + +print 'Roll complete' + +"; + + MessageBox.Show("This is Very ALPHA"); + + Form scriptedit = new Form(); + + scriptedit.Size = new System.Drawing.Size(500,500); + + TextBox tb = new TextBox(); + + tb.Dock = DockStyle.Fill; + + tb.ScrollBars = ScrollBars.Both; + tb.Multiline = true; + + tb.Location = new Point(0,0); + tb.Size = new System.Drawing.Size(scriptedit.Size.Width-30,scriptedit.Size.Height-30); + + scriptedit.Controls.Add(tb); + + tb.Text = myscript; + + scriptedit.ShowDialog(); + + if (DialogResult.Yes == MessageBox.Show("Run Script", "Run this script?", MessageBoxButtons.YesNo)) + { + + Script scr = new Script(); + + scr.runScript(tb.Text); + } + } } } \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx index 845fa8bfd8..6eb95e738d 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx @@ -208,7 +208,7 @@ hud1 - hud.HUD, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + hud.HUD, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc SubMainLeft.Panel1 @@ -228,6 +228,33 @@ 0 + + NoControl + + + 17, 93 + + + 66, 23 + + + 78 + + + Script + + + BUT_script + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + + + tabActions + + + 0 + NoControl @@ -253,13 +280,13 @@ BUT_joystick - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc tabActions - 0 + 1 NoControl @@ -283,13 +310,13 @@ BUT_quickmanual - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc tabActions - 1 + 2 NoControl @@ -313,13 +340,13 @@ BUT_quickrtl - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc tabActions - 2 + 3 NoControl @@ -343,13 +370,13 @@ BUT_quickauto - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc tabActions - 3 + 4 0 (Home) @@ -373,7 +400,7 @@ tabActions - 4 + 5 NoControl @@ -397,13 +424,13 @@ BUT_setwp - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc tabActions - 5 + 6 4, 64 @@ -424,7 +451,7 @@ tabActions - 6 + 7 NoControl @@ -448,13 +475,13 @@ BUT_setmode - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc tabActions - 7 + 8 NoControl @@ -478,13 +505,13 @@ BUT_clear_track - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc tabActions - 8 + 9 4, 6 @@ -505,7 +532,7 @@ tabActions - 9 + 10 NoControl @@ -529,13 +556,13 @@ BUT_Homealt - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc tabActions - 10 + 11 NoControl @@ -559,13 +586,13 @@ BUT_RAWSensor - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc tabActions - 11 + 12 NoControl @@ -589,13 +616,13 @@ BUTrestartmission - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc tabActions - 12 + 13 NoControl @@ -619,13 +646,13 @@ BUTactiondo - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc tabActions - 13 + 14 4, 22 @@ -673,7 +700,7 @@ Gvspeed - AGaugeApp.AGauge, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + AGaugeApp.AGauge, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc tabGauges @@ -703,7 +730,7 @@ Gheading - AGaugeApp.AGauge, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + AGaugeApp.AGauge, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc tabGauges @@ -733,7 +760,7 @@ Galt - AGaugeApp.AGauge, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + AGaugeApp.AGauge, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc tabGauges @@ -766,7 +793,7 @@ Gspeed - AGaugeApp.AGauge, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + AGaugeApp.AGauge, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc tabGauges @@ -847,7 +874,7 @@ lbl_logpercent - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc tabTLogs @@ -898,7 +925,7 @@ BUT_log2kml - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc tabTLogs @@ -949,7 +976,7 @@ BUT_playlog - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc tabTLogs @@ -976,7 +1003,7 @@ BUT_loadtelem - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc tabTLogs @@ -1162,7 +1189,7 @@ lbl_winddir - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc splitContainer1.Panel2 @@ -1192,7 +1219,7 @@ lbl_windvel - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc splitContainer1.Panel2 @@ -1427,7 +1454,7 @@ TXT_lat - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc panel1 @@ -1484,7 +1511,7 @@ label1 - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc panel1 @@ -1514,7 +1541,7 @@ TXT_long - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc panel1 @@ -1544,7 +1571,7 @@ TXT_alt - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc panel1 @@ -1745,7 +1772,7 @@ label6 - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc $this @@ -1823,6 +1850,6 @@ FlightData - System.Windows.Forms.MyUserControl, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + System.Windows.Forms.MyUserControl, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs index 8f65838cdf..55f50b788b 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs @@ -342,7 +342,7 @@ namespace ArdupilotMega.GCSViews { TXT_mouselat.Text = lat.ToString(); TXT_mouselong.Text = lng.ToString(); - TXT_mousealt.Text = srtm.getAltitude(lat, lng).ToString("0"); + TXT_mousealt.Text = srtm.getAltitude(lat, lng, MainMap.Zoom).ToString("0"); try { @@ -2754,16 +2754,18 @@ namespace ArdupilotMega.GCSViews private void BUT_zoomto_Click(object sender, EventArgs e) { string place = "Perth, Australia"; - Common.InputBox("Location", "Enter your location", ref place); + if (DialogResult.OK == Common.InputBox("Location", "Enter your location", ref place)) + { - GeoCoderStatusCode status = MainMap.SetCurrentPositionByKeywords(place); - if (status != GeoCoderStatusCode.G_GEO_SUCCESS) - { - MessageBox.Show("Google Maps Geocoder can't find: '" + place + "', reason: " + status.ToString(), "GMap.NET", MessageBoxButtons.OK, MessageBoxIcon.Exclamation); - } - else - { - MainMap.Zoom = 15; + GeoCoderStatusCode status = MainMap.SetCurrentPositionByKeywords(place); + if (status != GeoCoderStatusCode.G_GEO_SUCCESS) + { + MessageBox.Show("Google Maps Geocoder can't find: '" + place + "', reason: " + status.ToString(), "GMap.NET", MessageBoxButtons.OK, MessageBoxIcon.Exclamation); + } + else + { + MainMap.Zoom = 15; + } } } } diff --git a/Tools/ArdupilotMegaPlanner/HUD.cs b/Tools/ArdupilotMegaPlanner/HUD.cs index f22114f12b..58515757a5 100644 --- a/Tools/ArdupilotMegaPlanner/HUD.cs +++ b/Tools/ArdupilotMegaPlanner/HUD.cs @@ -264,7 +264,7 @@ namespace hud if (DateTime.Now.Second != countdate.Second) { countdate = DateTime.Now; - Console.WriteLine("HUD " + count + " hz drawtime " + (huddrawtime / count) + " gl " + opengl); + //Console.WriteLine("HUD " + count + " hz drawtime " + (huddrawtime / count) + " gl " + opengl); count = 0; huddrawtime = 0; } diff --git a/Tools/ArdupilotMegaPlanner/MainV2.cs b/Tools/ArdupilotMegaPlanner/MainV2.cs index 5288c28de1..c14c2567fe 100644 --- a/Tools/ArdupilotMegaPlanner/MainV2.cs +++ b/Tools/ArdupilotMegaPlanner/MainV2.cs @@ -21,7 +21,6 @@ using System.Threading; using System.Net.Sockets; using IronPython.Hosting; - namespace ArdupilotMega { public partial class MainV2 : Form @@ -109,15 +108,10 @@ namespace ArdupilotMega //return; - var engine = Python.CreateEngine(); - var scope = engine.CreateScope(); - scope.SetVariable("MainV2",this); - Console.WriteLine(DateTime.Now.Millisecond); - engine.CreateScriptSourceFromString("print 'hello world from python'").Execute(scope); - Console.WriteLine(DateTime.Now.Millisecond); - engine.CreateScriptSourceFromString("MainV2.testpython()").Execute(scope); - Console.WriteLine(DateTime.Now.Millisecond); + // preload + Python.CreateEngine(); + var t = Type.GetType("Mono.Runtime"); MONO = (t != null); @@ -678,7 +672,7 @@ namespace ArdupilotMega comPort.logfile = new BinaryWriter(File.Open(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs" + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd hh-mm-ss") + ".tlog", FileMode.CreateNew)); } catch { MessageBox.Show("Failed to create log - wont log this session"); } // soft fail - + comPort.BaseStream.PortName = CMB_serialport.Text; comPort.Open(true); @@ -1100,7 +1094,7 @@ namespace ArdupilotMega if (heatbeatsend.Second != DateTime.Now.Second) { - Console.WriteLine("remote lost {0}", cs.packetdropremote); +// Console.WriteLine("remote lost {0}", cs.packetdropremote); MAVLink.__mavlink_heartbeat_t htb = new MAVLink.__mavlink_heartbeat_t(); diff --git a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs index 41b0f8de1a..4c387fa873 100644 --- a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs +++ b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs @@ -34,5 +34,5 @@ using System.Resources; // by using the '*' as shown below: // [assembly: AssemblyVersion("1.0.*")] [assembly: AssemblyVersion("1.0.0.0")] -[assembly: AssemblyFileVersion("1.0.98")] +[assembly: AssemblyFileVersion("1.0.99")] [assembly: NeutralResourcesLanguageAttribute("")] diff --git a/Tools/ArdupilotMegaPlanner/Script.cs b/Tools/ArdupilotMegaPlanner/Script.cs index d18b5c9aac..534b3aa678 100644 --- a/Tools/ArdupilotMegaPlanner/Script.cs +++ b/Tools/ArdupilotMegaPlanner/Script.cs @@ -2,6 +2,7 @@ using System.Collections.Generic; using System.Linq; using System.Text; +using IronPython.Hosting; namespace ArdupilotMega { @@ -9,12 +10,27 @@ namespace ArdupilotMega { DateTime timeout = DateTime.Now; List items = new List(); + Microsoft.Scripting.Hosting.ScriptEngine engine; + Microsoft.Scripting.Hosting.ScriptScope scope; // keeps history MAVLink.__mavlink_rc_channels_override_t rc = new MAVLink.__mavlink_rc_channels_override_t(); public Script() { + engine = Python.CreateEngine(); + scope = engine.CreateScope(); + + scope.SetVariable("cs", MainV2.cs); + scope.SetVariable("Script", this); + + Console.WriteLine(DateTime.Now.Millisecond); + engine.CreateScriptSourceFromString("print 'hello world from python'").Execute(scope); + Console.WriteLine(DateTime.Now.Millisecond); + engine.CreateScriptSourceFromString("print cs.roll").Execute(scope); + Console.WriteLine(DateTime.Now.Millisecond); + + object thisBoxed = MainV2.cs; Type test = thisBoxed.GetType(); @@ -33,7 +49,24 @@ namespace ArdupilotMega items.Add(field.Name); } - } + } + + + public void Sleep(int ms) { + System.Threading.Thread.Sleep(ms); + } + + public void runScript(string script) + { + try + { + engine.CreateScriptSourceFromString(script).Execute(scope); + } + catch (Exception e) + { + System.Windows.Forms.MessageBox.Show("Error running script " + e.Message); + } + } public enum Conditional { @@ -51,36 +84,34 @@ namespace ArdupilotMega return MainV2.comPort.setParam(param, value); } + public float GetParam(string param) + { + if (MainV2.comPort.param[param] != null) + return (float)MainV2.comPort.param[param]; + + return 0.0f; + } + public bool ChangeMode(string mode) { MainV2.comPort.setMode(mode); return true; } - public bool WaitFor(string message) + public bool WaitFor(string message, int timeout) { - while (MainV2.cs.message != message) { + int timein = 0; + while (!MainV2.cs.message.Contains(message)) { System.Threading.Thread.Sleep(5); + timein += 5; + if (timein > timeout) + return false; } return true; } - - public bool WaitFor(string item, Conditional cond,double value ,int timeoutms) - { - timeout = DateTime.Now; - while (DateTime.Now < timeout.AddMilliseconds(timeoutms)) - { - //if (item) - { - - } - } - - return false; - } - - public bool sendRC(int channel, ushort pwm) + + public bool SendRC(int channel, ushort pwm,bool sendnow) { switch (channel) { @@ -118,18 +149,18 @@ namespace ArdupilotMega break; } - MainV2.comPort.sendPacket(rc); - System.Threading.Thread.Sleep(20); - MainV2.comPort.sendPacket(rc); - MainV2.comPort.sendPacket(rc); + rc.target_component = MainV2.comPort.compid; + rc.target_system = MainV2.comPort.sysid; + + if (sendnow) + { + MainV2.comPort.sendPacket(rc); + System.Threading.Thread.Sleep(20); + MainV2.comPort.sendPacket(rc); + MainV2.comPort.sendPacket(rc); + } return true; } - - void convertItemtoMessage() - { - - } - } } diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application b/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application index 862465c9a5..ab815306fe 100644 --- a/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application +++ b/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application @@ -11,7 +11,7 @@ - XqmS8DEyaXOEHAzbfxq+pbxDUg4= + QOAVY4eRCMREZxVv8+wq0bmXS7U= diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/dataflashlog.xml b/Tools/ArdupilotMegaPlanner/bin/Release/dataflashlog.xml index 6bb291dd77..dc4cc836fe 100644 --- a/Tools/ArdupilotMegaPlanner/bin/Release/dataflashlog.xml +++ b/Tools/ArdupilotMegaPlanner/bin/Release/dataflashlog.xml @@ -42,17 +42,19 @@ Nav Throttle Angle boost Manual boost + climb rate + rc3 servo out alt hold int thro int - control mode - yaw mode - r p mode - thro mode - thro cruise - thro int + time + gyro sat + adc const + renorm sqrt + renorm blowup + fix count Gyro X diff --git a/Tools/ArdupilotMegaPlanner/dataflashlog.xml b/Tools/ArdupilotMegaPlanner/dataflashlog.xml index 6bb291dd77..dc4cc836fe 100644 --- a/Tools/ArdupilotMegaPlanner/dataflashlog.xml +++ b/Tools/ArdupilotMegaPlanner/dataflashlog.xml @@ -42,17 +42,19 @@ Nav Throttle Angle boost Manual boost + climb rate + rc3 servo out alt hold int thro int - control mode - yaw mode - r p mode - thro mode - thro cruise - thro int + time + gyro sat + adc const + renorm sqrt + renorm blowup + fix count Gyro X diff --git a/Tools/ArdupilotMegaPlanner/srtm.cs b/Tools/ArdupilotMegaPlanner/srtm.cs index 1bc8ca401f..08e5bca0ef 100644 --- a/Tools/ArdupilotMegaPlanner/srtm.cs +++ b/Tools/ArdupilotMegaPlanner/srtm.cs @@ -3,6 +3,10 @@ using System.Collections.Generic; using System.Linq; using System.Text; using System.IO; +using System.Net; +using System.Text.RegularExpressions; +using ICSharpCode.SharpZipLib.Zip; +using System.Threading; namespace ArdupilotMega { @@ -10,7 +14,15 @@ namespace ArdupilotMega { public static string datadirectory; - public static int getAltitude(double lat, double lng) + static List allhgts = new List(); + + static object objlock = new object(); + + static Thread requestThread; + + static List queue = new List(); + + public static int getAltitude(double lat, double lng, double zoom) { short alt = 0; @@ -36,136 +48,296 @@ namespace ArdupilotMega string filename2 = "srtm_" + Math.Round((lng + 2.5 + 180) / 5, 0).ToString("00") + "_" + Math.Round((60 - lat + 2.5) / 5, 0).ToString("00") + ".asc"; - if (File.Exists(datadirectory + Path.DirectorySeparatorChar + filename)) - { // srtm hgt files - FileStream fs = new FileStream(datadirectory + Path.DirectorySeparatorChar + filename, FileMode.Open, FileAccess.Read); - - float posx = 0; - float row = 0; - - if (fs.Length <= (1201 * 1201 * 2)) - { - posx = (int)(((float)(lng - x)) * (1201 * 2)); - row = (int)(((float)(lat - y)) * 1201) * (1201 * 2); - row = (1201 * 1201 * 2) - row; - } - else - { - posx = (int)(((float)(lng - x)) * (3601 * 2)); - row = (int)(((float)(lat - y)) * 3601) * (3601 * 2); - row = (3601 * 3601 * 2) - row; - } - - if (posx % 2 == 1) - { - posx--; - } - - //Console.WriteLine(filename + " row " + row + " posx" + posx); - - byte[] data = new byte[2]; - - fs.Seek((int)(row + posx), SeekOrigin.Begin); - fs.Read(data, 0, data.Length); - - fs.Close(); - fs.Dispose(); - - Array.Reverse(data); - - alt = BitConverter.ToInt16(data, 0); - - return alt; - } - else if (File.Exists(datadirectory + Path.DirectorySeparatorChar + filename2)) + try { - // this is way to slow - and cacheing it will chew memory 6001 * 6001 * 4 = 144048004 bytes - FileStream fs = new FileStream(datadirectory + Path.DirectorySeparatorChar + filename2, FileMode.Open, FileAccess.Read); - StreamReader sr = new StreamReader(fs); + if (File.Exists(datadirectory + Path.DirectorySeparatorChar + filename)) + { // srtm hgt files + FileStream fs = new FileStream(datadirectory + Path.DirectorySeparatorChar + filename, FileMode.Open, FileAccess.Read, FileShare.Read); - int nox = 0; - int noy = 0; - float left = 0; - float top = 0; - int nodata = -9999; - float cellsize = 0; + float posx = 0; + float row = 0; - int rowcounter = 0; - - float wantrow = 0; - float wantcol = 0; - - - while (!sr.EndOfStream) - { - string line = sr.ReadLine(); - - if (line.StartsWith("ncols")) + if (fs.Length <= (1201 * 1201 * 2)) { - nox = int.Parse(line.Substring(line.IndexOf(' '))); - - //hgtdata = new int[nox * noy]; - } - else if (line.StartsWith("nrows")) - { - noy = int.Parse(line.Substring(line.IndexOf(' '))); - - //hgtdata = new int[nox * noy]; - } - else if (line.StartsWith("xllcorner")) - { - left = float.Parse(line.Substring(line.IndexOf(' '))); - } - else if (line.StartsWith("yllcorner")) - { - top = float.Parse(line.Substring(line.IndexOf(' '))); - } - else if (line.StartsWith("cellsize")) - { - cellsize = float.Parse(line.Substring(line.IndexOf(' '))); - } - else if (line.StartsWith("NODATA_value")) - { - nodata = int.Parse(line.Substring(line.IndexOf(' '))); + posx = (int)(((float)(lng - x)) * (1201 * 2)); + row = (int)(((float)(lat - y)) * 1201) * (1201 * 2); + row = (1201 * 1201 * 2) - row; } else { - string[] data = line.Split(new char[] { ' ' }); - - if (data.Length == (nox + 1)) - { - - - - wantcol = (float)((lng - Math.Round(left,0))); - - wantrow = (float)((lat - Math.Round(top,0))); - - wantrow =(int)( wantrow / cellsize); - wantcol =(int)( wantcol / cellsize); - - wantrow = noy - wantrow; - - if (rowcounter == wantrow) - { - Console.WriteLine("{0} {1} {2} {3} ans {4} x {5}", lng, lat, left, top, data[(int)wantcol], (nox + wantcol * cellsize)); - - return int.Parse(data[(int)wantcol]); - } - - rowcounter++; - } + posx = (int)(((float)(lng - x)) * (3601 * 2)); + row = (int)(((float)(lat - y)) * 3601) * (3601 * 2); + row = (3601 * 3601 * 2) - row; } + if (posx % 2 == 1) + { + posx--; + } + + //Console.WriteLine(filename + " row " + row + " posx" + posx); + + byte[] data = new byte[2]; + + fs.Seek((int)(row + posx), SeekOrigin.Begin); + fs.Read(data, 0, data.Length); + + fs.Close(); + fs.Dispose(); + + Array.Reverse(data); + + alt = BitConverter.ToInt16(data, 0); + + return alt; + } + else if (File.Exists(datadirectory + Path.DirectorySeparatorChar + filename2)) + { + // this is way to slow - and cacheing it will chew memory 6001 * 6001 * 4 = 144048004 bytes + FileStream fs = new FileStream(datadirectory + Path.DirectorySeparatorChar + filename2, FileMode.Open, FileAccess.Read); + + StreamReader sr = new StreamReader(fs); + + int nox = 0; + int noy = 0; + float left = 0; + float top = 0; + int nodata = -9999; + float cellsize = 0; + + int rowcounter = 0; + + float wantrow = 0; + float wantcol = 0; + while (!sr.EndOfStream) + { + string line = sr.ReadLine(); + + if (line.StartsWith("ncols")) + { + nox = int.Parse(line.Substring(line.IndexOf(' '))); + + //hgtdata = new int[nox * noy]; + } + else if (line.StartsWith("nrows")) + { + noy = int.Parse(line.Substring(line.IndexOf(' '))); + + //hgtdata = new int[nox * noy]; + } + else if (line.StartsWith("xllcorner")) + { + left = float.Parse(line.Substring(line.IndexOf(' '))); + } + else if (line.StartsWith("yllcorner")) + { + top = float.Parse(line.Substring(line.IndexOf(' '))); + } + else if (line.StartsWith("cellsize")) + { + cellsize = float.Parse(line.Substring(line.IndexOf(' '))); + } + else if (line.StartsWith("NODATA_value")) + { + nodata = int.Parse(line.Substring(line.IndexOf(' '))); + } + else + { + string[] data = line.Split(new char[] { ' ' }); + + if (data.Length == (nox + 1)) + { + + + + wantcol = (float)((lng - Math.Round(left, 0))); + + wantrow = (float)((lat - Math.Round(top, 0))); + + wantrow = (int)(wantrow / cellsize); + wantcol = (int)(wantcol / cellsize); + + wantrow = noy - wantrow; + + if (rowcounter == wantrow) + { + Console.WriteLine("{0} {1} {2} {3} ans {4} x {5}", lng, lat, left, top, data[(int)wantcol], (nox + wantcol * cellsize)); + + return int.Parse(data[(int)wantcol]); + } + + rowcounter++; + } + } + + + + } + + return alt; + } + else // get something + { + if (zoom >= 15) + { + if (requestThread == null) + { + Console.WriteLine("Getting " + filename); + queue.Add(filename); + + requestThread = new Thread(requestRunner); + requestThread.IsBackground = true; + requestThread.Start(); + } + else + { + lock (objlock) + { + if (!queue.Contains(filename)) + { + Console.WriteLine("Getting " + filename); + queue.Add(filename); + } + } + } + } } - return alt; } + catch { alt = 0; } return alt; } + + static void requestRunner() + { + while (true) + { + try + { + string item = ""; + lock (objlock) + { + if (queue.Count > 0) + { + item = queue[0]; + } + } + + if (item != "") + { + get3secfile(item); + lock (objlock) + { + queue.Remove(item); + } + } + } + catch { } + Thread.Sleep(100); + } + } + + static void get3secfile(object name) + { + string baseurl = "http://dds.cr.usgs.gov/srtm/version2_1/SRTM3/"; + + // check file doesnt already exist + if (File.Exists(datadirectory + Path.DirectorySeparatorChar + (string)name)) + { + return; + } + + List list = getListing(baseurl); + + foreach (string item in list) + { + List hgtfiles = getListing(item); + + foreach (string hgt in hgtfiles) + { + if (hgt.Contains((string)name)) + { + // get file + + gethgt(hgt, (string)name); + return; + } + } + } + } + + static void gethgt(string url, string filename) + { + try + { + + WebRequest req = HttpWebRequest.Create(url); + + WebResponse res = req.GetResponse(); + + Stream resstream = res.GetResponseStream(); + + BinaryWriter bw = new BinaryWriter(File.Create(datadirectory + Path.DirectorySeparatorChar + filename + ".zip")); + + byte[] buf1 = new byte[1024]; + + while (resstream.CanRead) + { + + int len = resstream.Read(buf1, 0, 1024); + if (len == 0) + break; + bw.Write(buf1, 0, len); + + } + + bw.Close(); + + FastZip fzip = new FastZip(); + + fzip.ExtractZip(datadirectory + Path.DirectorySeparatorChar + filename + ".zip", datadirectory, ""); + } + catch { } + } + + static List getListing(string url) + { + List list = new List(); + + try + { + + WebRequest req = HttpWebRequest.Create(url); + + WebResponse res = req.GetResponse(); + + StreamReader resstream = new StreamReader(res.GetResponseStream()); + + string data = resstream.ReadToEnd(); + + Regex regex = new Regex("href=\"([^\"]+)\"", RegexOptions.IgnoreCase); + if (regex.IsMatch(data)) + { + MatchCollection matchs = regex.Matches(data); + for (int i = 0; i < matchs.Count; i++) + { + if (matchs[i].Groups[1].Value.ToString().Contains("..")) + continue; + if (matchs[i].Groups[1].Value.ToString().Contains("http")) + continue; + + list.Add(url.TrimEnd(new char[] { '/', '\\' }) + "/" + matchs[i].Groups[1].Value.ToString()); + + } + } + } + catch { } + + return list; + } } -} +} \ No newline at end of file