mirror of https://github.com/ArduPilot/ardupilot
minor change to GPS
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1834 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
b84137ae42
commit
3494e73f8b
|
@ -235,6 +235,7 @@ boolean rate_yaw_flag; // used to transition yaw control from Rate control
|
||||||
// ----------
|
// ----------
|
||||||
boolean motor_light; // status of the Motor safety
|
boolean motor_light; // status of the Motor safety
|
||||||
boolean GPS_light; // status of the GPS light
|
boolean GPS_light; // status of the GPS light
|
||||||
|
boolean timer_light; // status of the Motor safety
|
||||||
|
|
||||||
// GPS variables
|
// GPS variables
|
||||||
// -------------
|
// -------------
|
||||||
|
@ -427,7 +428,7 @@ unsigned long elapsedTime; // for doing custom events
|
||||||
float load; // % MCU cycles used
|
float load; // % MCU cycles used
|
||||||
|
|
||||||
byte counter_one_herz;
|
byte counter_one_herz;
|
||||||
bool GPS_disabled = true;
|
bool GPS_enabled = false;
|
||||||
|
|
||||||
////////////////////////////////////////////////////////////////////////////////
|
////////////////////////////////////////////////////////////////////////////////
|
||||||
// Top-level logic
|
// Top-level logic
|
||||||
|
@ -448,9 +449,14 @@ void loop()
|
||||||
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
|
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
|
||||||
mainLoop_count++;
|
mainLoop_count++;
|
||||||
|
|
||||||
//if(delta_ms_fast_loop > 11){
|
/*
|
||||||
// Serial.println(delta_ms_fast_loop,DEC);
|
if(delta_ms_fast_loop > 11){
|
||||||
//}
|
update_timer_light(true);
|
||||||
|
//Serial.println(delta_ms_fast_loop,DEC);
|
||||||
|
}else{
|
||||||
|
update_timer_light(false);
|
||||||
|
}*/
|
||||||
|
|
||||||
// Execute the fast loop
|
// Execute the fast loop
|
||||||
// ---------------------
|
// ---------------------
|
||||||
fast_loop();
|
fast_loop();
|
||||||
|
@ -525,7 +531,7 @@ void medium_loop()
|
||||||
medium_loopCounter++;
|
medium_loopCounter++;
|
||||||
|
|
||||||
|
|
||||||
if(GPS_disabled == false){
|
if(GPS_enabled){
|
||||||
update_GPS();
|
update_GPS();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -972,7 +978,7 @@ void update_current_flight_mode(void)
|
||||||
if(flight_timer > 10){
|
if(flight_timer > 10){
|
||||||
flight_timer = 0;
|
flight_timer = 0;
|
||||||
|
|
||||||
if(GPS_disabled){
|
if(GPS_enabled == false){
|
||||||
current_loc.lat = home.lat = 0;
|
current_loc.lat = home.lat = 0;
|
||||||
current_loc.lng = home.lng = 0;
|
current_loc.lng = home.lng = 0;
|
||||||
}
|
}
|
||||||
|
|
|
@ -69,6 +69,7 @@ setup_show(uint8_t argc, const Menu::arg *argv)
|
||||||
// clear the area
|
// clear the area
|
||||||
print_blanks(8);
|
print_blanks(8);
|
||||||
|
|
||||||
|
report_version();
|
||||||
report_radio();
|
report_radio();
|
||||||
report_frame();
|
report_frame();
|
||||||
report_current();
|
report_current();
|
||||||
|
@ -807,6 +808,14 @@ void report_current()
|
||||||
print_blanks(2);
|
print_blanks(2);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void report_gps()
|
||||||
|
{
|
||||||
|
Serial.printf_P(PSTR("\nGPS\n"));
|
||||||
|
print_divider();
|
||||||
|
print_enabled(GPS_enabled);
|
||||||
|
print_blanks(2);
|
||||||
|
}
|
||||||
|
|
||||||
void report_sonar()
|
void report_sonar()
|
||||||
{
|
{
|
||||||
g.sonar_enabled.load();
|
g.sonar_enabled.load();
|
||||||
|
@ -816,6 +825,13 @@ void report_sonar()
|
||||||
print_blanks(2);
|
print_blanks(2);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void report_version()
|
||||||
|
{
|
||||||
|
Serial.printf_P(PSTR("FW Version %d\n"),(int)g.format_version.get());
|
||||||
|
print_divider();
|
||||||
|
print_blanks(2);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
void report_frame()
|
void report_frame()
|
||||||
{
|
{
|
||||||
|
|
|
@ -85,11 +85,15 @@ void init_ardupilot()
|
||||||
"\n\nFree RAM: %lu\n"),
|
"\n\nFree RAM: %lu\n"),
|
||||||
freeRAM());
|
freeRAM());
|
||||||
|
|
||||||
|
|
||||||
//
|
//
|
||||||
// Check the EEPROM format version before loading any parameters from EEPROM.
|
// Check the EEPROM format version before loading any parameters from EEPROM.
|
||||||
//
|
//
|
||||||
|
report_version();
|
||||||
|
|
||||||
if (!g.format_version.load() ||
|
if (!g.format_version.load() ||
|
||||||
g.format_version != Parameters::k_format_version) {
|
g.format_version != Parameters::k_format_version) {
|
||||||
|
Serial.printf_P(PSTR("\n\nForcing complete parameter reset..."));
|
||||||
|
|
||||||
/*Serial.printf_P(PSTR("\n\nEEPROM format version %d not compatible with this firmware (requires %d)"
|
/*Serial.printf_P(PSTR("\n\nEEPROM format version %d not compatible with this firmware (requires %d)"
|
||||||
"\n\nForcing complete parameter reset..."),
|
"\n\nForcing complete parameter reset..."),
|
||||||
|
@ -255,20 +259,22 @@ void startup_ground(void)
|
||||||
init_commands();
|
init_commands();
|
||||||
|
|
||||||
byte counter = 4;
|
byte counter = 4;
|
||||||
GPS_disabled = true;
|
GPS_enabled = false;
|
||||||
|
|
||||||
// Read in the GPS
|
// Read in the GPS
|
||||||
for (byte counter = 0; ; counter++) {
|
for (byte counter = 0; ; counter++) {
|
||||||
g_gps->update();
|
g_gps->update();
|
||||||
if (g_gps->status() != 0){
|
if (g_gps->status() != 0){
|
||||||
|
GPS_enabled = true;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
if (counter >= 4) {
|
if (counter >= 4) {
|
||||||
GPS_disabled = true;
|
GPS_enabled = false;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
report_gps();
|
||||||
|
SendDebug("\nReady to FLY ");
|
||||||
gcs.send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY."));
|
gcs.send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY."));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -410,6 +416,15 @@ void update_motor_light(void)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void update_timer_light(bool light)
|
||||||
|
{
|
||||||
|
if(light == true){
|
||||||
|
digitalWrite(B_LED_PIN, HIGH);
|
||||||
|
}else{
|
||||||
|
digitalWrite(B_LED_PIN, LOW);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
void resetPerfData(void) {
|
void resetPerfData(void) {
|
||||||
/*
|
/*
|
||||||
|
|
Loading…
Reference in New Issue