diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 444286aca3..8492a37c5c 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -391,7 +391,7 @@ struct Location prev_WP; // last waypoint struct Location current_loc; // current location struct Location next_WP; // next waypoint struct Location target_WP; // where do we want to you towards? -struct Location tell_command; // command for telemetry +struct Location simple_WP; // command for telemetry struct Location next_command; // command preloaded long target_altitude; // used for boolean home_is_set; // Flag for if we have g_gps lock and have set the home location @@ -965,16 +965,16 @@ void update_current_flight_mode(void) if(flight_timer > 4){ flight_timer = 0; - tell_command.lat = 0; - tell_command.lng = 0; + simple_WP.lat = 0; + simple_WP.lng = 0; next_WP.lng = (float)g.rc_1.control_in *.4; // X: 4500 / 2 = 2250 = 25 meteres next_WP.lat = -((float)g.rc_2.control_in *.4); // Y: 4500 / 2 = 2250 = 25 meteres // calc a new bearing - nav_bearing = get_bearing(&tell_command, &next_WP) + initial_simple_bearing; + nav_bearing = get_bearing(&simple_WP, &next_WP) + initial_simple_bearing; nav_bearing = wrap_360(nav_bearing); - wp_distance = get_distance(&tell_command, &next_WP); + wp_distance = get_distance(&simple_WP, &next_WP); calc_bearing_error(); /* Serial.printf("lat: %ld lon:%ld, bear:%ld, dist:%ld, init:%ld, err:%ld ", @@ -1183,10 +1183,10 @@ void update_alt() // XXX temp removed fr debugging //filter out bad sonar reads - int temp = sonar.read(); + int temp_sonar = sonar.read(); - if(abs(temp - sonar_alt) < 300){ - sonar_alt = temp; + if(abs(temp_sonar - sonar_alt) < 300){ + sonar_alt = temp_sonar; } //sonar_alt = sonar.read(); diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde index 44a003aaea..f6363d4021 100644 --- a/ArduCopterMega/Log.pde +++ b/ArduCopterMega/Log.pde @@ -221,10 +221,8 @@ byte get_num_logs(void) void start_new_log(byte num_existing_logs) { - int page; int start_pages[50] = {0,0,0}; int end_pages[50] = {0,0,0}; - byte data; if (num_existing_logs > 0) { for(int i=0;i 0) + while(Serial.available() > 0) { - incoming_val = Serial.read(); + incoming_val = Serial.read(); - if (incoming_val != 13 && incoming_val != 10 ) { - input_buffer[bufferPointer++] = incoming_val; + if (incoming_val != 13 && incoming_val != 10 ) { + input_buffer[bufferPointer++] = incoming_val; } if(bufferPointer >= INPUT_BUF_LEN){ @@ -43,7 +42,7 @@ void readCommands(void) } } parseCommand(input_buffer); - + // clear buffer of old data // ------------------------ memset(input_buffer,0,sizeof(input_buffer)); @@ -61,14 +60,14 @@ void parseCommand(char *buffer) Serial.println("got cmd "); Serial.println(buffer); char *token, *saveptr1, *saveptr2; - + for (int j = 1;; j++, buffer = NULL) { token = strtok_r(buffer, "|", &saveptr1); - if (token == NULL) break; - + if (token == NULL) break; + char * cmd = strtok_r(token, ":", &saveptr2); long value = strtol(strtok_r (NULL,":", &saveptr2), NULL,0); - + ///* Serial.print("cmd "); Serial.print(cmd[0]); @@ -94,7 +93,7 @@ void parseCommand(char *buffer) //g.pid_stabilize_roll.kD((float)value / 1000); //g.pid_stabilize_pitch.kD((float)value / 1000); break; - + case 'X': g.pid_stabilize_roll.imax(value * 100); g.pid_stabilize_pitch.imax(value * 100); @@ -106,7 +105,7 @@ void parseCommand(char *buffer) break; } init_pids(); - //*/ + //*/ } } diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index 5829e3be55..e579971eed 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -67,7 +67,6 @@ setup_mode(uint8_t argc, const Menu::arg *argv) static int8_t setup_show(uint8_t argc, const Menu::arg *argv) { - uint8_t i; // clear the area print_blanks(8); @@ -92,9 +91,7 @@ setup_show(uint8_t argc, const Menu::arg *argv) static int8_t setup_factory(uint8_t argc, const Menu::arg *argv) { - - uint8_t i; - int c; + int c; Serial.printf_P(PSTR("\n'Y' + Enter to factory reset, any other key to abort:\n")); @@ -200,7 +197,8 @@ setup_radio(uint8_t argc, const Menu::arg *argv) static int8_t setup_esc(uint8_t argc, const Menu::arg *argv) { - Serial.printf_P(PSTR("\nUnplug battery, calibrate as usual.\n Press Enter to cancel.\n")); + Serial.printf_P(PSTR("\nUnplug, then plug-in battery; Calibrate ESCs.\n Press Enter to cancel.\n")); + g.esc_calibrate.set_and_save(1); @@ -209,7 +207,7 @@ setup_esc(uint8_t argc, const Menu::arg *argv) if(Serial.available() > 0){ g.esc_calibrate.set_and_save(0); - break; + return(0); } } } @@ -409,7 +407,7 @@ setup_pid(uint8_t argc, const Menu::arg *argv) static int8_t setup_flightmodes(uint8_t argc, const Menu::arg *argv) { - byte switchPosition, oldSwitchPosition, mode; + byte switchPosition, _oldSwitchPosition, mode; Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes.")); print_hit_enter(); @@ -421,7 +419,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) // look for control switch change - if (oldSwitchPosition != switchPosition){ + if (_oldSwitchPosition != switchPosition){ mode = g.flight_modes[switchPosition]; mode = constrain(mode, 0, NUM_MODES-1); @@ -430,7 +428,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) print_switch(switchPosition, mode); // Remember switch position - oldSwitchPosition = switchPosition; + _oldSwitchPosition = switchPosition; } // look for stick input diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index c07b89627d..bc78fbce42 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -119,7 +119,7 @@ void init_ardupilot() } }else{ - unsigned long before = micros(); + // unsigned long before = micros(); // Load all auto-loaded EEPROM variables AP_Var::load_all(); @@ -263,7 +263,6 @@ void startup_ground(void) // ------------------- init_commands(); - byte counter = 4; GPS_enabled = false; // Read in the GPS diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index 9dd8e44da3..f89dccdc9b 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -842,8 +842,6 @@ test_sonar(uint8_t argc, const Menu::arg *argv) static int8_t test_mission(uint8_t argc, const Menu::arg *argv) { - //print_hit_enter(); - //delay(1000); //write out a basic mission to the EEPROM Location t; /*{ @@ -911,7 +909,7 @@ void fake_out_gps() g_gps->new_data = true; g_gps->fix = true; - int length = g.rc_6.control_in; + //int length = g.rc_6.control_in; rads += .05; if (rads > 6.28){