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

ACM: make it possible to run CLI on radio port in ArduCopter

This commit is contained in:
Andrew Tridgell 2012-11-21 17:08:03 +11:00
parent 937c485f91
commit 76e9adb8d4
16 changed files with 419 additions and 383 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/AP_State.pde
View File

@ -143,6 +143,6 @@ void set_gps_healthy(bool b)
void dump_state()
{
Serial.printf("st: %u\n",ap.value);
//Serial.printf("%u\n", *(uint16_t*)&ap);
cliSerial->printf("st: %u\n",ap.value);
//cliSerial->printf("%u\n", *(uint16_t*)&ap);
}

11
ArduCopter/ArduCopter.pde
View File

@ -143,6 +143,9 @@ FastSerialPort0(Serial); // FTDI/console
FastSerialPort1(Serial1); // GPS port
FastSerialPort3(Serial3); // Telemetry port
// 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
@ -1375,7 +1378,7 @@ static void super_slow_loop()
#endif
/*
* //Serial.printf("alt %d, next.alt %d, alt_err: %d, cruise: %d, Alt_I:%1.2f, wp_dist %d, tar_bear %d, home_d %d, homebear %d\n",
* //cliSerial->printf("alt %d, next.alt %d, alt_err: %d, cruise: %d, Alt_I:%1.2f, wp_dist %d, tar_bear %d, home_d %d, homebear %d\n",
* current_loc.alt,
* next_WP.alt,
* altitude_error,
@ -1502,7 +1505,7 @@ void update_yaw_mode(void)
case YAW_AUTO:
nav_yaw += constrain(wrap_180(auto_yaw - nav_yaw), -60, 60); // 40 deg a second
//Serial.printf("nav_yaw %d ", nav_yaw);
//cliSerial->printf("nav_yaw %d ", nav_yaw);
nav_yaw = wrap_360(nav_yaw);
get_stabilize_yaw(nav_yaw);
break;
@ -1941,7 +1944,7 @@ static void update_altitude_est()
climb_rate += climb_rate_error;
current_loc.alt += (climb_rate / 50);
}
//Serial.printf(" %d, %d, %d, %d\n", climb_rate_actual, climb_rate_error, climb_rate, current_loc.alt);
//cliSerial->printf(" %d, %d, %d, %d\n", climb_rate_actual, climb_rate_error, climb_rate, current_loc.alt);
}
static void tuning(){
@ -2154,7 +2157,7 @@ static void update_nav_wp()
//int16_t angleTest = degrees(circle_angle);
//int16_t nroll = nav_roll;
//int16_t npitch = nav_pitch;
//Serial.printf("CIRCLE: angle:%d, dist:%d, X:%d, Y:%d, P:%d, R:%d \n", angleTest, (int)wp_distance , (int)long_error, (int)lat_error, npitch, nroll);
//cliSerial->printf("CIRCLE: angle:%d, dist:%d, X:%d, Y:%d, P:%d, R:%d \n", angleTest, (int)wp_distance , (int)long_error, (int)lat_error, npitch, nroll);
}else if(wp_control == WP_MODE) {
// calc error to target

20
ArduCopter/GCS_Mavlink.pde
View File

@ -799,7 +799,7 @@ GCS_MAVLINK::update(void)
crlf_count = 0;
}
if (crlf_count == 3) {
run_cli();
run_cli(_port);
}
}
#endif
@ -894,7 +894,7 @@ GCS_MAVLINK::data_stream_send(void)
send_message(MSG_RAW_IMU1);
send_message(MSG_RAW_IMU2);
send_message(MSG_RAW_IMU3);
//Serial.printf("mav1 %d\n", (int)streamRateRawSensors.get());
//cliSerial->printf("mav1 %d\n", (int)streamRateRawSensors.get());
}
if (stream_trigger(STREAM_EXTENDED_STATUS)) {
@ -918,29 +918,29 @@ GCS_MAVLINK::data_stream_send(void)
if (stream_trigger(STREAM_POSITION)) {
// sent with GPS read
//Serial.printf("mav3 %d\n", (int)streamRatePosition.get());
//cliSerial->printf("mav3 %d\n", (int)streamRatePosition.get());
}
if (stream_trigger(STREAM_RAW_CONTROLLER)) {
send_message(MSG_SERVO_OUT);
//Serial.printf("mav4 %d\n", (int)streamRateRawController.get());
//cliSerial->printf("mav4 %d\n", (int)streamRateRawController.get());
}
if (stream_trigger(STREAM_RC_CHANNELS)) {
send_message(MSG_RADIO_OUT);
send_message(MSG_RADIO_IN);
//Serial.printf("mav5 %d\n", (int)streamRateRCChannels.get());
//cliSerial->printf("mav5 %d\n", (int)streamRateRCChannels.get());
}
if (stream_trigger(STREAM_EXTRA1)) {
send_message(MSG_ATTITUDE);
send_message(MSG_SIMSTATE);
//Serial.printf("mav6 %d\n", (int)streamRateExtra1.get());
//cliSerial->printf("mav6 %d\n", (int)streamRateExtra1.get());
}
if (stream_trigger(STREAM_EXTRA2)) {
send_message(MSG_VFR_HUD);
//Serial.printf("mav7 %d\n", (int)streamRateExtra2.get());
//cliSerial->printf("mav7 %d\n", (int)streamRateExtra2.get());
}
if (stream_trigger(STREAM_EXTRA3)) {
@ -1604,7 +1604,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
if (!waypoint_receiving) break;
//Serial.printf("req: %d, seq: %d, total: %d\n", waypoint_request_i,packet.seq, g.command_total.get());
//cliSerial->printf("req: %d, seq: %d, total: %d\n", waypoint_request_i,packet.seq, g.command_total.get());
// check if this is the requested waypoint
if (packet.seq != waypoint_request_i)
@ -1828,8 +1828,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:\t%d\t%d\t%d\n"), packet.xacc, packet.yacc, packet.zacc);
// Serial.printf_P(PSTR("gyro:\t%d\t%d\t%d\n"), packet.xgyro, packet.ygyro, packet.zgyro);
// cliSerial->printf_P(PSTR("accel:\t%d\t%d\t%d\n"), packet.xacc, packet.yacc, packet.zacc);
// cliSerial->printf_P(PSTR("gyro:\t%d\t%d\t%d\n"), packet.xgyro, packet.ygyro, packet.zgyro);
// rad/sec
Vector3f gyros;

150
ArduCopter/Log.pde
View File

@ -22,7 +22,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"
@ -69,45 +69,45 @@ 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{
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) Serial.printf_P(PSTR(" ATTITUDE_FAST"));
if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) Serial.printf_P(PSTR(" ATTITUDE_MED"));
if (g.log_bitmask & MASK_LOG_GPS) Serial.printf_P(PSTR(" GPS"));
if (g.log_bitmask & MASK_LOG_PM) Serial.printf_P(PSTR(" PM"));
if (g.log_bitmask & MASK_LOG_CTUN) Serial.printf_P(PSTR(" CTUN"));
if (g.log_bitmask & MASK_LOG_NTUN) Serial.printf_P(PSTR(" NTUN"));
if (g.log_bitmask & MASK_LOG_RAW) Serial.printf_P(PSTR(" RAW"));
if (g.log_bitmask & MASK_LOG_CMD) Serial.printf_P(PSTR(" CMD"));
if (g.log_bitmask & MASK_LOG_CUR) Serial.printf_P(PSTR(" CURRENT"));
if (g.log_bitmask & MASK_LOG_MOTORS) Serial.printf_P(PSTR(" MOTORS"));
if (g.log_bitmask & MASK_LOG_OPTFLOW) Serial.printf_P(PSTR(" OPTFLOW"));
if (g.log_bitmask & MASK_LOG_PID) Serial.printf_P(PSTR(" PID"));
if (g.log_bitmask & MASK_LOG_ITERM) Serial.printf_P(PSTR(" ITERM"));
if (g.log_bitmask & MASK_LOG_INAV) Serial.printf_P(PSTR(" INAV"));
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) cliSerial->printf_P(PSTR(" ATTITUDE_FAST"));
if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) cliSerial->printf_P(PSTR(" ATTITUDE_MED"));
if (g.log_bitmask & MASK_LOG_GPS) cliSerial->printf_P(PSTR(" GPS"));
if (g.log_bitmask & MASK_LOG_PM) cliSerial->printf_P(PSTR(" PM"));
if (g.log_bitmask & MASK_LOG_CTUN) cliSerial->printf_P(PSTR(" CTUN"));
if (g.log_bitmask & MASK_LOG_NTUN) cliSerial->printf_P(PSTR(" NTUN"));
if (g.log_bitmask & MASK_LOG_RAW) cliSerial->printf_P(PSTR(" RAW"));
if (g.log_bitmask & MASK_LOG_CMD) cliSerial->printf_P(PSTR(" CMD"));
if (g.log_bitmask & MASK_LOG_CUR) cliSerial->printf_P(PSTR(" CURRENT"));
if (g.log_bitmask & MASK_LOG_MOTORS) cliSerial->printf_P(PSTR(" MOTORS"));
if (g.log_bitmask & MASK_LOG_OPTFLOW) cliSerial->printf_P(PSTR(" OPTFLOW"));
if (g.log_bitmask & MASK_LOG_PID) cliSerial->printf_P(PSTR(" PID"));
if (g.log_bitmask & MASK_LOG_ITERM) cliSerial->printf_P(PSTR(" ITERM"));
if (g.log_bitmask & MASK_LOG_INAV) cliSerial->printf_P(PSTR(" INAV"));
}
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%u logs\n"), (unsigned)num_logs);
cliSerial->printf_P(PSTR("\n%u logs\n"), (unsigned)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);
}
@ -127,28 +127,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 number %d, start %d, end %d\n"),
/*cliSerial->printf_P(PSTR("Dumping Log number %d, start %d, end %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);
}
@ -174,7 +174,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);
}
@ -278,13 +278,13 @@ static void Log_Read_GPS()
int32_t temp8 = DataFlash.ReadLong(); // 8 ground course
// 1 2 3 4 5 6 7 8
Serial.printf_P(PSTR("GPS, %ld, %d, "),
cliSerial->printf_P(PSTR("GPS, %ld, %d, "),
(long)temp1, // 1 time
(int)temp2); // 2 sats
print_latlon(&Serial, temp3);
Serial.print_P(PSTR(", "));
cliSerial->print_P(PSTR(", "));
print_latlon(&Serial, temp4);
Serial.printf_P(PSTR(", %4.4f, %4.4f, %d, %ld\n"),
cliSerial->printf_P(PSTR(", %4.4f, %4.4f, %d, %ld\n"),
temp5, // 5 gps alt
temp6, // 6 sensor alt
(int)temp7, // 7 ground speed
@ -325,19 +325,19 @@ static void Log_Write_Raw()
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 = get_float(DataFlash.ReadLong());
Serial.print(logvar);
Serial.print_P(PSTR(", "));
cliSerial->print(logvar);
cliSerial->print_P(PSTR(", "));
}
Serial.println_P(PSTR(" "));
cliSerial->println_P(PSTR(" "));
/*
float temp1 = get_float(DataFlash.ReadLong());
float temp2 = get_float(DataFlash.ReadLong());
Serial.printf_P(PSTR("RAW, %4.4f, %4.4f\n"),
cliSerial->printf_P(PSTR("RAW, %4.4f, %4.4f\n"),
temp1,
temp2);
*/
@ -370,7 +370,7 @@ static void Log_Read_Current()
int16_t temp5 = DataFlash.ReadInt(); // 5
// 1 2 3 4 5
Serial.printf_P(PSTR("CURR, %d, %ld, %4.4f, %4.4f, %d\n"),
cliSerial->printf_P(PSTR("CURR, %d, %ld, %4.4f, %4.4f, %d\n"),
(int)temp1,
(long)temp2,
temp3,
@ -448,7 +448,7 @@ static void Log_Read_Motors()
int16_t temp5 = DataFlash.ReadInt(); // 5
int16_t temp6 = DataFlash.ReadInt(); // 6
// 1 2 3 4 5 6
Serial.printf_P(PSTR("MOT, %d, %d, %d, %d, %d, %d\n"),
cliSerial->printf_P(PSTR("MOT, %d, %d, %d, %d, %d, %d\n"),
(int)temp1, //1
(int)temp2, //2
(int)temp3, //3
@ -466,7 +466,7 @@ static void Log_Read_Motors()
int16_t temp7 = DataFlash.ReadInt(); // 7
int16_t temp8 = DataFlash.ReadInt(); // 8
// 1 2 3 4 5 6 7 8
Serial.printf_P(PSTR("MOT, %d, %d, %d, %d, %d, %d, %d, %d\n"),
cliSerial->printf_P(PSTR("MOT, %d, %d, %d, %d, %d, %d, %d, %d\n"),
(int)temp1, //1
(int)temp2, //2
(int)temp3, //3
@ -483,7 +483,7 @@ static void Log_Read_Motors()
int16_t temp4 = DataFlash.ReadInt(); // 4
int16_t temp5 = DataFlash.ReadInt(); // 5
// 1 2 3 4 5
Serial.printf_P(PSTR("MOT, %d, %d, %d, %d, %d\n"),
cliSerial->printf_P(PSTR("MOT, %d, %d, %d, %d, %d\n"),
(int)temp1, //1
(int)temp2, //2
(int)temp3, //3
@ -497,7 +497,7 @@ static void Log_Read_Motors()
int16_t temp4 = DataFlash.ReadInt(); // 4
// 1 2 3 4
Serial.printf_P(PSTR("MOT, %d, %d, %d, %d\n"),
cliSerial->printf_P(PSTR("MOT, %d, %d, %d, %d\n"),
(int)temp1, //1
(int)temp2, //2
(int)temp3, //3
@ -538,7 +538,7 @@ static void Log_Read_Optflow()
int32_t temp8 = DataFlash.ReadLong(); // 8
int32_t temp9 = DataFlash.ReadLong(); // 9
Serial.printf_P(PSTR("OF, %d, %d, %d, %d, %d, %4.7f, %4.7f, %ld, %ld\n"),
cliSerial->printf_P(PSTR("OF, %d, %d, %d, %d, %d, %4.7f, %4.7f, %ld, %ld\n"),
(int)temp1,
(int)temp2,
(int)temp3,
@ -577,15 +577,15 @@ static void Log_Read_Nav_Tuning()
{
int16_t temp;
Serial.printf_P(PSTR("NTUN, "));
cliSerial->printf_P(PSTR("NTUN, "));
for(int8_t i = 1; i < 8; i++ ) {
temp = DataFlash.ReadInt();
Serial.printf_P(PSTR("%d, "), (int)temp);
cliSerial->printf_P(PSTR("%d, "), (int)temp);
}
// read 8
temp = DataFlash.ReadInt();
Serial.printf_P(PSTR("%d\n"), (int)temp);
cliSerial->printf_P(PSTR("%d\n"), (int)temp);
}
@ -615,15 +615,15 @@ static void Log_Read_Control_Tuning()
{
int16_t temp;
Serial.printf_P(PSTR("CTUN, "));
cliSerial->printf_P(PSTR("CTUN, "));
for(uint8_t i = 1; i < 9; i++ ) {
temp = DataFlash.ReadInt();
Serial.printf_P(PSTR("%d, "), (int)temp);
cliSerial->printf_P(PSTR("%d, "), (int)temp);
}
// read 9
temp = DataFlash.ReadInt();
Serial.printf_P(PSTR("%d\n"), (int)temp);
cliSerial->printf_P(PSTR("%d\n"), (int)temp);
}
static void Log_Write_Iterm()
@ -653,15 +653,15 @@ static void Log_Read_Iterm()
{
int16_t temp;
Serial.printf_P(PSTR("ITERM, "));
cliSerial->printf_P(PSTR("ITERM, "));
for(uint8_t i = 1; i < 12; i++ ) {
temp = DataFlash.ReadInt();
Serial.printf_P(PSTR("%d, "), (int)temp);
cliSerial->printf_P(PSTR("%d, "), (int)temp);
}
// read 12
temp = DataFlash.ReadInt();
Serial.println((int)temp);
cliSerial->println((int)temp);
}
@ -691,7 +691,7 @@ static void Log_Read_Performance()
uint32_t temp6 = DataFlash.ReadLong();
// 1 2 3 4 5 6
Serial.printf_P(PSTR("PM, %d, %d, %d, %u, %u, %lu\n"),
cliSerial->printf_P(PSTR("PM, %d, %d, %d, %u, %u, %lu\n"),
(int)temp1,
(int)temp2,
(int)temp3,
@ -734,7 +734,7 @@ static void Log_Read_Cmd()
int32_t temp8 = DataFlash.ReadLong();
// 1 2 3 4 5 6 7 8
Serial.printf_P(PSTR( "CMD, %d, %d, %d, %d, %d, %ld, %ld, %ld\n"),
cliSerial->printf_P(PSTR( "CMD, %d, %d, %d, %d, %d, %ld, %ld, %ld\n"),
(int)temp1,
(int)temp2,
(int)temp3,
@ -774,7 +774,7 @@ static void Log_Read_Attitude()
uint16_t temp7 = DataFlash.ReadInt();
// 1 2 3 4 5 6 7 8 9
Serial.printf_P(PSTR("ATT, %d, %d, %d, %d, %d, %u, %u\n"),
cliSerial->printf_P(PSTR("ATT, %d, %d, %d, %d, %d, %u, %u\n"),
(int)temp1,
(int)temp2,
(int)temp3,
@ -837,7 +837,7 @@ static void Log_Read_INAV()
float temp17 = get_float(DataFlash.ReadLong()); // 17 accel based lat velocity
float temp18 = get_float(DataFlash.ReadLong()); // 18 accel based lon velocity
// 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18
Serial.printf_P(PSTR("INAV, %d, %d, %d, %d, %6.4f, %6.4f, %6.4f, %6.4f, %6.4f, %6.4f, %6.4f, %6.4f, %ld, %ld, %6.4f, %6.4f, %6.4f, %6.4f\n"),
cliSerial->printf_P(PSTR("INAV, %d, %d, %d, %d, %6.4f, %6.4f, %6.4f, %6.4f, %6.4f, %6.4f, %6.4f, %6.4f, %ld, %ld, %6.4f, %6.4f, %6.4f, %6.4f\n"),
(int)temp1,
(int)temp2,
(int)temp3,
@ -872,9 +872,9 @@ static void Log_Write_Mode(byte mode)
// Read a mode packet
static void Log_Read_Mode()
{
Serial.printf_P(PSTR("MOD:"));
cliSerial->printf_P(PSTR("MOD:"));
print_flight_mode(DataFlash.ReadByte());
Serial.printf_P(PSTR(", %d\n"),(int)DataFlash.ReadInt());
cliSerial->printf_P(PSTR(", %d\n"),(int)DataFlash.ReadInt());
}
// Write Startup packet. Total length : 4 bytes
@ -889,7 +889,7 @@ static void Log_Write_Startup()
// Read a startup packet
static void Log_Read_Startup()
{
Serial.printf_P(PSTR("START UP\n"));
cliSerial->printf_P(PSTR("START UP\n"));
}
#define DATA_INT32 0
@ -960,23 +960,23 @@ static void Log_Read_Data()
int8_t _type = DataFlash.ReadByte();
if(_type == DATA_EVENT) {
Serial.printf_P(PSTR("EV: %u\n"), _index);
cliSerial->printf_P(PSTR("EV: %u\n"), _index);
}else if(_type == DATA_FLOAT) {
float _value = get_float(DataFlash.ReadLong());
Serial.printf_P(PSTR("DATA: %u, %1.6f\n"), _index, _value);
cliSerial->printf_P(PSTR("DATA: %u, %1.6f\n"), _index, _value);
}else if(_type == DATA_INT16) {
int16_t _value = DataFlash.ReadInt();
Serial.printf_P(PSTR("DATA: %u, %d\n"), _index, _value);
cliSerial->printf_P(PSTR("DATA: %u, %d\n"), _index, _value);
}else if(_type == DATA_UINT16) {
uint16_t _value = DataFlash.ReadInt();
Serial.printf_P(PSTR("DATA: %u, %u\n"), _index, _value);
cliSerial->printf_P(PSTR("DATA: %u, %u\n"), _index, _value);
}else if(_type == DATA_INT32) {
int32_t _value = DataFlash.ReadLong();
Serial.printf_P(PSTR("DATA: %u, %ld\n"), _index, _value);
cliSerial->printf_P(PSTR("DATA: %u, %ld\n"), _index, _value);
}
}
@ -1010,7 +1010,7 @@ static void Log_Read_PID()
float temp7 = DataFlash.ReadLong() / 1000.f; // gain
// 1 2 3 4 5 6 7
Serial.printf_P(PSTR("PID-%d, %ld, %ld, %ld, %ld, %ld, %4.4f\n"),
cliSerial->printf_P(PSTR("PID-%d, %ld, %ld, %ld, %ld, %ld, %4.4f\n"),
(int)temp1, // pid id
(long)temp2, // error
(long)temp3, // p
@ -1049,7 +1049,7 @@ static void Log_Read_DMP()
uint16_t temp6 = DataFlash.ReadInt();
// 1 2 3 4 5 6
Serial.printf_P(PSTR("DMP, %d, %d, %d, %d, %u, %u\n"),
cliSerial->printf_P(PSTR("DMP, %d, %d, %d, %d, %u, %u\n"),
(int)temp1,
(int)temp2,
(int)temp3,
@ -1064,19 +1064,19 @@ 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"),
(unsigned) memcheck_available_memory());
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
Serial.printf_P(PSTR("APM 2\n"));
cliSerial->printf_P(PSTR("APM 2\n"));
#elif CONFIG_APM_HARDWARE == APM2_BETA_HARDWARE
Serial.printf_P(PSTR("APM 2Beta\n"));
cliSerial->printf_P(PSTR("APM 2Beta\n"));
#else
Serial.printf_P(PSTR("APM 1\n"));
cliSerial->printf_P(PSTR("APM 1\n"));
#endif
#if CLI_ENABLED == ENABLED
@ -1090,7 +1090,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
@ -1119,7 +1119,7 @@ static int16_t Log_Read_Process(int16_t start_page, int16_t end_page)
log_step++;
else{
log_step = 0;
Serial.println_P(PSTR("."));
cliSerial->println_P(PSTR("."));
}
break;
@ -1199,7 +1199,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"),data);
cliSerial->printf_P(PSTR("Error Reading END_BYTE: %d\n"),data);
}
log_step = 0; // Restart sequence: new packet...
break;

6
ArduCopter/Parameters.pde
View File

@ -465,18 +465,18 @@ 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);
default_dead_zones();
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);
}
}

4
ArduCopter/commands.pde
View File

@ -75,7 +75,7 @@ static void set_cmd_with_index(struct Location temp, int i)
{
i = constrain(i, 0, g.command_total.get());
//Serial.printf("set_command: %d with id: %d\n", i, temp.id);
//cliSerial->printf("set_command: %d with id: %d\n", i, temp.id);
// store home as 0 altitude!!!
// Home is always a MAV_CMD_NAV_WAYPOINT (16)
@ -146,7 +146,7 @@ static void set_next_WP(struct Location *wp)
prev_WP = current_loc;
}
//Serial.printf("set_next_WP #%d, ", command_nav_index);
//cliSerial->printf("set_next_WP #%d, ", command_nav_index);
//print_wp(&prev_WP, command_nav_index -1);
// Load the next_WP slot

40
ArduCopter/commands_logic.pde
View File

@ -298,7 +298,7 @@ static void do_loiter_unlimited()
{
wp_control = LOITER_MODE;
//Serial.println("dloi ");
//cliSerial->println("dloi ");
if(command_nav_queue.lat == 0) {
set_next_WP(&current_loc);
wp_control = LOITER_MODE;
@ -466,7 +466,7 @@ static bool verify_nav_wp()
if ((millis() - loiter_time) > loiter_time_max) {
wp_verify_byte |= NAV_DELAY;
//gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete"));
//Serial.println("vlt done");
//cliSerial->println("vlt done");
}
}
@ -508,7 +508,7 @@ static bool verify_loiter_time()
static bool verify_loiter_turns()
{
//Serial.printf("loiter_sum: %d \n", loiter_sum);
//cliSerial->printf("loiter_sum: %d \n", loiter_sum);
// have we rotated around the center enough times?
// -----------------------------------------------
if(abs(loiter_sum) > loiter_total) {
@ -542,10 +542,10 @@ static bool verify_RTL()
static void do_wait_delay()
{
//Serial.print("dwd ");
//cliSerial->print("dwd ");
condition_start = millis();
condition_value = command_cond_queue.lat * 1000; // convert to milliseconds
//Serial.println(condition_value,DEC);
//cliSerial->println(condition_value,DEC);
}
static void do_change_alt()
@ -564,7 +564,7 @@ static void do_within_distance()
static void do_yaw()
{
//Serial.println("dyaw ");
//cliSerial->println("dyaw ");
yaw_tracking = MAV_ROI_NONE;
// target angle in degrees
@ -624,19 +624,19 @@ static void do_yaw()
static bool verify_wait_delay()
{
//Serial.print("vwd");
//cliSerial->print("vwd");
if ((unsigned)(millis() - condition_start) > (unsigned)condition_value) {
//Serial.println("y");
//cliSerial->println("y");
condition_value = 0;
return true;
}
//Serial.println("n");
//cliSerial->println("n");
return false;
}
static bool verify_change_alt()
{
//Serial.printf("change_alt, ca:%d, na:%d\n", (int)current_loc.alt, (int)next_WP.alt);
//cliSerial->printf("change_alt, ca:%d, na:%d\n", (int)current_loc.alt, (int)next_WP.alt);
if ((int32_t)condition_start < next_WP.alt) {
// we are going higer
if(current_loc.alt > next_WP.alt) {
@ -653,7 +653,7 @@ static bool verify_change_alt()
static bool verify_within_distance()
{
//Serial.printf("cond dist :%d\n", (int)condition_value);
//cliSerial->printf("cond dist :%d\n", (int)condition_value);
if (wp_distance < condition_value) {
condition_value = 0;
return true;
@ -663,7 +663,7 @@ static bool verify_within_distance()
static bool verify_yaw()
{
//Serial.printf("vyaw %d\n", (int)(nav_yaw/100));
//cliSerial->printf("vyaw %d\n", (int)(nav_yaw/100));
if((millis() - command_yaw_start_time) > command_yaw_time) {
// time out
@ -674,7 +674,7 @@ static bool verify_yaw()
// TO-DO: there's still a problem with Condition_yaw, it will do it two times(probably more) sometimes, if it hasn't reached the next waypoint yet.
// it should only do it one time so there should be code here to prevent another Condition_Yaw.
//Serial.println("Y");
//cliSerial->println("Y");
return true;
}else{
@ -689,7 +689,7 @@ static bool verify_yaw()
}
nav_yaw = wrap_360(nav_yaw);
auto_yaw = nav_yaw;
//Serial.printf("ny %ld\n",nav_yaw);
//cliSerial->printf("ny %ld\n",nav_yaw);
return false;
}
}
@ -757,27 +757,27 @@ static void do_jump()
// when in use, it contains the current remaining jumps
static int8_t jump = -10; // used to track loops in jump command
//Serial.printf("do Jump: %d\n", jump);
//cliSerial->printf("do Jump: %d\n", jump);
if(jump == -10) {
//Serial.printf("Fresh Jump\n");
//cliSerial->printf("Fresh Jump\n");
// we use a locally stored index for jump
jump = command_cond_queue.lat;
}
//Serial.printf("Jumps left: %d\n",jump);
//cliSerial->printf("Jumps left: %d\n",jump);
if(jump > 0) {
//Serial.printf("Do Jump to %d\n",command_cond_queue.p1);
//cliSerial->printf("Do Jump to %d\n",command_cond_queue.p1);
jump--;
change_command(command_cond_queue.p1);
} else if (jump == 0) {
//Serial.printf("Did last jump\n");
//cliSerial->printf("Did last jump\n");
// we're done, move along
jump = -11;
} else if (jump == -1) {
//Serial.printf("jumpForever\n");
//cliSerial->printf("jumpForever\n");
// repeat forever
change_command(command_cond_queue.p1);
}

14
ArduCopter/commands_process.pde
View File

@ -4,14 +4,14 @@
//----------------------------------------
static void change_command(uint8_t cmd_index)
{
//Serial.printf("change_command: %d\n",cmd_index );
//cliSerial->printf("change_command: %d\n",cmd_index );
// limit range
cmd_index = min(g.command_total - 1, cmd_index);
// load command
struct Location temp = get_cmd_with_index(cmd_index);
//Serial.printf("loading cmd: %d with id:%d\n", cmd_index, temp.id);
//cliSerial->printf("loading cmd: %d with id:%d\n", cmd_index, temp.id);
// verify it's a nav command
if(temp.id > MAV_CMD_NAV_LAST) {
@ -32,12 +32,12 @@ static void change_command(uint8_t cmd_index)
// --------------------
static void update_commands()
{
//Serial.printf("update_commands: %d\n",increment );
//cliSerial->printf("update_commands: %d\n",increment );
// A: if we do not have any commands there is nothing to do
// B: We have completed the mission, don't redo the mission
// XXX debug
//uint8_t tmp = g.command_index.get();
//Serial.printf("command_index %u \n", tmp);
//cliSerial->printf("command_index %u \n", tmp);
if(g.command_total <= 1 || g.command_index >= 255)
return;
@ -172,7 +172,7 @@ static void execute_nav_command(void)
static void verify_commands(void)
{
if(verify_must()) {
//Serial.printf("verified must cmd %d\n" , command_nav_index);
//cliSerial->printf("verified must cmd %d\n" , command_nav_index);
command_nav_queue.id = NO_COMMAND;
// store our most recent executed nav command
@ -183,11 +183,11 @@ static void verify_commands(void)
command_cond_queue.id = NO_COMMAND;
}else{
//Serial.printf("verified must false %d\n" , command_nav_index);
//cliSerial->printf("verified must false %d\n" , command_nav_index);
}
if(verify_may()) {
//Serial.printf("verified may cmd %d\n" , command_cond_index);
//cliSerial->printf("verified may cmd %d\n" , command_cond_index);
command_cond_queue.id = NO_COMMAND;
}
}

4
ArduCopter/motors.pde
View File

@ -96,7 +96,7 @@ static void init_arm_motors()
// disable failsafe because initialising everything takes a while
failsafe_disable();
//Serial.printf("\nARM\n");
//cliSerial->printf("\nARM\n");
#if HIL_MODE != HIL_MODE_DISABLED || defined(DESKTOP_BUILD)
gcs_send_text_P(SEVERITY_HIGH, PSTR("ARMING MOTORS"));
#endif
@ -104,7 +104,7 @@ static void init_arm_motors()
// we don't want writes to the serial port to cause us to pause
// mid-flight, so set the serial ports non-blocking once we arm
// the motors
Serial.set_blocking_writes(false);
cliSerial->set_blocking_writes(false);
if (gcs3.initialised) {
Serial3.set_blocking_writes(false);
}

4
ArduCopter/navigation.pde
View File

@ -301,7 +301,7 @@ static void run_navigation_contollers()
if(home_distance > SUPER_SIMPLE_RADIUS) { // 10m from home
// we reset the angular offset to be a vector from home to the quad
initial_simple_bearing = home_to_copter_bearing;
//Serial.printf("ISB: %d\n", initial_simple_bearing);
//cliSerial->printf("ISB: %d\n", initial_simple_bearing);
}
}
@ -477,7 +477,7 @@ static void calc_nav_rate(int16_t max_speed)
// We use the DCM's matrix to precalculate these trig values at 50hz
static void calc_nav_pitch_roll()
{
//Serial.printf("ys %ld, cx %1.4f, _cx %1.4f | sy %1.4f, _sy %1.4f\n", dcm.yaw_sensor, cos_yaw_x, _cos_yaw_x, sin_yaw_y, _sin_yaw_y);
//cliSerial->printf("ys %ld, cx %1.4f, _cx %1.4f | sy %1.4f, _sy %1.4f\n", dcm.yaw_sensor, cos_yaw_x, _cos_yaw_x, sin_yaw_y, _sin_yaw_y);
// rotate the vector
auto_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x;
auto_pitch = (float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y;

4
ArduCopter/radio.pde
View File

@ -89,14 +89,14 @@ static void init_rc_out()
// send miinimum throttle out to ESC
motors.output_min();
// display message on console
Serial.printf_P(PSTR("Entering ESC Calibration: please restart APM.\n"));
cliSerial->printf_P(PSTR("Entering ESC Calibration: please restart APM.\n"));
// block until we restart
while(1) {
delay(200);
dancing_light();
}
}else{
Serial.printf_P(PSTR("ESC Calibration active: passing throttle through to ESCs.\n"));
cliSerial->printf_P(PSTR("ESC Calibration active: passing throttle through to ESCs.\n"));
// clear esc flag
g.esc_calibrate.set_and_save(0);
// block until we restart

4
ArduCopter/sensors.pde
View File

@ -39,7 +39,7 @@ static void init_compass()
compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft
if (!compass.init() || !compass.read()) {
// make sure we don't pass a broken compass to DCM
Serial.println_P(PSTR("COMPASS INIT ERROR"));
cliSerial->println_P(PSTR("COMPASS INIT ERROR"));
return;
}
ahrs.set_compass(&compass);
@ -53,7 +53,7 @@ static void init_optflow()
#if OPTFLOW == ENABLED
if( optflow.init(false, &timer_scheduler, &spi_semaphore, &spi3_semaphore) == false ) {
g.optflow_enabled = false;
Serial.print_P(PSTR("\nFailed to Init OptFlow "));
cliSerial->print_P(PSTR("\nFailed to Init OptFlow "));
}
// suspend timer while we set-up SPI communication
timer_scheduler.suspend_timer();

255
ArduCopter/setup.pde
View File

@ -61,7 +61,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\n\n"));
cliSerial->printf_P(PSTR("Setup Mode\n\n\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"
@ -70,9 +70,9 @@ setup_mode(uint8_t argc, const Menu::arg *argv)
if(g.rc_1.radio_min >= 1300) {
delay(1000);
Serial.printf_P(PSTR("\n!Warning, radio not configured!"));
cliSerial->printf_P(PSTR("\n!Warning, radio not configured!"));
delay(1000);
Serial.printf_P(PSTR("\n Type 'radio' now.\n\n"));
cliSerial->printf_P(PSTR("\n Type 'radio' now.\n\n"));
}
// Run the setup menu. When the menu exits, we will return to the main menu.
@ -118,17 +118,17 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
{
int16_t c;
Serial.printf_P(PSTR("\n'Y' = factory reset, any other key to abort:\n"));
cliSerial->printf_P(PSTR("\n'Y' = factory reset, any other key to abort:\n"));
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("\nReboot APM"));
cliSerial->printf_P(PSTR("\nReboot APM"));
delay(1000);
//default_gains();
@ -144,7 +144,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
static int8_t
setup_radio(uint8_t argc, const Menu::arg *argv)
{
Serial.println_P(PSTR("\n\nRadio Setup:"));
cliSerial->println_P(PSTR("\n\nRadio Setup:"));
uint8_t i;
for(i = 0; i < 100; i++) {
@ -154,7 +154,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
if(g.rc_1.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
}
@ -188,7 +188,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
g.rc_8.radio_trim = 1500;
Serial.printf_P(PSTR("\nMove all controls to extremes. Enter to save: "));
cliSerial->printf_P(PSTR("\nMove all controls to extremes. Enter to save: "));
while(1) {
delay(20);
@ -205,9 +205,9 @@ 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) {
if(cliSerial->available() > 0) {
delay(20);
Serial.flush();
cliSerial->flush();
g.rc_1.save_eeprom();
g.rc_2.save_eeprom();
@ -229,7 +229,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
static int8_t
setup_motors(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR(
cliSerial->printf_P(PSTR(
"Now connect the main lipo and follow the instruction on the wiki for your frame setup.\n"
"For security remember to disconnect the main lipo after the test, then hit any key to exit.\n"
"Any key to exit.\n"));
@ -237,7 +237,7 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
delay(20);
read_radio();
motors.output_test();
if(Serial.available() > 0) {
if(cliSerial->available() > 0) {
g.esc_calibrate.set_and_save(0);
return(0);
}
@ -253,12 +253,37 @@ setup_accel(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);
ins.calibrate_accel(delay, flash_leds, setup_printf_P, setup_wait_key);
report_ins();
return(0);
}
@ -275,7 +300,7 @@ setup_frame(uint8_t argc, const Menu::arg *argv)
} else if (!strcmp_P(argv[1].str, PSTR("v"))) {
g.frame_orientation.set_and_save(V_FRAME);
}else{
Serial.printf_P(PSTR("\nOp:[x,+,v]\n"));
cliSerial->printf_P(PSTR("\nOp:[x,+,v]\n"));
report_frame();
return 0;
}
@ -291,7 +316,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
byte _oldSwitchPosition = 0;
int8_t mode = 0;
Serial.printf_P(PSTR("\nMode switch to edit, aileron: select modes, rudder: Simple on/off\n"));
cliSerial->printf_P(PSTR("\nMode switch to edit, aileron: select modes, rudder: Simple on/off\n"));
print_hit_enter();
while(1) {
@ -344,7 +369,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
}
// escape hatch
if(Serial.available() > 0) {
if(cliSerial->available() > 0) {
for (mode = 0; mode < 6; mode++)
flight_modes[mode].save();
@ -377,7 +402,7 @@ setup_tune(uint8_t argc, const Menu::arg *argv)
static int8_t
setup_range(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("\nCH 6 Ranges are divided by 1000: [low, high]\n"));
cliSerial->printf_P(PSTR("\nCH 6 Ranges are divided by 1000: [low, high]\n"));
g.radio_tuning_low.set_and_save(argv[1].i);
g.radio_tuning_high.set_and_save(argv[2].i);
@ -406,7 +431,7 @@ setup_compass(uint8_t argc, const Menu::arg *argv)
g.compass_enabled.set_and_save(false);
}else{
Serial.printf_P(PSTR("\nOp:[on,off]\n"));
cliSerial->printf_P(PSTR("\nOp:[on,off]\n"));
report_compass();
return 0;
}
@ -426,7 +451,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("\nOp: off, 3-4"));
cliSerial->printf_P(PSTR("\nOp: off, 3-4"));
}
report_batt_monitor();
@ -447,7 +472,7 @@ setup_sonar(uint8_t argc, const Menu::arg *argv)
g.sonar_type.set_and_save(argv[1].i);
}else{
Serial.printf_P(PSTR("\nOp:[on, off, 0-3]\n"));
cliSerial->printf_P(PSTR("\nOp:[on, off, 0-3]\n"));
report_sonar();
return 0;
}
@ -480,18 +505,18 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
report_heli();
// display help
Serial.printf_P(PSTR("Instructions:"));
cliSerial->printf_P(PSTR("Instructions:"));
print_divider();
Serial.printf_P(PSTR("\td\t\tdisplay settings\n"));
Serial.printf_P(PSTR("\t1~4\t\tselect servo\n"));
Serial.printf_P(PSTR("\ta or z\t\tmove mid up/down\n"));
Serial.printf_P(PSTR("\tc\t\tset coll when blade pitch zero\n"));
Serial.printf_P(PSTR("\tm\t\tset roll, pitch, coll min/max\n"));
Serial.printf_P(PSTR("\tp<angle>\tset pos (i.e. p0 = front, p90 = right)\n"));
Serial.printf_P(PSTR("\tr\t\treverse servo\n"));
Serial.printf_P(PSTR("\tu a|d\t\tupdate rate (a=analog servo, d=digital)\n"));
Serial.printf_P(PSTR("\tt<angle>\tset trim (-500 ~ 500)\n"));
Serial.printf_P(PSTR("\tx\t\texit & save\n"));
cliSerial->printf_P(PSTR("\td\t\tdisplay settings\n"));
cliSerial->printf_P(PSTR("\t1~4\t\tselect servo\n"));
cliSerial->printf_P(PSTR("\ta or z\t\tmove mid up/down\n"));
cliSerial->printf_P(PSTR("\tc\t\tset coll when blade pitch zero\n"));
cliSerial->printf_P(PSTR("\tm\t\tset roll, pitch, coll min/max\n"));
cliSerial->printf_P(PSTR("\tp<angle>\tset pos (i.e. p0 = front, p90 = right)\n"));
cliSerial->printf_P(PSTR("\tr\t\treverse servo\n"));
cliSerial->printf_P(PSTR("\tu a|d\t\tupdate rate (a=analog servo, d=digital)\n"));
cliSerial->printf_P(PSTR("\tt<angle>\tset trim (-500 ~ 500)\n"));
cliSerial->printf_P(PSTR("\tx\t\texit & save\n"));
// start capturing
while( value != 'x' ) {
@ -516,8 +541,8 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
max_tail = max(g.rc_4.radio_out, max_tail);
}
if( Serial.available() ) {
value = Serial.read();
if( cliSerial->available() ) {
value = cliSerial->read();
// process the user's input
switch( value ) {
@ -541,7 +566,7 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
case 'C':
if( g.rc_3.radio_out >= 900 && g.rc_3.radio_out <= 2100 ) {
motors.collective_mid = g.rc_3.radio_out;
Serial.printf_P(PSTR("Collective when blade pitch at zero: %d\n"),(int)motors.collective_mid);
cliSerial->printf_P(PSTR("Collective when blade pitch at zero: %d\n"),(int)motors.collective_mid);
}
break;
case 'd':
@ -553,7 +578,7 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
case 'M':
if( state == 0 ) {
state = 1; // switch to capture min/max mode
Serial.printf_P(PSTR("Move coll, roll, pitch and tail to extremes, press 'm' when done\n"));
cliSerial->printf_P(PSTR("Move coll, roll, pitch and tail to extremes, press 'm' when done\n"));
// reset servo ranges
motors.roll_max = motors.pitch_max = 4500;
@ -572,7 +597,7 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
state = 0; // switch back to normal mode
// double check values aren't totally terrible
if( max_roll <= 1000 || max_pitch <= 1000 || (max_collective - min_collective < 200) || (max_tail - min_tail < 200) || min_tail < 1000 || max_tail > 2000 )
Serial.printf_P(PSTR("Invalid min/max captured roll:%d, pitch:%d, collective min: %d max: %d, tail min:%d max:%d\n"),max_roll,max_pitch,min_collective,max_collective,min_tail,max_tail);
cliSerial->printf_P(PSTR("Invalid min/max captured roll:%d, pitch:%d, collective min: %d max: %d, tail min:%d max:%d\n"),max_roll,max_pitch,min_collective,max_collective,min_tail,max_tail);
else{
motors.roll_max = max_roll;
motors.pitch_max = max_pitch;
@ -600,7 +625,7 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
if( active_servo == CH_3 )
motors.servo3_pos = temp;
motors.init_swash();
Serial.printf_P(PSTR("Servo %d\t\tpos:%d\n"),active_servo+1, temp);
cliSerial->printf_P(PSTR("Servo %d\t\tpos:%d\n"),active_servo+1, temp);
}
break;
case 'r':
@ -615,28 +640,28 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
if( temp > -500 && temp < 500 ) {
heli_get_servo(active_servo)->radio_trim = 1500 + temp;
motors.init_swash();
Serial.printf_P(PSTR("Servo %d\t\ttrim:%d\n"),active_servo+1, 1500 + temp);
cliSerial->printf_P(PSTR("Servo %d\t\ttrim:%d\n"),active_servo+1, 1500 + temp);
}
break;
case 'u':
case 'U':
temp = 0;
// delay up to 2 seconds for servo type from user
while( !Serial.available() && temp < 20 ) {
while( !cliSerial->available() && temp < 20 ) {
temp++;
delay(100);
}
if( Serial.available() ) {
value = Serial.read();
if( cliSerial->available() ) {
value = cliSerial->read();
if( value == 'a' || value == 'A' ) {
g.rc_speed.set_and_save(AP_MOTORS_HELI_SPEED_ANALOG_SERVOS);
//motors._speed_hz = AP_MOTORS_HELI_SPEED_ANALOG_SERVOS; // need to force this update to take effect immediately
Serial.printf_P(PSTR("Analog Servo %dhz\n"),(int)g.rc_speed);
cliSerial->printf_P(PSTR("Analog Servo %dhz\n"),(int)g.rc_speed);
}
if( value == 'd' || value == 'D' ) {
g.rc_speed.set_and_save(AP_MOTORS_HELI_SPEED_ANALOG_SERVOS);
//motors._speed_hz = AP_MOTORS_HELI_SPEED_ANALOG_SERVOS; // need to force this update to take effect immediately
Serial.printf_P(PSTR("Digital Servo %dhz\n"),(int)g.rc_speed);
cliSerial->printf_P(PSTR("Digital Servo %dhz\n"),(int)g.rc_speed);
}
}
break;
@ -696,7 +721,7 @@ setup_gyro(uint8_t argc, const Menu::arg *argv)
motors.ext_gyro_gain.save();
}else{
Serial.printf_P(PSTR("\nOp:[on, off] gain\n"));
cliSerial->printf_P(PSTR("\nOp:[on, off] gain\n"));
}
report_gyro();
@ -724,7 +749,7 @@ setup_optflow(uint8_t argc, const Menu::arg *argv)
g.optflow_enabled = false;
}else{
Serial.printf_P(PSTR("\nOp:[on, off]\n"));
cliSerial->printf_P(PSTR("\nOp:[on, off]\n"));
report_optflow();
return 0;
}
@ -743,11 +768,11 @@ setup_optflow(uint8_t argc, const Menu::arg *argv)
static void report_batt_monitor()
{
Serial.printf_P(PSTR("\nBatt Mon:\n"));
cliSerial->printf_P(PSTR("\nBatt Mon:\n"));
print_divider();
if(g.battery_monitoring == 0) print_enabled(false);
if(g.battery_monitoring == 3) Serial.printf_P(PSTR("volts"));
if(g.battery_monitoring == 4) Serial.printf_P(PSTR("volts and cur"));
if(g.battery_monitoring == 3) cliSerial->printf_P(PSTR("volts"));
if(g.battery_monitoring == 4) cliSerial->printf_P(PSTR("volts and cur"));
print_blanks(2);
}
@ -766,39 +791,39 @@ static void report_wp(byte index = 255)
static void report_sonar()
{
Serial.printf_P(PSTR("Sonar\n"));
cliSerial->printf_P(PSTR("Sonar\n"));
print_divider();
print_enabled(g.sonar_enabled.get());
Serial.printf_P(PSTR("Type: %d (0=XL, 1=LV, 2=XLL, 3=HRLV)"), (int)g.sonar_type);
cliSerial->printf_P(PSTR("Type: %d (0=XL, 1=LV, 2=XLL, 3=HRLV)"), (int)g.sonar_type);
print_blanks(2);
}
static void report_frame()
{
Serial.printf_P(PSTR("Frame\n"));
cliSerial->printf_P(PSTR("Frame\n"));
print_divider();
#if FRAME_CONFIG == QUAD_FRAME
Serial.printf_P(PSTR("Quad frame\n"));
cliSerial->printf_P(PSTR("Quad frame\n"));
#elif FRAME_CONFIG == TRI_FRAME
Serial.printf_P(PSTR("TRI frame\n"));
cliSerial->printf_P(PSTR("TRI frame\n"));
#elif FRAME_CONFIG == HEXA_FRAME
Serial.printf_P(PSTR("Hexa frame\n"));
cliSerial->printf_P(PSTR("Hexa frame\n"));
#elif FRAME_CONFIG == Y6_FRAME
Serial.printf_P(PSTR("Y6 frame\n"));
cliSerial->printf_P(PSTR("Y6 frame\n"));
#elif FRAME_CONFIG == OCTA_FRAME
Serial.printf_P(PSTR("Octa frame\n"));
cliSerial->printf_P(PSTR("Octa frame\n"));
#elif FRAME_CONFIG == HELI_FRAME
Serial.printf_P(PSTR("Heli frame\n"));
cliSerial->printf_P(PSTR("Heli frame\n"));
#endif
#if FRAME_CONFIG != HELI_FRAME
if(g.frame_orientation == X_FRAME)
Serial.printf_P(PSTR("X mode\n"));
cliSerial->printf_P(PSTR("X mode\n"));
else if(g.frame_orientation == PLUS_FRAME)
Serial.printf_P(PSTR("+ mode\n"));
cliSerial->printf_P(PSTR("+ mode\n"));
else if(g.frame_orientation == V_FRAME)
Serial.printf_P(PSTR("V mode\n"));
cliSerial->printf_P(PSTR("V mode\n"));
#endif
print_blanks(2);
@ -806,7 +831,7 @@ static void report_frame()
static void report_radio()
{
Serial.printf_P(PSTR("Radio\n"));
cliSerial->printf_P(PSTR("Radio\n"));
print_divider();
// radio
print_radio_values();
@ -815,7 +840,7 @@ static void report_radio()
static void report_ins()
{
Serial.printf_P(PSTR("INS\n"));
cliSerial->printf_P(PSTR("INS\n"));
print_divider();
print_gyro_offsets();
@ -825,19 +850,19 @@ static void report_ins()
static void report_compass()
{
Serial.printf_P(PSTR("Compass\n"));
cliSerial->printf_P(PSTR("Compass\n"));
print_divider();
print_enabled(g.compass_enabled);
// mag declination
Serial.printf_P(PSTR("Mag Dec: %4.4f\n"),
cliSerial->printf_P(PSTR("Mag Dec: %4.4f\n"),
degrees(compass.get_declination()));
Vector3f offsets = compass.get_offsets();
// mag offsets
Serial.printf_P(PSTR("Mag off: %4.4f, %4.4f, %4.4f"),
cliSerial->printf_P(PSTR("Mag off: %4.4f, %4.4f, %4.4f"),
offsets.x,
offsets.y,
offsets.z);
@ -846,7 +871,7 @@ static void report_compass()
static void report_flight_modes()
{
Serial.printf_P(PSTR("Flight modes\n"));
cliSerial->printf_P(PSTR("Flight modes\n"));
print_divider();
for(int16_t i = 0; i < 6; i++ ) {
@ -858,13 +883,13 @@ static void report_flight_modes()
void report_optflow()
{
#if OPTFLOW == ENABLED
Serial.printf_P(PSTR("OptFlow\n"));
cliSerial->printf_P(PSTR("OptFlow\n"));
print_divider();
print_enabled(g.optflow_enabled);
// field of view
//Serial.printf_P(PSTR("FOV: %4.0f\n"),
//cliSerial->printf_P(PSTR("FOV: %4.0f\n"),
// degrees(g.optflow_fov));
print_blanks(2);
@ -874,22 +899,22 @@ void report_optflow()
#if FRAME_CONFIG == HELI_FRAME
static void report_heli()
{
Serial.printf_P(PSTR("Heli\n"));
cliSerial->printf_P(PSTR("Heli\n"));
print_divider();
// main servo settings
Serial.printf_P(PSTR("Servo \tpos \tmin \tmax \trev\n"));
Serial.printf_P(PSTR("1:\t%d \t%d \t%d \t%d\n"),(int)motors.servo1_pos, (int)motors._servo_1->radio_min, (int)motors._servo_1->radio_max, (int)motors._servo_1->get_reverse());
Serial.printf_P(PSTR("2:\t%d \t%d \t%d \t%d\n"),(int)motors.servo2_pos, (int)motors._servo_2->radio_min, (int)motors._servo_2->radio_max, (int)motors._servo_2->get_reverse());
Serial.printf_P(PSTR("3:\t%d \t%d \t%d \t%d\n"),(int)motors.servo3_pos, (int)motors._servo_3->radio_min, (int)motors._servo_3->radio_max, (int)motors._servo_3->get_reverse());
Serial.printf_P(PSTR("tail:\t\t%d \t%d \t%d\n"), (int)motors._servo_4->radio_min, (int)motors._servo_4->radio_max, (int)motors._servo_4->get_reverse());
cliSerial->printf_P(PSTR("Servo \tpos \tmin \tmax \trev\n"));
cliSerial->printf_P(PSTR("1:\t%d \t%d \t%d \t%d\n"),(int)motors.servo1_pos, (int)motors._servo_1->radio_min, (int)motors._servo_1->radio_max, (int)motors._servo_1->get_reverse());
cliSerial->printf_P(PSTR("2:\t%d \t%d \t%d \t%d\n"),(int)motors.servo2_pos, (int)motors._servo_2->radio_min, (int)motors._servo_2->radio_max, (int)motors._servo_2->get_reverse());
cliSerial->printf_P(PSTR("3:\t%d \t%d \t%d \t%d\n"),(int)motors.servo3_pos, (int)motors._servo_3->radio_min, (int)motors._servo_3->radio_max, (int)motors._servo_3->get_reverse());
cliSerial->printf_P(PSTR("tail:\t\t%d \t%d \t%d\n"), (int)motors._servo_4->radio_min, (int)motors._servo_4->radio_max, (int)motors._servo_4->get_reverse());
Serial.printf_P(PSTR("roll max: \t%d\n"), (int)motors.roll_max);
Serial.printf_P(PSTR("pitch max: \t%d\n"), (int)motors.pitch_max);
Serial.printf_P(PSTR("coll min:\t%d\t mid:%d\t max:%d\n"),(int)motors.collective_min, (int)motors.collective_mid, (int)motors.collective_max);
cliSerial->printf_P(PSTR("roll max: \t%d\n"), (int)motors.roll_max);
cliSerial->printf_P(PSTR("pitch max: \t%d\n"), (int)motors.pitch_max);
cliSerial->printf_P(PSTR("coll min:\t%d\t mid:%d\t max:%d\n"),(int)motors.collective_min, (int)motors.collective_mid, (int)motors.collective_max);
// calculate and print servo rate
Serial.printf_P(PSTR("servo rate:\t%d hz\n"),(int)g.rc_speed);
cliSerial->printf_P(PSTR("servo rate:\t%d hz\n"),(int)g.rc_speed);
print_blanks(2);
}
@ -897,12 +922,12 @@ static void report_heli()
static void report_gyro()
{
Serial.printf_P(PSTR("Gyro:\n"));
cliSerial->printf_P(PSTR("Gyro:\n"));
print_divider();
print_enabled( motors.ext_gyro_enabled );
if( motors.ext_gyro_enabled )
Serial.printf_P(PSTR("gain: %d"),(int)motors.ext_gyro_gain);
cliSerial->printf_P(PSTR("gain: %d"),(int)motors.ext_gyro_gain);
print_blanks(2);
}
@ -916,7 +941,7 @@ static void report_gyro()
/*static void
* print_PID(PI * pid)
* {
* Serial.printf_P(PSTR("P: %4.2f, I:%4.2f, IMAX:%ld\n"),
* cliSerial->printf_P(PSTR("P: %4.2f, I:%4.2f, IMAX:%ld\n"),
* pid->kP(),
* pid->kI(),
* (long)pid->imax());
@ -926,32 +951,32 @@ static void report_gyro()
static void
print_radio_values()
{
Serial.printf_P(PSTR("CH1: %d | %d\n"), (int)g.rc_1.radio_min, (int)g.rc_1.radio_max);
Serial.printf_P(PSTR("CH2: %d | %d\n"), (int)g.rc_2.radio_min, (int)g.rc_2.radio_max);
Serial.printf_P(PSTR("CH3: %d | %d\n"), (int)g.rc_3.radio_min, (int)g.rc_3.radio_max);
Serial.printf_P(PSTR("CH4: %d | %d\n"), (int)g.rc_4.radio_min, (int)g.rc_4.radio_max);
Serial.printf_P(PSTR("CH5: %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_max);
Serial.printf_P(PSTR("CH6: %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_max);
Serial.printf_P(PSTR("CH7: %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_max);
//Serial.printf_P(PSTR("CH8: %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_max);
cliSerial->printf_P(PSTR("CH1: %d | %d\n"), (int)g.rc_1.radio_min, (int)g.rc_1.radio_max);
cliSerial->printf_P(PSTR("CH2: %d | %d\n"), (int)g.rc_2.radio_min, (int)g.rc_2.radio_max);
cliSerial->printf_P(PSTR("CH3: %d | %d\n"), (int)g.rc_3.radio_min, (int)g.rc_3.radio_max);
cliSerial->printf_P(PSTR("CH4: %d | %d\n"), (int)g.rc_4.radio_min, (int)g.rc_4.radio_max);
cliSerial->printf_P(PSTR("CH5: %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_max);
cliSerial->printf_P(PSTR("CH6: %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_max);
cliSerial->printf_P(PSTR("CH7: %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_max);
//cliSerial->printf_P(PSTR("CH8: %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_max);
}
static void
print_switch(byte p, byte m, bool b)
{
Serial.printf_P(PSTR("Pos %d:\t"),p);
cliSerial->printf_P(PSTR("Pos %d:\t"),p);
print_flight_mode(m);
Serial.printf_P(PSTR(",\t\tSimple: "));
cliSerial->printf_P(PSTR(",\t\tSimple: "));
if(b)
Serial.printf_P(PSTR("ON\n"));
cliSerial->printf_P(PSTR("ON\n"));
else
Serial.printf_P(PSTR("OFF\n"));
cliSerial->printf_P(PSTR("OFF\n"));
}
static void
print_done()
{
Serial.printf_P(PSTR("\nSaved\n"));
cliSerial->printf_P(PSTR("\nSaved\n"));
}
@ -959,13 +984,13 @@ static void zero_eeprom(void)
{
byte b = 0;
Serial.printf_P(PSTR("\nErasing EEPROM\n"));
cliSerial->printf_P(PSTR("\nErasing EEPROM\n"));
for (uintptr_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
@ -973,7 +998,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("A_off: %4.2f, %4.2f, %4.2f\tA_scale: %4.2f, %4.2f, %4.2f\n"),
cliSerial->printf_P(PSTR("A_off: %4.2f, %4.2f, %4.2f\tA_scale: %4.2f, %4.2f, %4.2f\n"),
(float)accel_offsets.x, // Pitch
(float)accel_offsets.y, // Roll
(float)accel_offsets.z, // YAW
@ -986,7 +1011,7 @@ static void
print_gyro_offsets(void)
{
Vector3f gyro_offsets = ins.get_gyro_offsets();
Serial.printf_P(PSTR("G_off: %4.2f, %4.2f, %4.2f\n"),
cliSerial->printf_P(PSTR("G_off: %4.2f, %4.2f, %4.2f\n"),
(float)gyro_offsets.x,
(float)gyro_offsets.y,
(float)gyro_offsets.z);
@ -1014,11 +1039,11 @@ static int16_t read_num_from_serial() {
char data[5] = "";
do {
if (Serial.available() == 0) {
if (cliSerial->available() == 0) {
delay(10);
timeout++;
}else{
data[index] = Serial.read();
data[index] = cliSerial->read();
timeout = 0;
index++;
}
@ -1035,7 +1060,7 @@ print_blanks(int16_t num)
{
while(num > 0) {
num--;
Serial.println("");
cliSerial->println("");
}
}
@ -1044,11 +1069,11 @@ static bool
wait_for_yes()
{
int c;
Serial.flush();
Serial.printf_P(PSTR("Y to save\n"));
cliSerial->flush();
cliSerial->printf_P(PSTR("Y to save\n"));
do {
c = Serial.read();
c = cliSerial->read();
} while (-1 == c);
if (('y' == c) || ('Y' == c))
@ -1061,18 +1086,18 @@ static void
print_divider(void)
{
for (int i = 0; i < 40; i++) {
Serial.print_P(PSTR("-"));
cliSerial->print_P(PSTR("-"));
}
Serial.println();
cliSerial->println();
}
static void print_enabled(bool b)
{
if(b)
Serial.print_P(PSTR("en"));
cliSerial->print_P(PSTR("en"));
else
Serial.print_P(PSTR("dis"));
Serial.print_P(PSTR("abled\n"));
cliSerial->print_P(PSTR("dis"));
cliSerial->print_P(PSTR("abled\n"));
}
@ -1094,7 +1119,7 @@ static void print_wp(struct Location *cmd, byte index)
//float t1 = (float)cmd->lat / t7;
//float t2 = (float)cmd->lng / t7;
Serial.printf_P(PSTR("cmd#: %d | %d, %d, %d, %ld, %ld, %ld\n"),
cliSerial->printf_P(PSTR("cmd#: %d | %d, %d, %d, %ld, %ld, %ld\n"),
index,
cmd->id,
cmd->options,
@ -1104,7 +1129,7 @@ static void print_wp(struct Location *cmd, byte index)
cmd->lng);
/*
Serial.printf_P(PSTR("cmd#: %d id:%d op:%d p1:%d p2:%ld p3:%4.7f p4:%4.7f \n"),
cliSerial->printf_P(PSTR("cmd#: %d id:%d op:%d p1:%d p2:%ld p3:%4.7f p4:%4.7f \n"),
(int)index,
(int)cmd->id,
(int)cmd->options,
@ -1117,7 +1142,7 @@ static void print_wp(struct Location *cmd, byte index)
static void report_version()
{
Serial.printf_P(PSTR("FW Ver: %d\n"),(int)g.k_format_version);
cliSerial->printf_P(PSTR("FW Ver: %d\n"),(int)g.k_format_version);
print_divider();
print_blanks(2);
}
@ -1125,14 +1150,14 @@ static void report_version()
static void report_tuning()
{
Serial.printf_P(PSTR("\nTUNE:\n"));
cliSerial->printf_P(PSTR("\nTUNE:\n"));
print_divider();
if (g.radio_tuning == 0) {
print_enabled(g.radio_tuning.get());
}else{
float low = (float)g.radio_tuning_low.get() / 1000;
float high = (float)g.radio_tuning_high.get() / 1000;
Serial.printf_P(PSTR(" %d, Low:%1.4f, High:%1.4f\n"),(int)g.radio_tuning.get(), low, high);
cliSerial->printf_P(PSTR(" %d, Low:%1.4f, High:%1.4f\n"),(int)g.radio_tuning.get(), low, high);
}
print_blanks(2);
}

64
ArduCopter/system.pde
View File

@ -17,7 +17,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\n"
" setup\n"
" test\n"
@ -42,8 +42,12 @@ 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)
{
cliSerial = port;
Menu::set_port(port);
port->set_blocking_writes(true);
while (1) {
main_menu.run();
}
@ -76,7 +80,7 @@ static void init_ardupilot()
// The console port buffers are defined to be sufficiently large to support
// the MAVLink protocol efficiently
//
Serial.begin(SERIAL0_BAUD, 128, 256);
cliSerial->begin(SERIAL0_BAUD, 128, 256);
// GPS serial port.
//
@ -86,7 +90,7 @@ static void init_ardupilot()
Serial1.begin(38400, 256, 16);
#endif
Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE
cliSerial->printf_P(PSTR("\n\nInit " THISFIRMWARE
"\n\nFree RAM: %u\n"),
memcheck_available_memory());
@ -164,7 +168,7 @@ static void init_ardupilot()
if (!ap_system.usb_connected) {
// we are not connected via USB, re-init UART0 with right
// baud rate
Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD));
cliSerial->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD));
}
#else
// we have a 2nd serial port for telemetry
@ -257,11 +261,15 @@ static void init_ardupilot()
//
if (check_startup_for_CLI()) {
digitalWrite(A_LED_PIN, LED_ON); // turn on setup-mode LED
Serial.printf_P(PSTR("\nCLI:\n\n"));
run_cli();
cliSerial->printf_P(PSTR("\nCLI:\n\n"));
run_cli(cliSerial);
}
#else
Serial.printf_P(PSTR("\nPress ENTER 3 times for CLI\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
#if HIL_MODE != HIL_MODE_ATTITUDE
@ -328,7 +336,7 @@ static void init_ardupilot()
#endif
Serial.print_P(PSTR("\nReady to FLY "));
cliSerial->print_P(PSTR("\nReady to FLY "));
}
@ -409,7 +417,7 @@ static void set_mode(byte mode)
// have we achieved the proper altitude before RTL is enabled
set_rtl_reached_alt(false);
// debug to Serial terminal
//Serial.println(flight_mode_strings[control_mode]);
//cliSerial->println(flight_mode_strings[control_mode]);
ap.loiter_override = false;
@ -603,7 +611,7 @@ static void update_throttle_cruise(int16_t tmp)
// recalc kp
//g.pid_throttle.kP((float)g.throttle_cruise.get() / 981.0);
//Serial.printf("kp:%1.4f\n",kp);
//cliSerial->printf("kp:%1.4f\n",kp);
}
#if CLI_SLIDER_ENABLED == ENABLED && CLI_ENABLED == ENABLED
@ -630,7 +638,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;
}
@ -645,9 +653,9 @@ static void check_usb_mux(void)
// the user has switched to/from the telemetry port
ap_system.usb_connected = usb_check;
if (ap_system.usb_connected) {
Serial.begin(SERIAL0_BAUD);
cliSerial->begin(SERIAL0_BAUD);
} else {
Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD));
cliSerial->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD));
}
}
#endif
@ -681,46 +689,46 @@ print_flight_mode(uint8_t mode)
{
switch (mode) {
case STABILIZE:
Serial.print_P(PSTR("STABILIZE"));
cliSerial->print_P(PSTR("STABILIZE"));
break;
case ACRO:
Serial.print_P(PSTR("ACRO"));
cliSerial->print_P(PSTR("ACRO"));
break;
case ALT_HOLD:
Serial.print_P(PSTR("ALT_HOLD"));
cliSerial->print_P(PSTR("ALT_HOLD"));
break;
case AUTO:
Serial.print_P(PSTR("AUTO"));
cliSerial->print_P(PSTR("AUTO"));
break;
case GUIDED:
Serial.print_P(PSTR("GUIDED"));
cliSerial->print_P(PSTR("GUIDED"));
break;
case LOITER:
Serial.print_P(PSTR("LOITER"));
cliSerial->print_P(PSTR("LOITER"));
break;
case RTL:
Serial.print_P(PSTR("RTL"));
cliSerial->print_P(PSTR("RTL"));
break;
case CIRCLE:
Serial.print_P(PSTR("CIRCLE"));
cliSerial->print_P(PSTR("CIRCLE"));
break;
case POSITION:
Serial.print_P(PSTR("POSITION"));
cliSerial->print_P(PSTR("POSITION"));
break;
case LAND:
Serial.print_P(PSTR("LAND"));
cliSerial->print_P(PSTR("LAND"));
break;
case OF_LOITER:
Serial.print_P(PSTR("OF_LOITER"));
cliSerial->print_P(PSTR("OF_LOITER"));
break;
case TOY_M:
Serial.print_P(PSTR("TOY_M"));
cliSerial->print_P(PSTR("TOY_M"));
break;
case TOY_A:
Serial.print_P(PSTR("TOY_A"));
cliSerial->print_P(PSTR("TOY_A"));
break;
default:
Serial.print_P(PSTR("---"));
cliSerial->print_P(PSTR("---"));
break;
}
}

214
ArduCopter/test.pde
View File

@ -44,7 +44,7 @@ extern void print_latlon(BetterStream *s, int32_t lat_or_lon);
// printf_P is a version of printf that reads from flash memory
/*static int8_t help_test(uint8_t argc, const Menu::arg *argv)
* {
* Serial.printf_P(PSTR("\n"
* cliSerial->printf_P(PSTR("\n"
* "Commands:\n"
* " radio\n"
* " servos\n"
@ -96,7 +96,7 @@ 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;
}
@ -108,10 +108,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);
}
@ -137,7 +137,7 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
// servo Yaw
//APM_RC.OutputCh(CH_7, g.rc_4.radio_out);
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"),
g.rc_1.radio_in,
g.rc_2.radio_in,
g.rc_3.radio_in,
@ -147,7 +147,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);
}
}
@ -170,13 +170,13 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
* g.rc_4.servo_out = g.rc_4.control_in;
* g.rc_4.calc_pwm();
*
* Serial.printf_P(PSTR("input: %d\toutput%d\n"),
* cliSerial->printf_P(PSTR("input: %d\toutput%d\n"),
* g.rc_4.control_in,
* g.rc_4.radio_out);
*
* APM_RC.OutputCh(CH_TRI_YAW, g.rc_4.radio_out);
*
* if(Serial.available() > 0){
* if(cliSerial->available() > 0){
* return (0);
* }
* }
@ -191,7 +191,7 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
for(altitude_error = 2000; altitude_error > -100; altitude_error--){
int16_t temp = get_desired_climb_rate();
Serial.printf("%ld, %d\n", altitude_error, temp);
cliSerial->printf("%ld, %d\n", altitude_error, temp);
}
return 0;
}
@ -202,7 +202,7 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
int32_t temp = 2 * 100 * (wp_distance - g.waypoint_radius * 100);
max_speed = sqrt((float)temp);
max_speed = min(max_speed, g.waypoint_speed_max);
Serial.printf("Zspeed: %ld, %d, %ld\n", temp, max_speed, wp_distance);
cliSerial->printf("Zspeed: %ld, %d, %ld\n", temp, max_speed, wp_distance);
wp_distance += 100;
}
return 0;
@ -220,17 +220,17 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
* g.toy_yaw_rate = 3;
* yaw_rate = g.rc_1.control_in / g.toy_yaw_rate;
* roll_rate = ((int32_t)g.rc_2.control_in * (yaw_rate/100)) /40;
* Serial.printf("yaw_rate, %d, roll_rate, %d\n", yaw_rate, roll_rate);
* cliSerial->printf("yaw_rate, %d, roll_rate, %d\n", yaw_rate, roll_rate);
*
* g.toy_yaw_rate = 2;
* yaw_rate = g.rc_1.control_in / g.toy_yaw_rate;
* roll_rate = ((int32_t)g.rc_2.control_in * (yaw_rate/100)) /40;
* Serial.printf("yaw_rate, %d, roll_rate, %d\n", yaw_rate, roll_rate);
* cliSerial->printf("yaw_rate, %d, roll_rate, %d\n", yaw_rate, roll_rate);
*
* g.toy_yaw_rate = 1;
* yaw_rate = g.rc_1.control_in / g.toy_yaw_rate;
* roll_rate = ((int32_t)g.rc_2.control_in * (yaw_rate/100)) /40;
* Serial.printf("yaw_rate, %d, roll_rate, %d\n", yaw_rate, roll_rate);
* cliSerial->printf("yaw_rate, %d, roll_rate, %d\n", yaw_rate, roll_rate);
* }*/
static int8_t
@ -244,7 +244,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
read_radio();
Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\n"),
cliSerial->printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\n"),
g.rc_1.control_in,
g.rc_2.control_in,
g.rc_3.control_in,
@ -253,9 +253,9 @@ test_radio(uint8_t argc, const Menu::arg *argv)
g.rc_6.control_in,
g.rc_7.control_in);
//Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d\n"), (g.rc_1.servo_out / 100), (g.rc_2.servo_out / 100), g.rc_3.servo_out, (g.rc_4.servo_out / 100));
//cliSerial->printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d\n"), (g.rc_1.servo_out / 100), (g.rc_2.servo_out / 100), g.rc_3.servo_out, (g.rc_4.servo_out / 100));
/*Serial.printf_P(PSTR( "min: %d"
/*cliSerial->printf_P(PSTR( "min: %d"
* "\t in: %d"
* "\t pwm_in: %d"
* "\t sout: %d"
@ -267,7 +267,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
* g.rc_3.pwm_out
* );
*/
if(Serial.available() > 0) {
if(cliSerial->available() > 0) {
return (0);
}
}
@ -288,7 +288,7 @@ test_radio(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.rc_3.control_in > 0){
* delay(20);
* read_radio();
@ -299,27 +299,27 @@ test_radio(uint8_t argc, const Menu::arg *argv)
* read_radio();
*
* if(g.rc_3.control_in > 0){
* Serial.printf_P(PSTR("THROTTLE CHANGED %d \n"), g.rc_3.control_in);
* cliSerial->printf_P(PSTR("THROTTLE CHANGED %d \n"), g.rc_3.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.rc_3.get_failsafe()){
* Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.rc_3.radio_in);
* Serial.println(flight_mode_strings[readSwitch()]);
* cliSerial->printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.rc_3.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 ACM.\n"));
* if(cliSerial->available() > 0){
* cliSerial->printf_P(PSTR("LOS caused no change in ACM.\n"));
* return (0);
* }
* }
@ -344,8 +344,8 @@ test_radio(uint8_t argc, const Menu::arg *argv)
* init_rc_in();
*
* control_mode = STABILIZE;
* Serial.printf_P(PSTR("g.pi_stabilize_roll.kP: %4.4f\n"), g.pi_stabilize_roll.kP());
* Serial.printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener);
* cliSerial->printf_P(PSTR("g.pi_stabilize_roll.kP: %4.4f\n"), g.pi_stabilize_roll.kP());
* cliSerial->printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener);
*
* motors.auto_armed(false);
* motors.armed(true);
@ -394,7 +394,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
* ts_num++;
* if (ts_num > 10){
* ts_num = 0;
* Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, "),
* cliSerial->printf_P(PSTR("r: %d, p:%d, rc1:%d, "),
* (int)(dcm.roll_sensor/100),
* (int)(dcm.pitch_sensor/100),
* g.rc_1.pwm_out);
@ -403,10 +403,10 @@ test_radio(uint8_t argc, const Menu::arg *argv)
* }
* // R: 1417, L: 1453 F: 1453 B: 1417
*
* //Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100));
* //Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100));
* //cliSerial->printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100));
* //cliSerial->printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100));
*
* if(Serial.available() > 0){
* if(cliSerial->available() > 0){
* if(g.compass_enabled){
* compass.save_offsets();
* report_compass();
@ -426,7 +426,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
* //test_adc(uint8_t argc, const Menu::arg *argv)
* {
* print_hit_enter();
* Serial.printf_P(PSTR("ADC\n"));
* cliSerial->printf_P(PSTR("ADC\n"));
* delay(1000);
*
* adc.Init(&timer_scheduler);
@ -435,11 +435,11 @@ test_radio(uint8_t argc, const Menu::arg *argv)
*
* while(1){
* for(int16_t i = 0; i < 9; i++){
* Serial.printf_P(PSTR("%.1f,"),adc.Ch(i));
* cliSerial->printf_P(PSTR("%.1f,"),adc.Ch(i));
* }
* Serial.println();
* cliSerial->println();
* delay(20);
* if(Serial.available() > 0){
* if(cliSerial->available() > 0){
* return (0);
* }
* }
@ -457,7 +457,7 @@ test_ins(uint8_t argc, const Menu::arg *argv)
Vector3f gyro, accel;
float temp;
print_hit_enter();
Serial.printf_P(PSTR("INS\n"));
cliSerial->printf_P(PSTR("INS\n"));
delay(1000);
ins.init(AP_InertialSensor::COLD_START, delay, flash_leds, &timer_scheduler);
@ -472,13 +472,13 @@ test_ins(uint8_t argc, const Menu::arg *argv)
float test = sqrt(sq(accel.x) + sq(accel.y) + sq(accel.z)) / 9.80665;
Serial.printf_P(PSTR("a %7.4f %7.4f %7.4f g %7.4f %7.4f %7.4f t %74f | %7.4f\n"),
cliSerial->printf_P(PSTR("a %7.4f %7.4f %7.4f g %7.4f %7.4f %7.4f t %74f | %7.4f\n"),
accel.x, accel.y, accel.z,
gyro.x, gyro.y, gyro.z,
temp, test);
delay(40);
if(Serial.available() > 0) {
if(cliSerial->available() > 0) {
return (0);
}
}
@ -506,18 +506,18 @@ test_gps(uint8_t argc, const Menu::arg *argv)
g_gps->update();
if (g_gps->new_data) {
Serial.printf_P(PSTR("Lat: "));
cliSerial->printf_P(PSTR("Lat: "));
print_latlon(&Serial, g_gps->latitude);
Serial.printf_P(PSTR(", Lon "));
cliSerial->printf_P(PSTR(", Lon "));
print_latlon(&Serial, g_gps->longitude);
Serial.printf_P(PSTR(", Alt: %ldm, #sats: %d\n"),
cliSerial->printf_P(PSTR(", Alt: %ldm, #sats: %d\n"),
g_gps->altitude/100,
g_gps->num_sats);
g_gps->new_data = false;
}else{
Serial.print_P(PSTR("."));
cliSerial->print_P(PSTR("."));
}
if(Serial.available() > 0) {
if(cliSerial->available() > 0) {
return (0);
}
}
@ -531,7 +531,7 @@ test_gps(uint8_t argc, const Menu::arg *argv)
* {
* print_hit_enter();
* delay(1000);
* Serial.printf_P(PSTR("Gyro | Accel\n"));
* cliSerial->printf_P(PSTR("Gyro | Accel\n"));
* Vector3f _cam_vector;
* Vector3f _out_vector;
*
@ -548,7 +548,7 @@ test_gps(uint8_t argc, const Menu::arg *argv)
* Matrix3f temp = dcm.get_dcm_matrix();
* Matrix3f temp_t = dcm.get_dcm_transposed();
*
* Serial.printf_P(PSTR("dcm\n"
* cliSerial->printf_P(PSTR("dcm\n"
* "%4.4f \t %4.4f \t %4.4f \n"
* "%4.4f \t %4.4f \t %4.4f \n"
* "%4.4f \t %4.4f \t %4.4f \n\n"),
@ -559,18 +559,18 @@ test_gps(uint8_t argc, const Menu::arg *argv)
* int16_t _pitch = degrees(-asin(temp.c.x));
* int16_t _roll = degrees(atan2(temp.c.y, temp.c.z));
* int16_t _yaw = degrees(atan2(temp.b.x, temp.a.x));
* Serial.printf_P(PSTR( "angles\n"
* cliSerial->printf_P(PSTR( "angles\n"
* "%d \t %d \t %d\n\n"),
* _pitch,
* _roll,
* _yaw);
*
* //_out_vector = _cam_vector * temp;
* //Serial.printf_P(PSTR( "cam\n"
* //cliSerial->printf_P(PSTR( "cam\n"
* // "%d \t %d \t %d\n\n"),
* // (int)temp.a.x * 100, (int)temp.a.y * 100, (int)temp.a.x * 100);
*
* if(Serial.available() > 0){
* if(cliSerial->available() > 0){
* return (0);
* }
* }
@ -582,18 +582,18 @@ test_gps(uint8_t argc, const Menu::arg *argv)
* {
* print_hit_enter();
* delay(1000);
* Serial.printf_P(PSTR("Gyro | Accel\n"));
* cliSerial->printf_P(PSTR("Gyro | Accel\n"));
* delay(1000);
*
* while(1){
* Vector3f accels = dcm.get_accel();
* Serial.print("accels.z:");
* Serial.print(accels.z);
* Serial.print("omega.z:");
* Serial.print(omega.z);
* cliSerial->print("accels.z:");
* cliSerial->print(accels.z);
* cliSerial->print("omega.z:");
* cliSerial->print(omega.z);
* delay(100);
*
* if(Serial.available() > 0){
* if(cliSerial->available() > 0){
* return (0);
* }
* }
@ -608,7 +608,7 @@ test_gps(uint8_t argc, const Menu::arg *argv)
*
* print_hit_enter();
* delay(1000);
* Serial.printf_P(PSTR("Omega"));
* cliSerial->printf_P(PSTR("Omega"));
* delay(1000);
*
* G_Dt = .02;
@ -626,11 +626,11 @@ test_gps(uint8_t argc, const Menu::arg *argv)
* ts_num++;
* if (ts_num > 2){
* ts_num = 0;
* //Serial.printf_P(PSTR("R: %4.4f\tP: %4.4f\tY: %4.4f\tY: %4.4f\n"), omega.x, omega.y, omega.z, my_oz);
* Serial.printf_P(PSTR(" Yaw: %ld\tY: %4.4f\tY: %4.4f\n"), dcm.yaw_sensor, omega.z, my_oz);
* //cliSerial->printf_P(PSTR("R: %4.4f\tP: %4.4f\tY: %4.4f\tY: %4.4f\n"), omega.x, omega.y, omega.z, my_oz);
* cliSerial->printf_P(PSTR(" Yaw: %ld\tY: %4.4f\tY: %4.4f\n"), dcm.yaw_sensor, omega.z, my_oz);
* }
*
* if(Serial.available() > 0){
* if(cliSerial->available() > 0){
* return (0);
* }
* }
@ -647,9 +647,9 @@ test_tuning(uint8_t argc, const Menu::arg *argv)
delay(200);
read_radio();
tuning();
Serial.printf_P(PSTR("tune: %1.3f\n"), tuning_value);
cliSerial->printf_P(PSTR("tune: %1.3f\n"), tuning_value);
if(Serial.available() > 0) {
if(cliSerial->available() > 0) {
return (0);
}
}
@ -662,12 +662,12 @@ test_battery(uint8_t argc, const Menu::arg *argv)
print_test_disabled();
return (0);
#else
Serial.printf_P(PSTR("\nCareful! Motors will spin! Press Enter to start.\n"));
Serial.flush();
while(!Serial.available()) {
cliSerial->printf_P(PSTR("\nCareful! Motors will spin! Press Enter to start.\n"));
cliSerial->flush();
while(!cliSerial->available()) {
delay(100);
}
Serial.flush();
cliSerial->flush();
print_hit_enter();
// allow motors to spin
@ -679,19 +679,19 @@ 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, Ah: %4.4f\n"),
cliSerial->printf_P(PSTR("V: %4.4f, A: %4.4f, Ah: %4.4f\n"),
battery_voltage1,
current_amps1,
current_total1);
}
motors.throttle_pass_through();
if(Serial.available() > 0) {
if(cliSerial->available() > 0) {
motors.armed(false);
return (0);
}
@ -712,17 +712,17 @@ static int8_t 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);
}
}
@ -736,16 +736,16 @@ test_wp(uint8_t argc, const Menu::arg *argv)
delay(1000);
// save the alitude above home option
Serial.printf_P(PSTR("Hold alt "));
cliSerial->printf_P(PSTR("Hold alt "));
if(g.RTL_altitude < 0) {
Serial.printf_P(PSTR("\n"));
cliSerial->printf_P(PSTR("\n"));
}else{
Serial.printf_P(PSTR("of %dm\n"), (int)g.RTL_altitude / 100);
cliSerial->printf_P(PSTR("of %dm\n"), (int)g.RTL_altitude / 100);
}
Serial.printf_P(PSTR("%d wp\n"), (int)g.command_total);
Serial.printf_P(PSTR("Hit rad: %dm\n"), (int)g.waypoint_radius);
//Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius);
cliSerial->printf_P(PSTR("%d wp\n"), (int)g.command_total);
cliSerial->printf_P(PSTR("Hit rad: %dm\n"), (int)g.waypoint_radius);
//cliSerial->printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius);
report_wp();
@ -767,7 +767,7 @@ test_wp(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);
* }
* }
@ -779,13 +779,13 @@ test_wp(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);
* }
* }
@ -811,10 +811,10 @@ test_baro(uint8_t argc, const Menu::arg *argv)
int16_t temp = barometer.get_temperature();
int32_t raw_pres = barometer.get_raw_pressure();
int32_t raw_temp = barometer.get_raw_temp();
Serial.printf_P(PSTR("alt: %ldcm, pres: %ldmbar, temp: %d/100degC,"
cliSerial->printf_P(PSTR("alt: %ldcm, pres: %ldmbar, temp: %d/100degC,"
" raw pres: %ld, raw temp: %ld\n"),
alt, pres,temp, raw_pres, raw_temp);
if(Serial.available() > 0) {
if(cliSerial->available() > 0) {
return (0);
}
}
@ -838,21 +838,21 @@ test_mag(uint8_t argc, const Menu::arg *argv)
delay(100);
if (compass.read()) {
float heading = compass.calculate_heading(ahrs.get_dcm_matrix());
Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"),
cliSerial->printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"),
(wrap_360(ToDeg(heading) * 100)) /100,
compass.mag_x,
compass.mag_y,
compass.mag_z);
} else {
Serial.println_P(PSTR("not healthy"));
cliSerial->println_P(PSTR("not healthy"));
}
if(Serial.available() > 0) {
if(cliSerial->available() > 0) {
return (0);
}
}
} else {
Serial.printf_P(PSTR("Compass: "));
cliSerial->printf_P(PSTR("Compass: "));
print_enabled(false);
return (0);
}
@ -876,7 +876,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
* g.rc_4.set_pwm(APM_RC.InputCh(CH_4));
* g.rc_4.servo_out = g.rc_4.control_in;
* g.rc_4.calc_pwm();
* Serial.printf_P(PSTR("PWM:%d input: %d\toutput%d "),
* cliSerial->printf_P(PSTR("PWM:%d input: %d\toutput%d "),
* APM_RC.InputCh(CH_4),
* g.rc_4.control_in,
* g.rc_4.radio_out);
@ -887,13 +887,13 @@ test_mag(uint8_t argc, const Menu::arg *argv)
* g.rc_4.set_pwm(APM_RC.InputCh(CH_4));
* g.rc_4.servo_out = g.rc_4.control_in;
* g.rc_4.calc_pwm();
* Serial.printf_P(PSTR("\trev input: %d\toutput%d\n"),
* cliSerial->printf_P(PSTR("\trev input: %d\toutput%d\n"),
* g.rc_4.control_in,
* g.rc_4.radio_out);
*
* APM_RC.OutputCh(CH_7, g.rc_4.radio_out);
*
* if(Serial.available() > 0){
* if(cliSerial->available() > 0){
* g.rc_4.set_reverse(0);
* return (0);
* }
@ -908,7 +908,7 @@ static int8_t
test_sonar(uint8_t argc, const Menu::arg *argv)
{
if(g.sonar_enabled == false) {
Serial.printf_P(PSTR("Sonar disabled\n"));
cliSerial->printf_P(PSTR("Sonar disabled\n"));
return (0);
}
@ -919,10 +919,10 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
while(1) {
delay(100);
Serial.printf_P(PSTR("Sonar: %d cm\n"), sonar.read());
//Serial.printf_P(PSTR("Sonar, %d, %d\n"), sonar.read(), sonar.raw_value);
cliSerial->printf_P(PSTR("Sonar: %d cm\n"), sonar.read());
//cliSerial->printf_P(PSTR("Sonar, %d, %d\n"), sonar.read(), sonar.raw_value);
if(Serial.available() > 0) {
if(cliSerial->available() > 0) {
return (0);
}
}
@ -937,26 +937,26 @@ test_optflow(uint8_t argc, const Menu::arg *argv)
{
#if OPTFLOW == ENABLED
if(g.optflow_enabled) {
Serial.printf_P(PSTR("man id: %d\t"),optflow.read_register(ADNS3080_PRODUCT_ID));
cliSerial->printf_P(PSTR("man id: %d\t"),optflow.read_register(ADNS3080_PRODUCT_ID));
print_hit_enter();
while(1) {
delay(200);
optflow.update(millis());
Log_Write_Optflow();
Serial.printf_P(PSTR("x/dx: %d/%d\t y/dy %d/%d\t squal:%d\n"),
cliSerial->printf_P(PSTR("x/dx: %d/%d\t y/dy %d/%d\t squal:%d\n"),
optflow.x,
optflow.dx,
optflow.y,
optflow.dy,
optflow.surface_quality);
if(Serial.available() > 0) {
if(cliSerial->available() > 0) {
return (0);
}
}
} else {
Serial.printf_P(PSTR("OptFlow: "));
cliSerial->printf_P(PSTR("OptFlow: "));
print_enabled(false);
}
return (0);
@ -979,7 +979,7 @@ test_wp_nav(uint8_t argc, const Menu::arg *argv)
// got 23506;, should be 22800
update_navigation();
Serial.printf_P(PSTR("bear: %ld\n"), target_bearing);
cliSerial->printf_P(PSTR("bear: %ld\n"), target_bearing);
return 0;
}
@ -994,20 +994,20 @@ test_logging(uint8_t argc, const Menu::arg *argv)
print_test_disabled();
return (0);
#else
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;
#endif
@ -1077,12 +1077,12 @@ test_logging(uint8_t argc, const Menu::arg *argv)
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 void print_test_disabled()
{
Serial.printf_P(PSTR("Sorry, not 1280 compat.\n"));
cliSerial->printf_P(PSTR("Sorry, not 1280 compat.\n"));
}
/*
@ -1110,7 +1110,7 @@ static void print_test_disabled()
*/
/*
* //static void print_motor_out(){
* Serial.printf("out: R: %d, L: %d F: %d B: %d\n",
* cliSerial->printf("out: R: %d, L: %d F: %d B: %d\n",
* (motor_out[CH_1] - g.rc_3.radio_min),
* (motor_out[CH_2] - g.rc_3.radio_min),
* (motor_out[CH_3] - g.rc_3.radio_min),

4
ArduCopter/toy.pde
View File

@ -156,7 +156,7 @@ void roll_pitch_toy()
#elif TOY_MIXER == TOY_LINEAR_MIXER
roll_rate = -((int32_t)g.rc_2.control_in * (yaw_rate/100)) /30;
//Serial.printf("roll_rate: %d\n",roll_rate);
//cliSerial->printf("roll_rate: %d\n",roll_rate);
roll_rate = constrain(roll_rate, -2000, 2000);
#elif TOY_MIXER == TOY_EXTERNAL_MIXER
@ -179,4 +179,4 @@ void roll_pitch_toy()
get_stabilize_pitch(g.rc_2.control_in);
#endif
}
}