mirror of https://github.com/ArduPilot/ardupilot
Update to get SIMPLE mode running.
Seems to work! git-svn-id: https://arducopter.googlecode.com/svn/trunk@1774 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort
parent
602a4fb423
commit
323f2bfa59
|
|
@ -407,17 +407,18 @@ byte slow_loopCounter;
|
|||
byte superslow_loopCounter;
|
||||
byte fbw_timer; // for limiting the execution of FBW input
|
||||
|
||||
unsigned long nav_loopTimer; // used to track the elapsed ime for GPS nav
|
||||
//unsigned long nav_loopTimer; // used to track the elapsed ime for GPS nav
|
||||
unsigned long nav2_loopTimer; // used to track the elapsed ime for GPS nav
|
||||
|
||||
unsigned long dTnav; // Delta Time in milliseconds for navigation computations
|
||||
//unsigned long dTnav; // Delta Time in milliseconds for navigation computations
|
||||
unsigned long dTnav2; // Delta Time in milliseconds for navigation computations
|
||||
unsigned long elapsedTime; // for doing custom events
|
||||
float load; // % MCU cycles used
|
||||
|
||||
byte counter_one_herz;
|
||||
|
||||
byte GPS_failure_counter = 255;
|
||||
byte GPS_failure_counter = 3;
|
||||
bool GPS_disabled = false;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Top-level logic
|
||||
|
|
@ -498,7 +499,6 @@ void medium_loop()
|
|||
// ----------
|
||||
read_radio(); // read the radio first
|
||||
|
||||
|
||||
// reads all of the necessary trig functions for cameras, throttle, etc.
|
||||
update_trig();
|
||||
|
||||
|
|
@ -506,13 +506,16 @@ void medium_loop()
|
|||
// -----------------------------------------
|
||||
switch(medium_loopCounter) {
|
||||
|
||||
// This case deals with the GPS
|
||||
//-------------------------------
|
||||
// This case deals with the GPS and Compass
|
||||
//-----------------------------------------
|
||||
case 0:
|
||||
medium_loopCounter++;
|
||||
|
||||
if(GPS_failure_counter > 0){
|
||||
update_GPS();
|
||||
|
||||
}else if(GPS_failure_counter == 0){
|
||||
GPS_disabled = true;
|
||||
}
|
||||
//readCommands();
|
||||
|
||||
|
|
@ -529,32 +532,39 @@ void medium_loop()
|
|||
case 1:
|
||||
medium_loopCounter++;
|
||||
|
||||
// hack to stop navigation in Simple mode
|
||||
if (control_mode == SIMPLE)
|
||||
break;
|
||||
|
||||
if(g_gps->new_data){
|
||||
g_gps->new_data = false;
|
||||
dTnav = millis() - nav_loopTimer;
|
||||
nav_loopTimer = millis();
|
||||
|
||||
// calculate the plane's desired bearing
|
||||
// -------------------------------------
|
||||
navigate();
|
||||
}
|
||||
|
||||
// calc pitch and roll to target
|
||||
// -----------------------------
|
||||
dTnav2 = millis() - nav2_loopTimer;
|
||||
nav2_loopTimer = millis();
|
||||
|
||||
// hack to stop navigation in Simple mode
|
||||
if (control_mode == SIMPLE)
|
||||
break;
|
||||
|
||||
if (control_mode == FBW)
|
||||
break;
|
||||
|
||||
// Auto control modes:
|
||||
if(g_gps->new_data){
|
||||
g_gps->new_data = false;
|
||||
|
||||
// we are not tracking I term on navigation, so this isn't needed
|
||||
//dTnav = millis() - nav_loopTimer;
|
||||
//nav_loopTimer = millis();
|
||||
|
||||
// calculate the copter's desired bearing and WP distance
|
||||
// ------------------------------------------------------
|
||||
navigate();
|
||||
}
|
||||
|
||||
// we call these regardless of GPS because of the rapid nature of the yaw sensor
|
||||
// -----------------------------------------------------------------------------
|
||||
if(wp_distance < 800){ // 8 meters
|
||||
calc_loiter_nav();
|
||||
}else{
|
||||
calc_waypoint_nav();
|
||||
}
|
||||
|
||||
|
||||
break;
|
||||
|
||||
// command processing
|
||||
|
|
@ -563,7 +573,7 @@ void medium_loop()
|
|||
medium_loopCounter++;
|
||||
|
||||
// Read altitude from sensors
|
||||
// ------------------
|
||||
// --------------------------
|
||||
update_alt();
|
||||
|
||||
// perform next command
|
||||
|
|
@ -581,41 +591,55 @@ void medium_loop()
|
|||
if (g.log_bitmask & MASK_LOG_ATTITUDE_MED && (g.log_bitmask & MASK_LOG_ATTITUDE_FAST == 0))
|
||||
Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (int)dcm.yaw_sensor);
|
||||
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
if (g.log_bitmask & MASK_LOG_CTUN)
|
||||
Log_Write_Control_Tuning();
|
||||
#endif
|
||||
#endif
|
||||
|
||||
if (g.log_bitmask & MASK_LOG_NTUN)
|
||||
Log_Write_Nav_Tuning();
|
||||
|
||||
if (g.log_bitmask & MASK_LOG_GPS){
|
||||
if(home_is_set)
|
||||
Log_Write_GPS(g_gps->time, current_loc.lat, current_loc.lng, g_gps->altitude, current_loc.alt, (long) g_gps->ground_speed, g_gps->ground_course, g_gps->fix, g_gps->num_sats);
|
||||
if(home_is_set){
|
||||
Log_Write_GPS(g_gps->time,
|
||||
current_loc.lat,
|
||||
current_loc.lng,
|
||||
g_gps->altitude,
|
||||
current_loc.alt,
|
||||
(long)g_gps->ground_speed,
|
||||
g_gps->ground_course,
|
||||
g_gps->fix,
|
||||
g_gps->num_sats);
|
||||
}
|
||||
}
|
||||
|
||||
gcs.send_message(MSG_ATTITUDE); // Sends attitude data
|
||||
break;
|
||||
|
||||
// This case controls the slow loop
|
||||
//---------------------------------
|
||||
case 4:
|
||||
medium_loopCounter = 0;
|
||||
|
||||
if (g.current_enabled){
|
||||
read_current();
|
||||
}
|
||||
|
||||
// shall we trim the copter?
|
||||
// ------------------------
|
||||
// Accel trims = hold > 2 seconds
|
||||
// Throttle cruise = switch less than 1 second
|
||||
// --------------------------------------------
|
||||
read_trim_switch();
|
||||
|
||||
// shall we check for engine start?
|
||||
// --------------------------------
|
||||
// Check for engine arming
|
||||
// -----------------------
|
||||
arm_motors();
|
||||
|
||||
medium_loopCounter = 0;
|
||||
slow_loop();
|
||||
break;
|
||||
|
||||
default:
|
||||
// this is just a catch all
|
||||
// ------------------------
|
||||
medium_loopCounter = 0;
|
||||
break;
|
||||
}
|
||||
|
|
@ -632,10 +656,10 @@ void medium_loop()
|
|||
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
|
||||
Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (int)dcm.yaw_sensor);
|
||||
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
if (g.log_bitmask & MASK_LOG_RAW)
|
||||
Log_Write_Raw();
|
||||
#endif
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
if (g.log_bitmask & MASK_LOG_RAW)
|
||||
Log_Write_Raw();
|
||||
#endif
|
||||
|
||||
#if GCS_PROTOCOL == 6 // This is here for Benjamin Pelletier. Please do not remove without checking with me. Doug W
|
||||
readgcsinput();
|
||||
|
|
@ -688,6 +712,9 @@ void slow_loop()
|
|||
slow_loopCounter = 0;
|
||||
update_events();
|
||||
|
||||
// blink if we are armed
|
||||
update_motor_light();
|
||||
|
||||
// XXX this should be a "GCS slow loop" interface
|
||||
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
|
||||
gcs.data_stream_send(1,5);
|
||||
|
|
@ -724,7 +751,7 @@ void update_GPS(void)
|
|||
update_GPS_light();
|
||||
|
||||
if (g_gps->new_data && g_gps->fix) {
|
||||
GPS_failure_counter = 255;
|
||||
GPS_failure_counter = 3;
|
||||
|
||||
// XXX We should be sending GPS data off one of the regular loops so that we send
|
||||
// no-GPS-fix data too
|
||||
|
|
@ -755,7 +782,7 @@ void update_GPS(void)
|
|||
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
|
||||
|
||||
// reset our nav loop timer
|
||||
nav_loopTimer = millis();
|
||||
//nav_loopTimer = millis();
|
||||
init_home();
|
||||
|
||||
// init altitude
|
||||
|
|
@ -866,17 +893,28 @@ void update_current_flight_mode(void)
|
|||
fbw_timer++;
|
||||
// 25 hz
|
||||
if(fbw_timer > 4){
|
||||
Location temp = current_loc;
|
||||
temp.lng += g.rc_1.control_in / 2;
|
||||
temp.lat -= g.rc_2.control_in / 2;
|
||||
fbw_timer = 0;
|
||||
|
||||
current_loc.lat = 0;
|
||||
current_loc.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(¤t_loc, &temp) + initial_simple_bearing;
|
||||
nav_bearing = get_bearing(¤t_loc, &next_WP) + initial_simple_bearing;
|
||||
nav_bearing = wrap_360(nav_bearing);
|
||||
wp_distance = get_distance(¤t_loc, &temp);
|
||||
|
||||
wp_distance = get_distance(¤t_loc, &next_WP);
|
||||
calc_bearing_error();
|
||||
|
||||
/*
|
||||
Serial.printf("lat: %ld lon:%ld, bear:%ld, dist:%ld, init:%ld, err:%ld ",
|
||||
next_WP.lat,
|
||||
next_WP.lng,
|
||||
nav_bearing,
|
||||
wp_distance,
|
||||
initial_simple_bearing,
|
||||
bearing_error);
|
||||
*/
|
||||
// get nav_pitch and nav_roll
|
||||
calc_waypoint_nav();
|
||||
}
|
||||
|
|
@ -905,20 +943,15 @@ void update_current_flight_mode(void)
|
|||
if(fbw_timer > 10){
|
||||
fbw_timer = 0;
|
||||
|
||||
if(home_is_set == false){
|
||||
scaleLongDown = 1;
|
||||
// we are not using GPS
|
||||
// reset the location
|
||||
// RTL won't function
|
||||
if(GPS_disabled){
|
||||
current_loc.lat = home.lat = 0;
|
||||
current_loc.lng = home.lng = 0;
|
||||
// set dTnav manually
|
||||
dTnav = 200;
|
||||
}
|
||||
|
||||
next_WP.lng = home.lng + g.rc_1.control_in / 2; // X: 4500 / 2 = 2250 = 25 meteres
|
||||
// forward is negative so we reverse it to get a positive North translation
|
||||
next_WP.lat = home.lat - g.rc_2.control_in / 2; // Y: 4500 / 2 = 2250 = 25 meteres
|
||||
|
||||
calc_loiter_nav();
|
||||
}
|
||||
|
||||
// Output Pitch, Roll, Yaw and Throttle
|
||||
|
|
|
|||
|
|
@ -144,7 +144,7 @@ output_yaw_with_hold(boolean hold)
|
|||
long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377
|
||||
rate = constrain(rate, -36000, 36000); // limit to something fun!
|
||||
|
||||
long error = ((long)g.rc_4.control_in * 6) - rate; // control is += 6000 * 6 = 36000
|
||||
long error = ((long)g.rc_4.control_in * 6) - rate; // control is += 6000 * 6 = 36000
|
||||
// -error = CCW, +error = CW
|
||||
|
||||
if(g.rc_4.control_in == 0){
|
||||
|
|
@ -231,7 +231,7 @@ void calc_nav_throttle()
|
|||
float scaler = 1.0;
|
||||
|
||||
if(error < 0){
|
||||
scaler = (altitude_sensor == BARO) ? .5 : .8;
|
||||
scaler = (altitude_sensor == BARO) ? .5 : .5;
|
||||
}
|
||||
|
||||
if(altitude_sensor == BARO){
|
||||
|
|
|
|||
|
|
@ -287,7 +287,7 @@
|
|||
#define ACRO_RATE_PITCH_IMAX_CENTIDEGREE ACRO_RATE_PITCH_IMAX * 100
|
||||
|
||||
#ifndef ACRO_RATE_YAW_P
|
||||
# define ACRO_RATE_YAW_P .5
|
||||
# define ACRO_RATE_YAW_P .1
|
||||
#endif
|
||||
#ifndef ACRO_RATE_YAW_I
|
||||
# define ACRO_RATE_YAW_I 0.0
|
||||
|
|
@ -394,7 +394,7 @@
|
|||
# define NAV_P 2.0
|
||||
#endif
|
||||
#ifndef NAV_I
|
||||
# define NAV_I 0.00
|
||||
# define NAV_I 0.1
|
||||
#endif
|
||||
#ifndef NAV_D
|
||||
# define NAV_D 0.00
|
||||
|
|
|
|||
|
|
@ -13,12 +13,14 @@ void arm_motors()
|
|||
if (arming_counter > ARM_DELAY) {
|
||||
motor_armed = true;
|
||||
} else{
|
||||
//Serial.printf("arm %d\n", arming_counter);
|
||||
arming_counter++;
|
||||
}
|
||||
}else if (g.rc_4.control_in < -2700) {
|
||||
if (arming_counter > DISARM_DELAY){
|
||||
motor_armed = false;
|
||||
}else{
|
||||
//Serial.printf("arm %d\n", arming_counter);
|
||||
arming_counter++;
|
||||
}
|
||||
}else{
|
||||
|
|
@ -67,10 +69,10 @@ set_servos_4()
|
|||
motor_out[CH_3] = g.rc_3.radio_out + g.rc_2.pwm_out;
|
||||
motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out;
|
||||
|
||||
motor_out[CH_1] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[CH_2] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[CH_3] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[CH_4] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[CH_1] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[CH_2] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[CH_3] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[CH_4] -= g.rc_4.pwm_out; // CW
|
||||
|
||||
}else if(g.frame_type == X_FRAME){
|
||||
//Serial.println("X_FRAME");
|
||||
|
|
@ -123,11 +125,11 @@ set_servos_4()
|
|||
//left side
|
||||
motor_out[CH_2] = g.rc_3.radio_out + g.rc_1.pwm_out; // CCW
|
||||
motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW
|
||||
motor_out[CH_8] = g.rc_3.radio_out + roll_out - pitch_out; // CW
|
||||
motor_out[CH_8] = g.rc_3.radio_out + roll_out - pitch_out; // CW
|
||||
|
||||
//right side
|
||||
motor_out[CH_1] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW
|
||||
motor_out[CH_7] = g.rc_3.radio_out - roll_out + pitch_out; // CCW
|
||||
motor_out[CH_7] = g.rc_3.radio_out - roll_out + pitch_out; // CCW
|
||||
motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW
|
||||
|
||||
motor_out[CH_7] += g.rc_4.pwm_out; // CCW
|
||||
|
|
@ -136,9 +138,9 @@ set_servos_4()
|
|||
|
||||
motor_out[CH_3] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[CH_1] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[CH_8] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[CH_8] -= g.rc_4.pwm_out; // CW
|
||||
|
||||
}else{
|
||||
}else{
|
||||
|
||||
Serial.print("frame error");
|
||||
|
||||
|
|
@ -157,6 +159,7 @@ set_servos_4()
|
|||
}
|
||||
|
||||
num++;
|
||||
|
||||
if (num > 50){
|
||||
num = 0;
|
||||
/*
|
||||
|
|
@ -172,8 +175,6 @@ set_servos_4()
|
|||
}
|
||||
*/
|
||||
|
||||
|
||||
|
||||
//Serial.print("!");
|
||||
//debugging with Channel 6
|
||||
|
||||
|
|
@ -196,7 +197,7 @@ set_servos_4()
|
|||
init_pids();
|
||||
//*/
|
||||
|
||||
/*
|
||||
///*
|
||||
write_int(motor_out[CH_1]);
|
||||
write_int(motor_out[CH_2]);
|
||||
write_int(motor_out[CH_3]);
|
||||
|
|
@ -214,14 +215,13 @@ set_servos_4()
|
|||
write_int((int)nav_roll);
|
||||
write_int((int)nav_pitch);
|
||||
|
||||
//24
|
||||
write_long(current_loc.lat); //28
|
||||
write_long(current_loc.lng); //32
|
||||
write_int((int)current_loc.alt); //34
|
||||
write_long(current_loc.lat); // 28
|
||||
write_long(current_loc.lng); // 32
|
||||
write_int((int)current_loc.alt); // 34
|
||||
|
||||
write_long(next_WP.lat);
|
||||
write_long(next_WP.lng);
|
||||
write_int((int)next_WP.alt); //44
|
||||
write_int((int)next_WP.alt); // 44
|
||||
|
||||
flush(10);
|
||||
//*/
|
||||
|
|
|
|||
|
|
@ -70,14 +70,12 @@ void calc_loiter_nav()
|
|||
lat_error = constrain(lat_error, -DIST_ERROR_MAX, DIST_ERROR_MAX); // +- 20m max error
|
||||
|
||||
// Convert distance into ROLL X
|
||||
nav_lon = long_error * g.pid_nav_lon.kP(); // 1800 * 2 = 3600 or 36°
|
||||
//nav_lon = g.pid_nav_lon.get_pid(long_error, dTnav2, 1.0);
|
||||
//nav_lon = constrain(nav_lon, -DIST_ERROR_MAX, DIST_ERROR_MAX); // Limit max command
|
||||
//nav_lon = long_error * g.pid_nav_lon.kP(); // 1800 * 2 = 3600 or 36°
|
||||
nav_lon = g.pid_nav_lon.get_pid(long_error, dTnav2, 1.0);
|
||||
|
||||
// PITCH Y
|
||||
//nav_lat = g.pid_nav_lat.get_pid(lat_error, dTnav2, 1.0);
|
||||
nav_lat = lat_error * g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36°
|
||||
//nav_lat = constrain(nav_lat, -DIST_ERROR_MAX, DIST_ERROR_MAX); // Limit max command
|
||||
//nav_lat = lat_error * g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36°
|
||||
nav_lat = g.pid_nav_lat.get_pid(lat_error, dTnav2, 1.0);
|
||||
|
||||
// rotate the vector
|
||||
nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x;
|
||||
|
|
@ -91,23 +89,30 @@ void calc_loiter_nav()
|
|||
|
||||
void calc_waypoint_nav()
|
||||
{
|
||||
nav_lat = constrain(wp_distance, -DIST_ERROR_MAX, DIST_ERROR_MAX); // +- 20m max error
|
||||
nav_lat = constrain((wp_distance * 100), -1800, 1800); // +- 20m max error
|
||||
//nav_lat = max(wp_distance, -DIST_ERROR_MAX);
|
||||
//nav_lat = min(wp_distance, DIST_ERROR_MAX);
|
||||
|
||||
//Serial.printf("nav_lat %ld, ", nav_lat);
|
||||
// Scale response by kP
|
||||
nav_lat *= g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36°
|
||||
//Serial.printf("%ld, ",nav_lat);
|
||||
|
||||
// get the sin and cos of the bearing error - rotated 90°
|
||||
sin_nav_y = sin(radians((float)(9000 - bearing_error) / 100));
|
||||
cos_nav_x = cos(radians((float)(bearing_error - 9000) / 100));
|
||||
//Serial.printf("X%2.4f, Y%2.4f ", cos_nav_x, sin_nav_y);
|
||||
|
||||
// rotate the vector
|
||||
nav_roll = (float)nav_lat * cos_nav_x;
|
||||
nav_pitch = (float)nav_lat * sin_nav_y;
|
||||
|
||||
// Scale response by kP
|
||||
nav_roll *= g.pid_nav_lon.kP(); // 1800 * 2 = 3600 or 36°
|
||||
nav_pitch *= g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36°
|
||||
//Serial.printf("R%ld, P%ld ", nav_roll, nav_pitch);
|
||||
|
||||
long pmax = g.pitch_max.get();
|
||||
nav_roll = constrain(nav_roll, -pmax, pmax);
|
||||
nav_pitch = constrain(nav_pitch, -pmax, pmax);
|
||||
//Serial.printf("R%ld, P%ld \n", nav_roll, nav_pitch);
|
||||
}
|
||||
|
||||
void calc_bearing_error()
|
||||
|
|
@ -205,10 +210,10 @@ long get_altitude_above_home(void)
|
|||
|
||||
long get_distance(struct Location *loc1, struct Location *loc2)
|
||||
{
|
||||
if(loc1->lat == 0 || loc1->lng == 0)
|
||||
return -1;
|
||||
if(loc2->lat == 0 || loc2->lng == 0)
|
||||
return -1;
|
||||
//if(loc1->lat == 0 || loc1->lng == 0)
|
||||
// return -1;
|
||||
//if(loc2->lat == 0 || loc2->lng == 0)
|
||||
// return -1;
|
||||
float dlat = (float)(loc2->lat - loc1->lat);
|
||||
float dlong = ((float)(loc2->lng - loc1->lng)) * scaleLongDown;
|
||||
return sqrt(sq(dlat) + sq(dlong)) * .01113195;
|
||||
|
|
|
|||
|
|
@ -82,7 +82,7 @@ void init_ardupilot()
|
|||
//
|
||||
Serial3.begin(SERIAL3_BAUD, 128, 128);
|
||||
|
||||
Serial.printf_P(PSTR("\n\nInit ArduPilotMega (unstable development version)"
|
||||
Serial.printf_P(PSTR("\n\nInit ArduCopterMega"
|
||||
"\n\nFree RAM: %lu\n"),
|
||||
freeRAM());
|
||||
|
||||
|
|
@ -92,9 +92,11 @@ void init_ardupilot()
|
|||
if (!g.format_version.load() ||
|
||||
g.format_version != Parameters::k_format_version) {
|
||||
|
||||
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..."),
|
||||
g.format_version.get(), Parameters::k_format_version);
|
||||
g.format_version.get(),
|
||||
Parameters::k_format_version);
|
||||
*/
|
||||
|
||||
// erase all parameters
|
||||
AP_Var::erase_all();
|
||||
|
|
@ -102,14 +104,14 @@ void init_ardupilot()
|
|||
// save the new format version
|
||||
g.format_version.set_and_save(Parameters::k_format_version);
|
||||
|
||||
Serial.println_P(PSTR("done."));
|
||||
//Serial.println_P(PSTR("done."));
|
||||
}else{
|
||||
unsigned long before = micros();
|
||||
// Load all auto-loaded EEPROM variables
|
||||
AP_Var::load_all();
|
||||
|
||||
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());
|
||||
// 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());
|
||||
}
|
||||
|
||||
#ifdef RADIO_OVERRIDE_DEFAULTS
|
||||
|
|
@ -171,11 +173,10 @@ void init_ardupilot()
|
|||
Serial.printf_P(PSTR("\n"
|
||||
"Entering interactive setup mode...\n"
|
||||
"\n"
|
||||
"If using the Arduino Serial Monitor, ensure Line Ending is set to Carriage Return.\n"
|
||||
"Type 'help' to list commands, 'exit' to leave a submenu.\n"
|
||||
"Visit the 'setup' menu for first-time configuration.\n"));
|
||||
for (;;) {
|
||||
Serial.println_P(PSTR("\nMove the slide switch and reset to FLY.\n"));
|
||||
//Serial.println_P(PSTR("\nMove the slide switch and reset to FLY.\n"));
|
||||
main_menu.run();
|
||||
}
|
||||
}
|
||||
|
|
@ -341,7 +342,8 @@ void update_GPS_light(void)
|
|||
{
|
||||
// GPS LED on if we have a fix or Blink GPS LED if we are receiving data
|
||||
// ---------------------------------------------------------------------
|
||||
switch (g_gps->status()) {
|
||||
switch (g_gps->status()){
|
||||
|
||||
case(2):
|
||||
digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix.
|
||||
break;
|
||||
|
|
@ -362,7 +364,10 @@ void update_GPS_light(void)
|
|||
digitalWrite(C_LED_PIN, LOW);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void update_motor_light(void)
|
||||
{
|
||||
if(motor_armed == true){
|
||||
motor_light = !motor_light;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue