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

APM: make it possible to run the CLI on any serial port

This commit is contained in:
Andrew Tridgell 2012-11-21 16:19:16 +11:00
parent 11141d0af0
commit 84300c85d6
10 changed files with 241 additions and 220 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ArduPlane/ArduPlane.pde
View File

@ -91,6 +91,9 @@ FastSerialPort2(Serial3);
FastSerialPort3(Serial3); // Telemetry port for APM1
#endif
// port to use for command line interface
static FastSerial *cliSerial = &Serial;
// 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
@ -828,15 +831,6 @@ static void medium_loop()
ahrs.set_compass(NULL);
}
#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"));
* 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);
* }*/
break;

4
ArduPlane/GCS_Mavlink.pde
View File

@ -797,7 +797,7 @@ GCS_MAVLINK::update(void)
crlf_count = 0;
}
if (crlf_count == 3) {
run_cli();
run_cli(_port);
}
}
#endif
@ -1868,8 +1868,6 @@ mission_failed:
// 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);
// rad/sec
Vector3f gyros;

90
ArduPlane/Log.pde
View File

@ -21,7 +21,7 @@ static int8_t select_logs(uint8_t argc, const Menu::arg *argv);
// 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"
@ -55,16 +55,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);
@ -78,24 +78,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"), (int)num_logs);
cliSerial->printf_P(PSTR("\n%d logs\n"), (int)num_logs);
for(int16_t i=num_logs; i>=1; i--) {
int16_t 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"), (int)temp, (int)log_start, (int)log_end);
cliSerial->printf_P(PSTR("Log %d, start %d, end %d\n"), (int)temp, (int)log_start, (int)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);
}
@ -115,28 +115,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"), (int)count);
Serial.printf_P(PSTR("%d,\t"), (int)DataFlash.GetFileNumber());
Serial.printf_P(PSTR("%d\n"), (int)DataFlash.GetFilePage());
cliSerial->printf_P(PSTR("DF page, log file #, log page: %d,\t"), (int)count);
cliSerial->printf_P(PSTR("%d,\t"), (int)DataFlash.GetFileNumber());
cliSerial->printf_P(PSTR("%d\n"), (int)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"),
(int)dump_log,
(int)dump_log_start,
(int)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;
}
@ -162,7 +162,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);
}
@ -387,7 +387,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"),
(int)DataFlash.ReadInt(),
((float)DataFlash.ReadInt() / 100.f),
((float)DataFlash.ReadInt() / 100.f),
@ -399,15 +399,15 @@ static void Log_Read_Control_Tuning()
{
float logvar;
Serial.printf_P(PSTR("CTUN:"));
cliSerial->printf_P(PSTR("CTUN:"));
for (int16_t y = 1; y < 10; y++) {
logvar = DataFlash.ReadInt();
if(y < 8) logvar = logvar/100.f;
if(y == 9) logvar = logvar/10000.f;
Serial.print(logvar);
cliSerial->print(logvar);
print_comma();
}
Serial.println();
cliSerial->println();
}
// Read a nav tuning packet
@ -417,7 +417,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,
(int)d[1],
((uint16_t)d[2])/100.0,
@ -433,9 +433,9 @@ static void Log_Read_Performance()
int32_t pm_time;
int16_t logvar;
Serial.printf_P(PSTR("PM:"));
cliSerial->printf_P(PSTR("PM:"));
pm_time = DataFlash.ReadLong();
Serial.print(pm_time);
cliSerial->print(pm_time);
print_comma();
for (int16_t y = 1; y <= 12; y++) {
if(y < 3 || y > 7) {
@ -443,10 +443,10 @@ static void Log_Read_Performance()
}else{
logvar = DataFlash.ReadByte();
}
Serial.print(logvar);
cliSerial->print(logvar);
print_comma();
}
Serial.println();
cliSerial->println();
}
// Read a command processing packet
@ -455,18 +455,18 @@ static void Log_Read_Cmd()
byte logvarb;
int32_t logvarl;
Serial.printf_P(PSTR("CMD:"));
cliSerial->printf_P(PSTR("CMD:"));
for(int16_t i = 1; i < 4; i++) {
logvarb = DataFlash.ReadByte();
Serial.print(logvarb, DEC);
cliSerial->print(logvarb, DEC);
print_comma();
}
for(int16_t i = 1; i < 4; i++) {
logvarl = DataFlash.ReadLong();
Serial.print(logvarl, DEC);
cliSerial->print(logvarl, DEC);
print_comma();
}
Serial.println();
cliSerial->println();
}
static void Log_Read_Startup()
@ -474,13 +474,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
@ -490,7 +490,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"),
(int)d[0], (int)d[1],
(unsigned)d[2]);
}
@ -498,7 +498,7 @@ static void Log_Read_Attitude()
// Read a mode packet
static void Log_Read_Mode()
{
Serial.printf_P(PSTR("MOD:"));
cliSerial->printf_P(PSTR("MOD:"));
print_flight_mode(DataFlash.ReadByte());
}
@ -518,7 +518,7 @@ static void Log_Read_GPS()
l[4] = DataFlash.ReadLong();
l[5] = DataFlash.ReadLong();
l[6] = DataFlash.ReadLong();
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,
@ -529,13 +529,13 @@ static void Log_Read_GPS()
static void Log_Read_Raw()
{
float logvar;
Serial.printf_P(PSTR("RAW:"));
cliSerial->printf_P(PSTR("RAW:"));
for (int16_t y = 0; y < 6; y++) {
logvar = (float)DataFlash.ReadLong() / t7;
Serial.print(logvar);
cliSerial->print(logvar);
print_comma();
}
Serial.println();
cliSerial->println();
}
// Read the DataFlash log memory : Packet Parser
@ -544,9 +544,9 @@ static void Log_Read(int16_t start_page, int16_t end_page)
int16_t 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());
@ -558,7 +558,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"), (int) packet_count);
cliSerial->printf_P(PSTR("Number of packets read: %d\n"), (int) packet_count);
}
// Read the DataFlash log memory : Packet Parser
@ -626,7 +626,7 @@ static int16_t 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...
}
}
@ -635,7 +635,7 @@ static int16_t 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"),(int)data);
cliSerial->printf_P(PSTR("Error Reading END_BYTE: %d\n"),(int)data);
}
log_step = 0; // Restart sequence: new packet...
break;

