Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1843 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-04-03 21:11:14 +00:00
parent e8c40234a6
commit 4660bb87e2
10 changed files with 182 additions and 134 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopterMega/APM_Config.h
View File

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

6
ArduCopterMega/ArduCopterMega.pde
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_sum; // deg : how far we have turned around a waypoint
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
// ----------------------------------------------------
@ -574,6 +574,10 @@ void medium_loop()
// calculate the copter's desired bearing and WP distance
// ------------------------------------------------------
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

6
ArduCopterMega/commands.pde
View File

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

223
ArduCopterMega/commands_logic.pde
View File

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

43
ArduCopterMega/commands_process.pde
View File

@ -1,7 +1,7 @@
/// -*- 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)
{
// 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
// generate a dynamic command for RTL
// --------------------------------------------
if(next_command.id == NO_COMMAND){
Serial.println("lnc_nc");
// we are out of commands!
gcs.send_text_P(SEVERITY_LOW,PSTR("out of commands!"));
handle_no_commands();
if(handle_no_commands() == true){
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
command_must_index = g.waypoint_index;
//SendDebug("MSG <process_next_command> new command_must_id ");
//SendDebug(next_command.id,DEC);
//SendDebug(" index:");
//SendDebugln(command_must_index,DEC);
SendDebug("MSG <pnc> new c_must_id ");
SendDebug(next_command.id,DEC);
SendDebug(" index:");
SendDebugln(command_must_index,DEC);
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index, &next_command);
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 ){
increment_WP_index(); // this command is from the command list in EEPROM
command_may_index = g.waypoint_index;
//SendDebug("MSG <process_next_command> new command_may_id ");
//SendDebug(next_command.id,DEC);
SendDebug("MSG <pnc> new may ");
SendDebugln(next_command.id,DEC);
//Serial.print("new command_may_index ");
//Serial.println(command_may_index,DEC);
if (g.log_bitmask & MASK_LOG_CMD)
@ -86,8 +87,9 @@ void process_next_command()
// ---------------------------
if (next_command.id > MAV_CMD_CONDITION_LAST){
increment_WP_index(); // this command is from the command list in EEPROM
//SendDebug("MSG <process_next_command> new command_now_id ");
//SendDebug(next_command.id,DEC);
SendDebug("MSG <pnc> new now ");
SendDebugln(next_command.id,DEC);
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index, &next_command);
process_now();
@ -100,8 +102,9 @@ void process_next_command()
/**************************************************/
void process_must()
{
gcs.send_text_P(SEVERITY_LOW,PSTR("New cmd: <process_must>"));
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
//gcs.send_text_P(SEVERITY_LOW,PSTR("New cmd: <process_must>"));
//gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
Serial.printf("pmst %d\n", (int)next_command.id);
// clear May indexes
command_may_index = NO_COMMAND;
@ -117,8 +120,9 @@ void process_must()
void process_may()
{
gcs.send_text_P(SEVERITY_LOW,PSTR("<process_may>"));
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
//gcs.send_text_P(SEVERITY_LOW,PSTR("<process_may>"));
//gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
Serial.print("pmay");
command_may_ID = next_command.id;
handle_process_may();
@ -130,13 +134,14 @@ void process_may()
void process_now()
{
Serial.print("pnow");
handle_process_now();
// invalidate command so a new one is loaded
// -----------------------------------------
next_command.id = NO_COMMAND;
gcs.send_text_P(SEVERITY_LOW, PSTR("<process_now>"));
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
//gcs.send_text_P(SEVERITY_LOW, PSTR("<process_now>"));
//gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
}

2
ArduCopterMega/defines.h
View File

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

4
ArduCopterMega/navigation.pde
View File

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

11
ArduCopterMega/setup.pde
View File

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

5
ArduCopterMega/system.pde
View File

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

14
ArduCopterMega/test.pde
View File

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