From 765e3d29350b08c96ef3ad8c39ac39b842ad7c47 Mon Sep 17 00:00:00 2001 From: jasonshort Date: Sat, 19 Feb 2011 07:21:42 +0000 Subject: [PATCH] updated to AP_Var git-svn-id: https://arducopter.googlecode.com/svn/trunk@1681 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/ArduCopterMega.pde | 8 +- ArduCopterMega/EEPROM.pde | 13 --- ArduCopterMega/commands.pde | 83 ++++++++-------- ArduCopterMega/commands_process.pde | 10 +- ArduCopterMega/radio.pde | 4 + ArduCopterMega/system.pde | 148 ++++++++++++---------------- 6 files changed, 119 insertions(+), 147 deletions(-) diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index d7957b5243..3f6e97894d 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -110,11 +110,13 @@ AP_GPS_None GPS(NULL); */ GPS *g_gps; + #if GPS_PROTOCOL == GPS_PROTOCOL_NONE -AP_GPS_None GPS(NULL); + AP_GPS_None GPS(NULL); #else -AP_GPS_Auto GPS(&Serial1, &g_gps); -#endif // GPS PROTOCOL + AP_GPS_Auto GPS(&Serial1, &g_gps); +#endif + diff --git a/ArduCopterMega/EEPROM.pde b/ArduCopterMega/EEPROM.pde index 070395ba33..8bb344d00b 100644 --- a/ArduCopterMega/EEPROM.pde +++ b/ArduCopterMega/EEPROM.pde @@ -25,19 +25,6 @@ } */ -void save_EEPROM_groundstart(void) -{ - g.rc_1.save_trim(); - g.rc_2.save_trim(); - g.rc_3.save_trim(); - g.rc_4.save_trim(); - g.rc_5.save_trim(); - g.rc_6.save_trim(); - g.rc_7.save_trim(); - g.rc_8.save_trim(); - - // pressure sensor data saved by init_home -} /********************************************************************************/ diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde index a6aa150982..ed5a8e95f8 100644 --- a/ArduCopterMega/commands.pde +++ b/ArduCopterMega/commands.pde @@ -2,7 +2,7 @@ void init_commands() { - read_EEPROM_waypoint_info(); + //read_EEPROM_waypoint_info(); g.waypoint_index.set_and_save(0); command_must_index = 0; command_may_index = 0; @@ -11,9 +11,9 @@ void init_commands() void update_auto() { - if (g.waypoint_index == g.waypoint_total){ + if (g.waypoint_index == g.waypoint_total) { return_to_launch(); - //g.waypoint_index = 0; + //wp_index = 0; } } @@ -24,7 +24,6 @@ void reload_commands() decrement_WP_index(); } - // Getters // ------- struct Location get_wp_with_index(int i) @@ -57,8 +56,7 @@ struct Location get_wp_with_index(int i) // ------- void set_wp_with_index(struct Location temp, int i) { - - i = constrain(i, 0, g.waypoint_total.get()); // XXX if i was unsigned this could be simply < g.waypoint_total + i = constrain(i, 0, g.waypoint_total.get()); uint32_t mem = WP_START_BYTE + (i * WP_SIZE); eeprom_write_byte((uint8_t *) mem, temp.id); @@ -78,20 +76,21 @@ void set_wp_with_index(struct Location temp, int i) void increment_WP_index() { - if(g.waypoint_index < g.waypoint_total){ + if (g.waypoint_index < g.waypoint_total) { g.waypoint_index.set_and_save(g.waypoint_index + 1); - Serial.printf_P(PSTR("WP index is incremented to %d\n"),(int)g.waypoint_index); + SendDebug("MSG WP index is incremented to "); }else{ - Serial.printf_P(PSTR("WP Failed to incremented WP index of %d\n"),(int)g.waypoint_index); + //SendDebug("MSG Failed to increment WP index of "); + // This message is used excessively at the end of a mission } SendDebugln(g.waypoint_index,DEC); } void decrement_WP_index() { - if(g.waypoint_index > 0){ + if (g.waypoint_index > 0) { g.waypoint_index.set_and_save(g.waypoint_index - 1); - } + } } long read_alt_to_hold() @@ -103,6 +102,15 @@ long read_alt_to_hold() return g.RTL_altitude + home.alt; } +void +set_current_loc_here() +{ + //struct Location temp; + Location l = current_loc; + l.alt = get_altitude_above_home(); + set_next_WP(&l); +} + //******************************************************************************** //This function sets the waypoint and modes for Return to Launch //******************************************************************************** @@ -126,6 +134,7 @@ return_to_launch(void) // Set by configuration tool // ------------------------- next_WP.alt = read_alt_to_hold(); + //send_message(SEVERITY_LOW,"Return To Launch"); } struct @@ -133,20 +142,11 @@ Location get_LOITER_home_wp() { // read home position struct Location temp = get_wp_with_index(0); - temp.id = CMD_LOITER; - temp.alt = read_alt_to_hold(); + temp.id = MAV_CMD_NAV_LOITER_UNLIM; + temp.alt = read_alt_to_hold() - home.alt; // will be incremented up by home.alt in set_next_WP return temp; } -void -set_current_loc_here() -{ - //struct Location temp; - Location l = current_loc; - l.alt = get_altitude_above_home(); - set_next_WP(&l); -} - /* This function stores waypoint commands It looks to see what the next command type is and finds the last command. @@ -154,12 +154,14 @@ It looks to see what the next command type is and finds the last command. void set_next_WP(struct Location *wp) { - Serial.printf_P(PSTR("set_next_WP, wp_index %d\n"), (int)g.waypoint_index); - send_message(MSG_COMMAND_LIST, g.waypoint_index); + //GCS.send_text(SEVERITY_LOW,"load WP"); + SendDebug("MSG wp_index: "); + SendDebugln(g.waypoint_index, DEC); + gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); // copy the current WP into the OldWP slot // --------------------------------------- - prev_WP = current_loc; + prev_WP = next_WP; // Load the next_WP slot // --------------------- @@ -172,31 +174,30 @@ set_next_WP(struct Location *wp) // used to control FBW and limit the rate of climb // ----------------------------------------------- target_altitude = current_loc.alt; // PH: target_altitude = 200 - offset_altitude = next_WP.alt - current_loc.alt; // PH: offset_altitude = 0 + offset_altitude = next_WP.alt - prev_WP.alt; // PH: offset_altitude = 0 // zero out our loiter vals to watch for missed waypoints - loiter_delta = 0; - loiter_sum = 0; - loiter_total = 0; + loiter_delta = 0; + loiter_sum = 0; + loiter_total = 0; - float rads = (abs(next_WP.lat)/t7) * 0.0174532925; - //377,173,810 / 10,000,000 = 37.717381 * 0.0174532925 = 0.658292482926943 - scaleLongDown = cos(rads); - scaleLongUp = 1.0f/cos(rads); + // this is used to offset the shrinking longitude as we go towards the poles + float rads = (abs(next_WP.lat)/t7) * 0.0174532925; + scaleLongDown = cos(rads); + scaleLongUp = 1.0f/cos(rads); // this is handy for the groundstation wp_totalDistance = getDistance(¤t_loc, &next_WP); - wp_distance = wp_totalDistance; - - print_current_waypoints(); - target_bearing = get_bearing(¤t_loc, &next_WP); + + wp_distance = wp_totalDistance; old_target_bearing = target_bearing; - // this is used to offset the shrinking longitude as we go towards the poles // set a new crosstrack bearing // ---------------------------- reset_crosstrack(); + + gcs.print_current_waypoints(); } @@ -204,7 +205,7 @@ set_next_WP(struct Location *wp) // ------------------------------- void init_home() { - Serial.printf_P(PSTR("init home\n")); + SendDebugln("MSG: init home"); // block until we get a good fix // ----------------------------- @@ -212,7 +213,7 @@ void init_home() g_gps->update(); } - home.id = CMD_WAYPOINT; + home.id = MAV_CMD_NAV_WAYPOINT; home.lng = g_gps->longitude; // Lon * 10**7 home.lat = g_gps->latitude; // Lat * 10**7 home.alt = g_gps->altitude; @@ -228,7 +229,7 @@ void init_home() // Save prev loc // ------------- - prev_WP = home; + next_WP = prev_WP = home; } diff --git a/ArduCopterMega/commands_process.pde b/ArduCopterMega/commands_process.pde index 376c153d1f..a561cb81c9 100644 --- a/ArduCopterMega/commands_process.pde +++ b/ArduCopterMega/commands_process.pde @@ -145,13 +145,13 @@ void process_must() case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint break; - case MAV_CMD_NAV_R_WAYPOINT: // Navigate to Waypoint - next_command.lat += home.lat; // offset from home location - next_command.lng += home.lng; // offset from home location + //case MAV_CMD_NAV_R_WAYPOINT: // Navigate to Waypoint + // next_command.lat += home.lat; // offset from home location + // next_command.lng += home.lng; // offset from home location // we've recalculated the WP so we need to set it again - set_next_WP(&next_command); - break; + // set_next_WP(&next_command); + // break; case MAV_CMD_NAV_LAND: // LAND to Waypoint velocity_land = 1000; diff --git a/ArduCopterMega/radio.pde b/ArduCopterMega/radio.pde index 76a4978366..80dce16b28 100644 --- a/ArduCopterMega/radio.pde +++ b/ArduCopterMega/radio.pde @@ -74,6 +74,10 @@ void trim_radio() g.rc_1.trim(); // roll g.rc_2.trim(); // pitch g.rc_4.trim(); // yaw + + g.rc_1.save_trim(); + g.rc_2.save_trim(); + g.rc_4.save_trim(); } void trim_yaw() diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index 426c97b713..2fb17b8299 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -80,16 +80,11 @@ void init_ardupilot() // the receive buffer, and the transmit buffer could also be // shrunk for protocols that don't send large messages. // -#if GCS_PORT == 3 Serial3.begin(SERIAL3_BAUD, 128, 128); -#endif - Serial.printf_P(PSTR("\n\n" - "Init ArduCopterMega 1.0.3 Public Alpha\n\n" -#if TELEMETRY_PORT == 3 - "Telemetry is on the xbee port\n" -#endif - "freeRAM: %d\n"),freeRAM()); + Serial.printf_P(PSTR("\n\nInit ArduPilotMega (unstable development version)" + "\n\nFree RAM: %lu\n"), + freeRAM()); // // Check the EEPROM format version before loading any parameters from EEPROM. @@ -109,12 +104,13 @@ void init_ardupilot() Serial.println_P(PSTR("done.")); } else { - + unsigned long before = micros(); // Load all auto-loaded EEPROM variables AP_Var::load_all(); - } - //read_EEPROM_startup(); // Read critical config information to start + Serial.printf_P(PSTR("load_all took %luus\n"), micros() - before); + Serial.printf_P(PSTR("using %u bytes of memory\n"), AP_Var::get_memory_use()); + } init_rc_in(); // sets up rc channels from radio init_rc_out(); // sets up the timer libs @@ -123,10 +119,19 @@ void init_ardupilot() APM_BMP085.Init(); // APM Abs Pressure sensor initialization DataFlash.Init(); // DataFlash log initialization - g_gps = &GPS; - g_gps->init(); + // Do GPS init + //g_gps = &GPS; + g_gps = &g_gps_driver; + g_gps->init(); // GPS Initialization + + // init the GCS + #if GCS_PORT == 3 + gcs.init(&Serial3); + #else + gcs.init(&Serial); + #endif + - imu.init(IMU::WARM_START); // offsets are loaded from EEPROM if(g.compass_enabled) @@ -137,6 +142,7 @@ void init_ardupilot() pinMode(B_LED_PIN, OUTPUT); // GPS status LED pinMode(SLIDE_SWITCH_PIN, INPUT); // To enter interactive mode pinMode(PUSHBUTTON_PIN, INPUT); // unused + DDRL |= B00000100; // Set Port L, pin 2 to output for the relay // If the switch is in 'menu' mode, run the main menu. // @@ -153,48 +159,26 @@ void init_ardupilot() "Type 'help' to list commands, 'exit' to leave a submenu.\n" "Visit the 'setup' menu for first-time configuration.\n")); for (;;) { - Serial.printf_P(PSTR("\n" - "Move the slide switch and reset to FLY.\n" - "\n")); + Serial.println_P(PSTR("\nMove the slide switch and reset to FLY.\n")); main_menu.run(); } } - if(g.log_bitmask > 0){ - // Here we will check on the length of the last log + // TODO - Here we will check on the length of the last log // We don't want to create a bunch of little logs due to powering on and off - last_log_num = eeprom_read_byte((uint8_t *) EE_LAST_LOG_NUM); - last_log_start = eeprom_read_word((uint16_t *) (EE_LOG_1_START+(last_log_num - 1) * 0x02)); - last_log_end = eeprom_read_word((uint16_t *) EE_LAST_LOG_PAGE); - - if(last_log_num == 0) { - // The log space is empty. Start a write session on page 1 - DataFlash.StartWrite(1); - eeprom_write_byte((uint8_t *) EE_LAST_LOG_NUM, (1)); - eeprom_write_word((uint16_t *) EE_LOG_1_START, (1)); - - } else if (last_log_end <= last_log_start + 10) { - // The last log is small. We consider it junk. Overwrite it. - DataFlash.StartWrite(last_log_start); - - } else { - // The last log is valid. Start a new log - if(last_log_num >= 19) { - Serial.println("Number of log files exceeds max. Log 19 will be overwritten."); - last_log_num --; - } - DataFlash.StartWrite(last_log_end + 1); - eeprom_write_byte((uint8_t *) EE_LAST_LOG_NUM, (last_log_num + 1)); - eeprom_write_word((uint16_t *) (EE_LOG_1_START+(last_log_num)*0x02), (last_log_end + 1)); - } + last_log_num = get_num_logs(); + start_new_log(last_log_num); } + // read in the flight switches + update_servo_switches(); + // read in the flight switches //update_servo_switches(); - //Serial.print("GROUND START"); - send_message(SEVERITY_LOW,"GROUND START"); + imu.init(IMU::WARM_START); // offsets are loaded from EEPROM + startup_ground(); if (g.log_bitmask & MASK_LOG_CMD) @@ -210,27 +194,10 @@ void init_ardupilot() //******************************************************************************** void startup_ground(void) { - /* - read_radio(); - while (g.rc_3.control_in > 0){ - delay(20); - read_radio(); - APM_RC.OutputCh(CH_1, g.rc_3.radio_in); - APM_RC.OutputCh(CH_2, g.rc_3.radio_in); - APM_RC.OutputCh(CH_3, g.rc_3.radio_in); - APM_RC.OutputCh(CH_4, g.rc_3.radio_in); - Serial.println("*") - } - */ - // read the radio to set trims - // --------------------------- - trim_radio(); - - if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Startup(TYPE_GROUNDSTART_MSG); + gcs.send_text(SEVERITY_LOW," GROUND START"); #if(GROUND_START_DELAY > 0) - send_message(SEVERITY_LOW,"With Delay"); + gcs.send_text(SEVERITY_LOW," With Delay"); delay(GROUND_START_DELAY * 1000); #endif @@ -240,28 +207,31 @@ void startup_ground(void) gcs.send_message(MSG_COMMAND_LIST, i); } - //IMU ground start - //------------------------ - init_pressure_ground(); - // Warm up and read Gyro offsets // ----------------------------- imu.init(IMU::COLD_START); - // Save the settings for in-air restart - // ------------------------------------ - save_EEPROM_groundstart(); + // read the radio to set trims + // --------------------------- + trim_radio(); + + if (g.log_bitmask & MASK_LOG_CMD) + Log_Write_Startup(TYPE_GROUNDSTART_MSG); + + + // read Baro pressure at ground + //----------------------------- + init_pressure_ground(); // initialize commands // ------------------- init_commands(); - send_message(SEVERITY_LOW,"\n\n Ready to FLY."); + gcs.send_text(SEVERITY_LOW,"\n\n Ready to FLY."); } void set_mode(byte mode) { - if(control_mode == mode){ // don't switch modes if we are already in the correct mode. return; @@ -314,7 +284,7 @@ void set_mode(byte mode) } // output control mode to the ground station - send_message(MSG_HEARTBEAT); + gcs.send_message(MSG_MODE_CHANGE); if (g.log_bitmask & MASK_LOG_MODE) Log_Write_Mode(control_mode); @@ -357,18 +327,26 @@ void update_GPS_light(void) { // GPS LED on if we have a fix or Blink GPS LED if we are receiving data // --------------------------------------------------------------------- - if(g_gps->fix == 0){ - GPS_light = !GPS_light; - if(GPS_light){ - digitalWrite(C_LED_PIN, HIGH); - }else{ + switch (g_gps->status()) { + case(2): + digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix. + break; + + case(1): + if (g_gps->valid_read == true){ + GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock + if (GPS_light){ + digitalWrite(C_LED_PIN, LOW); + } else { + digitalWrite(C_LED_PIN, HIGH); + } + g_gps->valid_read = false; + } + break; + + default: digitalWrite(C_LED_PIN, LOW); - } - }else{ - if(!GPS_light){ - GPS_light = true; - digitalWrite(C_LED_PIN, HIGH); - } + break; } if(motor_armed == true){