Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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 2011-03-15 05:54:48 +00:00
parent bd8c72958d
commit 41c9b1edd7
6 changed files with 138 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

125
ArduCopterMega/ArduCopterMega.pde
View File

@ -407,17 +407,18 @@ byte slow_loopCounter;
byte superslow_loopCounter; byte superslow_loopCounter;
byte fbw_timer; // for limiting the execution of FBW input 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 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 dTnav2; // Delta Time in milliseconds for navigation computations
unsigned long elapsedTime; // for doing custom events 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;
byte GPS_failure_counter = 255; byte GPS_failure_counter = 3;
bool GPS_disabled = false;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Top-level logic // Top-level logic
@ -498,7 +499,6 @@ void medium_loop()
// ---------- // ----------
read_radio(); // read the radio first read_radio(); // read the radio first
// reads all of the necessary trig functions for cameras, throttle, etc. // reads all of the necessary trig functions for cameras, throttle, etc.
update_trig(); update_trig();
@ -506,13 +506,16 @@ void medium_loop()
// ----------------------------------------- // -----------------------------------------
switch(medium_loopCounter) { switch(medium_loopCounter) {
// This case deals with the GPS // This case deals with the GPS and Compass
//------------------------------- //-----------------------------------------
case 0: case 0:
medium_loopCounter++; medium_loopCounter++;
if(GPS_failure_counter > 0){ if(GPS_failure_counter > 0){
update_GPS(); update_GPS();
}else if(GPS_failure_counter == 0){
GPS_disabled = true;
} }
//readCommands(); //readCommands();
@ -529,32 +532,39 @@ void medium_loop()
case 1: case 1:
medium_loopCounter++; 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 // calc pitch and roll to target
// ----------------------------- // -----------------------------
dTnav2 = millis() - nav2_loopTimer; dTnav2 = millis() - nav2_loopTimer;
nav2_loopTimer = millis(); 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 if(wp_distance < 800){ // 8 meters
calc_loiter_nav(); calc_loiter_nav();
}else{ }else{
calc_waypoint_nav(); calc_waypoint_nav();
} }
break; break;
// command processing // command processing
@ -563,7 +573,7 @@ void medium_loop()
medium_loopCounter++; medium_loopCounter++;
// Read altitude from sensors // Read altitude from sensors
// ------------------ // --------------------------
update_alt(); update_alt();
// perform next command // perform next command
@ -590,32 +600,46 @@ void medium_loop()
Log_Write_Nav_Tuning(); Log_Write_Nav_Tuning();
if (g.log_bitmask & MASK_LOG_GPS){ if (g.log_bitmask & MASK_LOG_GPS){
if(home_is_set) 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); 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 gcs.send_message(MSG_ATTITUDE); // Sends attitude data
break; break;
// This case controls the slow loop // This case controls the slow loop
//--------------------------------- //---------------------------------
case 4: case 4:
medium_loopCounter = 0;
if (g.current_enabled){ if (g.current_enabled){
read_current(); read_current();
} }
// shall we trim the copter? // Accel trims = hold > 2 seconds
// ------------------------ // Throttle cruise = switch less than 1 second
// --------------------------------------------
read_trim_switch(); read_trim_switch();
// shall we check for engine start? // Check for engine arming
// -------------------------------- // -----------------------
arm_motors(); arm_motors();
medium_loopCounter = 0;
slow_loop(); slow_loop();
break; break;
default: default:
// this is just a catch all
// ------------------------
medium_loopCounter = 0; medium_loopCounter = 0;
break; break;
} }
@ -688,6 +712,9 @@ void slow_loop()
slow_loopCounter = 0; slow_loopCounter = 0;
update_events(); update_events();
// blink if we are armed
update_motor_light();
// XXX this should be a "GCS slow loop" interface // XXX this should be a "GCS slow loop" interface
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK #if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
gcs.data_stream_send(1,5); gcs.data_stream_send(1,5);
@ -724,7 +751,7 @@ void update_GPS(void)
update_GPS_light(); update_GPS_light();
if (g_gps->new_data && g_gps->fix) { 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 // XXX We should be sending GPS data off one of the regular loops so that we send
// no-GPS-fix data too // no-GPS-fix data too
@ -755,7 +782,7 @@ void update_GPS(void)
Log_Write_Startup(TYPE_GROUNDSTART_MSG); Log_Write_Startup(TYPE_GROUNDSTART_MSG);
// reset our nav loop timer // reset our nav loop timer
nav_loopTimer = millis(); //nav_loopTimer = millis();
init_home(); init_home();
// init altitude // init altitude
@ -866,17 +893,28 @@ void update_current_flight_mode(void)
fbw_timer++; fbw_timer++;
// 25 hz // 25 hz
if(fbw_timer > 4){ if(fbw_timer > 4){
Location temp = current_loc; fbw_timer = 0;
temp.lng += g.rc_1.control_in / 2;
temp.lat -= g.rc_2.control_in / 2; 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 // calc a new bearing
nav_bearing = get_bearing(&current_loc, &temp) + initial_simple_bearing; nav_bearing = get_bearing(&current_loc, &next_WP) + initial_simple_bearing;
nav_bearing = wrap_360(nav_bearing); nav_bearing = wrap_360(nav_bearing);
wp_distance = get_distance(&current_loc, &temp); wp_distance = get_distance(&current_loc, &next_WP);
calc_bearing_error(); 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 // get nav_pitch and nav_roll
calc_waypoint_nav(); calc_waypoint_nav();
} }
@ -905,20 +943,15 @@ void update_current_flight_mode(void)
if(fbw_timer > 10){ if(fbw_timer > 10){
fbw_timer = 0; fbw_timer = 0;
if(home_is_set == false){ if(GPS_disabled){
scaleLongDown = 1;
// we are not using GPS
// reset the location
// RTL won't function
current_loc.lat = home.lat = 0; current_loc.lat = home.lat = 0;
current_loc.lng = home.lng = 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 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 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 // Output Pitch, Roll, Yaw and Throttle

2
ArduCopterMega/Attitude.pde
View File

@ -231,7 +231,7 @@ void calc_nav_throttle()
float scaler = 1.0; float scaler = 1.0;
if(error < 0){ if(error < 0){
scaler = (altitude_sensor == BARO) ? .5 : .8; scaler = (altitude_sensor == BARO) ? .5 : .5;
} }
if(altitude_sensor == BARO){ if(altitude_sensor == BARO){

4
ArduCopterMega/config.h
View File

@ -287,7 +287,7 @@
#define ACRO_RATE_PITCH_IMAX_CENTIDEGREE ACRO_RATE_PITCH_IMAX * 100 #define ACRO_RATE_PITCH_IMAX_CENTIDEGREE ACRO_RATE_PITCH_IMAX * 100
#ifndef ACRO_RATE_YAW_P #ifndef ACRO_RATE_YAW_P
# define ACRO_RATE_YAW_P .5 # define ACRO_RATE_YAW_P .1
#endif #endif
#ifndef ACRO_RATE_YAW_I #ifndef ACRO_RATE_YAW_I
# define ACRO_RATE_YAW_I 0.0 # define ACRO_RATE_YAW_I 0.0
@ -394,7 +394,7 @@
# define NAV_P 2.0 # define NAV_P 2.0
#endif #endif
#ifndef NAV_I #ifndef NAV_I
# define NAV_I 0.00 # define NAV_I 0.1
#endif #endif
#ifndef NAV_D #ifndef NAV_D
# define NAV_D 0.00 # define NAV_D 0.00

8
ArduCopterMega/motors.pde
View File

@ -13,12 +13,14 @@ void arm_motors()
if (arming_counter > ARM_DELAY) { if (arming_counter > ARM_DELAY) {
motor_armed = true; motor_armed = true;
} else{ } else{
//Serial.printf("arm %d\n", arming_counter);
arming_counter++; arming_counter++;
} }
}else if (g.rc_4.control_in < -2700) { }else if (g.rc_4.control_in < -2700) {
if (arming_counter > DISARM_DELAY){ if (arming_counter > DISARM_DELAY){
motor_armed = false; motor_armed = false;
}else{ }else{
//Serial.printf("arm %d\n", arming_counter);
arming_counter++; arming_counter++;
} }
}else{ }else{
@ -157,6 +159,7 @@ set_servos_4()
} }
num++; num++;
if (num > 50){ if (num > 50){
num = 0; num = 0;
/* /*
@ -172,8 +175,6 @@ set_servos_4()
} }
*/ */
//Serial.print("!"); //Serial.print("!");
//debugging with Channel 6 //debugging with Channel 6
@ -196,7 +197,7 @@ set_servos_4()
init_pids(); init_pids();
//*/ //*/
/* ///*
write_int(motor_out[CH_1]); write_int(motor_out[CH_1]);
write_int(motor_out[CH_2]); write_int(motor_out[CH_2]);
write_int(motor_out[CH_3]); write_int(motor_out[CH_3]);
@ -214,7 +215,6 @@ set_servos_4()
write_int((int)nav_roll); write_int((int)nav_roll);
write_int((int)nav_pitch); write_int((int)nav_pitch);
//24
write_long(current_loc.lat); // 28 write_long(current_loc.lat); // 28
write_long(current_loc.lng); // 32 write_long(current_loc.lng); // 32
write_int((int)current_loc.alt); // 34 write_int((int)current_loc.alt); // 34

33
ArduCopterMega/navigation.pde
View File

@ -70,14 +70,12 @@ void calc_loiter_nav()
lat_error = constrain(lat_error, -DIST_ERROR_MAX, DIST_ERROR_MAX); // +- 20m max error lat_error = constrain(lat_error, -DIST_ERROR_MAX, DIST_ERROR_MAX); // +- 20m max error
// Convert distance into ROLL X // Convert distance into ROLL X
nav_lon = long_error * g.pid_nav_lon.kP(); // 1800 * 2 = 3600 or 36° //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 = 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
// PITCH Y // 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 = 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);
//nav_lat = constrain(nav_lat, -DIST_ERROR_MAX, DIST_ERROR_MAX); // Limit max command
// rotate the vector // rotate the vector
nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x; 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() 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° // get the sin and cos of the bearing error - rotated 90°
sin_nav_y = sin(radians((float)(9000 - bearing_error) / 100)); sin_nav_y = sin(radians((float)(9000 - bearing_error) / 100));
cos_nav_x = cos(radians((float)(bearing_error - 9000) / 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 // rotate the vector
nav_roll = (float)nav_lat * cos_nav_x; nav_roll = (float)nav_lat * cos_nav_x;
nav_pitch = (float)nav_lat * sin_nav_y; nav_pitch = (float)nav_lat * sin_nav_y;
// Scale response by kP //Serial.printf("R%ld, P%ld ", nav_roll, nav_pitch);
nav_roll *= g.pid_nav_lon.kP(); // 1800 * 2 = 3600 or 36°
nav_pitch *= g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36°
long pmax = g.pitch_max.get(); long pmax = g.pitch_max.get();
nav_roll = constrain(nav_roll, -pmax, pmax); nav_roll = constrain(nav_roll, -pmax, pmax);
nav_pitch = constrain(nav_pitch, -pmax, pmax); nav_pitch = constrain(nav_pitch, -pmax, pmax);
//Serial.printf("R%ld, P%ld \n", nav_roll, nav_pitch);
} }
void calc_bearing_error() void calc_bearing_error()
@ -205,10 +210,10 @@ long get_altitude_above_home(void)
long get_distance(struct Location *loc1, struct Location *loc2) long get_distance(struct Location *loc1, struct Location *loc2)
{ {
if(loc1->lat == 0 || loc1->lng == 0) //if(loc1->lat == 0 || loc1->lng == 0)
return -1; // return -1;
if(loc2->lat == 0 || loc2->lng == 0) //if(loc2->lat == 0 || loc2->lng == 0)
return -1; // return -1;
float dlat = (float)(loc2->lat - loc1->lat); float dlat = (float)(loc2->lat - loc1->lat);
float dlong = ((float)(loc2->lng - loc1->lng)) * scaleLongDown; float dlong = ((float)(loc2->lng - loc1->lng)) * scaleLongDown;
return sqrt(sq(dlat) + sq(dlong)) * .01113195; return sqrt(sq(dlat) + sq(dlong)) * .01113195;

21
ArduCopterMega/system.pde
View File

@ -82,7 +82,7 @@ void init_ardupilot()
// //
Serial3.begin(SERIAL3_BAUD, 128, 128); 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"), "\n\nFree RAM: %lu\n"),
freeRAM()); freeRAM());
@ -92,9 +92,11 @@ void init_ardupilot()
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\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..."),
g.format_version.get(), Parameters::k_format_version); g.format_version.get(),
Parameters::k_format_version);
*/
// erase all parameters // erase all parameters
AP_Var::erase_all(); AP_Var::erase_all();
@ -102,14 +104,14 @@ void init_ardupilot()
// save the new format version // save the new format version
g.format_version.set_and_save(Parameters::k_format_version); g.format_version.set_and_save(Parameters::k_format_version);
Serial.println_P(PSTR("done.")); //Serial.println_P(PSTR("done."));
}else{ }else{
unsigned long before = micros(); unsigned long before = micros();
// Load all auto-loaded EEPROM variables // Load all auto-loaded EEPROM variables
AP_Var::load_all(); AP_Var::load_all();
Serial.printf_P(PSTR("load_all took %luus\n"), micros() - before); // 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("using %u bytes of memory\n"), AP_Var::get_memory_use());
} }
#ifdef RADIO_OVERRIDE_DEFAULTS #ifdef RADIO_OVERRIDE_DEFAULTS
@ -171,11 +173,10 @@ void init_ardupilot()
Serial.printf_P(PSTR("\n" Serial.printf_P(PSTR("\n"
"Entering interactive setup mode...\n" "Entering interactive setup mode...\n"
"\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" "Type 'help' to list commands, 'exit' to leave a submenu.\n"
"Visit the 'setup' menu for first-time configuration.\n")); "Visit the 'setup' menu for first-time configuration.\n"));
for (;;) { 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(); main_menu.run();
} }
} }
@ -342,6 +343,7 @@ void update_GPS_light(void)
// GPS LED on if we have a fix or Blink GPS LED if we are receiving data // 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): case(2):
digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix. digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix.
break; break;
@ -362,7 +364,10 @@ void update_GPS_light(void)
digitalWrite(C_LED_PIN, LOW); digitalWrite(C_LED_PIN, LOW);
break; break;
} }
}
void update_motor_light(void)
{
if(motor_armed == true){ if(motor_armed == true){
motor_light = !motor_light; motor_light = !motor_light;