mirror of https://github.com/ArduPilot/ardupilot
Rover: make it possible to run the CLI over a radio link
This commit is contained in:
Andrew Tridgell
parent
bcae83c4f6
commit
24adc1aaa3
|
|
@ -134,6 +134,8 @@ FastSerialPort1(Serial1); // GPS port
|
|||
FastSerialPort3(Serial3); // Telemetry port for APM1
|
||||
#endif
|
||||
|
||||
static FastSerial *cliSerial = &Serial3;
|
||||
|
||||
// this sets up the parameter table, and sets the default values. This
|
||||
// must be the first AP_Param variable declared to ensure its
|
||||
// constructor runs before the constructors of the other AP_Param
|
||||
|
|
@ -812,13 +814,13 @@ static void medium_loop()
|
|||
#endif
|
||||
//#endif
|
||||
/*{
|
||||
Serial.print(ahrs.roll_sensor, DEC); Serial.printf_P(PSTR("\t"));
|
||||
Serial.print(ahrs.pitch_sensor, DEC); Serial.printf_P(PSTR("\t"));
|
||||
Serial.print(ahrs.yaw_sensor, DEC); Serial.printf_P(PSTR("\t"));
|
||||
cliSerial->print(ahrs.roll_sensor, DEC); cliSerial->printf_P(PSTR("\t"));
|
||||
cliSerial->print(ahrs.pitch_sensor, DEC); cliSerial->printf_P(PSTR("\t"));
|
||||
cliSerial->print(ahrs.yaw_sensor, DEC); cliSerial->printf_P(PSTR("\t"));
|
||||
Vector3f tempaccel = ins.get_accel();
|
||||
Serial.print(tempaccel.x, DEC); Serial.printf_P(PSTR("\t"));
|
||||
Serial.print(tempaccel.y, DEC); Serial.printf_P(PSTR("\t"));
|
||||
Serial.println(tempaccel.z, DEC);
|
||||
cliSerial->print(tempaccel.x, DEC); cliSerial->printf_P(PSTR("\t"));
|
||||
cliSerial->print(tempaccel.y, DEC); cliSerial->printf_P(PSTR("\t"));
|
||||
cliSerial->println(tempaccel.z, DEC);
|
||||
}*/
|
||||
|
||||
break;
|
||||
|
|
@ -939,11 +941,11 @@ static void slow_loop()
|
|||
#endif
|
||||
|
||||
#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);
|
||||
// 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);
|
||||
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
|
||||
break;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -756,7 +756,7 @@ GCS_MAVLINK::update(void)
|
|||
crlf_count = 0;
|
||||
}
|
||||
if (crlf_count == 3) {
|
||||
run_cli();
|
||||
run_cli(_port);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
@ -1692,8 +1692,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||
// TODO: check scaling for temp/absPress
|
||||
float temp = 70;
|
||||
float absPress = 1;
|
||||
//Serial.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("accel: %d %d %d\n"), packet.xacc, packet.yacc, packet.zacc);
|
||||
//cliSerial->printf_P(PSTR("gyro: %d %d %d\n"), packet.xgyro, packet.ygyro, packet.zgyro);
|
||||
|
||||
// rad/sec
|
||||
Vector3f gyros;
|
||||
|
|
|
|||
|
|
@ -23,7 +23,7 @@ static int16_t cur_throttle =0;
|
|||
// 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)
|
||||
/*{
|
||||
Serial.printf_P(PSTR("\n"
|
||||
cliSerial->printf_P(PSTR("\n"
|
||||
"Commands:\n"
|
||||
" dump <n>"
|
||||
" erase (all logs)\n"
|
||||
|
|
@ -57,16 +57,16 @@ print_log_menu(void)
|
|||
|
||||
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) {
|
||||
Serial.printf_P(PSTR("none"));
|
||||
cliSerial->printf_P(PSTR("none"));
|
||||
}else{
|
||||
// Macro to make the following code a bit easier on the eye.
|
||||
// Pass it the capitalised name of the log option, as defined
|
||||
// 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.
|
||||
#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_MED);
|
||||
PLOG(GPS);
|
||||
|
|
@ -80,24 +80,24 @@ print_log_menu(void)
|
|||
#undef PLOG
|
||||
}
|
||||
|
||||
Serial.println();
|
||||
cliSerial->println();
|
||||
|
||||
if (num_logs == 0) {
|
||||
Serial.printf_P(PSTR("\nNo logs\n\n"));
|
||||
cliSerial->printf_P(PSTR("\nNo logs\n\n"));
|
||||
}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--) {
|
||||
int last_log_start = log_start, last_log_end = log_end;
|
||||
temp = last_log_num-i+1;
|
||||
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) {
|
||||
// we are printing bogus logs
|
||||
break;
|
||||
}
|
||||
}
|
||||
Serial.println();
|
||||
cliSerial->println();
|
||||
}
|
||||
return(true);
|
||||
}
|
||||
|
|
@ -117,28 +117,28 @@ dump_log(uint8_t argc, const Menu::arg *argv)
|
|||
if (dump_log == -2) {
|
||||
for(uint16_t count=1; count<=DataFlash.df_NumPages; count++) {
|
||||
DataFlash.StartRead(count);
|
||||
Serial.printf_P(PSTR("DF page, log file #, log page: %d,\t"), count);
|
||||
Serial.printf_P(PSTR("%d,\t"), DataFlash.GetFileNumber());
|
||||
Serial.printf_P(PSTR("%d\n"), DataFlash.GetFilePage());
|
||||
cliSerial->printf_P(PSTR("DF page, log file #, log page: %d,\t"), count);
|
||||
cliSerial->printf_P(PSTR("%d,\t"), DataFlash.GetFileNumber());
|
||||
cliSerial->printf_P(PSTR("%d\n"), DataFlash.GetFilePage());
|
||||
}
|
||||
return(-1);
|
||||
} 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);
|
||||
return(-1);
|
||||
} 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);
|
||||
}
|
||||
|
||||
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_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;
|
||||
}
|
||||
|
||||
|
|
@ -146,15 +146,15 @@ dump_log(uint8_t argc, const Menu::arg *argv)
|
|||
void erase_callback(unsigned long t) {
|
||||
mavlink_delay(t);
|
||||
if (DataFlash.GetWritePage() % 128 == 0) {
|
||||
Serial.printf_P(PSTR("+"));
|
||||
cliSerial->printf_P(PSTR("+"));
|
||||
}
|
||||
}
|
||||
|
||||
static void do_erase_logs(void)
|
||||
{
|
||||
Serial.printf_P(PSTR("\nErasing log...\n"));
|
||||
cliSerial->printf_P(PSTR("\nErasing log...\n"));
|
||||
DataFlash.EraseAll(erase_callback);
|
||||
Serial.printf_P(PSTR("\nLog erased.\n"));
|
||||
cliSerial->printf_P(PSTR("\nLog erased.\n"));
|
||||
}
|
||||
|
||||
static int8_t
|
||||
|
|
@ -172,7 +172,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
|
|||
uint16_t bits;
|
||||
|
||||
if (argc != 2) {
|
||||
Serial.printf_P(PSTR("missing log type\n"));
|
||||
cliSerial->printf_P(PSTR("missing log type\n"));
|
||||
return(-1);
|
||||
}
|
||||
|
||||
|
|
@ -400,7 +400,7 @@ static void Log_Write_Current()
|
|||
// Read a Current packet
|
||||
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(),
|
||||
((float)DataFlash.ReadInt() / 100.f),
|
||||
((float)DataFlash.ReadInt() / 100.f),
|
||||
|
|
@ -412,17 +412,17 @@ static void Log_Read_Control_Tuning()
|
|||
{
|
||||
float logvar;
|
||||
|
||||
Serial.printf_P(PSTR("CTUN: "));
|
||||
cliSerial->printf_P(PSTR("CTUN: "));
|
||||
for (int y = 1; y < 10; y++) {
|
||||
logvar = DataFlash.ReadInt();
|
||||
if(y == 7) cur_throttle = logvar;
|
||||
if(y < 8) logvar = logvar/100.f;
|
||||
if(y == 9) logvar = logvar/10000.f;
|
||||
Serial.print(logvar);
|
||||
Serial.print(comma);
|
||||
Serial.print(" ");
|
||||
cliSerial->print(logvar);
|
||||
cliSerial->print(comma);
|
||||
cliSerial->print(" ");
|
||||
}
|
||||
Serial.println("");
|
||||
cliSerial->println("");
|
||||
}
|
||||
|
||||
// Read a nav tuning packet
|
||||
|
|
@ -432,7 +432,7 @@ static void Log_Read_Nav_Tuning()
|
|||
for (int8_t i=0; i<7; i++) {
|
||||
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[1],
|
||||
((uint16_t)d[2])/100.0,
|
||||
|
|
@ -448,21 +448,21 @@ static void Log_Read_Performance()
|
|||
int32_t pm_time;
|
||||
int logvar;
|
||||
|
||||
Serial.printf_P(PSTR("PM: "));
|
||||
cliSerial->printf_P(PSTR("PM: "));
|
||||
pm_time = DataFlash.ReadLong();
|
||||
Serial.print(pm_time);
|
||||
Serial.print(comma);
|
||||
cliSerial->print(pm_time);
|
||||
cliSerial->print(comma);
|
||||
for (int y = 1; y <= 12; y++) {
|
||||
if(y < 3 || y > 7){
|
||||
logvar = DataFlash.ReadInt();
|
||||
}else{
|
||||
logvar = DataFlash.ReadByte();
|
||||
}
|
||||
Serial.print(logvar);
|
||||
Serial.print(comma);
|
||||
Serial.print(" ");
|
||||
cliSerial->print(logvar);
|
||||
cliSerial->print(comma);
|
||||
cliSerial->print(" ");
|
||||
}
|
||||
Serial.println("");
|
||||
cliSerial->println("");
|
||||
}
|
||||
|
||||
// Read a command processing packet
|
||||
|
|
@ -471,20 +471,20 @@ static void Log_Read_Cmd()
|
|||
byte logvarb;
|
||||
int32_t logvarl;
|
||||
|
||||
Serial.printf_P(PSTR("CMD: "));
|
||||
cliSerial->printf_P(PSTR("CMD: "));
|
||||
for(int i = 1; i < 4; i++) {
|
||||
logvarb = DataFlash.ReadByte();
|
||||
Serial.print(logvarb, DEC);
|
||||
Serial.print(comma);
|
||||
Serial.print(" ");
|
||||
cliSerial->print(logvarb, DEC);
|
||||
cliSerial->print(comma);
|
||||
cliSerial->print(" ");
|
||||
}
|
||||
for(int i = 1; i < 4; i++) {
|
||||
logvarl = DataFlash.ReadLong();
|
||||
Serial.print(logvarl, DEC);
|
||||
Serial.print(comma);
|
||||
Serial.print(" ");
|
||||
cliSerial->print(logvarl, DEC);
|
||||
cliSerial->print(comma);
|
||||
cliSerial->print(" ");
|
||||
}
|
||||
Serial.println("");
|
||||
cliSerial->println("");
|
||||
}
|
||||
|
||||
static void Log_Read_Startup()
|
||||
|
|
@ -492,13 +492,13 @@ static void Log_Read_Startup()
|
|||
byte logbyte = DataFlash.ReadByte();
|
||||
|
||||
if (logbyte == TYPE_AIRSTART_MSG)
|
||||
Serial.printf_P(PSTR("AIR START - "));
|
||||
cliSerial->printf_P(PSTR("AIR START - "));
|
||||
else if (logbyte == TYPE_GROUNDSTART_MSG)
|
||||
Serial.printf_P(PSTR("GROUND START - "));
|
||||
cliSerial->printf_P(PSTR("GROUND START - "));
|
||||
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
|
||||
|
|
@ -508,7 +508,7 @@ static void Log_Read_Attitude()
|
|||
d[0] = DataFlash.ReadInt();
|
||||
d[1] = 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],
|
||||
(uint16_t)d[2]);
|
||||
}
|
||||
|
|
@ -516,8 +516,8 @@ static void Log_Read_Attitude()
|
|||
// Read a mode packet
|
||||
static void Log_Read_Mode()
|
||||
{
|
||||
Serial.printf_P(PSTR("MOD: "));
|
||||
Serial.println(flight_mode_strings[DataFlash.ReadByte()]);
|
||||
cliSerial->printf_P(PSTR("MOD: "));
|
||||
cliSerial->println(flight_mode_strings[DataFlash.ReadByte()]);
|
||||
}
|
||||
|
||||
// Read a GPS packet
|
||||
|
|
@ -540,17 +540,17 @@ static void Log_Read_GPS()
|
|||
k = 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],
|
||||
l[1]/t7, l[2]/t7,
|
||||
(int)i,
|
||||
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],
|
||||
l[1]/t7, l[2]/t7,
|
||||
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,
|
||||
(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()
|
||||
{
|
||||
float logvar;
|
||||
Serial.printf_P(PSTR("RAW: "));
|
||||
cliSerial->printf_P(PSTR("RAW: "));
|
||||
for (int y = 0; y < 6; y++) {
|
||||
logvar = (float)DataFlash.ReadLong() / t7;
|
||||
Serial.print(logvar);
|
||||
Serial.print(comma);
|
||||
Serial.print(" ");
|
||||
cliSerial->print(logvar);
|
||||
cliSerial->print(comma);
|
||||
cliSerial->print(" ");
|
||||
}
|
||||
Serial.println("");
|
||||
cliSerial->println("");
|
||||
}
|
||||
|
||||
// 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;
|
||||
|
||||
#ifdef AIRFRAME_NAME
|
||||
Serial.printf_P(PSTR((AIRFRAME_NAME)
|
||||
cliSerial->printf_P(PSTR((AIRFRAME_NAME)
|
||||
#endif
|
||||
Serial.printf_P(PSTR("\n" THISFIRMWARE
|
||||
cliSerial->printf_P(PSTR("\n" THISFIRMWARE
|
||||
"\nFree RAM: %u\n"),
|
||||
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);
|
||||
}
|
||||
|
||||
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
|
||||
|
|
@ -657,7 +657,7 @@ static int Log_Read_Process(int16_t start_page, int16_t end_page)
|
|||
Log_Read_GPS();
|
||||
log_step++;
|
||||
}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...
|
||||
}
|
||||
}
|
||||
|
|
@ -666,7 +666,7 @@ static int Log_Read_Process(int16_t start_page, int16_t end_page)
|
|||
if(data == END_BYTE){
|
||||
packet_count++;
|
||||
}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...
|
||||
break;
|
||||
|
|
|
|||
|
|
@ -150,17 +150,17 @@ static void load_parameters(void)
|
|||
g.format_version != Parameters::k_format_version) {
|
||||
|
||||
// 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();
|
||||
|
||||
// save the current format version
|
||||
g.format_version.set_and_save(Parameters::k_format_version);
|
||||
Serial.println_P(PSTR("done."));
|
||||
cliSerial->println_P(PSTR("done."));
|
||||
} else {
|
||||
unsigned long before = micros();
|
||||
// Load all auto-loaded EEPROM variables
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
{
|
||||
wp_radius = ground_speed * 150 / g.roll_limit.get();
|
||||
//Serial.println(wp_radius, DEC);
|
||||
//cliSerial->println(wp_radius, DEC);
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
ground_start_count = 5;
|
||||
#if X_PLANE == ENABLED
|
||||
Serial.printf_P(PSTR("*** RESET the FPL\n"));
|
||||
cliSerial->printf_P(PSTR("*** RESET the FPL\n"));
|
||||
#endif
|
||||
CH7_wp_index = 1;
|
||||
return;
|
||||
|
|
@ -94,7 +94,7 @@ if (g.ch7_option == CH7_SAVE_WP){ // set to 1
|
|||
set_cmd_with_index(current_loc, CH7_wp_index);
|
||||
|
||||
#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
|
||||
|
||||
// increment index
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@ static void navigate()
|
|||
|
||||
if (wp_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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -18,7 +18,7 @@ MENU(planner_menu, "planner", planner_menu_commands);
|
|||
static int8_t
|
||||
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();
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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_2.control_in,
|
||||
g.rc_3.control_in,
|
||||
|
|
|
|||
|
|
@ -36,7 +36,7 @@ static int8_t
|
|||
setup_mode(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
// Give the user some guidance
|
||||
Serial.printf_P(PSTR("Setup Mode\n"
|
||||
cliSerial->printf_P(PSTR("Setup Mode\n"
|
||||
"\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"
|
||||
|
|
@ -65,7 +65,7 @@ setup_show(uint8_t argc, const Menu::arg *argv)
|
|||
report_ins();
|
||||
report_compass();
|
||||
|
||||
Serial.printf_P(PSTR("Raw Values\n"));
|
||||
cliSerial->printf_P(PSTR("Raw Values\n"));
|
||||
print_divider();
|
||||
|
||||
AP_Param::show_all();
|
||||
|
|
@ -80,16 +80,16 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
|
|||
{
|
||||
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 {
|
||||
c = Serial.read();
|
||||
c = cliSerial->read();
|
||||
} while (-1 == c);
|
||||
|
||||
if (('y' != c) && ('Y' != c))
|
||||
return(-1);
|
||||
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()
|
||||
|
||||
|
|
@ -105,7 +105,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
|
|||
static int8_t
|
||||
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;
|
||||
|
||||
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){
|
||||
while(1){
|
||||
Serial.printf_P(PSTR("\nNo radio; Check connectors."));
|
||||
cliSerial->printf_P(PSTR("\nNo radio; Check connectors."));
|
||||
delay(1000);
|
||||
// stop here
|
||||
}
|
||||
|
|
@ -148,7 +148,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
|
|||
g.rc_7.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){
|
||||
|
||||
delay(20);
|
||||
|
|
@ -165,8 +165,8 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
|
|||
g.rc_7.update_min_max();
|
||||
g.rc_8.update_min_max();
|
||||
|
||||
if(Serial.available() > 0){
|
||||
Serial.flush();
|
||||
if(cliSerial->available() > 0){
|
||||
cliSerial->flush();
|
||||
g.channel_roll.save_eeprom();
|
||||
g.channel_pitch.save_eeprom();
|
||||
g.channel_throttle.save_eeprom();
|
||||
|
|
@ -190,7 +190,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
|||
{
|
||||
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();
|
||||
trim_radio();
|
||||
|
||||
|
|
@ -253,7 +253,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
|||
}
|
||||
|
||||
// escape hatch
|
||||
if(Serial.available() > 0){
|
||||
if(cliSerial->available() > 0){
|
||||
// save changes
|
||||
for (mode=0; mode<6; mode++)
|
||||
flight_modes[mode].save();
|
||||
|
|
@ -278,10 +278,10 @@ setup_erase(uint8_t argc, const Menu::arg *argv)
|
|||
{
|
||||
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 {
|
||||
c = Serial.read();
|
||||
c = cliSerial->read();
|
||||
} while (-1 == c);
|
||||
|
||||
if (('y' != c) && ('Y' != c))
|
||||
|
|
@ -290,11 +290,41 @@ setup_erase(uint8_t argc, const Menu::arg *argv)
|
|||
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
|
||||
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.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();
|
||||
return(0);
|
||||
}
|
||||
|
|
@ -305,7 +335,7 @@ setup_compass(uint8_t argc, const Menu::arg *argv)
|
|||
if (!strcmp_P(argv[1].str, PSTR("on"))) {
|
||||
compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft
|
||||
if (!compass.init()) {
|
||||
Serial.println_P(PSTR("Compass initialisation failed!"));
|
||||
cliSerial->println_P(PSTR("Compass initialisation failed!"));
|
||||
g.compass_enabled = false;
|
||||
} else {
|
||||
g.compass_enabled = true;
|
||||
|
|
@ -317,7 +347,7 @@ setup_compass(uint8_t argc, const Menu::arg *argv)
|
|||
compass.set_offsets(0,0,0);
|
||||
|
||||
} else {
|
||||
Serial.printf_P(PSTR("\nOptions:[on,off,reset]\n"));
|
||||
cliSerial->printf_P(PSTR("\nOptions:[on,off,reset]\n"));
|
||||
report_compass();
|
||||
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);
|
||||
|
||||
} else {
|
||||
Serial.printf_P(PSTR("\nOptions: 3-4"));
|
||||
cliSerial->printf_P(PSTR("\nOptions: 3-4"));
|
||||
}
|
||||
|
||||
report_batt_monitor();
|
||||
|
|
@ -348,17 +378,17 @@ setup_batt_monitor(uint8_t argc, const Menu::arg *argv)
|
|||
static void report_batt_monitor()
|
||||
{
|
||||
//print_blanks(2);
|
||||
Serial.printf_P(PSTR("Batt Mointor\n"));
|
||||
cliSerial->printf_P(PSTR("Batt Mointor\n"));
|
||||
print_divider();
|
||||
if(g.battery_monitoring == 0) Serial.printf_P(PSTR("Batt monitoring disabled"));
|
||||
if(g.battery_monitoring == 3) Serial.printf_P(PSTR("Monitoring batt volts"));
|
||||
if(g.battery_monitoring == 4) Serial.printf_P(PSTR("Monitoring volts and current"));
|
||||
if(g.battery_monitoring == 0) cliSerial->printf_P(PSTR("Batt monitoring disabled"));
|
||||
if(g.battery_monitoring == 3) cliSerial->printf_P(PSTR("Monitoring batt volts"));
|
||||
if(g.battery_monitoring == 4) cliSerial->printf_P(PSTR("Monitoring volts and current"));
|
||||
print_blanks(2);
|
||||
}
|
||||
static void report_radio()
|
||||
{
|
||||
//print_blanks(2);
|
||||
Serial.printf_P(PSTR("Radio\n"));
|
||||
cliSerial->printf_P(PSTR("Radio\n"));
|
||||
print_divider();
|
||||
// radio
|
||||
print_radio_values();
|
||||
|
|
@ -368,28 +398,28 @@ static void report_radio()
|
|||
static void report_gains()
|
||||
{
|
||||
//print_blanks(2);
|
||||
Serial.printf_P(PSTR("Gains\n"));
|
||||
cliSerial->printf_P(PSTR("Gains\n"));
|
||||
print_divider();
|
||||
|
||||
Serial.printf_P(PSTR("servo roll:\n"));
|
||||
cliSerial->printf_P(PSTR("servo roll:\n"));
|
||||
print_PID(&g.pidServoRoll);
|
||||
|
||||
Serial.printf_P(PSTR("servo pitch:\n"));
|
||||
cliSerial->printf_P(PSTR("servo pitch:\n"));
|
||||
print_PID(&g.pidServoPitch);
|
||||
|
||||
Serial.printf_P(PSTR("servo rudder:\n"));
|
||||
cliSerial->printf_P(PSTR("servo rudder:\n"));
|
||||
print_PID(&g.pidServoRudder);
|
||||
|
||||
Serial.printf_P(PSTR("nav roll:\n"));
|
||||
cliSerial->printf_P(PSTR("nav roll:\n"));
|
||||
print_PID(&g.pidNavRoll);
|
||||
|
||||
Serial.printf_P(PSTR("nav pitch airspeed:\n"));
|
||||
cliSerial->printf_P(PSTR("nav pitch airspeed:\n"));
|
||||
print_PID(&g.pidNavPitchAirspeed);
|
||||
|
||||
Serial.printf_P(PSTR("energry throttle:\n"));
|
||||
cliSerial->printf_P(PSTR("energry throttle:\n"));
|
||||
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_blanks(2);
|
||||
|
|
@ -398,10 +428,10 @@ static void report_gains()
|
|||
static void report_xtrack()
|
||||
{
|
||||
//print_blanks(2);
|
||||
Serial.printf_P(PSTR("Crosstrack\n"));
|
||||
cliSerial->printf_P(PSTR("Crosstrack\n"));
|
||||
print_divider();
|
||||
// radio
|
||||
Serial.printf_P(PSTR("XTRACK: %4.2f\n"
|
||||
cliSerial->printf_P(PSTR("XTRACK: %4.2f\n"
|
||||
"XTRACK angle: %d\n"),
|
||||
(float)g.crosstrack_gain,
|
||||
(int)g.crosstrack_entry_angle);
|
||||
|
|
@ -411,10 +441,10 @@ static void report_xtrack()
|
|||
static void report_throttle()
|
||||
{
|
||||
//print_blanks(2);
|
||||
Serial.printf_P(PSTR("Throttle\n"));
|
||||
cliSerial->printf_P(PSTR("Throttle\n"));
|
||||
print_divider();
|
||||
|
||||
Serial.printf_P(PSTR("min: %d\n"
|
||||
cliSerial->printf_P(PSTR("min: %d\n"
|
||||
"max: %d\n"
|
||||
"cruise: %d\n"
|
||||
"failsafe_enabled: %d\n"
|
||||
|
|
@ -430,7 +460,7 @@ static void report_throttle()
|
|||
static void report_ins()
|
||||
{
|
||||
//print_blanks(2);
|
||||
Serial.printf_P(PSTR("INS\n"));
|
||||
cliSerial->printf_P(PSTR("INS\n"));
|
||||
print_divider();
|
||||
|
||||
print_gyro_offsets();
|
||||
|
|
@ -441,20 +471,20 @@ static void report_ins()
|
|||
static void report_compass()
|
||||
{
|
||||
//print_blanks(2);
|
||||
Serial.printf_P(PSTR("Compass: "));
|
||||
cliSerial->printf_P(PSTR("Compass: "));
|
||||
|
||||
switch (compass.product_id) {
|
||||
case AP_COMPASS_TYPE_HMC5883L:
|
||||
Serial.println_P(PSTR("HMC5883L"));
|
||||
cliSerial->println_P(PSTR("HMC5883L"));
|
||||
break;
|
||||
case AP_COMPASS_TYPE_HMC5843:
|
||||
Serial.println_P(PSTR("HMC5843"));
|
||||
cliSerial->println_P(PSTR("HMC5843"));
|
||||
break;
|
||||
case AP_COMPASS_TYPE_HIL:
|
||||
Serial.println_P(PSTR("HIL"));
|
||||
cliSerial->println_P(PSTR("HIL"));
|
||||
break;
|
||||
default:
|
||||
Serial.println_P(PSTR("??"));
|
||||
cliSerial->println_P(PSTR("??"));
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
@ -463,13 +493,13 @@ static void report_compass()
|
|||
print_enabled(g.compass_enabled);
|
||||
|
||||
// mag declination
|
||||
Serial.printf_P(PSTR("Mag Declination: %4.4f\n"),
|
||||
cliSerial->printf_P(PSTR("Mag Declination: %4.4f\n"),
|
||||
degrees(compass.get_declination()));
|
||||
|
||||
Vector3f offsets = compass.get_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.y,
|
||||
offsets.z);
|
||||
|
|
@ -479,7 +509,7 @@ static void report_compass()
|
|||
static void report_flight_modes()
|
||||
{
|
||||
//print_blanks(2);
|
||||
Serial.printf_P(PSTR("Flight modes\n"));
|
||||
cliSerial->printf_P(PSTR("Flight modes\n"));
|
||||
print_divider();
|
||||
|
||||
for(int i = 0; i < 6; i++ ){
|
||||
|
|
@ -495,7 +525,7 @@ static void report_flight_modes()
|
|||
static void
|
||||
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->kI(),
|
||||
pid->kD(),
|
||||
|
|
@ -505,28 +535,28 @@ print_PID(PID * pid)
|
|||
static void
|
||||
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);
|
||||
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);
|
||||
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);
|
||||
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);
|
||||
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);
|
||||
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);
|
||||
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);
|
||||
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("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("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("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("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("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("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("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("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
|
||||
print_switch(byte p, byte m)
|
||||
{
|
||||
Serial.printf_P(PSTR("Pos %d: "),p);
|
||||
Serial.println(flight_mode_strings[m]);
|
||||
cliSerial->printf_P(PSTR("Pos %d: "),p);
|
||||
cliSerial->println(flight_mode_strings[m]);
|
||||
}
|
||||
|
||||
static void
|
||||
print_done()
|
||||
{
|
||||
Serial.printf_P(PSTR("\nSaved Settings\n\n"));
|
||||
cliSerial->printf_P(PSTR("\nSaved Settings\n\n"));
|
||||
}
|
||||
|
||||
static void
|
||||
|
|
@ -534,7 +564,7 @@ print_blanks(int num)
|
|||
{
|
||||
while(num > 0){
|
||||
num--;
|
||||
Serial.println("");
|
||||
cliSerial->println("");
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -542,9 +572,9 @@ static void
|
|||
print_divider(void)
|
||||
{
|
||||
for (int i = 0; i < 40; i++) {
|
||||
Serial.printf_P(PSTR("-"));
|
||||
cliSerial->printf_P(PSTR("-"));
|
||||
}
|
||||
Serial.println("");
|
||||
cliSerial->println("");
|
||||
}
|
||||
|
||||
static int8_t
|
||||
|
|
@ -577,20 +607,20 @@ radio_input_switch(void)
|
|||
static void zero_eeprom(void)
|
||||
{
|
||||
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++) {
|
||||
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)
|
||||
{
|
||||
if(b)
|
||||
Serial.printf_P(PSTR("en"));
|
||||
cliSerial->printf_P(PSTR("en"));
|
||||
else
|
||||
Serial.printf_P(PSTR("dis"));
|
||||
Serial.printf_P(PSTR("abled\n"));
|
||||
cliSerial->printf_P(PSTR("dis"));
|
||||
cliSerial->printf_P(PSTR("abled\n"));
|
||||
}
|
||||
|
||||
static void
|
||||
|
|
@ -598,7 +628,7 @@ print_accel_offsets_and_scaling(void)
|
|||
{
|
||||
Vector3f accel_offsets = ins.get_accel_offsets();
|
||||
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.y, // Roll
|
||||
(float)accel_offsets.z, // YAW
|
||||
|
|
@ -610,7 +640,7 @@ print_accel_offsets_and_scaling(void)
|
|||
static 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.gy(),
|
||||
(float)ins.gz());
|
||||
|
|
|
|||
|
|
@ -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
|
||||
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"
|
||||
" setup setup 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);
|
||||
|
||||
// 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
|
||||
timer_scheduler.set_failsafe(NULL);
|
||||
|
||||
cliSerial = port;
|
||||
Menu::set_port(port);
|
||||
port->set_blocking_writes(true);
|
||||
|
||||
while (1) {
|
||||
main_menu.run();
|
||||
}
|
||||
|
|
@ -89,7 +93,7 @@ static void init_ardupilot()
|
|||
// XXX This could be optimised to reduce the buffer sizes in the cases
|
||||
// where they are not otherwise required.
|
||||
//
|
||||
Serial.begin(SERIAL0_BAUD, 128, 128);
|
||||
cliSerial->begin(SERIAL0_BAUD, 128, 128);
|
||||
|
||||
// GPS serial port.
|
||||
//
|
||||
|
|
@ -103,7 +107,7 @@ static void init_ardupilot()
|
|||
// standard gps running
|
||||
Serial1.begin(115200, 128, 16);
|
||||
|
||||
Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE
|
||||
cliSerial->printf_P(PSTR("\n\nInit " THISFIRMWARE
|
||||
"\n\nFree RAM: %u\n"),
|
||||
memcheck_available_memory());
|
||||
|
||||
|
|
@ -149,7 +153,7 @@ static void init_ardupilot()
|
|||
if (!usb_connected) {
|
||||
// we are not connected via USB, re-init UART0 with right
|
||||
// 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
|
||||
// we have a 2nd serial port for telemetry
|
||||
|
|
@ -185,7 +189,7 @@ static void init_ardupilot()
|
|||
if (g.compass_enabled==true) {
|
||||
compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft
|
||||
if (!compass.init()|| !compass.read()) {
|
||||
Serial.println_P(PSTR("Compass initialisation failed!"));
|
||||
cliSerial->println_P(PSTR("Compass initialisation failed!"));
|
||||
g.compass_enabled = false;
|
||||
} else {
|
||||
ahrs.set_compass(&compass);
|
||||
|
|
@ -199,7 +203,7 @@ static void init_ardupilot()
|
|||
// I2c.setSpeed(true);
|
||||
|
||||
if (!compass.init()) {
|
||||
Serial.println("compass initialisation failed!");
|
||||
cliSerial->println("compass initialisation failed!");
|
||||
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_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 ) {
|
||||
case AP_COMPASS_TYPE_HIL:
|
||||
Serial.println("HIL");
|
||||
cliSerial->println("HIL");
|
||||
break;
|
||||
case AP_COMPASS_TYPE_HMC5843:
|
||||
Serial.println("HMC5843");
|
||||
cliSerial->println("HMC5843");
|
||||
break;
|
||||
case AP_COMPASS_TYPE_HMC5883L:
|
||||
Serial.println("HMC5883L");
|
||||
cliSerial->println("HMC5883L");
|
||||
break;
|
||||
default:
|
||||
Serial.println("unknown");
|
||||
cliSerial->println("unknown");
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
@ -276,17 +280,21 @@ static void init_ardupilot()
|
|||
#if CLI_ENABLED == ENABLED && CLI_SLIDER_ENABLED == ENABLED
|
||||
if (digitalRead(SLIDE_SWITCH_PIN) == 0) {
|
||||
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"
|
||||
"\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"
|
||||
"Visit the 'setup' menu for first-time configuration.\n"));
|
||||
Serial.println_P(PSTR("\nMove the slide switch and reset to FLY.\n"));
|
||||
run_cli();
|
||||
cliSerial->println_P(PSTR("\nMove the slide switch and reset to FLY.\n"));
|
||||
run_cli(&cliSerial);
|
||||
}
|
||||
#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
|
||||
|
||||
// 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 115: return 115200;
|
||||
}
|
||||
Serial.println_P(PSTR("Invalid SERIAL3_BAUD"));
|
||||
cliSerial->println_P(PSTR("Invalid SERIAL3_BAUD"));
|
||||
return default_baud;
|
||||
}
|
||||
|
||||
|
|
@ -542,9 +550,9 @@ static void check_usb_mux(void)
|
|||
// the user has switched to/from the telemetry port
|
||||
usb_connected = usb_check;
|
||||
if (usb_connected) {
|
||||
Serial.begin(SERIAL0_BAUD, 128, 128);
|
||||
cliSerial->begin(SERIAL0_BAUD, 128, 128);
|
||||
} else {
|
||||
Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
|
||||
cliSerial->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -78,14 +78,14 @@ MENU(test_menu, "test", test_menu_commands);
|
|||
static int8_t
|
||||
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();
|
||||
return 0;
|
||||
}
|
||||
|
||||
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
|
||||
|
|
@ -95,10 +95,10 @@ test_eedump(uint8_t argc, const Menu::arg *argv)
|
|||
|
||||
// hexdump the EEPROM
|
||||
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++)
|
||||
Serial.printf_P(PSTR(" %02x"), eeprom_read_byte((const uint8_t *)(i + j)));
|
||||
Serial.println();
|
||||
cliSerial->printf_P(PSTR(" %02x"), eeprom_read_byte((const uint8_t *)(i + j)));
|
||||
cliSerial->println();
|
||||
}
|
||||
return(0);
|
||||
}
|
||||
|
|
@ -116,7 +116,7 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
|
|||
// ----------------------------------------------------------
|
||||
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_pitch.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_8.radio_in);
|
||||
|
||||
if(Serial.available() > 0){
|
||||
if(cliSerial->available() > 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)
|
||||
if (APM_RC.GetState() == 1){
|
||||
Serial.print("CH:");
|
||||
cliSerial->print("CH:");
|
||||
for(int i = 0; i < 8; i++){
|
||||
Serial.print(APM_RC.InputCh(i)); // Print channel values
|
||||
Serial.print(",");
|
||||
cliSerial->print(APM_RC.InputCh(i)); // Print channel values
|
||||
cliSerial->print(",");
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
|
@ -166,7 +166,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
|
|||
delay(1000);
|
||||
|
||||
#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);
|
||||
#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);
|
||||
|
||||
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_pitch.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,
|
||||
tuning_value);
|
||||
|
||||
if(Serial.available() > 0){
|
||||
if(cliSerial->available() > 0){
|
||||
return (0);
|
||||
}
|
||||
}
|
||||
|
|
@ -223,7 +223,7 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
|
|||
|
||||
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){
|
||||
delay(20);
|
||||
read_radio();
|
||||
|
|
@ -234,27 +234,27 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
|
|||
read_radio();
|
||||
|
||||
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++;
|
||||
}
|
||||
|
||||
if(oldSwitchPosition != readSwitch()){
|
||||
Serial.printf_P(PSTR("CONTROL MODE CHANGED: "));
|
||||
Serial.println(flight_mode_strings[readSwitch()]);
|
||||
cliSerial->printf_P(PSTR("CONTROL MODE CHANGED: "));
|
||||
cliSerial->println(flight_mode_strings[readSwitch()]);
|
||||
fail_test++;
|
||||
}
|
||||
|
||||
if(g.throttle_fs_enabled && g.channel_throttle.get_failsafe()){
|
||||
Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.channel_throttle.radio_in);
|
||||
Serial.println(flight_mode_strings[readSwitch()]);
|
||||
cliSerial->printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.channel_throttle.radio_in);
|
||||
cliSerial->println(flight_mode_strings[readSwitch()]);
|
||||
fail_test++;
|
||||
}
|
||||
|
||||
if(fail_test > 0){
|
||||
return (0);
|
||||
}
|
||||
if(Serial.available() > 0){
|
||||
Serial.printf_P(PSTR("LOS caused no change in APM.\n"));
|
||||
if(cliSerial->available() > 0){
|
||||
cliSerial->printf_P(PSTR("LOS caused no change in APM.\n"));
|
||||
return (0);
|
||||
}
|
||||
}
|
||||
|
|
@ -272,12 +272,12 @@ if (g.battery_monitoring == 3 || g.battery_monitoring == 4) {
|
|||
read_radio();
|
||||
read_battery();
|
||||
if (g.battery_monitoring == 3){
|
||||
Serial.printf_P(PSTR("V: %4.4f\n"),
|
||||
cliSerial->printf_P(PSTR("V: %4.4f\n"),
|
||||
battery_voltage1,
|
||||
current_amps1,
|
||||
current_total1);
|
||||
} 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,
|
||||
current_amps1,
|
||||
current_total1);
|
||||
|
|
@ -287,12 +287,12 @@ if (g.battery_monitoring == 3 || g.battery_monitoring == 4) {
|
|||
// ------------------------------
|
||||
set_servos();
|
||||
|
||||
if(Serial.available() > 0){
|
||||
if(cliSerial->available() > 0){
|
||||
return (0);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
Serial.printf_P(PSTR("Not enabled\n"));
|
||||
cliSerial->printf_P(PSTR("Not enabled\n"));
|
||||
return (0);
|
||||
}
|
||||
|
||||
|
|
@ -305,17 +305,17 @@ test_relay(uint8_t argc, const Menu::arg *argv)
|
|||
delay(1000);
|
||||
|
||||
while(1){
|
||||
Serial.printf_P(PSTR("Relay on\n"));
|
||||
cliSerial->printf_P(PSTR("Relay on\n"));
|
||||
relay.on();
|
||||
delay(3000);
|
||||
if(Serial.available() > 0){
|
||||
if(cliSerial->available() > 0){
|
||||
return (0);
|
||||
}
|
||||
|
||||
Serial.printf_P(PSTR("Relay off\n"));
|
||||
cliSerial->printf_P(PSTR("Relay off\n"));
|
||||
relay.off();
|
||||
delay(3000);
|
||||
if(Serial.available() > 0){
|
||||
if(cliSerial->available() > 0){
|
||||
return (0);
|
||||
}
|
||||
}
|
||||
|
|
@ -326,9 +326,9 @@ test_wp(uint8_t argc, const Menu::arg *argv)
|
|||
{
|
||||
delay(1000);
|
||||
|
||||
Serial.printf_P(PSTR("%d waypoints\n"), (int)g.command_total);
|
||||
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);
|
||||
cliSerial->printf_P(PSTR("%d waypoints\n"), (int)g.command_total);
|
||||
cliSerial->printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius);
|
||||
cliSerial->printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius);
|
||||
|
||||
for(byte i = 0; i <= g.command_total; i++){
|
||||
struct Location temp = get_cmd_with_index(i);
|
||||
|
|
@ -341,7 +341,7 @@ test_wp(uint8_t argc, const Menu::arg *argv)
|
|||
static void
|
||||
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)cmd->id,
|
||||
(int)cmd->options,
|
||||
|
|
@ -356,14 +356,14 @@ test_xbee(uint8_t argc, const Menu::arg *argv)
|
|||
{
|
||||
print_hit_enter();
|
||||
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){
|
||||
|
||||
if (Serial3.available())
|
||||
Serial3.write(Serial3.read());
|
||||
|
||||
if(Serial.available() > 0){
|
||||
if(cliSerial->available() > 0){
|
||||
return (0);
|
||||
}
|
||||
}
|
||||
|
|
@ -376,18 +376,18 @@ test_modeswitch(uint8_t argc, const Menu::arg *argv)
|
|||
print_hit_enter();
|
||||
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){
|
||||
delay(20);
|
||||
byte switchPosition = readSwitch();
|
||||
if (oldSwitchPosition != switchPosition){
|
||||
Serial.printf_P(PSTR("Position %d\n"), switchPosition);
|
||||
cliSerial->printf_P(PSTR("Position %d\n"), switchPosition);
|
||||
oldSwitchPosition = switchPosition;
|
||||
}
|
||||
if(Serial.available() > 0){
|
||||
if(cliSerial->available() > 0){
|
||||
return (0);
|
||||
}
|
||||
}
|
||||
|
|
@ -399,20 +399,20 @@ test_modeswitch(uint8_t argc, const Menu::arg *argv)
|
|||
static int8_t
|
||||
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()) {
|
||||
Serial.println_P(PSTR("ERR: No dataflash inserted"));
|
||||
cliSerial->println_P(PSTR("ERR: No dataflash inserted"));
|
||||
return 0;
|
||||
}
|
||||
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_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_PageSize);
|
||||
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);
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -425,7 +425,7 @@ test_dipswitches(uint8_t argc, const Menu::arg *argv)
|
|||
delay(1000);
|
||||
|
||||
if (!g.switch_enable) {
|
||||
Serial.println_P(PSTR("dip switches disabled, using EEPROM"));
|
||||
cliSerial->println_P(PSTR("dip switches disabled, using EEPROM"));
|
||||
}
|
||||
|
||||
while(1){
|
||||
|
|
@ -433,17 +433,17 @@ test_dipswitches(uint8_t argc, const Menu::arg *argv)
|
|||
update_servo_switches();
|
||||
|
||||
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_pitch.get_reverse(),
|
||||
(int)g.channel_rudder.get_reverse());
|
||||
} 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_ch1_elevon,
|
||||
(int)g.reverse_ch2_elevon);
|
||||
}
|
||||
if(Serial.available() > 0){
|
||||
if(cliSerial->available() > 0){
|
||||
return (0);
|
||||
}
|
||||
}
|
||||
|
|
@ -463,14 +463,14 @@ test_adc(uint8_t argc, const Menu::arg *argv)
|
|||
print_hit_enter();
|
||||
adc.Init(&timer_scheduler);
|
||||
delay(1000);
|
||||
Serial.printf_P(PSTR("ADC\n"));
|
||||
cliSerial->printf_P(PSTR("ADC\n"));
|
||||
delay(1000);
|
||||
|
||||
while(1){
|
||||
for (int i=0;i<9;i++) Serial.printf_P(PSTR("%.1f\t"),adc.Ch(i));
|
||||
Serial.println();
|
||||
for (int i=0;i<9;i++) cliSerial->printf_P(PSTR("%.1f\t"),adc.Ch(i));
|
||||
cliSerial->println();
|
||||
delay(100);
|
||||
if(Serial.available() > 0){
|
||||
if(cliSerial->available() > 0){
|
||||
return (0);
|
||||
}
|
||||
}
|
||||
|
|
@ -493,15 +493,15 @@ test_gps(uint8_t argc, const Menu::arg *argv)
|
|||
g_gps->update();
|
||||
|
||||
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->longitude,
|
||||
g_gps->altitude/100,
|
||||
g_gps->num_sats);
|
||||
}else{
|
||||
Serial.printf_P(PSTR("."));
|
||||
cliSerial->printf_P(PSTR("."));
|
||||
}
|
||||
if(Serial.available() > 0){
|
||||
if(cliSerial->available() > 0){
|
||||
return (0);
|
||||
}
|
||||
}
|
||||
|
|
@ -510,7 +510,7 @@ test_gps(uint8_t argc, const Menu::arg *argv)
|
|||
static int8_t
|
||||
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);
|
||||
ahrs.reset();
|
||||
|
||||
|
|
@ -540,14 +540,14 @@ test_ins(uint8_t argc, const Menu::arg *argv)
|
|||
// ---------------------
|
||||
Vector3f gyros = ins.get_gyro();
|
||||
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.pitch_sensor / 100,
|
||||
(uint16_t)ahrs.yaw_sensor / 100,
|
||||
gyros.x, gyros.y, gyros.z,
|
||||
accels.x, accels.y, accels.z);
|
||||
}
|
||||
if(Serial.available() > 0){
|
||||
if(cliSerial->available() > 0){
|
||||
return (0);
|
||||
}
|
||||
}
|
||||
|
|
@ -558,14 +558,14 @@ static int8_t
|
|||
test_mag(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
if (!g.compass_enabled) {
|
||||
Serial.printf_P(PSTR("Compass: "));
|
||||
cliSerial->printf_P(PSTR("Compass: "));
|
||||
print_enabled(false);
|
||||
return (0);
|
||||
}
|
||||
|
||||
compass.set_orientation(MAG_ORIENTATION);
|
||||
if (!compass.init()) {
|
||||
Serial.println_P(PSTR("Compass initialisation failed!"));
|
||||
cliSerial->println_P(PSTR("Compass initialisation failed!"));
|
||||
return 0;
|
||||
}
|
||||
ahrs.set_compass(&compass);
|
||||
|
|
@ -578,7 +578,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
|
|||
int counter = 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();
|
||||
|
||||
|
|
@ -608,7 +608,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
|
|||
if (counter>20) {
|
||||
if (compass.healthy) {
|
||||
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,
|
||||
(int)compass.mag_x,
|
||||
(int)compass.mag_y,
|
||||
|
|
@ -617,19 +617,19 @@ test_mag(uint8_t argc, const Menu::arg *argv)
|
|||
maggy.y,
|
||||
maggy.z);
|
||||
} else {
|
||||
Serial.println_P(PSTR("compass not healthy"));
|
||||
cliSerial->println_P(PSTR("compass not healthy"));
|
||||
}
|
||||
counter=0;
|
||||
}
|
||||
}
|
||||
if (Serial.available() > 0) {
|
||||
if (cliSerial->available() > 0) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// save offsets. This allows you to get sane offset values using
|
||||
// the CLI before you go flying.
|
||||
Serial.println_P(PSTR("saving offsets"));
|
||||
cliSerial->println_P(PSTR("saving offsets"));
|
||||
compass.save_offsets();
|
||||
return (0);
|
||||
}
|
||||
|
|
@ -658,7 +658,7 @@ test_rawgps(uint8_t argc, const Menu::arg *argv)
|
|||
Serial3.write(Serial1.read());
|
||||
digitalWrite(C_LED_PIN, LED_OFF);
|
||||
}
|
||||
if(Serial.available() > 0){
|
||||
if(cliSerial->available() > 0){
|
||||
return (0);
|
||||
}
|
||||
}
|
||||
|
|
@ -678,9 +678,9 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
|
|||
if(g.sonar_enabled){
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue