zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

updated test mission. It now passes the tests. Be sure to verify things are working well before flying. 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1843 f9c3cf11-9bcb-44bc-f272-b75c42450872
mission-4.1.18
jasonshort 14 years ago
parent
e8c40234a6
commit
4660bb87e2
10 changed files with 183 additions and 135 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      ArduCopterMega/APM_Config.h
  2. 6
      ArduCopterMega/ArduCopterMega.pde
  3. 6
      ArduCopterMega/commands.pde
  4. 225
      ArduCopterMega/commands_logic.pde
  5. 43
      ArduCopterMega/commands_process.pde
  6. 2
      ArduCopterMega/defines.h
  7. 4
      ArduCopterMega/navigation.pde
  8. 11
      ArduCopterMega/setup.pde
  9. 5
      ArduCopterMega/system.pde
  10. 14
      ArduCopterMega/test.pde

2
ArduCopterMega/APM_Config.h
Unescape View File

@ -4,7 +4,7 @@
// GPS is auto-selected // GPS is auto-selected
#define GCS_PROTOCOL GCS_PROTOCOL_NONE //#define GCS_PROTOCOL GCS_PROTOCOL_NONE
//#define GCS_PORT 0 //#define GCS_PORT 0
#define SERIAL0_BAUD 38400 #define SERIAL0_BAUD 38400

6
ArduCopterMega/ArduCopterMega.pde
Unescape View File

@ -324,7 +324,7 @@ int loiter_total; // deg : how many times to loiter * 360
int loiter_delta; // deg : how far we just turned int loiter_delta; // deg : how far we just turned
int loiter_sum; // deg : how far we have turned around a waypoint int loiter_sum; // deg : how far we have turned around a waypoint
long loiter_time; // millis : when we started LOITER mode long loiter_time; // millis : when we started LOITER mode
int loiter_time_max; // millis : how long to stay in LOITER mode long loiter_time_max; // millis : how long to stay in LOITER mode
// these are the values for navigation control functions // these are the values for navigation control functions
// ---------------------------------------------------- // ----------------------------------------------------
@ -574,6 +574,10 @@ void medium_loop()
// calculate the copter's desired bearing and WP distance // calculate the copter's desired bearing and WP distance
// ------------------------------------------------------ // ------------------------------------------------------
navigate(); navigate();
// control mode specific updates to nav_bearing
// --------------------------------------------
update_navigation();
} }
// we call these regardless of GPS because of the rapid nature of the yaw sensor // we call these regardless of GPS because of the rapid nature of the yaw sensor

6
ArduCopterMega/commands.pde
Unescape View File

@ -9,20 +9,22 @@ void init_commands()
next_command.id = CMD_BLANK; next_command.id = CMD_BLANK;
} }
void update_auto() void init_auto()
{ {
if (g.waypoint_index == g.waypoint_total) { if (g.waypoint_index == g.waypoint_total) {
Serial.println("ia_f");
do_RTL(); do_RTL();
} }
} }
// this is only used by an air-start // this is only used by an air-start
void reload_commands_airstart() /*void reload_commands_airstart()
{ {
init_commands(); init_commands();
g.waypoint_index.load(); // XXX can we assume it's been loaded already by ::load_all? g.waypoint_index.load(); // XXX can we assume it's been loaded already by ::load_all?
decrement_WP_index(); decrement_WP_index();
} }
*/
// Getters // Getters
// ------- // -------

225
ArduCopterMega/commands_logic.pde
Unescape View File

