Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 17:38:32 -04:00
Code Issues Packages Projects Releases Wiki Activity

updates - support

Browse Source 
new test mission  - Liftoff, spin, land
updated scripted Yaw control

Public Beta candidate...

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1814 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-03-26 06:35:52 +00:00
parent db3b7ba12a
commit bcc1bf65f2
17 changed files with 290 additions and 179 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduCopterMega/APM_Config.h
View File

@ -5,10 +5,9 @@
// 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 MAG_ORIENTATION AP_COMPASS_COMPONENTS_UP_PINS_BACK #define SERIAL0_BAUD 38400
# define SERIAL0_BAUD 38400
//# define STABILIZE_ROLL_P 0.4 //# define STABILIZE_ROLL_P 0.4
//# define STABILIZE_PITCH_P 0.4 //# define STABILIZE_PITCH_P 0.4

27
ArduCopterMega/ArduCopterMega.pde
View File

@ -42,6 +42,8 @@ version 2.1 of the License, or (at your option) any later version.
#define MAVLINK_COMM_NUM_BUFFERS 2 #define MAVLINK_COMM_NUM_BUFFERS 2
#include <GCS_MAVLink.h> // MAVLink GCS definitions #include <GCS_MAVLink.h> // MAVLink GCS definitions
//#include <GCS_SIMPLE.h>
// Configuration // Configuration
#include "config.h" #include "config.h"
@ -175,6 +177,8 @@ GPS *g_gps;
GCS_Class gcs; GCS_Class gcs;
#endif #endif
//GCS_SIMPLE gcs_simple(&Serial);
AP_RangeFinder_MaxsonarXL sonar; AP_RangeFinder_MaxsonarXL sonar;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
@ -337,6 +341,7 @@ long command_yaw_end; // what angle are we trying to be
long command_yaw_delta; // how many degrees will we turn long command_yaw_delta; // how many degrees will we turn
int command_yaw_speed; // how fast to turn int command_yaw_speed; // how fast to turn
byte command_yaw_dir; byte command_yaw_dir;
byte command_yaw_relative;
// Waypoints // Waypoints
// --------- // ---------
@ -758,6 +763,19 @@ void super_slow_loop()
gcs.send_message(MSG_HEARTBEAT); // XXX This is running at 3 1/3 Hz instead of 1 Hz gcs.send_message(MSG_HEARTBEAT); // XXX This is running at 3 1/3 Hz instead of 1 Hz
// gcs.send_message(MSG_CPU_LOAD, load*100); // gcs.send_message(MSG_CPU_LOAD, load*100);
//if(gcs_simple.read()){
// Serial.print("!");
/*
Location temp;
temp.id = gcs_simple.id;
temp.p1 = gcs_simple.p1;
temp.alt = gcs_simple.altitude;
temp.lat = gcs_simple.latitude;
temp.lng = gcs_simple.longitude;
set_wp_with_index(temp, gcs_simple.index);
gcs_simple.ack();
*/
//}
} }
void update_GPS(void) void update_GPS(void)
@ -1127,11 +1145,9 @@ void update_alt()
// decide which sensor we're usings // decide which sensor we're usings
sonar_alt = sonar.read(); sonar_alt = sonar.read();
if(baro_alt < 550){ if(baro_alt < 500 && sonar_alt < 600){
altitude_sensor = SONAR; altitude_sensor = SONAR;
} }else{
if(sonar_alt > 600){
altitude_sensor = BARO; altitude_sensor = BARO;
} }
@ -1162,4 +1178,5 @@ void update_alt()
// Amount of throttle to apply for hovering // Amount of throttle to apply for hovering
// ---------------------------------------- // ----------------------------------------
calc_nav_throttle(); calc_nav_throttle();
} }

9
ArduCopterMega/Attitude.pde
View File

