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

Rover: make it possible to run the CLI over a radio link

This commit is contained in:
Andrew Tridgell 2012-11-21 17:25:11 +11:00
parent 76e9adb8d4
commit 4e415424a2
12 changed files with 278 additions and 238 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
APMrover2/APMrover2.pde
View File

@ -134,6 +134,8 @@ FastSerialPort1(Serial1); // GPS port
FastSerialPort3(Serial3); // Telemetry port for APM1 FastSerialPort3(Serial3); // Telemetry port for APM1
#endif #endif
static FastSerial *cliSerial = &Serial3;
// this sets up the parameter table, and sets the default values. This // this sets up the parameter table, and sets the default values. This
// must be the first AP_Param variable declared to ensure its // must be the first AP_Param variable declared to ensure its
// constructor runs before the constructors of the other AP_Param // constructor runs before the constructors of the other AP_Param
@ -812,13 +814,13 @@ static void medium_loop()
#endif #endif
//#endif //#endif
/*{ /*{
Serial.print(ahrs.roll_sensor, DEC); Serial.printf_P(PSTR("\t")); cliSerial->print(ahrs.roll_sensor, DEC); cliSerial->printf_P(PSTR("\t"));
Serial.print(ahrs.pitch_sensor, DEC); Serial.printf_P(PSTR("\t")); cliSerial->print(ahrs.pitch_sensor, DEC); cliSerial->printf_P(PSTR("\t"));
Serial.print(ahrs.yaw_sensor, DEC); Serial.printf_P(PSTR("\t")); cliSerial->print(ahrs.yaw_sensor, DEC); cliSerial->printf_P(PSTR("\t"));
Vector3f tempaccel = ins.get_accel(); Vector3f tempaccel = ins.get_accel();
Serial.print(tempaccel.x, DEC); Serial.printf_P(PSTR("\t")); cliSerial->print(tempaccel.x, DEC); cliSerial->printf_P(PSTR("\t"));
Serial.print(tempaccel.y, DEC); Serial.printf_P(PSTR("\t")); cliSerial->print(tempaccel.y, DEC); cliSerial->printf_P(PSTR("\t"));
Serial.println(tempaccel.z, DEC); cliSerial->println(tempaccel.z, DEC);
}*/ }*/
break; break;
@ -939,11 +941,11 @@ static void slow_loop()
#endif #endif
#if TRACE == ENABLED #if TRACE == ENABLED
// Serial.printf_P(PSTR("NAV->gnd_crs=%3.0f, nav_brg=%3.0f, tgt_brg=%3.0f, brg_err=%3.0f, nav_rll=%3.1f rsvo=%3.1f\n"), // cliSerial->printf_P(PSTR("NAV->gnd_crs=%3.0f, nav_brg=%3.0f, tgt_brg=%3.0f, brg_err=%3.0f, nav_rll=%3.1f rsvo=%3.1f\n"),
// ahrs.yaw_sensor*0.01, (float)nav_bearing/100, (float)target_bearing/100, (float)bearing_error/100, (float)nav_roll/100, (float)g.channel_roll.servo_out/100); // ahrs.yaw_sensor*0.01, (float)nav_bearing/100, (float)target_bearing/100, (float)bearing_error/100, (float)nav_roll/100, (float)g.channel_roll.servo_out/100);
// Serial.printf_P(PSTR("WPL->g.command_total=%d, g.command_index=%d, nav_command_index=%d\n"), // cliSerial->printf_P(PSTR("WPL->g.command_total=%d, g.command_index=%d, nav_command_index=%d\n"),
// g.command_total, g.command_index, nav_command_index); // g.command_total, g.command_index, nav_command_index);
Serial.printf_P(PSTR("NAV->gnd_crs=%3.0f, sonar_dist = %d obstacle = %d\n"), ahrs.yaw_sensor*0.01, (int)sonar_dist, obstacle); cliSerial->printf_P(PSTR("NAV->gnd_crs=%3.0f, sonar_dist = %d obstacle = %d\n"), ahrs.yaw_sensor*0.01, (int)sonar_dist, obstacle);
#endif #endif
break; break;
} }

6
APMrover2/GCS_Mavlink.pde
View File

@ -756,7 +756,7 @@ GCS_MAVLINK::update(void)
crlf_count = 0; crlf_count = 0;
} }
if (crlf_count == 3) { if (crlf_count == 3) {
run_cli(); run_cli(_port);
} }
} }
#endif #endif
@ -1692,8 +1692,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// TODO: check scaling for temp/absPress // TODO: check scaling for temp/absPress
float temp = 70; float temp = 70;
float absPress = 1; float absPress = 1;
//Serial.printf_P(PSTR("accel: %d %d %d\n"), packet.xacc, packet.yacc, packet.zacc); //cliSerial->printf_P(PSTR("accel: %d %d %d\n"), packet.xacc, packet.yacc, packet.zacc);
//Serial.printf_P(PSTR("gyro: %d %d %d\n"), packet.xgyro, packet.ygyro, packet.zgyro); //cliSerial->printf_P(PSTR("gyro: %d %d %d\n"), packet.xgyro, packet.ygyro, packet.zgyro);
// rad/sec // rad/sec
Vector3f gyros; Vector3f gyros;

124
APMrover2/Log.pde
View File