@ -101,22 +101,23 @@ void handle_process_now()
do_repeat_relay(); do_repeat_relay();
break; break;
case MAV_CMD_NAV_ORIENTATION_TARGET: case MAV_CMD_NAV_ORIENTATION_TARGET:
do_target_yaw(); do_target_yaw();
} }
} }
void handle_no_commands() bool handle_no_commands()
{ {
if (command_must_ID) if (command_must_ID)
return; return false;
switch (control_mode){ switch (control_mode){
default: default:
set_mode(RTL); //set_mode(RTL);
break; break;
} }
return true;
} }
/********************************************************************************/ /********************************************************************************/
@ -125,6 +126,8 @@ void handle_no_commands()
bool verify_must() bool verify_must()
{ {
Serial.printf("vmust ::%d", nav_throttle);
switch(command_must_ID) { switch(command_must_ID) {
case MAV_CMD_NAV_TAKEOFF: case MAV_CMD_NAV_TAKEOFF:
@ -190,14 +193,14 @@ bool verify_may()
} }
/********************************************************************************/ /********************************************************************************/
// Nav (Must) commands // Nav (Must) commands
/********************************************************************************/ /********************************************************************************/
void do_RTL(void) void do_RTL(void)
{ {
control_mode = LOITER; control_mode = LOITER;
Location temp = home; Location temp = home;
temp.alt = read_alt_to_hold(); temp.alt = read_alt_to_hold();
//so we know where we are navigating from //so we know where we are navigating from
next_WP = current_loc; next_WP = current_loc;
@ -215,9 +218,12 @@ void do_RTL(void)
void do_takeoff() void do_takeoff()
{ {
Location temp = current_loc; Location temp = current_loc;
temp.alt = next_command.alt; temp.alt += next_command.alt;
takeoff_complete = false; // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction takeoff_complete = false; // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction
Serial.print("dt ");
Serial.println(temp.alt,DEC);
set_next_WP(&temp); set_next_WP(&temp);
} }
@ -229,63 +235,86 @@ void do_nav_wp()
void do_land() void do_land()
{ {
land_complete = false; // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction Serial.println("dlnd ");
velocity_land = 1000; land_complete = false; // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction
velocity_land = 2000;
Location temp = current_loc; Location temp = current_loc;
//temp.alt = home.alt; //temp.alt = home.alt;
temp.alt = -1000; // just go down far
temp.alt = -100000;
set_next_WP(&temp); set_next_WP(&temp);
} }
void do_loiter_unlimited() void do_loiter_unlimited()
{ {
set_next_WP(&next_command); Serial.println("dloi ");
if(next_command.lat == 0)
set_next_WP(&current_loc);
else
set_next_WP(&next_command);
} }
void do_loiter_turns() void do_loiter_turns()
{ {
set_next_WP(&next_command); if(next_command.lat == 0)
set_next_WP(&current_loc);
else
set_next_WP(&next_command);
loiter_total = next_command.p1 * 360; loiter_total = next_command.p1 * 360;
} }
void do_loiter_time() void do_loiter_time()
{ {
set_next_WP(&next_command); if(next_command.lat == 0)
loiter_time = millis(); set_next_WP(&current_loc);
loiter_time_max = next_command.p1; // units are (seconds * 10) else
set_next_WP(&next_command);
loiter_time = millis();
loiter_time_max = next_command.p1 * 1000; // units are (seconds)
Serial.printf("dlt %ld, max %ld\n",loiter_time, loiter_time_max);
} }
/********************************************************************************/ /********************************************************************************/
// Verify Nav (Must) commands // Verify Nav (Must) commands
/********************************************************************************/ /********************************************************************************/
bool verify_takeoff() bool verify_takeoff()
{ {
Serial.print("vt ");
if (current_loc.alt > next_WP.alt){ if (current_loc.alt > next_WP.alt){
Serial.println("Y");
takeoff_complete = true; takeoff_complete = true;
return true; return true;
}else{ }else{
Serial.println("N");
return false; return false;
} }
} }
bool verify_land() bool verify_land()
{ {
Serial.print("vlnd ");
velocity_land = ((old_alt - current_loc.alt) *.2) + (velocity_land * .8); velocity_land = ((old_alt - current_loc.alt) *.2) + (velocity_land * .8);
old_alt = current_loc.alt; old_alt = current_loc.alt;
if(g.sonar_enabled){ if(g.sonar_enabled){
// decide which sensor we're usings // decide which sensor we're usings
if(sonar_alt < 20){ if(sonar_alt < 20){
land_complete = true; land_complete = true;
return true; Serial.println("Y");
} return true;
} else { }
//land_complete = true; }
//return true;
} if(velocity_land <= 0)
land_complete = true;
return true;
}
Serial.printf("N, %d\n", velocity_land);
//update_crosstrack(); //update_crosstrack();
return false; return false;
@ -318,8 +347,11 @@ bool verify_loiter_unlim()
bool verify_loiter_time() bool verify_loiter_time()
{ {
if ((millis() - loiter_time) > (long)loiter_time_max * 10000l) { // scale loiter_time_max from (sec*10) to milliseconds Serial.printf("vlt %ld\n",(millis() - loiter_time));
if ((millis() - loiter_time) > loiter_time_max) { // scale loiter_time_max from (sec*10) to milliseconds
gcs.send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete")); gcs.send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete"));
Serial.println("vlt done");
return true; return true;
} }
return false; return false;
@ -336,44 +368,47 @@ bool verify_RTL()
} }
/********************************************************************************/ /********************************************************************************/
// Condition (May) commands // Condition (May) commands
/********************************************************************************/ /********************************************************************************/
void do_wait_delay() void do_wait_delay()
{ {
Serial.print("dwd ");
condition_start = millis(); condition_start = millis();
condition_value = next_command.lat * 1000; // convert to milliseconds condition_value = next_command.lat * 1000; // convert to milliseconds
Serial.println(condition_value,DEC);
} }
void do_change_alt() void do_change_alt()
{ {
Location temp = next_WP; Location temp = next_WP;
condition_start = current_loc.alt; condition_start = current_loc.alt;
condition_value = next_command.alt + home.alt; condition_value = next_command.alt + home.alt;
temp.alt = condition_value; temp.alt = condition_value;
set_next_WP(&temp); set_next_WP(&temp);
} }
void do_within_distance() void do_within_distance()
{ {
condition_value = next_command.lat; condition_value = next_command.lat;
} }
void do_yaw() void do_yaw()
{ {
yaw_tracking = TRACK_NONE; Serial.println("dyaw ");
yaw_tracking = TRACK_NONE;
// target angle in degrees // target angle in degrees
command_yaw_start = nav_yaw; // current position command_yaw_start = nav_yaw; // current position
command_yaw_start_time = millis(); command_yaw_start_time = millis();
command_yaw_dir = next_command.p1; // 1 = clockwise, 0 = counterclockwise command_yaw_dir = next_command.p1; // 1 = clockwise, 0 = counterclockwise
command_yaw_relative = next_command.lng; // 1 = Relative, 0 = Absolute command_yaw_relative = next_command.lng; // 1 = Relative, 0 = Absolute
command_yaw_speed = next_command.lat * 100; command_yaw_speed = next_command.lat * 100; // ms * 100
// if unspecified go 10° a second // if unspecified go 60° a second
if(command_yaw_speed == 0) if(command_yaw_speed == 0)
command_yaw_speed = 6000; command_yaw_speed = 6000;
@ -383,40 +418,34 @@ void do_yaw()
if (command_yaw_relative){ if (command_yaw_relative){
// relative // relative
//command_yaw_dir = (command_yaw_end > 0) ? 1 : -1; //command_yaw_dir = (command_yaw_end > 0) ? 1 : -1;
//command_yaw_end += nav_yaw; //command_yaw_end += nav_yaw;
//command_yaw_end = wrap_360(command_yaw_end); //command_yaw_end = wrap_360(command_yaw_end);
command_yaw_delta = next_command.alt * 100; command_yaw_delta = next_command.alt * 100;
}else{ }else{
// absolute // absolute
command_yaw_end = next_command.alt * 100; command_yaw_end = next_command.alt * 100;
// calculate the delta travel in deg * 100 // calculate the delta travel in deg * 100
if(command_yaw_dir == 1){ if(command_yaw_dir == 1){
if(command_yaw_start >= command_yaw_end){ if(command_yaw_start >= command_yaw_end){
command_yaw_delta = 36000 - (command_yaw_start - command_yaw_end); command_yaw_delta = 36000 - (command_yaw_start - command_yaw_end);
}else{ }else{
command_yaw_delta = command_yaw_end - command_yaw_start; command_yaw_delta = command_yaw_end - command_yaw_start;
} }
}else{ }else{
if(command_yaw_start > command_yaw_end){ if(command_yaw_start > command_yaw_end){
command_yaw_delta = command_yaw_start - command_yaw_end; command_yaw_delta = command_yaw_start - command_yaw_end;
}else{ }else{
command_yaw_delta = 36000 + (command_yaw_start - command_yaw_end); command_yaw_delta = 36000 + (command_yaw_start - command_yaw_end);
} }
} }
command_yaw_delta = wrap_360(command_yaw_delta); command_yaw_delta = wrap_360(command_yaw_delta);
} }
// rate to turn deg per second - default is ten // rate to turn deg per second - default is ten
command_yaw_time = command_yaw_delta / command_yaw_speed; command_yaw_time = (command_yaw_delta / command_yaw_speed) * 1000;
command_yaw_time *= 1000;
//
//9000 turn in 10 seconds
//command_yaw_time = 9000/ 10 = 900° per second
} }
@ -426,10 +455,13 @@ void do_yaw()
bool verify_wait_delay() bool verify_wait_delay()
{ {
Serial.print("vwd");
if ((millis() - condition_start) > condition_value){ if ((millis() - condition_start) > condition_value){
condition_value = 0; Serial.println("y");
condition_value = 0;
return true; return true;
} }
Serial.println("n");
return false; return false;
} }
@ -462,9 +494,13 @@ bool verify_within_distance()
bool verify_yaw() bool verify_yaw()
{ {
Serial.print("vyaw ");
if((millis() - command_yaw_start_time) > command_yaw_time){ if((millis() - command_yaw_start_time) > command_yaw_time){
// time out // time out
// make sure we hold at the final desired yaw angle
nav_yaw = command_yaw_end; nav_yaw = command_yaw_end;
Serial.println("Y");
return true; return true;
}else{ }else{
@ -472,23 +508,24 @@ bool verify_yaw()
// power is a ratio of the time : .5 = half done // power is a ratio of the time : .5 = half done
float power = (float)(millis() - command_yaw_start_time) / (float)command_yaw_time; float power = (float)(millis() - command_yaw_start_time) / (float)command_yaw_time;
nav_yaw = command_yaw_start + ((float)command_yaw_delta * power * command_yaw_dir); nav_yaw = command_yaw_start + ((float)command_yaw_delta * power * command_yaw_dir);
nav_yaw = wrap_360(nav_yaw); nav_yaw = wrap_360(nav_yaw);
Serial.printf("ny %ld\n",nav_yaw);
return false; return false;
} }
} }
/********************************************************************************/ /********************************************************************************/
// Do (Now) commands // Do (Now) commands
/********************************************************************************/ /********************************************************************************/
void do_target_yaw() void do_target_yaw()
{ {
yaw_tracking = next_command.p1; yaw_tracking = next_command.p1;
if(yaw_tracking & TRACK_TARGET_WP){ if(yaw_tracking & TRACK_TARGET_WP){
target_WP = next_command; target_WP = next_command;
} }
} }
void do_loiter_at_location() void do_loiter_at_location()
@ -501,10 +538,10 @@ void do_jump()
struct Location temp; struct Location temp;
if(next_command.lat > 0) { if(next_command.lat > 0) {
command_must_index = 0; command_must_index = 0;
command_may_index = 0; command_may_index = 0;
temp = get_wp_with_index(g.waypoint_index); temp = get_wp_with_index(g.waypoint_index);
temp.lat = next_command.lat - 1; // Decrement repeat counter temp.lat = next_command.lat - 1; // Decrement repeat counter
set_wp_with_index(temp, g.waypoint_index); set_wp_with_index(temp, g.waypoint_index);
g.waypoint_index.set_and_save(next_command.p1 - 1); g.waypoint_index.set_and_save(next_command.p1 - 1);
@ -516,10 +553,10 @@ void do_set_home()
if(next_command.p1 == 1) { if(next_command.p1 == 1) {
init_home(); init_home();
} else { } else {
home.id = MAV_CMD_NAV_WAYPOINT; home.id = MAV_CMD_NAV_WAYPOINT;
home.lng = next_command.lng; // Lon * 10**7 home.lng = next_command.lng; // Lon * 10**7
home.lat = next_command.lat; // Lat * 10**7 home.lat = next_command.lat; // Lat * 10**7
home.alt = max(next_command.alt, 0); home.alt = max(next_command.alt, 0);
home_is_set = true; home_is_set = true;
} }
} }
@ -546,10 +583,10 @@ void do_repeat_servo()
if(next_command.p1 >= CH_5 + 1 && next_command.p1 <= CH_8 + 1) { if(next_command.p1 >= CH_5 + 1 && next_command.p1 <= CH_8 + 1) {
event_timer = 0; event_timer = 0;
event_delay = next_command.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) event_delay = next_command.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
event_repeat = next_command.lat * 2; event_repeat = next_command.lat * 2;
event_value = next_command.alt; event_value = next_command.alt;
switch(next_command.p1) { switch(next_command.p1) {
case CH_5: case CH_5:
@ -571,9 +608,9 @@ void do_repeat_servo()
void do_repeat_relay() void do_repeat_relay()
{ {
event_id = RELAY_TOGGLE; event_id = RELAY_TOGGLE;
event_timer = 0; event_timer = 0;
event_delay = next_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) event_delay = next_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
event_repeat = next_command.alt * 2; event_repeat = next_command.alt * 2;
update_events(); update_events();
} }

43
ArduCopterMega/commands_process.pde
Unescape View File

@ -1,7 +1,7 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// called by 10 Hz loop // called by 10 Hz Medium loop
// -------------------- // ---------------------------
void update_commands(void) void update_commands(void)
{ {
// This function loads commands into three buffers // This function loads commands into three buffers
@ -38,12 +38,13 @@ void load_next_command_from_EEPROM()
} }
// If the preload failed, return or just Loiter // If the preload failed, return or just Loiter
// generate a dynamic command for RTL
// -------------------------------------------- // --------------------------------------------
if(next_command.id == NO_COMMAND){ if(next_command.id == NO_COMMAND){
Serial.println("lnc_nc");
// we are out of commands! // we are out of commands!
gcs.send_text_P(SEVERITY_LOW,PSTR("out of commands!")); if(handle_no_commands() == true){
handle_no_commands(); gcs.send_text_P(SEVERITY_LOW,PSTR("out of commands!"));
}
} }
} }
@ -57,10 +58,10 @@ void process_next_command()
//save_command_index(); // TO DO - fix - to Recover from in air Restart //save_command_index(); // TO DO - fix - to Recover from in air Restart
command_must_index = g.waypoint_index; command_must_index = g.waypoint_index;
//SendDebug("MSG <process_next_command> new command_must_id "); SendDebug("MSG <pnc> new c_must_id ");
//SendDebug(next_command.id,DEC); SendDebug(next_command.id,DEC);
//SendDebug(" index:"); SendDebug(" index:");
//SendDebugln(command_must_index,DEC); SendDebugln(command_must_index,DEC);
if (g.log_bitmask & MASK_LOG_CMD) if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index, &next_command); Log_Write_Cmd(g.waypoint_index, &next_command);
process_must(); process_must();
@ -73,8 +74,8 @@ void process_next_command()
if (next_command.id > MAV_CMD_NAV_LAST && next_command.id < MAV_CMD_CONDITION_LAST ){ if (next_command.id > MAV_CMD_NAV_LAST && next_command.id < MAV_CMD_CONDITION_LAST ){
increment_WP_index(); // this command is from the command list in EEPROM increment_WP_index(); // this command is from the command list in EEPROM
command_may_index = g.waypoint_index; command_may_index = g.waypoint_index;
//SendDebug("MSG <process_next_command> new command_may_id "); SendDebug("MSG <pnc> new may ");
//SendDebug(next_command.id,DEC); SendDebugln(next_command.id,DEC);
//Serial.print("new command_may_index "); //Serial.print("new command_may_index ");
//Serial.println(command_may_index,DEC); //Serial.println(command_may_index,DEC);
if (g.log_bitmask & MASK_LOG_CMD) if (g.log_bitmask & MASK_LOG_CMD)
@ -86,8 +87,9 @@ void process_next_command()
// --------------------------- // ---------------------------
if (next_command.id > MAV_CMD_CONDITION_LAST){ if (next_command.id > MAV_CMD_CONDITION_LAST){
increment_WP_index(); // this command is from the command list in EEPROM increment_WP_index(); // this command is from the command list in EEPROM
//SendDebug("MSG <process_next_command> new command_now_id "); SendDebug("MSG <pnc> new now ");
//SendDebug(next_command.id,DEC); SendDebugln(next_command.id,DEC);
if (g.log_bitmask & MASK_LOG_CMD) if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index, &next_command); Log_Write_Cmd(g.waypoint_index, &next_command);
process_now(); process_now();
@ -100,8 +102,9 @@ void process_next_command()
/**************************************************/ /**************************************************/
void process_must() void process_must()
{ {
gcs.send_text_P(SEVERITY_LOW,PSTR("New cmd: <process_must>")); //gcs.send_text_P(SEVERITY_LOW,PSTR("New cmd: <process_must>"));
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); //gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
Serial.printf("pmst %d\n", (int)next_command.id);
// clear May indexes // clear May indexes
command_may_index = NO_COMMAND; command_may_index = NO_COMMAND;
@ -117,8 +120,9 @@ void process_must()
void process_may() void process_may()
{ {
gcs.send_text_P(SEVERITY_LOW,PSTR("<process_may>")); //gcs.send_text_P(SEVERITY_LOW,PSTR("<process_may>"));
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); //gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
Serial.print("pmay");
command_may_ID = next_command.id; command_may_ID = next_command.id;
handle_process_may(); handle_process_may();
@ -130,13 +134,14 @@ void process_may()
void process_now() void process_now()
{ {
Serial.print("pnow");
handle_process_now(); handle_process_now();
// invalidate command so a new one is loaded // invalidate command so a new one is loaded
// ----------------------------------------- // -----------------------------------------
next_command.id = NO_COMMAND; next_command.id = NO_COMMAND;
gcs.send_text_P(SEVERITY_LOW, PSTR("<process_now>")); //gcs.send_text_P(SEVERITY_LOW, PSTR("<process_now>"));
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); //gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
} }

2
ArduCopterMega/defines.h
Unescape View File

@ -114,7 +114,7 @@
#define RELAY_TOGGLE 5 #define RELAY_TOGGLE 5
#define STOP_REPEAT 10 #define STOP_REPEAT 10
#define MAV_CMD_CONDITION_YAW 23 //#define MAV_CMD_CONDITION_YAW 23
// GCS Message ID's // GCS Message ID's
#define MSG_ACKNOWLEDGE 0x00 #define MSG_ACKNOWLEDGE 0x00

4
ArduCopterMega/navigation.pde
Unescape View File

@ -45,10 +45,6 @@ void navigate()
if (loiter_delta > 180) loiter_delta -= 360; if (loiter_delta > 180) loiter_delta -= 360;
if (loiter_delta < -180) loiter_delta += 360; if (loiter_delta < -180) loiter_delta += 360;
loiter_sum += abs(loiter_delta); loiter_sum += abs(loiter_delta);
// control mode specific updates to nav_bearing
// --------------------------------------------
update_navigation();
} }
#define DIST_ERROR_MAX 1800 #define DIST_ERROR_MAX 1800

11
ArduCopterMega/setup.pde
Unescape View File

@ -799,7 +799,7 @@ void print_wp(struct Location *cmd, byte index)
void report_current() void report_current()
{ {
read_EEPROM_current(); //read_EEPROM_current();
Serial.printf_P(PSTR("Current \n")); Serial.printf_P(PSTR("Current \n"));
print_divider(); print_divider();
print_enabled(g.current_enabled.get()); print_enabled(g.current_enabled.get());
@ -859,7 +859,7 @@ void report_radio()
Serial.printf_P(PSTR("Radio\n")); Serial.printf_P(PSTR("Radio\n"));
print_divider(); print_divider();
// radio // radio
read_EEPROM_radio(); //read_EEPROM_radio();
print_radio_values(); print_radio_values();
print_blanks(2); print_blanks(2);
} }
@ -869,7 +869,8 @@ void report_gains()
Serial.printf_P(PSTR("Gains\n")); Serial.printf_P(PSTR("Gains\n"));
print_divider(); print_divider();
read_EEPROM_PID(); //read_EEPROM_PID();
// Acro // Acro
Serial.printf_P(PSTR("Acro:\nroll:\n")); Serial.printf_P(PSTR("Acro:\nroll:\n"));
print_PID(&g.pid_acro_rate_roll); print_PID(&g.pid_acro_rate_roll);
@ -906,7 +907,7 @@ void report_xtrack()
Serial.printf_P(PSTR("XTrack\n")); Serial.printf_P(PSTR("XTrack\n"));
print_divider(); print_divider();
// radio // radio
read_EEPROM_nav(); //read_EEPROM_nav();
Serial.printf_P(PSTR("XTRACK: %4.2f\n" Serial.printf_P(PSTR("XTRACK: %4.2f\n"
"XTRACK angle: %d\n" "XTRACK angle: %d\n"
"PITCH_MAX: %ld"), "PITCH_MAX: %ld"),
@ -921,7 +922,7 @@ void report_throttle()
Serial.printf_P(PSTR("Throttle\n")); Serial.printf_P(PSTR("Throttle\n"));
print_divider(); print_divider();
read_EEPROM_throttle(); //read_EEPROM_throttle();
Serial.printf_P(PSTR("min: %d\n" Serial.printf_P(PSTR("min: %d\n"
"max: %d\n" "max: %d\n"
"cruise: %d\n" "cruise: %d\n"

5
ArduCopterMega/system.pde
Unescape View File

@ -227,6 +227,9 @@ void startup_ground(void)
setup_show(NULL,NULL); setup_show(NULL,NULL);
// setup up PID value max
init_pids();
// Output waypoints for confirmation // Output waypoints for confirmation
// -------------------------------- // --------------------------------
for(int i = 1; i < g.waypoint_total + 1; i++) { for(int i = 1; i < g.waypoint_total + 1; i++) {
@ -315,7 +318,7 @@ void set_mode(byte mode)
break; break;
case AUTO: case AUTO:
update_auto(); init_auto();
break; break;
case SIMPLE: case SIMPLE:

14
ArduCopterMega/test.pde
Unescape View File

@ -736,7 +736,7 @@ static int8_t
test_wp(uint8_t argc, const Menu::arg *argv) test_wp(uint8_t argc, const Menu::arg *argv)
{ {
delay(1000); delay(1000);
read_EEPROM_waypoint_info(); //read_EEPROM_waypoint_info();
// save the alitude above home option // save the alitude above home option
@ -917,8 +917,8 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
static int8_t static int8_t
test_mission(uint8_t argc, const Menu::arg *argv) test_mission(uint8_t argc, const Menu::arg *argv)
{ {
print_hit_enter(); //print_hit_enter();
delay(1000); //delay(1000);
//write out a basic mission to the EEPROM //write out a basic mission to the EEPROM
Location t; Location t;
/*{ /*{
@ -938,12 +938,12 @@ test_mission(uint8_t argc, const Menu::arg *argv)
{Location t = {MAV_CMD_NAV_TAKEOFF, 0, 0, 300, 0, 0}; {Location t = {MAV_CMD_NAV_TAKEOFF, 0, 0, 300, 0, 0};
set_wp_with_index(t,1);} set_wp_with_index(t,1);}
// CMD opt time/ms // CMD opt time/s
{Location t = {MAV_CMD_CONDITION_DELAY, 0, 0, 0, 3000, 0}; {Location t = {MAV_CMD_NAV_LOITER_TIME, 0, 15, 0, 0, 0};
set_wp_with_index(t,2);} set_wp_with_index(t,2);}
// CMD opt dir angle/deg time/ms relative // CMD opt dir angle/deg deg/s relative
{Location t = {MAV_CMD_CONDITION_YAW, 0, 1, 360, 1000, 1}; {Location t = {MAV_CMD_CONDITION_YAW, 0, 1, 360, 60, 1};
set_wp_with_index(t,3);} set_wp_with_index(t,3);}
// CMD opt // CMD opt

Write Preview
Loading…
Cancel
Save