@ -89,7 +89,6 @@ clear_yaw_control()
void void
output_yaw_with_hold(boolean hold) output_yaw_with_hold(boolean hold)
{ {
//digitalWrite(B_LED_PIN, LOW);
if(hold){ if(hold){
// look to see if we have exited rate control properly - ie stopped turning // look to see if we have exited rate control properly - ie stopped turning
if(rate_yaw_flag){ if(rate_yaw_flag){
@ -101,8 +100,6 @@ output_yaw_with_hold(boolean hold)
}else{ }else{
//digitalWrite(B_LED_PIN, HIGH);
// return to rate control until we slow down. // return to rate control until we slow down.
hold = false; hold = false;
//Serial.print("D"); //Serial.print("D");
@ -119,7 +116,6 @@ output_yaw_with_hold(boolean hold)
} }
if(hold){ if(hold){
//Serial.println("H");
// try and hold the current nav_yaw setting // try and hold the current nav_yaw setting
yaw_error = nav_yaw - dcm.yaw_sensor; // +- 60° yaw_error = nav_yaw - dcm.yaw_sensor; // +- 60°
yaw_error = wrap_180(yaw_error); yaw_error = wrap_180(yaw_error);
@ -139,11 +135,9 @@ output_yaw_with_hold(boolean hold)
}else{ }else{
//Serial.println("R");
// rate control // rate control
long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377 long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377
rate = constrain(rate, -36000, 36000); // limit to something fun! 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 // -error = CCW, +error = CW
@ -161,10 +155,9 @@ output_yaw_with_hold(boolean hold)
g.rc_4.servo_out = constrain(g.rc_4.servo_out, -1800, 1800); // limit to 24° g.rc_4.servo_out = constrain(g.rc_4.servo_out, -1800, 1800); // limit to 24°
} }
} }
// slight left rudder give right roll. // slight left rudder gives right roll.
void void
output_rate_roll() output_rate_roll()

14
ArduCopterMega/GCS.pde
View File

@ -16,7 +16,7 @@ B Checksum byte 2
*/ */
/*
#if GCS_PORT == 3 #if GCS_PORT == 3
# define SendSerw Serial3.write # define SendSerw Serial3.write
# define SendSer Serial3.print # define SendSer Serial3.print
@ -80,23 +80,15 @@ void flush(byte id)
byte mess_ck_a = 0; byte mess_ck_a = 0;
byte mess_ck_b = 0; byte mess_ck_b = 0;
byte i; byte i;
SendSer("4D"); // This is the message preamble SendSer("4D"); // This is the message preamble
SendSerw(buff_pointer); // Length SendSerw(buff_pointer); // Length
SendSerw(2); // id SendSerw(2); // id
//SendSerw(0x01); // Version
for (i = 0; i < buff_pointer; i++) { for (i = 0; i < buff_pointer; i++) {
SendSerw(mess_buffer[i]); SendSerw(mess_buffer[i]);
} }
//for (i = 0; i < buff_pointer; i++) {
// mess_ck_a += mess_buffer[i]; // Calculates checksums
// mess_ck_b += mess_ck_a;
//}
//SendSerw(mess_ck_a);
//SendSerw(mess_ck_b);
buff_pointer = 0; buff_pointer = 0;
} }
*/

7
ArduCopterMega/GCS_Mavlink.pde
View File

@ -34,8 +34,6 @@ GCS_MAVLINK::update(void)
{ {
uint8_t c = comm_receive_ch(chan); uint8_t c = comm_receive_ch(chan);
// Try to get a new message // Try to get a new message
if(mavlink_parse_char(chan, c, &msg, &status)) handleMessage(&msg); if(mavlink_parse_char(chan, c, &msg, &status)) handleMessage(&msg);
} }
@ -43,11 +41,6 @@ GCS_MAVLINK::update(void)
// Update packet drops counter // Update packet drops counter
packet_drops += status.packet_rx_drop_count; packet_drops += status.packet_rx_drop_count;
// send out queued params/ waypoints // send out queued params/ waypoints
_queued_send(); _queued_send();

24
ArduCopterMega/Log.pde
View File

@ -22,10 +22,10 @@ static int8_t help_log(uint8_t argc, const Menu::arg *argv)
{ {
Serial.printf_P(PSTR("\n" Serial.printf_P(PSTR("\n"
"Commands:\n" "Commands:\n"
" dump <n> dump log <n>\n" " dump <n>"
" erase erase all logs\n" " erase (all logs)\n"
" enable <name>|all enable logging <name> or everything\n" " enable <name> | all\n"
" disable <name>|all disable logging <name> or everything\n" " disable <name> | all\n"
"\n")); "\n"));
} }
@ -75,13 +75,13 @@ print_log_menu(void)
Serial.println(); Serial.println();
if (last_log_num == 0) { if (last_log_num == 0) {
Serial.printf_P(PSTR("\nNo logs available for download\n")); Serial.printf_P(PSTR("\nNo logs\n"));
}else{ }else{
Serial.printf_P(PSTR("\n%d logs available for download\n"), last_log_num); Serial.printf_P(PSTR("\n%d logs\n"), last_log_num);
for(int i=1;i<last_log_num+1;i++) { for(int i=1;i<last_log_num+1;i++) {
get_log_boundaries(last_log_num, i, log_start, log_end); get_log_boundaries(last_log_num, i, log_start, log_end);
Serial.printf_P(PSTR("Log number %d, start page %d, end page %d\n"), Serial.printf_P(PSTR("Log # %d, start %d, end %d\n"),
i, log_start, log_end); i, log_start, log_end);
} }
Serial.println(); Serial.println();
@ -106,20 +106,20 @@ dump_log(uint8_t argc, const Menu::arg *argv)
} }
get_log_boundaries(last_log_num, dump_log, dump_log_start, dump_log_end); get_log_boundaries(last_log_num, dump_log, dump_log_start, dump_log_end);
Serial.printf_P(PSTR("Dumping Log number %d, start page %d, end page %d\n"), Serial.printf_P(PSTR("Dumping Log number %d, start %d, end %d\n"),
dump_log, dump_log,
dump_log_start, dump_log_start,
dump_log_end); dump_log_end);
Log_Read(dump_log_start, dump_log_end); Log_Read(dump_log_start, dump_log_end);
Serial.printf_P(PSTR("Log read complete\n")); Serial.printf_P(PSTR("Complete\n"));
} }
static int8_t static int8_t
erase_logs(uint8_t argc, const Menu::arg *argv) erase_logs(uint8_t argc, const Menu::arg *argv)
{ {
for(int i = 10 ; i > 0; i--) { for(int i = 10 ; i > 0; i--) {
Serial.printf_P(PSTR("ATTENTION - Erasing log in %d seconds. Power off now to save log! \n"), i); Serial.printf_P(PSTR("ATTENTION - Erasing log in %d seconds.\n"), i);
delay(1000); delay(1000);
} }
Serial.printf_P(PSTR("\nErasing log...\n")); Serial.printf_P(PSTR("\nErasing log...\n"));
@ -253,7 +253,7 @@ void start_new_log(byte num_existing_logs)
DataFlash.FinishWrite(); DataFlash.FinishWrite();
DataFlash.StartWrite(start_pages[num_existing_logs-1]); DataFlash.StartWrite(start_pages[num_existing_logs-1]);
}else{ }else{
gcs.send_text_P(SEVERITY_LOW,PSTR("<start_new_log> Logs full - logging discontinued")); gcs.send_text_P(SEVERITY_LOW,PSTR("<start_new_log> Logs full"));
} }
} }
@ -708,7 +708,7 @@ void Log_Read(int start_page, int end_page)
} }
page = DataFlash.GetPage(); page = DataFlash.GetPage();
} }
Serial.printf_P(PSTR("Number of packets read: %d\n"), packet_count); Serial.printf_P(PSTR("# of packets read: %d\n"), packet_count);
} }

2
ArduCopterMega/Mavlink_Common.h
View File

@ -8,7 +8,6 @@
uint16_t system_type = MAV_FIXED_WING; uint16_t system_type = MAV_FIXED_WING;
byte mavdelay = 0; byte mavdelay = 0;
// what does this do? // what does this do?
static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid) static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
{ {
@ -24,7 +23,6 @@ static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
} }
} }
void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, uint16_t packet_drops) void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, uint16_t packet_drops)
{ {
uint64_t timeStamp = micros(); uint64_t timeStamp = micros();

12
ArduCopterMega/command description.txt
View File

@ -23,22 +23,16 @@ Commands below MAV_CMD_NAV_LAST are commands that have a end criteria, eg "reach
*********************************** ***********************************
Command ID Name Parameter 1 Altitude Latitude Longitude Command ID Name Parameter 1 Altitude Latitude Longitude
0x10 / 16 MAV_CMD_NAV_WAYPOINT - altitude lat lon 0x10 / 16 MAV_CMD_NAV_WAYPOINT - altitude lat lon
0x11 / 17 MAV_CMD_NAV_LOITER_UNLIM (forever) altitude lat lon
0x11 / 17 MAV_CMD_NAV_LOITER_UNLIM (indefinitely) altitude lat lon
0x12 / 18 MAV_CMD_NAV_LOITER_TURNS turns altitude lat lon 0x12 / 18 MAV_CMD_NAV_LOITER_TURNS turns altitude lat lon
0x13 / 19 MAV_CMD_NAV_LOITER_TIME time (seconds*10) altitude lat lon 0x13 / 19 MAV_CMD_NAV_LOITER_TIME time (seconds*10) altitude lat lon
0x14 / 20 MAV_CMD_NAV_RETURN_TO_LAUNCH - altitude lat lon 0x14 / 20 MAV_CMD_NAV_RETURN_TO_LAUNCH - altitude lat lon
0x15 / 21 MAV_CMD_NAV_LAND - altitude lat lon 0x15 / 21 MAV_CMD_NAV_LAND - altitude lat lon
0x16 / 22 MAV_CMD_NAV_TAKEOFF takeoff pitch altitude - - 0x16 / 22 MAV_CMD_NAV_TAKEOFF takeoff pitch altitude - -
NOTE: for command 0x16 the value takeoff pitch specifies the minimum pitch for the case with airspeed sensor and the target pitch for the case without.
0x17 / 23 MAV_CMD_NAV_TARGET - altitude lat lon 0x17 / 23 MAV_CMD_NAV_TARGET - altitude lat lon
NOTE: for command 0x16 the value takeoff pitch specifies the minimum pitch for the case with airspeed sensor and the target pitch for the case without.
*********************************** ***********************************
May Commands - these commands are optional to finish May Commands - these commands are optional to finish

8
ArduCopterMega/commands.pde
View File

@ -40,6 +40,9 @@ struct Location get_wp_with_index(int i)
mem = (WP_START_BYTE) + (i * WP_SIZE); mem = (WP_START_BYTE) + (i * WP_SIZE);
temp.id = eeprom_read_byte((uint8_t*)mem); temp.id = eeprom_read_byte((uint8_t*)mem);
mem++;
temp.options = eeprom_read_byte((uint8_t*)mem);
mem++; mem++;
temp.p1 = eeprom_read_byte((uint8_t*)mem); temp.p1 = eeprom_read_byte((uint8_t*)mem);
@ -70,6 +73,9 @@ void set_wp_with_index(struct Location temp, int i)
eeprom_write_byte((uint8_t *) mem, temp.id); eeprom_write_byte((uint8_t *) mem, temp.id);
mem++;
eeprom_write_byte((uint8_t *) mem, temp.options);
mem++; mem++;
eeprom_write_byte((uint8_t *) mem, temp.p1); eeprom_write_byte((uint8_t *) mem, temp.p1);
@ -194,7 +200,7 @@ void init_home()
home.alt = max(g_gps->altitude, 0); home.alt = max(g_gps->altitude, 0);
home_is_set = true; home_is_set = true;
Serial.printf("gps alt: %ld", home.alt); //Serial.printf_P(PSTR("gps alt: %ld\n"), home.alt);
// Save Home to EEPROM // Save Home to EEPROM
// ------------------- // -------------------

98
ArduCopterMega/commands_logic.pde
View File

@ -230,7 +230,8 @@ void do_land()
velocity_land = 1000; velocity_land = 1000;
Location temp = current_loc; Location temp = current_loc;
temp.alt = home.alt; //temp.alt = home.alt;
temp.alt = -1000;
set_next_WP(&temp); set_next_WP(&temp);
} }
@ -269,17 +270,21 @@ bool verify_takeoff()
bool verify_land() bool verify_land()
{ {
// XXX not sure
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((current_loc.alt < home.alt + 100) && velocity_land == 0){ if(g.sonar_enabled){
land_complete = true; // decide which sensor we're usings
return true; if(sonar_alt < 20){
} land_complete = true;
return true;
}
} else {
//land_complete = true;
//return true;
}
update_crosstrack(); //update_crosstrack();
return false; return false;
} }
@ -353,62 +358,67 @@ void do_within_distance()
void do_yaw() void do_yaw()
{ {
// p1: bearing // { // CMD opt dir angle/deg deg/s relative
// alt: speed // Location t = {MAV_CMD_CONDITION_YAW, 0, 1, 360, 60, 1};
// lat: direction (-1,1),
// lng: rel (1) abs (0)
// 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();
// which direction to turn command_yaw_dir = next_command.p1; // 1 = clockwise, 0 = counterclockwise
// 1 = clockwise, -1 = counterclockwise command_yaw_relative = next_command.lng; // 1 = Relative, 0 = Absolute
command_yaw_dir = next_command.lat;
// 1 = Relative or 0 = Absolute command_yaw_speed = next_command.lat * 100;
if (next_command.lng == 1) {
// relative
command_yaw_dir = (command_yaw_end > 0) ? 1 : -1;
command_yaw_end += nav_yaw;
command_yaw_end = wrap_360(command_yaw_end);
}else{
// absolute
command_yaw_end = next_command.p1 * 100;
}
// if unspecified go 10° a second // if unspecified go 10° a second
if(command_yaw_speed == 0) if(command_yaw_speed == 0)
command_yaw_speed = 10; command_yaw_speed = 6000;
// if unspecified go clockwise // if unspecified go counterclockwise
if(command_yaw_dir == 0) if(command_yaw_dir == 0)
command_yaw_dir = 1; command_yaw_dir = -1;
// calculate the delta travel if (command_yaw_relative){
if(command_yaw_dir == 1){ // relative
if(command_yaw_start > command_yaw_end){ //command_yaw_dir = (command_yaw_end > 0) ? 1 : -1;
command_yaw_delta = 36000 - (command_yaw_start - command_yaw_end); //command_yaw_end += nav_yaw;
}else{ //command_yaw_end = wrap_360(command_yaw_end);
command_yaw_delta = command_yaw_end - command_yaw_start; command_yaw_delta = next_command.alt * 100;
}
}else{ }else{
if(command_yaw_start > command_yaw_end){ // absolute
command_yaw_delta = command_yaw_start - command_yaw_end; command_yaw_end = next_command.alt * 100;
}else{
command_yaw_delta = 36000 + (command_yaw_start - command_yaw_end); // 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);
} }
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_speed = next_command.alt;
command_yaw_time = command_yaw_delta / command_yaw_speed; command_yaw_time = command_yaw_delta / command_yaw_speed;
command_yaw_time *= 1000;
//
//9000 turn in 10 seconds //9000 turn in 10 seconds
//command_yaw_time = 9000/ 10 = 900° per second //command_yaw_time = 9000/ 10 = 900° per second
} }
/********************************************************************************/ /********************************************************************************/
// Verify Condition (May) commands // Verify Condition (May) commands
/********************************************************************************/ /********************************************************************************/
@ -452,13 +462,17 @@ bool verify_within_distance()
bool verify_yaw() bool verify_yaw()
{ {
if((millis() - command_yaw_start_time) > command_yaw_time){ if((millis() - command_yaw_start_time) > command_yaw_time){
// time out
nav_yaw = command_yaw_end; nav_yaw = command_yaw_end;
return true; return true;
}else{ }else{
// else we need to be at a certain place // else we need to be at a certain place
// 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);
return false; return false;
} }
} }

