mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-22 16:48:29 -04:00
updated to AP_Var
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1681 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort
parent
acb7d78a79
commit
765e3d2935
@ -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
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -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
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
|
||||
|
||||
@ -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 <increment_WP_index> 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 <increment_WP_index> 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 <set_next_wp> 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> 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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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()
|
||||
|
||||
@ -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,"<startup_ground> GROUND START");
|
||||
|
||||
#if(GROUND_START_DELAY > 0)
|
||||
send_message(SEVERITY_LOW,"With Delay");
|
||||
gcs.send_text(SEVERITY_LOW,"<startup_ground> 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){
|
||||
|
||||
Loading…
Reference in New Issue
Block a user