@ -23,7 +23,7 @@ static int16_t cur_throttle =0;
// printf_P is a version of print_f that reads from flash memory // printf_P is a version of print_f that reads from flash memory
//static int8_t help_log(uint8_t argc, const Menu::arg *argv) //static int8_t help_log(uint8_t argc, const Menu::arg *argv)
/*{ /*{
Serial.printf_P(PSTR("\n" cliSerial->printf_P(PSTR("\n"
"Commands:\n" "Commands:\n"
" dump <n>" " dump <n>"
" erase (all logs)\n" " erase (all logs)\n"
@ -57,16 +57,16 @@ print_log_menu(void)
uint16_t num_logs = DataFlash.get_num_logs(); uint16_t num_logs = DataFlash.get_num_logs();
Serial.printf_P(PSTR("logs enabled: ")); cliSerial->printf_P(PSTR("logs enabled: "));
if (0 == g.log_bitmask) { if (0 == g.log_bitmask) {
Serial.printf_P(PSTR("none")); cliSerial->printf_P(PSTR("none"));
}else{ }else{
// Macro to make the following code a bit easier on the eye. // Macro to make the following code a bit easier on the eye.
// Pass it the capitalised name of the log option, as defined // Pass it the capitalised name of the log option, as defined
// in defines.h but without the LOG_ prefix. It will check for // in defines.h but without the LOG_ prefix. It will check for
// the bit being set and print the name of the log option to suit. // the bit being set and print the name of the log option to suit.
#define PLOG(_s) if (g.log_bitmask & MASK_LOG_ ## _s) Serial.printf_P(PSTR(" %S"), PSTR(#_s)) #define PLOG(_s) if (g.log_bitmask & MASK_LOG_ ## _s) cliSerial->printf_P(PSTR(" %S"), PSTR(#_s))
PLOG(ATTITUDE_FAST); PLOG(ATTITUDE_FAST);
PLOG(ATTITUDE_MED); PLOG(ATTITUDE_MED);
PLOG(GPS); PLOG(GPS);
@ -80,24 +80,24 @@ print_log_menu(void)
#undef PLOG #undef PLOG
} }
Serial.println(); cliSerial->println();
if (num_logs == 0) { if (num_logs == 0) {
Serial.printf_P(PSTR("\nNo logs\n\n")); cliSerial->printf_P(PSTR("\nNo logs\n\n"));
}else{ }else{
Serial.printf_P(PSTR("\n%d logs\n"), num_logs); cliSerial->printf_P(PSTR("\n%d logs\n"), num_logs);
for(int i=num_logs;i>=1;i--) { for(int i=num_logs;i>=1;i--) {
int last_log_start = log_start, last_log_end = log_end; int last_log_start = log_start, last_log_end = log_end;
temp = last_log_num-i+1; temp = last_log_num-i+1;
DataFlash.get_log_boundaries(temp, log_start, log_end); DataFlash.get_log_boundaries(temp, log_start, log_end);
Serial.printf_P(PSTR("Log %d, start %d, end %d\n"), temp, log_start, log_end); cliSerial->printf_P(PSTR("Log %d, start %d, end %d\n"), temp, log_start, log_end);
if (last_log_start == log_start && last_log_end == log_end) { if (last_log_start == log_start && last_log_end == log_end) {
// we are printing bogus logs // we are printing bogus logs
break; break;
} }
} }
Serial.println(); cliSerial->println();
} }
return(true); return(true);
} }
@ -117,28 +117,28 @@ dump_log(uint8_t argc, const Menu::arg *argv)
if (dump_log == -2) { if (dump_log == -2) {
for(uint16_t count=1; count<=DataFlash.df_NumPages; count++) { for(uint16_t count=1; count<=DataFlash.df_NumPages; count++) {
DataFlash.StartRead(count); DataFlash.StartRead(count);
Serial.printf_P(PSTR("DF page, log file #, log page: %d,\t"), count); cliSerial->printf_P(PSTR("DF page, log file #, log page: %d,\t"), count);
Serial.printf_P(PSTR("%d,\t"), DataFlash.GetFileNumber()); cliSerial->printf_P(PSTR("%d,\t"), DataFlash.GetFileNumber());
Serial.printf_P(PSTR("%d\n"), DataFlash.GetFilePage()); cliSerial->printf_P(PSTR("%d\n"), DataFlash.GetFilePage());
} }
return(-1); return(-1);
} else if (dump_log <= 0) { } else if (dump_log <= 0) {
Serial.printf_P(PSTR("dumping all\n")); cliSerial->printf_P(PSTR("dumping all\n"));
Log_Read(1, DataFlash.df_NumPages); Log_Read(1, DataFlash.df_NumPages);
return(-1); return(-1);
} else if ((argc != 2) || (dump_log <= (last_log_num - DataFlash.get_num_logs())) || (dump_log > last_log_num)) { } else if ((argc != 2) || (dump_log <= (last_log_num - DataFlash.get_num_logs())) || (dump_log > last_log_num)) {
Serial.printf_P(PSTR("bad log number\n")); cliSerial->printf_P(PSTR("bad log number\n"));
return(-1); return(-1);
} }
DataFlash.get_log_boundaries(dump_log, dump_log_start, dump_log_end); DataFlash.get_log_boundaries(dump_log, dump_log_start, dump_log_end);
Serial.printf_P(PSTR("Dumping Log %d, start pg %d, end pg %d\n"), cliSerial->printf_P(PSTR("Dumping Log %d, start pg %d, end pg %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("Done\n")); cliSerial->printf_P(PSTR("Done\n"));
return 0; return 0;
} }
@ -146,15 +146,15 @@ dump_log(uint8_t argc, const Menu::arg *argv)
void erase_callback(unsigned long t) { void erase_callback(unsigned long t) {
mavlink_delay(t); mavlink_delay(t);
if (DataFlash.GetWritePage() % 128 == 0) { if (DataFlash.GetWritePage() % 128 == 0) {
Serial.printf_P(PSTR("+")); cliSerial->printf_P(PSTR("+"));
} }
} }
static void do_erase_logs(void) static void do_erase_logs(void)
{ {
Serial.printf_P(PSTR("\nErasing log...\n")); cliSerial->printf_P(PSTR("\nErasing log...\n"));
DataFlash.EraseAll(erase_callback); DataFlash.EraseAll(erase_callback);
Serial.printf_P(PSTR("\nLog erased.\n")); cliSerial->printf_P(PSTR("\nLog erased.\n"));
} }
static int8_t static int8_t
@ -172,7 +172,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
uint16_t bits; uint16_t bits;
if (argc != 2) { if (argc != 2) {
Serial.printf_P(PSTR("missing log type\n")); cliSerial->printf_P(PSTR("missing log type\n"));
return(-1); return(-1);
} }
@ -400,7 +400,7 @@ static void Log_Write_Current()
// Read a Current packet // Read a Current packet
static void Log_Read_Current() static void Log_Read_Current()
{ {
Serial.printf_P(PSTR("CURR: %d, %4.4f, %4.4f, %d\n"), cliSerial->printf_P(PSTR("CURR: %d, %4.4f, %4.4f, %d\n"),
DataFlash.ReadInt(), DataFlash.ReadInt(),
((float)DataFlash.ReadInt() / 100.f), ((float)DataFlash.ReadInt() / 100.f),
((float)DataFlash.ReadInt() / 100.f), ((float)DataFlash.ReadInt() / 100.f),
@ -412,17 +412,17 @@ static void Log_Read_Control_Tuning()
{ {
float logvar; float logvar;
Serial.printf_P(PSTR("CTUN: ")); cliSerial->printf_P(PSTR("CTUN: "));
for (int y = 1; y < 10; y++) { for (int y = 1; y < 10; y++) {
logvar = DataFlash.ReadInt(); logvar = DataFlash.ReadInt();
if(y == 7) cur_throttle = logvar; if(y == 7) cur_throttle = logvar;
if(y < 8) logvar = logvar/100.f; if(y < 8) logvar = logvar/100.f;
if(y == 9) logvar = logvar/10000.f; if(y == 9) logvar = logvar/10000.f;
Serial.print(logvar); cliSerial->print(logvar);
Serial.print(comma); cliSerial->print(comma);
Serial.print(" "); cliSerial->print(" ");
} }
Serial.println(""); cliSerial->println("");
} }
// Read a nav tuning packet // Read a nav tuning packet
@ -432,7 +432,7 @@ static void Log_Read_Nav_Tuning()
for (int8_t i=0; i<7; i++) { for (int8_t i=0; i<7; i++) {
d[i] = DataFlash.ReadInt(); d[i] = DataFlash.ReadInt();
} }
Serial.printf_P(PSTR("NTUN: %4.4f, %d, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f,\n"), // \n cliSerial->printf_P(PSTR("NTUN: %4.4f, %d, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f,\n"), // \n
d[0]/100.0, d[0]/100.0,
d[1], d[1],
((uint16_t)d[2])/100.0, ((uint16_t)d[2])/100.0,
@ -448,21 +448,21 @@ static void Log_Read_Performance()
int32_t pm_time; int32_t pm_time;
int logvar; int logvar;
Serial.printf_P(PSTR("PM: ")); cliSerial->printf_P(PSTR("PM: "));
pm_time = DataFlash.ReadLong(); pm_time = DataFlash.ReadLong();
Serial.print(pm_time); cliSerial->print(pm_time);
Serial.print(comma); cliSerial->print(comma);
for (int y = 1; y <= 12; y++) { for (int y = 1; y <= 12; y++) {
if(y < 3 || y > 7){ if(y < 3 || y > 7){
logvar = DataFlash.ReadInt(); logvar = DataFlash.ReadInt();
}else{ }else{
logvar = DataFlash.ReadByte(); logvar = DataFlash.ReadByte();
} }
Serial.print(logvar); cliSerial->print(logvar);
Serial.print(comma); cliSerial->print(comma);
Serial.print(" "); cliSerial->print(" ");
} }
Serial.println(""); cliSerial->println("");
} }
// Read a command processing packet // Read a command processing packet
@ -471,20 +471,20 @@ static void Log_Read_Cmd()
byte logvarb; byte logvarb;
int32_t logvarl; int32_t logvarl;
Serial.printf_P(PSTR("CMD: ")); cliSerial->printf_P(PSTR("CMD: "));
for(int i = 1; i < 4; i++) { for(int i = 1; i < 4; i++) {
logvarb = DataFlash.ReadByte(); logvarb = DataFlash.ReadByte();
Serial.print(logvarb, DEC); cliSerial->print(logvarb, DEC);
Serial.print(comma); cliSerial->print(comma);
Serial.print(" "); cliSerial->print(" ");
} }
for(int i = 1; i < 4; i++) { for(int i = 1; i < 4; i++) {
logvarl = DataFlash.ReadLong(); logvarl = DataFlash.ReadLong();
Serial.print(logvarl, DEC); cliSerial->print(logvarl, DEC);
Serial.print(comma); cliSerial->print(comma);
Serial.print(" "); cliSerial->print(" ");
} }
Serial.println(""); cliSerial->println("");
} }
static void Log_Read_Startup() static void Log_Read_Startup()
@ -492,13 +492,13 @@ static void Log_Read_Startup()
byte logbyte = DataFlash.ReadByte(); byte logbyte = DataFlash.ReadByte();
if (logbyte == TYPE_AIRSTART_MSG) if (logbyte == TYPE_AIRSTART_MSG)
Serial.printf_P(PSTR("AIR START - ")); cliSerial->printf_P(PSTR("AIR START - "));
else if (logbyte == TYPE_GROUNDSTART_MSG) else if (logbyte == TYPE_GROUNDSTART_MSG)
Serial.printf_P(PSTR("GROUND START - ")); cliSerial->printf_P(PSTR("GROUND START - "));
else else
Serial.printf_P(PSTR("UNKNOWN STARTUP - ")); cliSerial->printf_P(PSTR("UNKNOWN STARTUP - "));
Serial.printf_P(PSTR(" %d commands in memory\n"),(int)DataFlash.ReadByte()); cliSerial->printf_P(PSTR(" %d commands in memory\n"),(int)DataFlash.ReadByte());
} }
// Read an attitude packet // Read an attitude packet
@ -508,7 +508,7 @@ static void Log_Read_Attitude()
d[0] = DataFlash.ReadInt(); d[0] = DataFlash.ReadInt();
d[1] = DataFlash.ReadInt(); d[1] = DataFlash.ReadInt();
d[2] = DataFlash.ReadInt(); d[2] = DataFlash.ReadInt();
Serial.printf_P(PSTR("ATT: %d, %d, %u\n"), cliSerial->printf_P(PSTR("ATT: %d, %d, %u\n"),
d[0], d[1], d[0], d[1],
(uint16_t)d[2]); (uint16_t)d[2]);
} }
@ -516,8 +516,8 @@ static void Log_Read_Attitude()
// Read a mode packet // Read a mode packet
static void Log_Read_Mode() static void Log_Read_Mode()
{ {
Serial.printf_P(PSTR("MOD: ")); cliSerial->printf_P(PSTR("MOD: "));
Serial.println(flight_mode_strings[DataFlash.ReadByte()]); cliSerial->println(flight_mode_strings[DataFlash.ReadByte()]);
} }
// Read a GPS packet // Read a GPS packet
@ -540,17 +540,17 @@ static void Log_Read_GPS()
k = DataFlash.ReadInt(); k = DataFlash.ReadInt();
m = DataFlash.ReadInt(); m = DataFlash.ReadInt();
/* /*
Serial.printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %d, %4.4f, %4.4f, %4.4f, %4.4f\n"), cliSerial->printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %d, %4.4f, %4.4f, %4.4f, %4.4f\n"),
(long)l[0], (int)b[0], (int)b[1], (long)l[0], (int)b[0], (int)b[1],
l[1]/t7, l[2]/t7, l[1]/t7, l[2]/t7,
(int)i, (int)i,
l[3]/100.0, l[4]/100.0, l[5]/100.0, l[6]/100.0); */ l[3]/100.0, l[4]/100.0, l[5]/100.0, l[6]/100.0); */
Serial.printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %4.4f, %4.4f, %4.4f, %4.4f\n"), cliSerial->printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %4.4f, %4.4f, %4.4f, %4.4f\n"),
(long)l[0], (int)b[0], (int)b[1], (long)l[0], (int)b[0], (int)b[1],
l[1]/t7, l[2]/t7, l[1]/t7, l[2]/t7,
l[4]/100.0, l[3]/100.0, l[5]/100.0, l[6]/100.0); l[4]/100.0, l[3]/100.0, l[5]/100.0, l[6]/100.0);
Serial.printf_P(PSTR("THP: %4.7f, %4.7f, %4.4f, %2.1f, %2.1f, %2.1f, %d\n"), cliSerial->printf_P(PSTR("THP: %4.7f, %4.7f, %4.4f, %2.1f, %2.1f, %2.1f, %d\n"),
l[1]/t7, l[2]/t7, l[3]/100.0, l[1]/t7, l[2]/t7, l[3]/100.0,
(float)j/100.0, (float)k/100.0, (float)m/100.0, cur_throttle); (float)j/100.0, (float)k/100.0, (float)m/100.0, cur_throttle);
} }
@ -559,14 +559,14 @@ static void Log_Read_GPS()
static void Log_Read_Raw() static void Log_Read_Raw()
{ {
float logvar; float logvar;
Serial.printf_P(PSTR("RAW: ")); cliSerial->printf_P(PSTR("RAW: "));
for (int y = 0; y < 6; y++) { for (int y = 0; y < 6; y++) {
logvar = (float)DataFlash.ReadLong() / t7; logvar = (float)DataFlash.ReadLong() / t7;
Serial.print(logvar); cliSerial->print(logvar);
Serial.print(comma); cliSerial->print(comma);
Serial.print(" "); cliSerial->print(" ");
} }
Serial.println(""); cliSerial->println("");
} }
// Read the DataFlash log memory : Packet Parser // Read the DataFlash log memory : Packet Parser
@ -575,9 +575,9 @@ static void Log_Read(int16_t start_page, int16_t end_page)
int packet_count = 0; int packet_count = 0;
#ifdef AIRFRAME_NAME #ifdef AIRFRAME_NAME
Serial.printf_P(PSTR((AIRFRAME_NAME) cliSerial->printf_P(PSTR((AIRFRAME_NAME)
#endif #endif
Serial.printf_P(PSTR("\n" THISFIRMWARE cliSerial->printf_P(PSTR("\n" THISFIRMWARE
"\nFree RAM: %u\n"), "\nFree RAM: %u\n"),
memcheck_available_memory()); memcheck_available_memory());
@ -589,7 +589,7 @@ static void Log_Read(int16_t start_page, int16_t end_page)
packet_count = Log_Read_Process(start_page, end_page); packet_count = Log_Read_Process(start_page, end_page);
} }
Serial.printf_P(PSTR("Number of packets read: %d\n"), packet_count); cliSerial->printf_P(PSTR("Number of packets read: %d\n"), packet_count);
} }
// Read the DataFlash log memory : Packet Parser // Read the DataFlash log memory : Packet Parser
@ -657,7 +657,7 @@ static int Log_Read_Process(int16_t start_page, int16_t end_page)
Log_Read_GPS(); Log_Read_GPS();
log_step++; log_step++;
}else{ }else{
Serial.printf_P(PSTR("Error Reading Packet: %d\n"),packet_count); cliSerial->printf_P(PSTR("Error Reading Packet: %d\n"),packet_count);
log_step = 0; // Restart, we have a problem... log_step = 0; // Restart, we have a problem...
} }
} }
@ -666,7 +666,7 @@ static int Log_Read_Process(int16_t start_page, int16_t end_page)
if(data == END_BYTE){ if(data == END_BYTE){
packet_count++; packet_count++;
}else{ }else{
Serial.printf_P(PSTR("Error Reading END_BYTE: %d\n"),data); cliSerial->printf_P(PSTR("Error Reading END_BYTE: %d\n"),data);
} }
log_step = 0; // Restart sequence: new packet... log_step = 0; // Restart sequence: new packet...
break; break;