6
ArduPlane/Parameters.pde
View File

@ -699,17 +699,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 {
uint32_t 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);
}
}

3
ArduPlane/commands_logic.pde
View File

@ -244,10 +244,7 @@ static void do_takeoff()
set_next_WP(&next_nav_command);
// pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
takeoff_pitch_cd = (int)next_nav_command.p1 * 100;
//Serial.printf_P(PSTR("TO pitch:")); Serial.println(takeoff_pitch);
//Serial.printf_P(PSTR("home.alt:")); Serial.println(home.alt);
takeoff_altitude = next_nav_command.alt;
//Serial.printf_P(PSTR("takeoff_altitude:")); Serial.println(takeoff_altitude);
next_WP.lat = home.lat + 1000; // so we don't have bad calcs
next_WP.lng = home.lng + 1000; // so we don't have bad calcs
takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction

1
ArduPlane/navigation.pde
View File

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

2
ArduPlane/radio.pde
View File

@ -108,7 +108,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"),
* (int)g.rc_1.control_in,
* (int)g.rc_2.control_in,
* (int)g.rc_3.control_in,

157
ArduPlane/setup.pde
View File

@ -40,7 +40,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"
@ -69,7 +69,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();
@ -84,16 +84,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()
@ -109,7 +109,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++) {
@ -120,7 +120,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
}
@ -152,7 +152,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);
@ -169,8 +169,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();
@ -194,7 +194,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();
@ -257,7 +257,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();
@ -282,10 +282,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))
@ -302,13 +302,38 @@ setup_level(uint8_t argc, const Menu::arg *argv)
}
#if !defined( __AVR_ATmega1280__ )
/*
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);
if (ins.calibrate_accel(delay, flash_leds, NULL)) {
if (ins.calibrate_accel(delay, flash_leds, setup_printf_P, setup_wait_key)) {
if (g.manual_level == 0) {
Serial.println_P(PSTR("Setting MANUAL_LEVEL to 1"));
cliSerial->println_P(PSTR("Setting MANUAL_LEVEL to 1"));
g.manual_level.set_and_save(1);
}
}
@ -323,7 +348,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;
@ -335,7 +360,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;
}
@ -352,7 +377,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();
@ -366,17 +391,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();
@ -386,30 +411,30 @@ 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();
#if APM_CONTROL == DISABLED
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);
#endif
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);
@ -418,10 +443,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);
@ -431,10 +456,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"
@ -450,7 +475,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();
@ -461,20 +486,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;
}
@ -483,13 +508,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);
@ -499,7 +524,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(int16_t i = 0; i < 6; i++ ) {
@ -515,7 +540,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(),
@ -525,28 +550,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);
cliSerial->printf_P(PSTR("Pos %d: "),p);
print_flight_mode(m);
}
static void
print_done()
{
Serial.printf_P(PSTR("\nSaved Settings\n\n"));
cliSerial->printf_P(PSTR("\nSaved Settings\n\n"));
}
static void
@ -554,7 +579,7 @@ print_blanks(int16_t num)
{
while(num > 0) {
num--;
Serial.println();
cliSerial->println();
}
}
@ -562,9 +587,9 @@ static void
print_divider(void)
{
for (int16_t i = 0; i < 40; i++) {
Serial.printf_P(PSTR("-"));
cliSerial->printf_P(PSTR("-"));
}
Serial.println();
cliSerial->println();
}
static int8_t
@ -597,20 +622,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
@ -618,7 +643,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
@ -631,7 +656,7 @@ static void
print_gyro_offsets(void)
{
Vector3f gyro_offsets = ins.get_gyro_offsets();
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)gyro_offsets.x,
(float)gyro_offsets.y,
(float)gyro_offsets.z);

42
ArduPlane/system.pde
View File

@ -18,7 +18,7 @@ static int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in te
// 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"
@ -42,11 +42,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();
}
@ -86,7 +90,7 @@ static void init_ardupilot()
// standard gps running
Serial1.begin(38400, 256, 16);
Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE
cliSerial->printf_P(PSTR("\n\nInit " THISFIRMWARE
"\n\nFree RAM: %u\n"),
memcheck_available_memory());
@ -169,7 +173,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);
@ -220,7 +224,11 @@ static void init_ardupilot()
*/
timer_scheduler.set_failsafe(failsafe_check);
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
if (ENABLE_AIR_START == 1) {
// Perform an air start and get back to flying
@ -504,7 +512,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;
}
@ -554,7 +562,7 @@ uint16_t board_voltage(void)
*/
static void reboot_apm(void)
{
Serial.println_P(PSTR("REBOOTING"));
cliSerial->println_P(PSTR("REBOOTING"));
delay(100);
// see http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1250663814/
// for the method
@ -570,36 +578,36 @@ print_flight_mode(uint8_t mode)
{
switch (mode) {
case MANUAL:
Serial.println_P(PSTR("Manual"));
cliSerial->println_P(PSTR("Manual"));
break;
case CIRCLE:
Serial.println_P(PSTR("Circle"));
cliSerial->println_P(PSTR("Circle"));
break;
case STABILIZE:
Serial.println_P(PSTR("Stabilize"));
cliSerial->println_P(PSTR("Stabilize"));
break;
case FLY_BY_WIRE_A:
Serial.println_P(PSTR("FBW_A"));
cliSerial->println_P(PSTR("FBW_A"));
break;
case FLY_BY_WIRE_B:
Serial.println_P(PSTR("FBW_B"));
cliSerial->println_P(PSTR("FBW_B"));
break;
case AUTO:
Serial.println_P(PSTR("AUTO"));
cliSerial->println_P(PSTR("AUTO"));
break;
case RTL:
Serial.println_P(PSTR("RTL"));
cliSerial->println_P(PSTR("RTL"));
break;
case LOITER:
Serial.println_P(PSTR("Loiter"));
cliSerial->println_P(PSTR("Loiter"));
break;
default:
Serial.println_P(PSTR("---"));
cliSerial->println_P(PSTR("---"));
break;
}
}
static void print_comma(void)
{
Serial.print_P(PSTR(","));
cliSerial->print_P(PSTR(","));
}

144
ArduPlane/test.pde
View File

@ -70,14 +70,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
@ -87,10 +87,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);
}
@ -108,7 +108,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"),
(int)g.channel_roll.radio_in,
(int)g.channel_pitch.radio_in,
(int)g.channel_throttle.radio_in,
@ -118,7 +118,7 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
(int)g.rc_7.radio_in,
(int)g.rc_8.radio_in);
if(Serial.available() > 0) {
if(cliSerial->available() > 0) {
return (0);
}
}
@ -136,15 +136,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_P(PSTR("CH:"));
cliSerial->print_P(PSTR("CH:"));
for(int16_t i = 0; i < 8; i++) {
Serial.print(APM_RC.InputCh(i)); // Print channel values
cliSerial->print(APM_RC.InputCh(i)); // Print channel values
print_comma();
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);
}
}
@ -174,7 +174,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
// ------------------------------
set_servos();
Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
cliSerial->printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
(int)g.channel_roll.control_in,
(int)g.channel_pitch.control_in,
(int)g.channel_throttle.control_in,
@ -184,7 +184,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
(int)g.rc_7.control_in,
(int)g.rc_8.control_in);
if(Serial.available() > 0) {
if(cliSerial->available() > 0) {
return (0);
}
}
@ -206,7 +206,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();
@ -217,18 +217,18 @@ 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"), (int)g.channel_throttle.control_in);
cliSerial->printf_P(PSTR("THROTTLE CHANGED %d \n"), (int)g.channel_throttle.control_in);
fail_test++;
}
if(oldSwitchPosition != readSwitch()) {
Serial.printf_P(PSTR("CONTROL MODE CHANGED: "));
cliSerial->printf_P(PSTR("CONTROL MODE CHANGED: "));
print_flight_mode(readSwitch());
fail_test++;
}
if(g.throttle_fs_enabled && g.channel_throttle.get_failsafe()) {
Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), (int)g.channel_throttle.radio_in);
cliSerial->printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), (int)g.channel_throttle.radio_in);
print_flight_mode(readSwitch());
fail_test++;
}
@ -236,8 +236,8 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
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);
}
}
@ -255,12 +255,12 @@ test_battery(uint8_t argc, const Menu::arg *argv)
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);
@ -270,12 +270,12 @@ test_battery(uint8_t argc, const Menu::arg *argv)
// ------------------------------
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);
}
@ -288,17 +288,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);
}
}
@ -311,14 +311,14 @@ test_wp(uint8_t argc, const Menu::arg *argv)
// save the alitude above home option
if (g.RTL_altitude_cm < 0) {
Serial.printf_P(PSTR("Hold current altitude\n"));
cliSerial->printf_P(PSTR("Hold current altitude\n"));
}else{
Serial.printf_P(PSTR("Hold altitude of %dm\n"), (int)g.RTL_altitude_cm/100);
cliSerial->printf_P(PSTR("Hold altitude of %dm\n"), (int)g.RTL_altitude_cm/100);
}
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);
@ -331,7 +331,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,
@ -346,14 +346,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);
}
}
@ -366,18 +366,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"), (int)switchPosition);
cliSerial->printf_P(PSTR("Position %d\n"), (int)switchPosition);
oldSwitchPosition = switchPosition;
}
if(Serial.available() > 0) {
if(cliSerial->available() > 0) {
return (0);
}
}
@ -389,20 +389,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;
}
@ -419,14 +419,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 (int16_t i=0; i<9; i++) Serial.printf_P(PSTR("%.1f\t"),adc.Ch(i));
Serial.println();
for (int16_t 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);
}
}
@ -449,15 +449,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"),
(long)g_gps->latitude,
(long)g_gps->longitude,
(long)g_gps->altitude/100,
(int)g_gps->num_sats);
}else{
Serial.printf_P(PSTR("."));
cliSerial->printf_P(PSTR("."));
}
if(Serial.available() > 0) {
if(cliSerial->available() > 0) {
return (0);
}
}
@ -466,7 +466,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();
@ -496,14 +496,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);
}
}
@ -514,14 +514,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);
@ -534,7 +534,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
int16_t 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();
@ -564,7 +564,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_cd(ToDeg(heading) * 100)) /100,
(int)compass.mag_x,
(int)compass.mag_y,
@ -573,19 +573,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);
}
@ -601,26 +601,26 @@ static int8_t
test_airspeed(uint8_t argc, const Menu::arg *argv)
{
float airspeed_ch = pitot_analog_source.read();
// Serial.println(pitot_analog_source.read());
Serial.printf_P(PSTR("airspeed_ch: %.1f\n"), airspeed_ch);
// cliSerial->println(pitot_analog_source.read());
cliSerial->printf_P(PSTR("airspeed_ch: %.1f\n"), airspeed_ch);
if (!airspeed.enabled()) {
Serial.printf_P(PSTR("airspeed: "));
cliSerial->printf_P(PSTR("airspeed: "));
print_enabled(false);
return (0);
}else{
print_hit_enter();
zero_airspeed();
Serial.printf_P(PSTR("airspeed: "));
cliSerial->printf_P(PSTR("airspeed: "));
print_enabled(true);
while(1) {
delay(20);
read_airspeed();
Serial.printf_P(PSTR("%.1f m/s\n"), airspeed.get_airspeed());
cliSerial->printf_P(PSTR("%.1f m/s\n"), airspeed.get_airspeed());
if(Serial.available() > 0) {
if(cliSerial->available() > 0) {
return (0);
}
}
@ -631,7 +631,7 @@ test_airspeed(uint8_t argc, const Menu::arg *argv)
static int8_t
test_pressure(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("Uncalibrated relative airpressure\n"));
cliSerial->printf_P(PSTR("Uncalibrated relative airpressure\n"));
print_hit_enter();
home.alt = 0;
@ -643,14 +643,14 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
current_loc.alt = read_barometer() + home.alt;
if (!barometer.healthy) {
Serial.println_P(PSTR("not healthy"));
cliSerial->println_P(PSTR("not healthy"));
} else {
Serial.printf_P(PSTR("Alt: %0.2fm, Raw: %ld Temperature: %.1f\n"),
cliSerial->printf_P(PSTR("Alt: %0.2fm, Raw: %ld Temperature: %.1f\n"),
current_loc.alt / 100.0,
barometer.get_pressure(), 0.1*barometer.get_temperature());
}
if(Serial.available() > 0) {
if(cliSerial->available() > 0) {
return (0);
}
}
@ -673,7 +673,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);
}
}