4
ArduCopterMega/config.h
View File

@ -121,7 +121,7 @@
// GCS_PORT // GCS_PORT
// //
#ifndef GCS_PROTOCOL #ifndef GCS_PROTOCOL
# define GCS_PROTOCOL GCS_PROTOCOL_NONE # define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK
#endif #endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
@ -346,7 +346,7 @@
// YAW Control // YAW Control
// //
#ifndef YAW_P #ifndef YAW_P
# define YAW_P 0.5 # define YAW_P 0.25
#endif #endif
#ifndef YAW_I #ifndef YAW_I
# define YAW_I 0.0 # define YAW_I 0.0

36
ArduCopterMega/control_modes.pde
View File

@ -19,17 +19,17 @@ void read_control_switch()
} }
byte readSwitch(void){ byte readSwitch(void){
#if FLIGHT_MODE_CHANNEL == CH_5 #if FLIGHT_MODE_CHANNEL == CH_5
int pulsewidth = g.rc_5.radio_in; // default for Arducopter int pulsewidth = g.rc_5.radio_in; // default for Arducopter
#elif FLIGHT_MODE_CHANNEL == CH_6 #elif FLIGHT_MODE_CHANNEL == CH_6
int pulsewidth = g.rc_6.radio_in; // int pulsewidth = g.rc_6.radio_in; //
#elif FLIGHT_MODE_CHANNEL == CH_7 #elif FLIGHT_MODE_CHANNEL == CH_7
int pulsewidth = g.rc_7.radio_in; // int pulsewidth = g.rc_7.radio_in; //
#elif FLIGHT_MODE_CHANNEL == CH_8 #elif FLIGHT_MODE_CHANNEL == CH_8
int pulsewidth = g.rc_8.radio_in; // default for Ardupilot. Don't use for Arducopter! it has a hardware failsafe mux! int pulsewidth = g.rc_8.radio_in; // default for Ardupilot. Don't use for Arducopter! it has a hardware failsafe mux!
#else #else
# error Must define FLIGHT_MODE_CHANNEL as CH_5 - CH_8 # error Must define FLIGHT_MODE_CHANNEL as CH_5 - CH_8
#endif #endif
if (pulsewidth > 1230 && pulsewidth <= 1360) return 1; if (pulsewidth > 1230 && pulsewidth <= 1360) return 1;
if (pulsewidth > 1360 && pulsewidth <= 1490) return 2; if (pulsewidth > 1360 && pulsewidth <= 1490) return 2;
@ -75,14 +75,14 @@ void read_trim_switch()
if(trim_flag){ if(trim_flag){
// switch was just released // switch was just released
if((millis() - trim_timer) > 2000){ if((millis() - trim_timer) > 2000){
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
imu.save(); imu.save();
#endif #endif
}else{ }else{
// set the throttle nominal // set the throttle nominal
if(g.rc_3.control_in > 50){ if(g.rc_3.control_in > 50){
g.throttle_cruise.set(g.rc_3.control_in); g.throttle_cruise.set(g.rc_3.control_in);
Serial.printf("tnom %d\n", g.throttle_cruise.get()); //Serial.printf("tnom %d\n", g.throttle_cruise.get());
//save_EEPROM_throttle_cruise(); //save_EEPROM_throttle_cruise();
g.throttle_cruise.save(); g.throttle_cruise.save();
@ -114,7 +114,11 @@ void trim_accel()
imu.ax(imu.ax() - 1); imu.ax(imu.ax() - 1);
} }
Serial.printf_P(PSTR("r:%ld p:%ld ax:%f, ay:%f, az:%f\n"), dcm.roll_sensor, dcm.pitch_sensor, /*Serial.printf_P(PSTR("r:%ld p:%ld ax:%f, ay:%f, az:%f\n"),
(double)imu.ax(), (double)imu.ay(), (double)imu.az()); dcm.roll_sensor,
dcm.pitch_sensor,
(double)imu.ax(),
(double)imu.ay(),
(double)imu.az());*/
} }

2
ArduCopterMega/defines.h
View File

@ -255,4 +255,4 @@
#define EEPROM_MAX_ADDR 4096 #define EEPROM_MAX_ADDR 4096
// parameters get the first 1KiB of EEPROM, remainder is for waypoints // parameters get the first 1KiB of EEPROM, remainder is for waypoints
#define WP_START_BYTE 0x400 // where in memory home WP is stored + all other WP #define WP_START_BYTE 0x400 // where in memory home WP is stored + all other WP
#define WP_SIZE 14 #define WP_SIZE 15

1
ArduCopterMega/navigation.pde
View File

@ -208,6 +208,7 @@ long get_altitude_above_home(void)
return current_loc.alt - home.alt; return current_loc.alt - home.alt;
} }
// distance is returned in meters
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)

2
ArduCopterMega/sensors.pde
View File

@ -20,7 +20,7 @@ void init_barometer(void)
ground_pressure = barometer.Press; ground_pressure = barometer.Press;
ground_temperature = barometer.Temp; ground_temperature = barometer.Temp;
delay(20); delay(20);
Serial.printf("barometer.Press %ld\n", barometer.Press); //Serial.printf("barometer.Press %ld\n", barometer.Press);
} }
for(int i = 0; i < 30; i++){ // We take some readings... for(int i = 0; i < 30; i++){ // We take some readings...

138
ArduCopterMega/setup.pde
View File

@ -12,7 +12,6 @@ static int8_t setup_frame (uint8_t argc, const Menu::arg *argv);
static int8_t setup_current (uint8_t argc, const Menu::arg *argv); static int8_t setup_current (uint8_t argc, const Menu::arg *argv);
static int8_t setup_sonar (uint8_t argc, const Menu::arg *argv); static int8_t setup_sonar (uint8_t argc, const Menu::arg *argv);
static int8_t setup_compass (uint8_t argc, const Menu::arg *argv); static int8_t setup_compass (uint8_t argc, const Menu::arg *argv);
//static int8_t setup_mag_offset (uint8_t argc, const Menu::arg *argv);
static int8_t setup_declination (uint8_t argc, const Menu::arg *argv); static int8_t setup_declination (uint8_t argc, const Menu::arg *argv);
static int8_t setup_show (uint8_t argc, const Menu::arg *argv); static int8_t setup_show (uint8_t argc, const Menu::arg *argv);
@ -31,7 +30,6 @@ const struct Menu::command setup_menu_commands[] PROGMEM = {
{"current", setup_current}, {"current", setup_current},
{"sonar", setup_sonar}, {"sonar", setup_sonar},
{"compass", setup_compass}, {"compass", setup_compass},
// {"mag_offset", setup_mag_offset},
{"declination", setup_declination}, {"declination", setup_declination},
{"show", setup_show} {"show", setup_show}
}; };
@ -44,12 +42,12 @@ int8_t
setup_mode(uint8_t argc, const Menu::arg *argv) setup_mode(uint8_t argc, const Menu::arg *argv)
{ {
// Give the user some guidance // Give the user some guidance
Serial.printf_P(PSTR("Setup Mode\n" Serial.printf_P(PSTR("Setup Mode\n"));
"\n" //"\n"
"IMPORTANT: if you have not previously set this system up, use the\n" //"IMPORTANT: if you have not previously set this system up, use the\n"
"'reset' command to initialize the EEPROM to sensible default values\n" //"'reset' command to initialize the EEPROM to sensible default values\n"
"and then the 'radio' command to configure for your radio.\n" //"and then the 'radio' command to configure for your radio.\n"
"\n")); //"\n"));
// Run the setup menu. When the menu exits, we will return to the main menu. // Run the setup menu. When the menu exits, we will return to the main menu.
setup_menu.run(); setup_menu.run();
@ -88,7 +86,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
uint8_t i; uint8_t i;
int c; int c;
Serial.printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort:\n")); Serial.printf_P(PSTR("\n'Y' + Enter to factory reset, any other key to abort:\n"));
do { do {
c = Serial.read(); c = Serial.read();
@ -97,7 +95,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
if (('y' != c) && ('Y' != c)) if (('y' != c) && ('Y' != c))
return(-1); return(-1);
AP_Var::erase_all(); AP_Var::erase_all();
Serial.printf_P(PSTR("\nFACTORY RESET complete - please reset APM to continue")); Serial.printf_P(PSTR("\nFACTORY RESET complete - reboot APM"));
delay(1000); delay(1000);
default_log_bitmask(); default_log_bitmask();
@ -124,7 +122,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
if(g.rc_1.radio_in < 500){ if(g.rc_1.radio_in < 500){
while(1){ while(1){
Serial.printf_P(PSTR("\nNo radio; Check connectors.")); //Serial.printf_P(PSTR("\nNo radio; Check connectors."));
delay(1000); delay(1000);
// stop here // stop here
} }
@ -303,7 +301,7 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
static int8_t static int8_t
setup_accel(uint8_t argc, const Menu::arg *argv) setup_accel(uint8_t argc, const Menu::arg *argv)
{ {
Serial.printf_P(PSTR("\nHold ArduCopter completely still and level.\n")); //Serial.printf_P(PSTR("\nHold ArduCopter completely still and level.\n"));
imu.init_accel(); imu.init_accel();
print_accel_offsets(); print_accel_offsets();
@ -318,35 +316,52 @@ setup_pid(uint8_t argc, const Menu::arg *argv)
if (!strcmp_P(argv[1].str, PSTR("default"))) { if (!strcmp_P(argv[1].str, PSTR("default"))) {
default_gains(); default_gains();
}else if (!strcmp_P(argv[1].str, PSTR("s_kp"))) { }else if (!strcmp_P(argv[1].str, PSTR("stabilize"))) {
g.pid_stabilize_roll.kP(argv[2].f); g.pid_stabilize_roll.kP(argv[2].f);
g.pid_stabilize_pitch.kP(argv[2].f); g.pid_stabilize_pitch.kP(argv[2].f);
save_EEPROM_PID(); g.stabilize_dampener.set_and_save(argv[3].f);
}else if (!strcmp_P(argv[1].str, PSTR("s_kd"))) { g.pid_stabilize_roll.save_gains();
g.stabilize_dampener = argv[2].f; g.pid_stabilize_pitch.save_gains();
save_EEPROM_PID();
}else if (!strcmp_P(argv[1].str, PSTR("y_kp"))) { }else if (!strcmp_P(argv[1].str, PSTR("yaw"))) {
g.pid_yaw.kP(argv[2].f); g.pid_yaw.kP(argv[2].f);
save_EEPROM_PID();
}else if (!strcmp_P(argv[1].str, PSTR("y_kd"))) { g.pid_yaw.save_gains();
g.pid_yaw.kD(argv[2].f); g.hold_yaw_dampener.set_and_save(argv[3].f);
save_EEPROM_PID();
}else if (!strcmp_P(argv[1].str, PSTR("t_kp"))) { }else if (!strcmp_P(argv[1].str, PSTR("nav"))) {
g.pid_nav_lat.kP(argv[2].f);
g.pid_nav_lat.kI(argv[3].f);
g.pid_nav_lat.imax(argv[4].i);
g.pid_nav_lon.kP(argv[2].f);
g.pid_nav_lon.kI(argv[3].f);
g.pid_nav_lon.imax(argv[4].i);
g.pid_nav_lon.save_gains();
g.pid_nav_lat.save_gains();
}else if (!strcmp_P(argv[1].str, PSTR("baro"))) {
g.pid_baro_throttle.kP(argv[2].f); g.pid_baro_throttle.kP(argv[2].f);
save_EEPROM_PID(); g.pid_baro_throttle.kI(argv[3].f);
g.pid_baro_throttle.kD(0);
g.pid_baro_throttle.imax(argv[4].i);
g.pid_baro_throttle.save_gains();
}else if (!strcmp_P(argv[1].str, PSTR("sonar"))) {
g.pid_sonar_throttle.kP(argv[2].f);
g.pid_sonar_throttle.kI(argv[3].f);
g.pid_sonar_throttle.kD(argv[4].f);
g.pid_sonar_throttle.imax(argv[5].i);
g.pid_sonar_throttle.save_gains();
}else if (!strcmp_P(argv[1].str, PSTR("t_kd"))) {
g.pid_baro_throttle.kD(argv[2].f);
save_EEPROM_PID();
}else{ }else{
default_gains(); default_gains();
} }
report_gains(); report_gains();
} }
@ -379,7 +394,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
} }
// look for stick input // look for stick input
if (radio_input_switch() == true){ if (radio_input_switch() == true){
mode++; mode++;
if(mode >= NUM_MODES) if(mode >= NUM_MODES)
mode = 0; mode = 0;
@ -408,6 +423,7 @@ setup_declination(uint8_t argc, const Menu::arg *argv)
report_compass(); report_compass();
} }
static int8_t static int8_t
setup_erase(uint8_t argc, const Menu::arg *argv) setup_erase(uint8_t argc, const Menu::arg *argv)
{ {
@ -747,11 +763,34 @@ default_gains()
/***************************************************************************/ /***************************************************************************/
// CLI reports // CLI reports
/***************************************************************************/ /***************************************************************************/
void report_wp(byte index = 255)
{
if(index == 255){
for(byte i = 0; i <= g.waypoint_total; i++){
struct Location temp = get_wp_with_index(i);
print_wp(&temp, i);
}
}else{
struct Location temp = get_wp_with_index(index);
print_wp(&temp, index);
}
}
void print_wp(struct Location *cmd, byte index)
{
Serial.printf_P(PSTR("command #: %d id:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
(int)index,
(int)cmd->id,
(int)cmd->p1,
cmd->alt,
cmd->lat,
cmd->lng);
}
void report_current() void report_current()
{ {
read_EEPROM_current(); read_EEPROM_current();
Serial.printf_P(PSTR("Current Sensor\n")); Serial.printf_P(PSTR("Current \n"));
print_divider(); print_divider();
print_enabled(g.current_enabled.get()); print_enabled(g.current_enabled.get());
@ -762,7 +801,7 @@ void report_current()
void report_sonar() void report_sonar()
{ {
g.sonar_enabled.load(); g.sonar_enabled.load();
Serial.printf_P(PSTR("Sonar Sensor\n")); Serial.printf_P(PSTR("Sonar\n"));
print_divider(); print_divider();
print_enabled(g.sonar_enabled.get()); print_enabled(g.sonar_enabled.get());
print_blanks(2); print_blanks(2);
@ -822,8 +861,8 @@ void report_gains()
Serial.printf_P(PSTR("yaw:\n")); Serial.printf_P(PSTR("yaw:\n"));
print_PID(&g.pid_yaw); print_PID(&g.pid_yaw);
Serial.printf_P(PSTR("Stabilize dampener: %4.3f\n"), (float)g.stabilize_dampener); Serial.printf_P(PSTR("Stab D: %4.3f\n"), (float)g.stabilize_dampener);
Serial.printf_P(PSTR("Yaw Dampener: %4.3f\n\n"), (float)g.hold_yaw_dampener); Serial.printf_P(PSTR("Yaw D: %4.3f\n\n"), (float)g.hold_yaw_dampener);
// Nav // Nav
Serial.printf_P(PSTR("Nav:\nlat:\n")); Serial.printf_P(PSTR("Nav:\nlat:\n"));
@ -839,7 +878,7 @@ void report_gains()
void report_xtrack() void report_xtrack()
{ {
Serial.printf_P(PSTR("Crosstrack\n")); Serial.printf_P(PSTR("XTrack\n"));
print_divider(); print_divider();
// radio // radio
read_EEPROM_nav(); read_EEPROM_nav();
@ -889,7 +928,7 @@ void report_compass()
print_enabled(g.compass_enabled); print_enabled(g.compass_enabled);
// mag declination // mag declination
Serial.printf_P(PSTR("Mag Delination: %4.4f\n"), Serial.printf_P(PSTR("Mag Dec: %4.4f\n"),
degrees(compass.get_declination())); degrees(compass.get_declination()));
Vector3f offsets = compass.get_offsets(); Vector3f offsets = compass.get_offsets();
@ -920,12 +959,35 @@ void report_flight_modes()
void void
print_PID(PID * pid) print_PID(PID * pid)
{ {
Serial.printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%ld\n"), pid->kP(), pid->kI(), pid->kD(), (long)pid->imax()); Serial.printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%ld\n"),
pid->kP(),
pid->kI(),
pid->kD(),
(long)pid->imax());
} }
void void
print_radio_values() print_radio_values()
{ {
/*Serial.printf_P(PSTR( "CH1: %d | %d\n"
"CH2: %d | %d\n"
"CH3: %d | %d\n"
"CH4: %d | %d\n"
"CH5: %d | %d\n"
"CH6: %d | %d\n"
"CH7: %d | %d\n"
"CH8: %d | %d\n"),
g.rc_1.radio_min, g.rc_1.radio_max,
g.rc_2.radio_min, g.rc_2.radio_max,
g.rc_3.radio_min, g.rc_3.radio_max,
g.rc_4.radio_min, g.rc_4.radio_max,
g.rc_5.radio_min, g.rc_5.radio_max,
g.rc_6.radio_min, g.rc_6.radio_max,
g.rc_7.radio_min, g.rc_7.radio_max,
g.rc_8.radio_min, g.rc_8.radio_max);*/
///*
Serial.printf_P(PSTR("CH1: %d | %d\n"), (int)g.rc_1.radio_min, (int)g.rc_1.radio_max); Serial.printf_P(PSTR("CH1: %d | %d\n"), (int)g.rc_1.radio_min, (int)g.rc_1.radio_max);
Serial.printf_P(PSTR("CH2: %d | %d\n"), (int)g.rc_2.radio_min, (int)g.rc_2.radio_max); Serial.printf_P(PSTR("CH2: %d | %d\n"), (int)g.rc_2.radio_min, (int)g.rc_2.radio_max);
Serial.printf_P(PSTR("CH3: %d | %d\n"), (int)g.rc_3.radio_min, (int)g.rc_3.radio_max); Serial.printf_P(PSTR("CH3: %d | %d\n"), (int)g.rc_3.radio_min, (int)g.rc_3.radio_max);
@ -934,6 +996,7 @@ print_radio_values()
Serial.printf_P(PSTR("CH6: %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_max); Serial.printf_P(PSTR("CH6: %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_max);
Serial.printf_P(PSTR("CH7: %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_max); Serial.printf_P(PSTR("CH7: %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_max);
Serial.printf_P(PSTR("CH8: %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_max); Serial.printf_P(PSTR("CH8: %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_max);
//*/
} }
void void
@ -967,7 +1030,8 @@ print_divider(void)
Serial.println(""); Serial.println("");
} }
int8_t // read at 50Hz
bool
radio_input_switch(void) radio_input_switch(void)
{ {
static int8_t bouncer = 0; static int8_t bouncer = 0;

80
ArduCopterMega/test.pde
View File

@ -6,7 +6,7 @@ static int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv);
static int8_t test_radio(uint8_t argc, const Menu::arg *argv); static int8_t test_radio(uint8_t argc, const Menu::arg *argv);
static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv); static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv);
static int8_t test_stabilize(uint8_t argc, const Menu::arg *argv); static int8_t test_stabilize(uint8_t argc, const Menu::arg *argv);
static int8_t test_fbw(uint8_t argc, const Menu::arg *argv); //static int8_t test_fbw(uint8_t argc, const Menu::arg *argv);
static int8_t test_gps(uint8_t argc, const Menu::arg *argv); static int8_t test_gps(uint8_t argc, const Menu::arg *argv);
static int8_t test_adc(uint8_t argc, const Menu::arg *argv); static int8_t test_adc(uint8_t argc, const Menu::arg *argv);
static int8_t test_imu(uint8_t argc, const Menu::arg *argv); static int8_t test_imu(uint8_t argc, const Menu::arg *argv);
@ -22,6 +22,7 @@ static int8_t test_sonar(uint8_t argc, const Menu::arg *argv);
static int8_t test_xbee(uint8_t argc, const Menu::arg *argv); static int8_t test_xbee(uint8_t argc, const Menu::arg *argv);
static int8_t test_eedump(uint8_t argc, const Menu::arg *argv); static int8_t test_eedump(uint8_t argc, const Menu::arg *argv);
static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv); static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv);
static int8_t test_mission(uint8_t argc, const Menu::arg *argv);
// This is the help function // This is the help function
// PSTR is an AVR macro to read strings from flash memory // PSTR is an AVR macro to read strings from flash memory
@ -47,7 +48,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
{"radio", test_radio}, {"radio", test_radio},
{"failsafe", test_failsafe}, {"failsafe", test_failsafe},
{"stabilize", test_stabilize}, {"stabilize", test_stabilize},
{"fbw", test_fbw}, // {"fbw", test_fbw},
{"gps", test_gps}, {"gps", test_gps},
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
{"adc", test_adc}, {"adc", test_adc},
@ -67,6 +68,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
{"xbee", test_xbee}, {"xbee", test_xbee},
{"eedump", test_eedump}, {"eedump", test_eedump},
{"rawgps", test_rawgps}, {"rawgps", test_rawgps},
{"mission", test_mission},
}; };
// A Macro to create the Menu // A Macro to create the Menu
@ -315,7 +317,7 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
} }
} }
} }
/*
static int8_t static int8_t
test_fbw(uint8_t argc, const Menu::arg *argv) test_fbw(uint8_t argc, const Menu::arg *argv)
{ {
@ -408,6 +410,7 @@ test_fbw(uint8_t argc, const Menu::arg *argv)
} }
} }
} }
*/
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
static int8_t static int8_t
@ -737,36 +740,22 @@ test_wp(uint8_t argc, const Menu::arg *argv)
// save the alitude above home option // save the alitude above home option
Serial.printf_P(PSTR("Hold altitude "));
if(g.RTL_altitude < 0){ if(g.RTL_altitude < 0){
Serial.printf_P(PSTR("Hold current altitude\n")); Serial.printf_P(PSTR("\n"));
}else{ }else{
Serial.printf_P(PSTR("Hold altitude of %dm\n"), (int)g.RTL_altitude / 100); Serial.printf_P(PSTR("of %dm\n"), (int)g.RTL_altitude / 100);
} }
Serial.printf_P(PSTR("%d waypoints\n"), (int)g.waypoint_total); Serial.printf_P(PSTR("%d waypoints\n"), (int)g.waypoint_total);
Serial.printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius); Serial.printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius);
Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius); //Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius);
for(byte i = 0; i <= g.waypoint_total; i++){ report_wp();
struct Location temp = get_wp_with_index(i);
test_wp_print(&temp, i);
}
return (0); return (0);
} }
void
test_wp_print(struct Location *cmd, byte index)
{
Serial.printf_P(PSTR("command #: %d id:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
(int)index,
(int)cmd->id,
(int)cmd->p1,
cmd->alt,
cmd->lat,
cmd->lng);
}
static int8_t static int8_t
test_rawgps(uint8_t argc, const Menu::arg *argv) test_rawgps(uint8_t argc, const Menu::arg *argv)
{ {
@ -925,6 +914,53 @@ 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);
//write out a basic mission to the EEPROM
Location t;
/*{
uint8_t id; ///< command id
uint8_t options; ///< options bitmask (1<<0 = relative altitude)
uint8_t p1; ///< param 1
int32_t alt; ///< param 2 - Altitude in centimeters (meters * 100)
int32_t lat; ///< param 3 - Lattitude * 10**7
int32_t lng; ///< param 4 - Longitude * 10**7
}*/
{
Location t = {0, 0, 0, 0, 0, 0};
set_wp_with_index(t,0);
}
{ // CMD opt pitch alt/cm
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};
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};
set_wp_with_index(t,3);
}
{ // CMD opt
Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0};
set_wp_with_index(t,4);
}
g.RTL_altitude.set_and_save(300);
g.waypoint_total.set_and_save(4);
g.waypoint_radius.set_and_save(3);
test_wp(NULL, NULL);
}
void print_hit_enter() void print_hit_enter()
{ {
Serial.printf_P(PSTR("Hit Enter to exit.\n\n")); Serial.printf_P(PSTR("Hit Enter to exit.\n\n"));