6
APMrover2/Parameters.pde
View File

@ -150,17 +150,17 @@ static void load_parameters(void)
g.format_version != Parameters::k_format_version) { g.format_version != Parameters::k_format_version) {
// erase all parameters // erase all parameters
Serial.printf_P(PSTR("Firmware change: erasing EEPROM...\n")); cliSerial->printf_P(PSTR("Firmware change: erasing EEPROM...\n"));
AP_Param::erase_all(); AP_Param::erase_all();
// save the current format version // save the current 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.")); cliSerial->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_Param::load_all(); AP_Param::load_all();
Serial.printf_P(PSTR("load_all took %luus\n"), micros() - before); cliSerial->printf_P(PSTR("load_all took %luus\n"), micros() - before);
} }
} }

2
APMrover2/commands_logic.pde
View File

@ -248,7 +248,7 @@ static bool verify_takeoff()
static void calc_turn_radius(void) // JLN update - adjut automaticaly the wp_radius Vs the speed and the turn angle static void calc_turn_radius(void) // JLN update - adjut automaticaly the wp_radius Vs the speed and the turn angle
{ {
wp_radius = ground_speed * 150 / g.roll_limit.get(); wp_radius = ground_speed * 150 / g.roll_limit.get();
//Serial.println(wp_radius, DEC); //cliSerial->println(wp_radius, DEC);
} }

4
APMrover2/control_modes.pde
View File

@ -73,7 +73,7 @@ if (g.ch7_option == CH7_SAVE_WP){ // set to 1
if(g.channel_roll.control_in > 3000) // if roll is full right store the current location as home if(g.channel_roll.control_in > 3000) // if roll is full right store the current location as home
ground_start_count = 5; ground_start_count = 5;
#if X_PLANE == ENABLED #if X_PLANE == ENABLED
Serial.printf_P(PSTR("*** RESET the FPL\n")); cliSerial->printf_P(PSTR("*** RESET the FPL\n"));
#endif #endif
CH7_wp_index = 1; CH7_wp_index = 1;
return; return;
@ -94,7 +94,7 @@ if (g.ch7_option == CH7_SAVE_WP){ // set to 1
set_cmd_with_index(current_loc, CH7_wp_index); set_cmd_with_index(current_loc, CH7_wp_index);
#if X_PLANE == ENABLED #if X_PLANE == ENABLED
Serial.printf_P(PSTR("*** Store WP #%d\n"),CH7_wp_index); cliSerial->printf_P(PSTR("*** Store WP #%d\n"),CH7_wp_index);
#endif #endif
// increment index // increment index

2
APMrover2/navigation.pde
View File

@ -29,7 +29,7 @@ static void navigate()
if (wp_distance < 0){ if (wp_distance < 0){
gcs_send_text_P(SEVERITY_HIGH,PSTR("<navigate> WP error - distance < 0")); gcs_send_text_P(SEVERITY_HIGH,PSTR("<navigate> WP error - distance < 0"));
//Serial.println(wp_distance,DEC); //cliSerial->println(wp_distance,DEC);
return; return;
} }

2
APMrover2/planner.pde
View File

@ -18,7 +18,7 @@ MENU(planner_menu, "planner", planner_menu_commands);
static int8_t static int8_t
planner_mode(uint8_t argc, const Menu::arg *argv) planner_mode(uint8_t argc, const Menu::arg *argv)
{ {
Serial.printf_P(PSTR("Planner Mode\n\nThis mode is not intended for manual use\n\n")); cliSerial->printf_P(PSTR("Planner Mode\n\nThis mode is not intended for manual use\n\n"));
planner_menu.run(); planner_menu.run();
return 0; return 0;
} }

2
APMrover2/radio.pde
View File

@ -104,7 +104,7 @@ static void read_radio()
} }
/* /*
Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"), cliSerial->printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"),
g.rc_1.control_in, g.rc_1.control_in,
g.rc_2.control_in, g.rc_2.control_in,
g.rc_3.control_in, g.rc_3.control_in,

162
APMrover2/setup.pde
View File

@ -36,7 +36,7 @@ static 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" cliSerial->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"
@ -65,7 +65,7 @@ setup_show(uint8_t argc, const Menu::arg *argv)
report_ins(); report_ins();
report_compass(); report_compass();
Serial.printf_P(PSTR("Raw Values\n")); cliSerial->printf_P(PSTR("Raw Values\n"));
print_divider(); print_divider();
AP_Param::show_all(); AP_Param::show_all();
@ -80,16 +80,16 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
{ {
int c; int c;
Serial.printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort: ")); cliSerial->printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort: "));
do { do {
c = Serial.read(); c = cliSerial->read();
} while (-1 == c); } while (-1 == c);
if (('y' != c) && ('Y' != c)) if (('y' != c) && ('Y' != c))
return(-1); return(-1);
AP_Param::erase_all(); AP_Param::erase_all();
Serial.printf_P(PSTR("\nFACTORY RESET complete - please reset APM to continue")); cliSerial->printf_P(PSTR("\nFACTORY RESET complete - please reset APM to continue"));
//default_flight_modes(); // This will not work here. Replacement code located in init_ardupilot() //default_flight_modes(); // This will not work here. Replacement code located in init_ardupilot()
@ -105,7 +105,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
static int8_t static int8_t
setup_radio(uint8_t argc, const Menu::arg *argv) setup_radio(uint8_t argc, const Menu::arg *argv)
{ {
Serial.printf_P(PSTR("\n\nRadio Setup:\n")); cliSerial->printf_P(PSTR("\n\nRadio Setup:\n"));
uint8_t i; uint8_t i;
for(i = 0; i < 100;i++){ for(i = 0; i < 100;i++){
@ -116,7 +116,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
if(g.channel_roll.radio_in < 500){ if(g.channel_roll.radio_in < 500){
while(1){ while(1){
Serial.printf_P(PSTR("\nNo radio; Check connectors.")); cliSerial->printf_P(PSTR("\nNo radio; Check connectors."));
delay(1000); delay(1000);
// stop here // stop here
} }
@ -148,7 +148,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
g.rc_7.radio_trim = 1500; g.rc_7.radio_trim = 1500;
g.rc_8.radio_trim = 1500; g.rc_8.radio_trim = 1500;
Serial.printf_P(PSTR("\nMove all controls to each extreme. Hit Enter to save: \n")); cliSerial->printf_P(PSTR("\nMove all controls to each extreme. Hit Enter to save: \n"));
while(1){ while(1){
delay(20); delay(20);
@ -165,8 +165,8 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
g.rc_7.update_min_max(); g.rc_7.update_min_max();
g.rc_8.update_min_max(); g.rc_8.update_min_max();
if(Serial.available() > 0){ if(cliSerial->available() > 0){
Serial.flush(); cliSerial->flush();
g.channel_roll.save_eeprom(); g.channel_roll.save_eeprom();
g.channel_pitch.save_eeprom(); g.channel_pitch.save_eeprom();
g.channel_throttle.save_eeprom(); g.channel_throttle.save_eeprom();
@ -190,7 +190,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
{ {
byte switchPosition, mode = 0; byte switchPosition, mode = 0;
Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes.")); cliSerial->printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes."));
print_hit_enter(); print_hit_enter();
trim_radio(); trim_radio();
@ -253,7 +253,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
} }
// escape hatch // escape hatch
if(Serial.available() > 0){ if(cliSerial->available() > 0){
// save changes // save changes
for (mode=0; mode<6; mode++) for (mode=0; mode<6; mode++)
flight_modes[mode].save(); flight_modes[mode].save();
@ -278,10 +278,10 @@ setup_erase(uint8_t argc, const Menu::arg *argv)
{ {
int c; int c;
Serial.printf_P(PSTR("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: ")); cliSerial->printf_P(PSTR("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: "));
do { do {
c = Serial.read(); c = cliSerial->read();
} while (-1 == c); } while (-1 == c);
if (('y' != c) && ('Y' != c)) if (('y' != c) && ('Y' != c))
@ -290,11 +290,41 @@ setup_erase(uint8_t argc, const Menu::arg *argv)
return 0; return 0;
} }
/*
handle full accelerometer calibration via user dialog
*/
static void setup_printf_P(const prog_char_t *fmt, ...)
{
va_list arg_list;
va_start(arg_list, fmt);
cliSerial->vprintf_P(fmt, arg_list);
va_end(arg_list);
}
static void setup_wait_key(void)
{
// wait for user input
while (!cliSerial->available()) {
delay(20);
}
// clear input buffer
while( cliSerial->available() ) {
cliSerial->read();
}
}
static int8_t static int8_t
setup_accel_scale(uint8_t argc, const Menu::arg *argv) setup_accel_scale(uint8_t argc, const Menu::arg *argv)
{ {
cliSerial->println_P(PSTR("Initialising gyros"));
ins.init(AP_InertialSensor::COLD_START, delay, flash_leds, &timer_scheduler); ins.init(AP_InertialSensor::COLD_START, delay, flash_leds, &timer_scheduler);
ins.calibrate_accel(delay, flash_leds, gcs_send_text_fmt); if (ins.calibrate_accel(delay, flash_leds, setup_printf_P, setup_wait_key)) {
if (g.manual_level == 0) {
cliSerial->println_P(PSTR("Setting MANUAL_LEVEL to 1"));
g.manual_level.set_and_save(1);
}
}
report_ins(); report_ins();
return(0); return(0);
} }
@ -305,7 +335,7 @@ setup_compass(uint8_t argc, const Menu::arg *argv)
if (!strcmp_P(argv[1].str, PSTR("on"))) { if (!strcmp_P(argv[1].str, PSTR("on"))) {
compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft
if (!compass.init()) { if (!compass.init()) {
Serial.println_P(PSTR("Compass initialisation failed!")); cliSerial->println_P(PSTR("Compass initialisation failed!"));
g.compass_enabled = false; g.compass_enabled = false;
} else { } else {
g.compass_enabled = true; g.compass_enabled = true;
@ -317,7 +347,7 @@ setup_compass(uint8_t argc, const Menu::arg *argv)
compass.set_offsets(0,0,0); compass.set_offsets(0,0,0);
} else { } else {
Serial.printf_P(PSTR("\nOptions:[on,off,reset]\n")); cliSerial->printf_P(PSTR("\nOptions:[on,off,reset]\n"));
report_compass(); report_compass();
return 0; return 0;
} }
@ -334,7 +364,7 @@ setup_batt_monitor(uint8_t argc, const Menu::arg *argv)
g.battery_monitoring.set_and_save(argv[1].i); g.battery_monitoring.set_and_save(argv[1].i);
} else { } else {
Serial.printf_P(PSTR("\nOptions: 3-4")); cliSerial->printf_P(PSTR("\nOptions: 3-4"));
} }
report_batt_monitor(); report_batt_monitor();
@ -348,17 +378,17 @@ setup_batt_monitor(uint8_t argc, const Menu::arg *argv)
static void report_batt_monitor() static void report_batt_monitor()
{ {
//print_blanks(2); //print_blanks(2);
Serial.printf_P(PSTR("Batt Mointor\n")); cliSerial->printf_P(PSTR("Batt Mointor\n"));
print_divider(); print_divider();
if(g.battery_monitoring == 0) Serial.printf_P(PSTR("Batt monitoring disabled")); if(g.battery_monitoring == 0) cliSerial->printf_P(PSTR("Batt monitoring disabled"));
if(g.battery_monitoring == 3) Serial.printf_P(PSTR("Monitoring batt volts")); if(g.battery_monitoring == 3) cliSerial->printf_P(PSTR("Monitoring batt volts"));
if(g.battery_monitoring == 4) Serial.printf_P(PSTR("Monitoring volts and current")); if(g.battery_monitoring == 4) cliSerial->printf_P(PSTR("Monitoring volts and current"));
print_blanks(2); print_blanks(2);
} }
static void report_radio() static void report_radio()
{ {
//print_blanks(2); //print_blanks(2);
Serial.printf_P(PSTR("Radio\n")); cliSerial->printf_P(PSTR("Radio\n"));
print_divider(); print_divider();
// radio // radio
print_radio_values(); print_radio_values();
@ -368,28 +398,28 @@ static void report_radio()
static void report_gains() static void report_gains()
{ {
//print_blanks(2); //print_blanks(2);
Serial.printf_P(PSTR("Gains\n")); cliSerial->printf_P(PSTR("Gains\n"));
print_divider(); print_divider();
Serial.printf_P(PSTR("servo roll:\n")); cliSerial->printf_P(PSTR("servo roll:\n"));
print_PID(&g.pidServoRoll); print_PID(&g.pidServoRoll);
Serial.printf_P(PSTR("servo pitch:\n")); cliSerial->printf_P(PSTR("servo pitch:\n"));
print_PID(&g.pidServoPitch); print_PID(&g.pidServoPitch);
Serial.printf_P(PSTR("servo rudder:\n")); cliSerial->printf_P(PSTR("servo rudder:\n"));
print_PID(&g.pidServoRudder); print_PID(&g.pidServoRudder);
Serial.printf_P(PSTR("nav roll:\n")); cliSerial->printf_P(PSTR("nav roll:\n"));
print_PID(&g.pidNavRoll); print_PID(&g.pidNavRoll);
Serial.printf_P(PSTR("nav pitch airspeed:\n")); cliSerial->printf_P(PSTR("nav pitch airspeed:\n"));
print_PID(&g.pidNavPitchAirspeed); print_PID(&g.pidNavPitchAirspeed);
Serial.printf_P(PSTR("energry throttle:\n")); cliSerial->printf_P(PSTR("energry throttle:\n"));
print_PID(&g.pidTeThrottle); print_PID(&g.pidTeThrottle);
Serial.printf_P(PSTR("nav pitch alt:\n")); cliSerial->printf_P(PSTR("nav pitch alt:\n"));
print_PID(&g.pidNavPitchAltitude); print_PID(&g.pidNavPitchAltitude);
print_blanks(2); print_blanks(2);
@ -398,10 +428,10 @@ static void report_gains()
static void report_xtrack() static void report_xtrack()
{ {
//print_blanks(2); //print_blanks(2);
Serial.printf_P(PSTR("Crosstrack\n")); cliSerial->printf_P(PSTR("Crosstrack\n"));
print_divider(); print_divider();
// radio // radio
Serial.printf_P(PSTR("XTRACK: %4.2f\n" cliSerial->printf_P(PSTR("XTRACK: %4.2f\n"
"XTRACK angle: %d\n"), "XTRACK angle: %d\n"),
(float)g.crosstrack_gain, (float)g.crosstrack_gain,
(int)g.crosstrack_entry_angle); (int)g.crosstrack_entry_angle);
@ -411,10 +441,10 @@ static void report_xtrack()
static void report_throttle() static void report_throttle()
{ {
//print_blanks(2); //print_blanks(2);
Serial.printf_P(PSTR("Throttle\n")); cliSerial->printf_P(PSTR("Throttle\n"));
print_divider(); print_divider();
Serial.printf_P(PSTR("min: %d\n" cliSerial->printf_P(PSTR("min: %d\n"
"max: %d\n" "max: %d\n"
"cruise: %d\n" "cruise: %d\n"
"failsafe_enabled: %d\n" "failsafe_enabled: %d\n"
@ -430,7 +460,7 @@ static void report_throttle()
static void report_ins() static void report_ins()
{ {
//print_blanks(2); //print_blanks(2);
Serial.printf_P(PSTR("INS\n")); cliSerial->printf_P(PSTR("INS\n"));
print_divider(); print_divider();
print_gyro_offsets(); print_gyro_offsets();
@ -441,20 +471,20 @@ static void report_ins()
static void report_compass() static void report_compass()
{ {
//print_blanks(2); //print_blanks(2);
Serial.printf_P(PSTR("Compass: ")); cliSerial->printf_P(PSTR("Compass: "));
switch (compass.product_id) { switch (compass.product_id) {
case AP_COMPASS_TYPE_HMC5883L: case AP_COMPASS_TYPE_HMC5883L:
Serial.println_P(PSTR("HMC5883L")); cliSerial->println_P(PSTR("HMC5883L"));
break; break;
case AP_COMPASS_TYPE_HMC5843: case AP_COMPASS_TYPE_HMC5843:
Serial.println_P(PSTR("HMC5843")); cliSerial->println_P(PSTR("HMC5843"));
break; break;
case AP_COMPASS_TYPE_HIL: case AP_COMPASS_TYPE_HIL:
Serial.println_P(PSTR("HIL")); cliSerial->println_P(PSTR("HIL"));
break; break;
default: default:
Serial.println_P(PSTR("??")); cliSerial->println_P(PSTR("??"));
break; break;
} }
@ -463,13 +493,13 @@ static void report_compass()
print_enabled(g.compass_enabled); print_enabled(g.compass_enabled);
// mag declination // mag declination
Serial.printf_P(PSTR("Mag Declination: %4.4f\n"), cliSerial->printf_P(PSTR("Mag Declination: %4.4f\n"),
degrees(compass.get_declination())); degrees(compass.get_declination()));
Vector3f offsets = compass.get_offsets(); Vector3f offsets = compass.get_offsets();
// mag offsets // mag offsets
Serial.printf_P(PSTR("Mag offsets: %4.4f, %4.4f, %4.4f\n"), cliSerial->printf_P(PSTR("Mag offsets: %4.4f, %4.4f, %4.4f\n"),
offsets.x, offsets.x,
offsets.y, offsets.y,
offsets.z); offsets.z);
@ -479,7 +509,7 @@ static void report_compass()
static void report_flight_modes() static void report_flight_modes()
{ {
//print_blanks(2); //print_blanks(2);
Serial.printf_P(PSTR("Flight modes\n")); cliSerial->printf_P(PSTR("Flight modes\n"));
print_divider(); print_divider();
for(int i = 0; i < 6; i++ ){ for(int i = 0; i < 6; i++ ){
@ -495,7 +525,7 @@ static void report_flight_modes()
static void static 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"), cliSerial->printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%ld\n"),
pid->kP(), pid->kP(),
pid->kI(), pid->kI(),
pid->kD(), pid->kD(),
@ -505,28 +535,28 @@ print_PID(PID * pid)
static void static void
print_radio_values() print_radio_values()
{ {
Serial.printf_P(PSTR("CH1: %d | %d | %d\n"), (int)g.channel_roll.radio_min, (int)g.channel_roll.radio_trim, (int)g.channel_roll.radio_max); cliSerial->printf_P(PSTR("CH1: %d | %d | %d\n"), (int)g.channel_roll.radio_min, (int)g.channel_roll.radio_trim, (int)g.channel_roll.radio_max);
Serial.printf_P(PSTR("CH2: %d | %d | %d\n"), (int)g.channel_pitch.radio_min, (int)g.channel_pitch.radio_trim, (int)g.channel_pitch.radio_max); cliSerial->printf_P(PSTR("CH2: %d | %d | %d\n"), (int)g.channel_pitch.radio_min, (int)g.channel_pitch.radio_trim, (int)g.channel_pitch.radio_max);
Serial.printf_P(PSTR("CH3: %d | %d | %d\n"), (int)g.channel_throttle.radio_min, (int)g.channel_throttle.radio_trim, (int)g.channel_throttle.radio_max); cliSerial->printf_P(PSTR("CH3: %d | %d | %d\n"), (int)g.channel_throttle.radio_min, (int)g.channel_throttle.radio_trim, (int)g.channel_throttle.radio_max);
Serial.printf_P(PSTR("CH4: %d | %d | %d\n"), (int)g.channel_rudder.radio_min, (int)g.channel_rudder.radio_trim, (int)g.channel_rudder.radio_max); cliSerial->printf_P(PSTR("CH4: %d | %d | %d\n"), (int)g.channel_rudder.radio_min, (int)g.channel_rudder.radio_trim, (int)g.channel_rudder.radio_max);
Serial.printf_P(PSTR("CH5: %d | %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_trim, (int)g.rc_5.radio_max); cliSerial->printf_P(PSTR("CH5: %d | %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_trim, (int)g.rc_5.radio_max);
Serial.printf_P(PSTR("CH6: %d | %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_trim, (int)g.rc_6.radio_max); cliSerial->printf_P(PSTR("CH6: %d | %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_trim, (int)g.rc_6.radio_max);
Serial.printf_P(PSTR("CH7: %d | %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_trim, (int)g.rc_7.radio_max); cliSerial->printf_P(PSTR("CH7: %d | %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_trim, (int)g.rc_7.radio_max);
Serial.printf_P(PSTR("CH8: %d | %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_trim, (int)g.rc_8.radio_max); cliSerial->printf_P(PSTR("CH8: %d | %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_trim, (int)g.rc_8.radio_max);
} }
static void static void
print_switch(byte p, byte m) print_switch(byte p, byte m)
{ {
Serial.printf_P(PSTR("Pos %d: "),p); cliSerial->printf_P(PSTR("Pos %d: "),p);
Serial.println(flight_mode_strings[m]); cliSerial->println(flight_mode_strings[m]);
} }
static void static void
print_done() print_done()
{ {
Serial.printf_P(PSTR("\nSaved Settings\n\n")); cliSerial->printf_P(PSTR("\nSaved Settings\n\n"));
} }
static void static void
@ -534,7 +564,7 @@ print_blanks(int num)
{ {
while(num > 0){ while(num > 0){
num--; num--;
Serial.println(""); cliSerial->println("");
} }
} }
@ -542,9 +572,9 @@ static void
print_divider(void) print_divider(void)
{ {
for (int i = 0; i < 40; i++) { for (int i = 0; i < 40; i++) {
Serial.printf_P(PSTR("-")); cliSerial->printf_P(PSTR("-"));
} }
Serial.println(""); cliSerial->println("");
} }
static int8_t static int8_t
@ -577,20 +607,20 @@ radio_input_switch(void)
static void zero_eeprom(void) static void zero_eeprom(void)
{ {
byte b = 0; byte b = 0;
Serial.printf_P(PSTR("\nErasing EEPROM\n")); cliSerial->printf_P(PSTR("\nErasing EEPROM\n"));
for (intptr_t i = 0; i < EEPROM_MAX_ADDR; i++) { for (intptr_t i = 0; i < EEPROM_MAX_ADDR; i++) {
eeprom_write_byte((uint8_t *) i, b); eeprom_write_byte((uint8_t *) i, b);
} }
Serial.printf_P(PSTR("done\n")); cliSerial->printf_P(PSTR("done\n"));
} }
static void print_enabled(bool b) static void print_enabled(bool b)
{ {
if(b) if(b)
Serial.printf_P(PSTR("en")); cliSerial->printf_P(PSTR("en"));
else else
Serial.printf_P(PSTR("dis")); cliSerial->printf_P(PSTR("dis"));
Serial.printf_P(PSTR("abled\n")); cliSerial->printf_P(PSTR("abled\n"));
} }
static void static void
@ -598,7 +628,7 @@ print_accel_offsets_and_scaling(void)
{ {
Vector3f accel_offsets = ins.get_accel_offsets(); Vector3f accel_offsets = ins.get_accel_offsets();
Vector3f accel_scale = ins.get_accel_scale(); Vector3f accel_scale = ins.get_accel_scale();
Serial.printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\tscale: %4.2f, %4.2f, %4.2f\n"), cliSerial->printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\tscale: %4.2f, %4.2f, %4.2f\n"),
(float)accel_offsets.x, // Pitch (float)accel_offsets.x, // Pitch
(float)accel_offsets.y, // Roll (float)accel_offsets.y, // Roll
(float)accel_offsets.z, // YAW (float)accel_offsets.z, // YAW
@ -610,7 +640,7 @@ print_accel_offsets_and_scaling(void)
static void static void
print_gyro_offsets(void) print_gyro_offsets(void)
{ {
Serial.printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"), cliSerial->printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"),
(float)ins.gx(), (float)ins.gx(),
(float)ins.gy(), (float)ins.gy(),
(float)ins.gz()); (float)ins.gz());

46
APMrover2/system.pde
View File

@ -21,7 +21,7 @@ static int8_t planner_mode(uint8_t argc, const Menu::arg *argv); // in planner.p
// printf_P is a version of print_f that reads from flash memory // printf_P is a version of print_f that reads from flash memory
static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv) static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv)
{ {
Serial.printf_P(PSTR("Commands:\n" cliSerial->printf_P(PSTR("Commands:\n"
" logs log readback/setup mode\n" " logs log readback/setup mode\n"
" setup setup mode\n" " setup setup mode\n"
" test test mode\n" " test test mode\n"
@ -48,11 +48,15 @@ static const struct Menu::command main_menu_commands[] PROGMEM = {
MENU(main_menu, THISFIRMWARE, main_menu_commands); MENU(main_menu, THISFIRMWARE, main_menu_commands);
// the user wants the CLI. It never exits // the user wants the CLI. It never exits
static void run_cli(void) static void run_cli(FastSerial *port)
{ {
// disable the failsafe code in the CLI // disable the failsafe code in the CLI
timer_scheduler.set_failsafe(NULL); timer_scheduler.set_failsafe(NULL);
cliSerial = port;
Menu::set_port(port);
port->set_blocking_writes(true);
while (1) { while (1) {
main_menu.run(); main_menu.run();
} }
@ -89,7 +93,7 @@ static void init_ardupilot()
// XXX This could be optimised to reduce the buffer sizes in the cases // XXX This could be optimised to reduce the buffer sizes in the cases
// where they are not otherwise required. // where they are not otherwise required.
// //
Serial.begin(SERIAL0_BAUD, 128, 128); cliSerial->begin(SERIAL0_BAUD, 128, 128);
// GPS serial port. // GPS serial port.
// //
@ -103,7 +107,7 @@ static void init_ardupilot()
// standard gps running // standard gps running
Serial1.begin(115200, 128, 16); Serial1.begin(115200, 128, 16);
Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE cliSerial->printf_P(PSTR("\n\nInit " THISFIRMWARE
"\n\nFree RAM: %u\n"), "\n\nFree RAM: %u\n"),
memcheck_available_memory()); memcheck_available_memory());
@ -149,7 +153,7 @@ static void init_ardupilot()
if (!usb_connected) { if (!usb_connected) {
// we are not connected via USB, re-init UART0 with right // we are not connected via USB, re-init UART0 with right
// baud rate // baud rate
Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128); cliSerial->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
} }
#else #else
// we have a 2nd serial port for telemetry // we have a 2nd serial port for telemetry
@ -185,7 +189,7 @@ static void init_ardupilot()
if (g.compass_enabled==true) { if (g.compass_enabled==true) {
compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft
if (!compass.init()|| !compass.read()) { if (!compass.init()|| !compass.read()) {
Serial.println_P(PSTR("Compass initialisation failed!")); cliSerial->println_P(PSTR("Compass initialisation failed!"));
g.compass_enabled = false; g.compass_enabled = false;
} else { } else {
ahrs.set_compass(&compass); ahrs.set_compass(&compass);
@ -199,7 +203,7 @@ static void init_ardupilot()
// I2c.setSpeed(true); // I2c.setSpeed(true);
if (!compass.init()) { if (!compass.init()) {
Serial.println("compass initialisation failed!"); cliSerial->println("compass initialisation failed!");
while (1) ; while (1) ;
} }
@ -207,19 +211,19 @@ static void init_ardupilot()
compass.set_offsets(0,0,0); // set offsets to account for surrounding interference compass.set_offsets(0,0,0); // set offsets to account for surrounding interference
compass.set_declination(ToRad(0.0)); // set local difference between magnetic north and true north compass.set_declination(ToRad(0.0)); // set local difference between magnetic north and true north
Serial.print("Compass auto-detected as: "); cliSerial->print("Compass auto-detected as: ");
switch( compass.product_id ) { switch( compass.product_id ) {
case AP_COMPASS_TYPE_HIL: case AP_COMPASS_TYPE_HIL:
Serial.println("HIL"); cliSerial->println("HIL");
break; break;
case AP_COMPASS_TYPE_HMC5843: case AP_COMPASS_TYPE_HMC5843:
Serial.println("HMC5843"); cliSerial->println("HMC5843");
break; break;
case AP_COMPASS_TYPE_HMC5883L: case AP_COMPASS_TYPE_HMC5883L:
Serial.println("HMC5883L"); cliSerial->println("HMC5883L");
break; break;
default: default:
Serial.println("unknown"); cliSerial->println("unknown");
break; break;
} }
@ -276,17 +280,21 @@ static void init_ardupilot()
#if CLI_ENABLED == ENABLED && CLI_SLIDER_ENABLED == ENABLED #if CLI_ENABLED == ENABLED && CLI_SLIDER_ENABLED == ENABLED
if (digitalRead(SLIDE_SWITCH_PIN) == 0) { if (digitalRead(SLIDE_SWITCH_PIN) == 0) {
digitalWrite(A_LED_PIN,LED_ON); // turn on setup-mode LED digitalWrite(A_LED_PIN,LED_ON); // turn on setup-mode LED
Serial.printf_P(PSTR("\n" cliSerial->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" "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"));
Serial.println_P(PSTR("\nMove the slide switch and reset to FLY.\n")); cliSerial->println_P(PSTR("\nMove the slide switch and reset to FLY.\n"));
run_cli(); run_cli(&cliSerial);
} }
#else #else
Serial.printf_P(PSTR("\nPress ENTER 3 times to start interactive setup\n\n")); const prog_char_t *msg = PSTR("\nPress ENTER 3 times to start interactive setup\n");
cliSerial->println_P(msg);
#if USB_MUX_PIN == 0
Serial3.println_P(msg);
#endif
#endif // CLI_ENABLED #endif // CLI_ENABLED
// read in the flight switches // read in the flight switches
@ -526,7 +534,7 @@ static uint32_t map_baudrate(int8_t rate, uint32_t default_baud)
case 111: return 111100; case 111: return 111100;
case 115: return 115200; case 115: return 115200;
} }
Serial.println_P(PSTR("Invalid SERIAL3_BAUD")); cliSerial->println_P(PSTR("Invalid SERIAL3_BAUD"));
return default_baud; return default_baud;
} }
@ -542,9 +550,9 @@ static void check_usb_mux(void)
// the user has switched to/from the telemetry port // the user has switched to/from the telemetry port
usb_connected = usb_check; usb_connected = usb_check;
if (usb_connected) { if (usb_connected) {
Serial.begin(SERIAL0_BAUD, 128, 128); cliSerial->begin(SERIAL0_BAUD, 128, 128);
} else { } else {
Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128); cliSerial->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
} }
} }
#endif #endif

140
APMrover2/test.pde
View File

@ -78,14 +78,14 @@ MENU(test_menu, "test", test_menu_commands);
static int8_t static int8_t
test_mode(uint8_t argc, const Menu::arg *argv) test_mode(uint8_t argc, const Menu::arg *argv)
{ {
Serial.printf_P(PSTR("Test Mode\n\n")); cliSerial->printf_P(PSTR("Test Mode\n\n"));
test_menu.run(); test_menu.run();
return 0; return 0;
} }
static void print_hit_enter() static void print_hit_enter()
{ {
Serial.printf_P(PSTR("Hit Enter to exit.\n\n")); cliSerial->printf_P(PSTR("Hit Enter to exit.\n\n"));
} }
static int8_t static int8_t
@ -95,10 +95,10 @@ test_eedump(uint8_t argc, const Menu::arg *argv)
// hexdump the EEPROM // hexdump the EEPROM
for (i = 0; i < EEPROM_MAX_ADDR; i += 16) { for (i = 0; i < EEPROM_MAX_ADDR; i += 16) {
Serial.printf_P(PSTR("%04x:"), i); cliSerial->printf_P(PSTR("%04x:"), i);
for (j = 0; j < 16; j++) for (j = 0; j < 16; j++)
Serial.printf_P(PSTR(" %02x"), eeprom_read_byte((const uint8_t *)(i + j))); cliSerial->printf_P(PSTR(" %02x"), eeprom_read_byte((const uint8_t *)(i + j)));
Serial.println(); cliSerial->println();
} }
return(0); return(0);
} }
@ -116,7 +116,7 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
// ---------------------------------------------------------- // ----------------------------------------------------------
read_radio(); read_radio();
Serial.printf_P(PSTR("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), cliSerial->printf_P(PSTR("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
g.channel_roll.radio_in, g.channel_roll.radio_in,
g.channel_pitch.radio_in, g.channel_pitch.radio_in,
g.channel_throttle.radio_in, g.channel_throttle.radio_in,
@ -126,7 +126,7 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
g.rc_7.radio_in, g.rc_7.radio_in,
g.rc_8.radio_in); g.rc_8.radio_in);
if(Serial.available() > 0){ if(cliSerial->available() > 0){
return (0); return (0);
} }
} }
@ -144,15 +144,15 @@ test_passthru(uint8_t argc, const Menu::arg *argv)
// New radio frame? (we could use also if((millis()- timer) > 20) // New radio frame? (we could use also if((millis()- timer) > 20)
if (APM_RC.GetState() == 1){ if (APM_RC.GetState() == 1){
Serial.print("CH:"); cliSerial->print("CH:");
for(int i = 0; i < 8; i++){ for(int i = 0; i < 8; i++){
Serial.print(APM_RC.InputCh(i)); // Print channel values cliSerial->print(APM_RC.InputCh(i)); // Print channel values
Serial.print(","); cliSerial->print(",");
APM_RC.OutputCh(i, APM_RC.InputCh(i)); // Copy input to Servos APM_RC.OutputCh(i, APM_RC.InputCh(i)); // Copy input to Servos
} }
Serial.println(); cliSerial->println();
} }
if (Serial.available() > 0){ if (cliSerial->available() > 0){
return (0); return (0);
} }
} }
@ -166,7 +166,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
delay(1000); delay(1000);
#if THROTTLE_REVERSE == 1 #if THROTTLE_REVERSE == 1
Serial.printf_P(PSTR("Throttle is reversed in config: \n")); cliSerial->printf_P(PSTR("Throttle is reversed in config: \n"));
delay(1000); delay(1000);
#endif #endif
@ -190,7 +190,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
tuning_value = constrain(((float)(g.rc_7.radio_in - g.rc_7.radio_min) / (float)(g.rc_7.radio_max - g.rc_7.radio_min)),0,1); tuning_value = constrain(((float)(g.rc_7.radio_in - g.rc_7.radio_min) / (float)(g.rc_7.radio_max - g.rc_7.radio_min)),0,1);
Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d Tuning = %2.3f\n"), cliSerial->printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d Tuning = %2.3f\n"),
g.channel_roll.control_in, g.channel_roll.control_in,
g.channel_pitch.control_in, g.channel_pitch.control_in,
g.channel_throttle.control_in, g.channel_throttle.control_in,
@ -201,7 +201,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
g.rc_8.control_in, g.rc_8.control_in,
tuning_value); tuning_value);
if(Serial.available() > 0){ if(cliSerial->available() > 0){
return (0); return (0);
} }
} }
@ -223,7 +223,7 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
oldSwitchPosition = readSwitch(); oldSwitchPosition = readSwitch();
Serial.printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n")); cliSerial->printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n"));
while(g.channel_throttle.control_in > 0){ while(g.channel_throttle.control_in > 0){
delay(20); delay(20);
read_radio(); read_radio();
@ -234,27 +234,27 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
read_radio(); read_radio();
if(g.channel_throttle.control_in > 0){ if(g.channel_throttle.control_in > 0){
Serial.printf_P(PSTR("THROTTLE CHANGED %d \n"), g.channel_throttle.control_in); cliSerial->printf_P(PSTR("THROTTLE CHANGED %d \n"), g.channel_throttle.control_in);
fail_test++; fail_test++;
} }
if(oldSwitchPosition != readSwitch()){ if(oldSwitchPosition != readSwitch()){
Serial.printf_P(PSTR("CONTROL MODE CHANGED: ")); cliSerial->printf_P(PSTR("CONTROL MODE CHANGED: "));
Serial.println(flight_mode_strings[readSwitch()]); cliSerial->println(flight_mode_strings[readSwitch()]);
fail_test++; fail_test++;
} }
if(g.throttle_fs_enabled && g.channel_throttle.get_failsafe()){ if(g.throttle_fs_enabled && g.channel_throttle.get_failsafe()){
Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.channel_throttle.radio_in); cliSerial->printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.channel_throttle.radio_in);
Serial.println(flight_mode_strings[readSwitch()]); cliSerial->println(flight_mode_strings[readSwitch()]);
fail_test++; fail_test++;
} }
if(fail_test > 0){ if(fail_test > 0){
return (0); return (0);
} }
if(Serial.available() > 0){ if(cliSerial->available() > 0){
Serial.printf_P(PSTR("LOS caused no change in APM.\n")); cliSerial->printf_P(PSTR("LOS caused no change in APM.\n"));
return (0); return (0);
} }
} }
@ -272,12 +272,12 @@ if (g.battery_monitoring == 3 || g.battery_monitoring == 4) {
read_radio(); read_radio();
read_battery(); read_battery();
if (g.battery_monitoring == 3){ if (g.battery_monitoring == 3){
Serial.printf_P(PSTR("V: %4.4f\n"), cliSerial->printf_P(PSTR("V: %4.4f\n"),
battery_voltage1, battery_voltage1,
current_amps1, current_amps1,
current_total1); current_total1);
} else { } else {
Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"), cliSerial->printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"),
battery_voltage1, battery_voltage1,
current_amps1, current_amps1,
current_total1); current_total1);
@ -287,12 +287,12 @@ if (g.battery_monitoring == 3 || g.battery_monitoring == 4) {
// ------------------------------ // ------------------------------
set_servos(); set_servos();
if(Serial.available() > 0){ if(cliSerial->available() > 0){
return (0); return (0);
} }
} }
} else { } else {
Serial.printf_P(PSTR("Not enabled\n")); cliSerial->printf_P(PSTR("Not enabled\n"));
return (0); return (0);
} }
@ -305,17 +305,17 @@ test_relay(uint8_t argc, const Menu::arg *argv)
delay(1000); delay(1000);
while(1){ while(1){
Serial.printf_P(PSTR("Relay on\n")); cliSerial->printf_P(PSTR("Relay on\n"));
relay.on(); relay.on();
delay(3000); delay(3000);
if(Serial.available() > 0){ if(cliSerial->available() > 0){
return (0); return (0);
} }
Serial.printf_P(PSTR("Relay off\n")); cliSerial->printf_P(PSTR("Relay off\n"));
relay.off(); relay.off();
delay(3000); delay(3000);
if(Serial.available() > 0){ if(cliSerial->available() > 0){
return (0); return (0);
} }
} }
@ -326,9 +326,9 @@ test_wp(uint8_t argc, const Menu::arg *argv)
{ {
delay(1000); delay(1000);
Serial.printf_P(PSTR("%d waypoints\n"), (int)g.command_total); cliSerial->printf_P(PSTR("%d waypoints\n"), (int)g.command_total);
Serial.printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius); cliSerial->printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius);
Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius); cliSerial->printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius);
for(byte i = 0; i <= g.command_total; i++){ for(byte i = 0; i <= g.command_total; i++){
struct Location temp = get_cmd_with_index(i); struct Location temp = get_cmd_with_index(i);
@ -341,7 +341,7 @@ test_wp(uint8_t argc, const Menu::arg *argv)
static void static void
test_wp_print(struct Location *cmd, byte wp_index) test_wp_print(struct Location *cmd, byte wp_index)
{ {
Serial.printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"), cliSerial->printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
(int)wp_index, (int)wp_index,
(int)cmd->id, (int)cmd->id,
(int)cmd->options, (int)cmd->options,
@ -356,14 +356,14 @@ test_xbee(uint8_t argc, const Menu::arg *argv)
{ {
print_hit_enter(); print_hit_enter();
delay(1000); delay(1000);
Serial.printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n")); cliSerial->printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n"));
while(1){ while(1){
if (Serial3.available()) if (Serial3.available())
Serial3.write(Serial3.read()); Serial3.write(Serial3.read());
if(Serial.available() > 0){ if(cliSerial->available() > 0){
return (0); return (0);
} }
} }
@ -376,18 +376,18 @@ test_modeswitch(uint8_t argc, const Menu::arg *argv)
print_hit_enter(); print_hit_enter();
delay(1000); delay(1000);
Serial.printf_P(PSTR("Control CH ")); cliSerial->printf_P(PSTR("Control CH "));
Serial.println(FLIGHT_MODE_CHANNEL, DEC); cliSerial->println(FLIGHT_MODE_CHANNEL, DEC);
while(1){ while(1){
delay(20); delay(20);
byte switchPosition = readSwitch(); byte switchPosition = readSwitch();
if (oldSwitchPosition != switchPosition){ if (oldSwitchPosition != switchPosition){
Serial.printf_P(PSTR("Position %d\n"), switchPosition); cliSerial->printf_P(PSTR("Position %d\n"), switchPosition);
oldSwitchPosition = switchPosition; oldSwitchPosition = switchPosition;
} }
if(Serial.available() > 0){ if(cliSerial->available() > 0){
return (0); return (0);
} }
} }
@ -399,20 +399,20 @@ test_modeswitch(uint8_t argc, const Menu::arg *argv)
static int8_t static int8_t
test_logging(uint8_t argc, const Menu::arg *argv) test_logging(uint8_t argc, const Menu::arg *argv)
{ {
Serial.println_P(PSTR("Testing dataflash logging")); cliSerial->println_P(PSTR("Testing dataflash logging"));
if (!DataFlash.CardInserted()) { if (!DataFlash.CardInserted()) {
Serial.println_P(PSTR("ERR: No dataflash inserted")); cliSerial->println_P(PSTR("ERR: No dataflash inserted"));
return 0; return 0;
} }
DataFlash.ReadManufacturerID(); DataFlash.ReadManufacturerID();
Serial.printf_P(PSTR("Manufacturer: 0x%02x Device: 0x%04x\n"), cliSerial->printf_P(PSTR("Manufacturer: 0x%02x Device: 0x%04x\n"),
(unsigned)DataFlash.df_manufacturer, (unsigned)DataFlash.df_manufacturer,
(unsigned)DataFlash.df_device); (unsigned)DataFlash.df_device);
Serial.printf_P(PSTR("NumPages: %u PageSize: %u\n"), cliSerial->printf_P(PSTR("NumPages: %u PageSize: %u\n"),
(unsigned)DataFlash.df_NumPages+1, (unsigned)DataFlash.df_NumPages+1,
(unsigned)DataFlash.df_PageSize); (unsigned)DataFlash.df_PageSize);
DataFlash.StartRead(DataFlash.df_NumPages+1); DataFlash.StartRead(DataFlash.df_NumPages+1);
Serial.printf_P(PSTR("Format version: %lx Expected format version: %lx\n"), cliSerial->printf_P(PSTR("Format version: %lx Expected format version: %lx\n"),
(unsigned long)DataFlash.ReadLong(), (unsigned long)DF_LOGGING_FORMAT); (unsigned long)DataFlash.ReadLong(), (unsigned long)DF_LOGGING_FORMAT);
return 0; return 0;
} }
@ -425,7 +425,7 @@ test_dipswitches(uint8_t argc, const Menu::arg *argv)
delay(1000); delay(1000);
if (!g.switch_enable) { if (!g.switch_enable) {
Serial.println_P(PSTR("dip switches disabled, using EEPROM")); cliSerial->println_P(PSTR("dip switches disabled, using EEPROM"));
} }
while(1){ while(1){
@ -433,17 +433,17 @@ test_dipswitches(uint8_t argc, const Menu::arg *argv)
update_servo_switches(); update_servo_switches();
if (g.mix_mode == 0) { if (g.mix_mode == 0) {
Serial.printf_P(PSTR("Mix:standard \trev roll:%d, rev pitch:%d, rev rudder:%d\n"), cliSerial->printf_P(PSTR("Mix:standard \trev roll:%d, rev pitch:%d, rev rudder:%d\n"),
(int)g.channel_roll.get_reverse(), (int)g.channel_roll.get_reverse(),
(int)g.channel_pitch.get_reverse(), (int)g.channel_pitch.get_reverse(),
(int)g.channel_rudder.get_reverse()); (int)g.channel_rudder.get_reverse());
} else { } else {
Serial.printf_P(PSTR("Mix:elevons \trev elev:%d, rev ch1:%d, rev ch2:%d\n"), cliSerial->printf_P(PSTR("Mix:elevons \trev elev:%d, rev ch1:%d, rev ch2:%d\n"),
(int)g.reverse_elevons, (int)g.reverse_elevons,
(int)g.reverse_ch1_elevon, (int)g.reverse_ch1_elevon,
(int)g.reverse_ch2_elevon); (int)g.reverse_ch2_elevon);
} }
if(Serial.available() > 0){ if(cliSerial->available() > 0){
return (0); return (0);
} }
} }
@ -463,14 +463,14 @@ test_adc(uint8_t argc, const Menu::arg *argv)
print_hit_enter(); print_hit_enter();
adc.Init(&timer_scheduler); adc.Init(&timer_scheduler);
delay(1000); delay(1000);
Serial.printf_P(PSTR("ADC\n")); cliSerial->printf_P(PSTR("ADC\n"));
delay(1000); delay(1000);
while(1){ while(1){
for (int i=0;i<9;i++) Serial.printf_P(PSTR("%.1f\t"),adc.Ch(i)); for (int i=0;i<9;i++) cliSerial->printf_P(PSTR("%.1f\t"),adc.Ch(i));
Serial.println(); cliSerial->println();
delay(100); delay(100);
if(Serial.available() > 0){ if(cliSerial->available() > 0){
return (0); return (0);
} }
} }
@ -493,15 +493,15 @@ test_gps(uint8_t argc, const Menu::arg *argv)
g_gps->update(); g_gps->update();
if (g_gps->new_data){ if (g_gps->new_data){
Serial.printf_P(PSTR("Lat: %ld, Lon %ld, Alt: %ldm, #sats: %d\n"), cliSerial->printf_P(PSTR("Lat: %ld, Lon %ld, Alt: %ldm, #sats: %d\n"),
g_gps->latitude, g_gps->latitude,
g_gps->longitude, g_gps->longitude,
g_gps->altitude/100, g_gps->altitude/100,
g_gps->num_sats); g_gps->num_sats);
}else{ }else{
Serial.printf_P(PSTR(".")); cliSerial->printf_P(PSTR("."));
} }
if(Serial.available() > 0){ if(cliSerial->available() > 0){
return (0); return (0);
} }
} }
@ -510,7 +510,7 @@ test_gps(uint8_t argc, const Menu::arg *argv)
static int8_t static int8_t
test_ins(uint8_t argc, const Menu::arg *argv) test_ins(uint8_t argc, const Menu::arg *argv)
{ {
//Serial.printf_P(PSTR("Calibrating.")); //cliSerial->printf_P(PSTR("Calibrating."));
ins.init(AP_InertialSensor::COLD_START, delay, flash_leds, &timer_scheduler); ins.init(AP_InertialSensor::COLD_START, delay, flash_leds, &timer_scheduler);
ahrs.reset(); ahrs.reset();
@ -540,14 +540,14 @@ test_ins(uint8_t argc, const Menu::arg *argv)
// --------------------- // ---------------------
Vector3f gyros = ins.get_gyro(); Vector3f gyros = ins.get_gyro();
Vector3f accels = ins.get_accel(); Vector3f accels = ins.get_accel();
Serial.printf_P(PSTR("r:%4d p:%4d y:%3d g=(%5.1f %5.1f %5.1f) a=(%5.1f %5.1f %5.1f)\n"), cliSerial->printf_P(PSTR("r:%4d p:%4d y:%3d g=(%5.1f %5.1f %5.1f) a=(%5.1f %5.1f %5.1f)\n"),
(int)ahrs.roll_sensor / 100, (int)ahrs.roll_sensor / 100,
(int)ahrs.pitch_sensor / 100, (int)ahrs.pitch_sensor / 100,
(uint16_t)ahrs.yaw_sensor / 100, (uint16_t)ahrs.yaw_sensor / 100,
gyros.x, gyros.y, gyros.z, gyros.x, gyros.y, gyros.z,
accels.x, accels.y, accels.z); accels.x, accels.y, accels.z);
} }
if(Serial.available() > 0){ if(cliSerial->available() > 0){
return (0); return (0);
} }
} }
@ -558,14 +558,14 @@ static int8_t
test_mag(uint8_t argc, const Menu::arg *argv) test_mag(uint8_t argc, const Menu::arg *argv)
{ {
if (!g.compass_enabled) { if (!g.compass_enabled) {
Serial.printf_P(PSTR("Compass: ")); cliSerial->printf_P(PSTR("Compass: "));
print_enabled(false); print_enabled(false);
return (0); return (0);
} }
compass.set_orientation(MAG_ORIENTATION); compass.set_orientation(MAG_ORIENTATION);
if (!compass.init()) { if (!compass.init()) {
Serial.println_P(PSTR("Compass initialisation failed!")); cliSerial->println_P(PSTR("Compass initialisation failed!"));
return 0; return 0;
} }
ahrs.set_compass(&compass); ahrs.set_compass(&compass);
@ -578,7 +578,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
int counter = 0; int counter = 0;
float heading = 0; float heading = 0;
//Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION); //cliSerial->printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION);
print_hit_enter(); print_hit_enter();
@ -608,7 +608,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
if (counter>20) { if (counter>20) {
if (compass.healthy) { if (compass.healthy) {
Vector3f maggy = compass.get_offsets(); Vector3f maggy = compass.get_offsets();
Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"), cliSerial->printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
(wrap_360(ToDeg(heading) * 100)) /100, (wrap_360(ToDeg(heading) * 100)) /100,
(int)compass.mag_x, (int)compass.mag_x,
(int)compass.mag_y, (int)compass.mag_y,
@ -617,19 +617,19 @@ test_mag(uint8_t argc, const Menu::arg *argv)
maggy.y, maggy.y,
maggy.z); maggy.z);
} else { } else {
Serial.println_P(PSTR("compass not healthy")); cliSerial->println_P(PSTR("compass not healthy"));
} }
counter=0; counter=0;
} }
} }
if (Serial.available() > 0) { if (cliSerial->available() > 0) {
break; break;
} }
} }
// save offsets. This allows you to get sane offset values using // save offsets. This allows you to get sane offset values using
// the CLI before you go flying. // the CLI before you go flying.
Serial.println_P(PSTR("saving offsets")); cliSerial->println_P(PSTR("saving offsets"));
compass.save_offsets(); compass.save_offsets();
return (0); return (0);
} }
@ -658,7 +658,7 @@ test_rawgps(uint8_t argc, const Menu::arg *argv)
Serial3.write(Serial1.read()); Serial3.write(Serial1.read());
digitalWrite(C_LED_PIN, LED_OFF); digitalWrite(C_LED_PIN, LED_OFF);
} }
if(Serial.available() > 0){ if(cliSerial->available() > 0){
return (0); return (0);
} }
} }
@ -678,9 +678,9 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
if(g.sonar_enabled){ if(g.sonar_enabled){
sonar_dist = sonar.read(); sonar_dist = sonar.read();
} }
Serial.printf_P(PSTR("sonar_dist = %d\n"), (int)sonar_dist); cliSerial->printf_P(PSTR("sonar_dist = %d\n"), (int)sonar_dist);
if(Serial.available() > 0){ if(cliSerial->available() > 0){
break; break;
} }
} }