2011-09-08 22:31:32 -03:00
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/* This GCS protocol sends text-based information over the GCS port
*/
#if GCS_PROTOCOL == GCS_PROTOCOL_DEBUGTERMINAL
#define ERR(a) ((DEBUGTERMINAL_VERBOSE)>0?(PSTR(a)):(0))
void
GCS_DEBUGTERMINAL::update()
{
byte numc, i, c;
numc = _port->available();
for (i=0;i<numc;i++) {
c = _port->read();
processgcsinput(c);
}
}
void
GCS_DEBUGTERMINAL::processgcsinput(char c)
{
if (c==8) {
//Received a backspace character; move the buffer index backwards
if (bufferidx > 0) bufferidx--;
} else if (c==10) {
//Received a linefeed; do nothing
} else if (c==13) {
//Received a carriage return; process command
gcsinputbuffer[bufferidx] = 0;
run_debugt_command(gcsinputbuffer);
bufferidx = 0;
} else {
gcsinputbuffer[bufferidx++] = c;
if (bufferidx >= sizeof(gcsinputbuffer)) bufferidx = 0;
}
}
void
GCS_DEBUGTERMINAL::run_debugt_command(char *buf)
{
BetterStream *port = _port;
//*********** Ignore comments ***********
if (strmatch(buf, PSTR("//"))) {
//*********** Process 'show' commands ***********
} else if (strmatch(buf, PSTR("show "))) {
if (strmatch(buf+5, PSTR("heartbeat")))
report_heartbeat = 1;
else if (strmatch(buf+5, PSTR("attitude")))
report_attitude = 1;
else if (strmatch(buf+5, PSTR("location")))
report_location = 1;
else if (strmatch(buf+5, PSTR("command")))
report_command = 1;
else if (strmatch(buf+5, PSTR("cpuload")))
report_cpu_load = 1;
else if (strmatch(buf+5, PSTR("severity")))
report_severity = atoi(buf+14);
else
print_error(ERR("USAGE: show {heartbeat|attitude|location|command|severity <X>}"));
//*********** Process 'hide' commands ***********
} else if (strmatch(buf, PSTR("hide "))) {
if (strmatch(buf+5, PSTR("heartbeat")))
report_heartbeat = 0;
else if (strmatch(buf+5, PSTR("attitude")))
report_attitude = 0;
else if (strmatch(buf+5, PSTR("location")))
report_location = 0;
else if (strmatch(buf+5, PSTR("command")))
report_command = 0;
else if (strmatch(buf+5, PSTR("cpuload")))
report_cpu_load = 0;
else if (strmatch(buf+5, PSTR("all"))) {
report_heartbeat = 0;
report_attitude = 0;
report_location = 0;
report_command = 0;
report_cpu_load = 0;
} else
print_error(ERR("USAGE: hide {heartbeat|attitude|location|command|all}"));
//*********** Process 'copy' command ***********
} else if (strmatch(buf, PSTR("copy "))) {
//------- copy cmd <N1> <N2> -------
if (strmatch(buf+5, PSTR("cmd "))) {
unsigned char i = 9, index1, index2;
while (buf[i] != 0) {
i++;
if (buf[i] == ' ') break;
}
if (buf[i] == ' ') {
buf[i] = 0;
index1 = atoi(buf+9);
index2 = atoi(buf+i+1);
Location temp = get_wp_with_index(index1);
set_wp_with_index(temp, index2);
port->print_P(PSTR("Copied command index ")); port->print(index1,DEC); port->print_P(PSTR(" to ")); port->println(index2,DEC);
} else {
print_error(ERR("USAGE: copy cmd <srcindex> <targetindex>"));
}
}
//*********** Process 'echo' command ***********
} else if (strmatch(buf, PSTR("echo "))) {
port->println(buf+5);
//*********** Process 'groundstart' command ***********
} else if (strmatch(buf, PSTR("groundstart"))) {
startup_ground();
//*********** Process 'inithome' command ***********
} else if (strmatch(buf, PSTR("inithome"))) {
init_home();
port->println_P(PSTR("Home set."));
//------- k -? -------
} else if (strmatch(buf, PSTR("k -?"))) {
print_error(ERR("USAGE: {print|set} {k{p|i|d}|imax} {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt}"));
print_error(ERR("USAGE: {print|set} kff {pitchcomp|ruddermix|pitchtothrottle}"));
//*********** Process 'print' commands ***********
} else if (strmatch(buf, PSTR("print "))) {
//------- print airspeedtrim -------
if (strmatch(buf+6, PSTR("airspeedtrim"))) {
port->printf_P("Trim airspeed = %f\n", (float)get(PARAM_TRIM_AIRSPEED)/100);
//------- print airspeednudge -------
} else if (strmatch(buf+6, PSTR("airspeednudge"))) {
port->printf_P("Airspeed nudge = %f\n", (float)airspeed_nudge/100);
//------- print altitude -------
} else if (strmatch(buf+6, PSTR("altitude"))) {
recalc_climb_rate();
port->printf_P(PSTR("Altitude:\n"
" Pressure: %.2fm\n"
" GPS: %.2fm\n"
" Mix ratio: %.3f\n"
" Mix: %.2fm\n"
" Above home: %.1fm\n"
" Climb rate: %.2fm/s\n"),
(float)press_alt / 100,
(float)gps.altitude / 100,
get(PARAM_ALT_MIX),
(((1.0 - get(PARAM_ALT_MIX)) * gps.altitude) + (get(PARAM_ALT_MIX) * press_alt)) / 100,
(float)get_altitude_above_home()/100,
(float)climb_rate/100);
//------- print attitude -------
} else if (strmatch(buf+6, PSTR("attitude"))) {
print_attitude();
//------- print commands[ <N1>-<N2>] -------
} else if (strmatch(buf+6, PSTR("commands"))) {
unsigned char dash, index1, index2;
for (dash=14; dash<sizeof(gcsinputbuffer); dash++) {
if (buf[dash] == 0) break;
if (buf[dash] == '-') break;
}
if (buf[dash] == 0) {
print_commands();
} else {
buf[dash] = 0;
index1 = atoi(buf+14);
index2 = atoi(buf+dash+1);
if (index2 < index1) index2 = get(PARAM_WP_TOTAL);
print_commands(index1, index2);
}
//------- print ctrlmode -------
} else if (strmatch(buf+6, PSTR("ctrlmode"))) {
print_control_mode();
//------- print curwaypts -------
} else if (strmatch(buf+6, PSTR("curwaypts"))) {
print_current_waypoints();
//------- print gains -------
} else if (strmatch(buf+6, PSTR("gains"))) {
print_gains();
//------- print flightmodes -------
} else if (strmatch(buf+6, PSTR("flightmodes"))) {
int i;
port->print_P(PSTR("EEPROM read: "));
for (i=0; i<6; i++) {
port->print_P(PSTR("Ch ")); port->print(i,DEC); port->print_P(PSTR(" = ")); port->print(get(uint8_param_t(PARAM_FLIGHT_MODE_1+i)),DEC); port->print_P(PSTR(", "));
}
port->println(" ");
//------- print holdalt -------
} else if (strmatch(buf+6, PSTR("holdalt"))) {
port->print_P(PSTR("Altitude above home set to ")); port->println(get(PARAM_ALT_HOLD_HOME),2);
//------- print imax {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} -------
} else if (strmatch(buf+6, PSTR("imax "))) {
int i;
unsigned char j;
if (get_pid_offset_name(buf+11, &i, &j)) {
port->print_P(PSTR("Integrator maximum for "));
port->print(buf+9);
port->print_P(PSTR(" = "));
port->println(pid_index[i]->imax(),DEC);
} else
print_gain_keyword_error();
//------- print kp {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} -------
} else if (strmatch(buf+6, PSTR("kp "))) {
int i;
unsigned char j;
if (get_pid_offset_name(buf+9, &i, &j)) {
port->print_P(PSTR("P gain for "));
port->print(buf+9);
port->print_P(PSTR(" = "));
port->println(pid_index[i]->kP(),DEC);
} else
print_gain_keyword_error();
//------- print ki {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} -------
} else if (strmatch(buf+6, PSTR("ki "))) {
int i;
unsigned char j;
if (get_pid_offset_name(buf+9, &i, &j)) {
port->print_P(PSTR("I gain for "));
port->print(buf+9);
port->print_P(PSTR(" = "));
port->println(pid_index[i]->kI(),DEC);
} else
print_gain_keyword_error();
//------- print kd {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} -------
} else if (strmatch(buf+6, PSTR("kd "))) {
int i;
unsigned char j;
if (get_pid_offset_name(buf+9, &i, &j)) {
port->print_P(PSTR("D gain for "));
port->print(buf+9);
port->print_P(PSTR(" = "));
port->println(pid_index[i]->kD(),DEC);
} else
print_gain_keyword_error();
//------- print kff {pitchcomp|ruddermix|pitchtothrottle} -------
} else if (strmatch(buf+6, PSTR("kff "))) {
if (strmatch(buf+10, PSTR("pitchcomp"))) {
port->print_P(PSTR("FF gain for pitchcomp = "));
port->println(get(PARAM_KFF_PTCHCOMP),DEC);
} else if (strmatch(buf+10, PSTR("ruddermix"))) {
port->print_P(PSTR("FF gain for ruddermix = "));
port->println(get(PARAM_KFF_RDDRMIX),DEC);
} else if (strmatch(buf+10, PSTR("pitchtothrottle"))) {
port->print_P(PSTR("FF gain for pitchtothrottle = "));
port->println(get(PARAM_KFF_PTCH2THR),DEC);
/*} else if (strmatch(buf+10, PSTR("throttletopitch"))) {
port->print_P(PSTR("FF gain for throttletopitch = "));
port->println(kff[CASE_T_TO_P],DEC);*/
} else
print_gain_keyword_error();
//------- print location -------
} else if (strmatch(buf+6, PSTR("location"))) {
print_position();
//------- print navrolllimit -------
} else if (strmatch(buf+6, PSTR("navrolllimit"))) {
port->print_P(PSTR("Nav roll limit = ")); port->println((float)get(PARAM_LIM_ROLL)/100,2);
//------- print navsettings -------
} else if (strmatch(buf+6, PSTR("navsettings"))) {
port->printf_P(PSTR("Navigation settings:\n"
#if AIRSPEED_SENSOR == ENABLED
" Cruise airspeed: %.2f\n"
#else
" Cruise throttle: %d\n"
#endif
" Hold altitude above home: %ld\n"
" Loiter radius: %d\n"
" Waypoint radius: %d\n"),
#if AIRSPEED_SENSOR == ENABLED
(float)get(PARAM_TRIM_AIRSPEED) / 100,
#else
get(PARAM_TRIM_THROTTLE),
#endif
get(PARAM_ALT_HOLD_HOME),
get(PARAM_LOITER_RADIUS),
get(PARAM_WP_RADIUS));
//------- print pitchmax -------
} else if (strmatch(buf+6, PSTR("pitchmax"))) {
port->print_P(PSTR("Maximum pitch = ")); port->println((float)get(PARAM_LIM_PITCH_MAX)/100,2);
//------- print pitchmin -------
} else if (strmatch(buf+6, PSTR("pitchmin"))) {
port->print_P(PSTR("Minimum pitch = ")); port->println((float)get(PARAM_LIM_PITCH_MIN)/100,2);
//------- print pitchtarget -------
} else if (strmatch(buf+6, PSTR("pitchtarget"))) {
port->print_P(PSTR("Target pitch = ")); port->println((float)get(PARAM_TRIM_PITCH)/100,2);
#if HIL_MODE != HIL_MODE_ATTITUDE
//------- print pressure -------
} else if (strmatch(buf+6, PSTR("pressure"))) {
port->println_P(PSTR("BMP085 pressure sensor:"));
port->print_P(PSTR(" Temperature: ")); port->println(pitot.Temp,DEC);
port->print_P(PSTR(" Pressure: ")); port->println(pitot.Press,DEC);
#endif
//------- print rlocation home -------
} else if (strmatch(buf+6, PSTR("rlocation home"))) {
print_rlocation(&home);
//------- print rlocation -------
//(implication is "relative to next waypoint")
} else if (strmatch(buf+6, PSTR("rlocation"))) {
print_rlocation(&next_WP);
//------- print speed -------
} else if (strmatch(buf+6, PSTR("speed"))) {
port->println_P(PSTR("Speed:"));
port->print_P(PSTR(" Ground: ")); port->println((float)gps.ground_speed/100.0,2);
#if AIRSPEED_SENSOR == ENABLED
port->print_P(PSTR(" Air: ")); port->println((float)airspeed/100.0,2);
port->print_P(PSTR(" Cruise: ")); port->println((float)get(PARAM_TRIM_AIRSPEED)/100.0,2);
#endif
//------- print throttlecruise -------
} else if (strmatch(buf+6, PSTR("throttlecruise"))) {
port->print_P(PSTR("Throttle cruise = ")); port->print(get(PARAM_TRIM_THROTTLE),DEC); port->println_P(PSTR("%"));
//------- print throttlemax -------
} else if (strmatch(buf+6, PSTR("throttlemax"))) {
port->print_P(PSTR("Throttle max = ")); port->print(get(PARAM_THR_MAX),DEC); port->println_P(PSTR("%"));
//------- print throttlemin -------
} else if (strmatch(buf+6, PSTR("throttlemin"))) {
port->print_P(PSTR("Throttle min = ")); port->print(get(PARAM_THR_MIN),DEC); port->println_P(PSTR("%"));
//------- print tuning -------
} else if (strmatch(buf+6, PSTR("tuning"))) {
print_tuning();
} else
print_error(ERR("USAGE: print {altitude|attitude|commands|ctrlmode|curwaypts|flightmodes|k -?|kp|ki|kd|kff|location|navsettings|pressure|rlocation [home]|speed|tuning|}"));
//*********** Process 'reset' commands ***********
} else if (strmatch(buf, PSTR("reset "))) {
//------- reset commands -------
if (strmatch(buf+6, PSTR("commands"))) {
reload_commands();
port->println_P(PSTR("Commands reloaded."));
} else
print_error(ERR("USAGE: reset commands"));
//*********** Process 'rtl' command ***********
} else if (strmatch(buf, PSTR("rtl"))) {
return_to_launch();
port->println_P(PSTR("Returning to launch..."));
//*********** Process 'set' commands ***********
} else if (strmatch(buf, PSTR("set "))) {
//------- set cmd -------
if (strmatch(buf+4, PSTR("cmd "))) {
process_set_cmd(buf+8, bufferidx-8);
//------- set cmdcount -------
} else if (strmatch(buf+4, PSTR("cmdcount "))) {
set(PARAM_WP_TOTAL, atoi(buf+13));
port->print_P(PSTR("PARAM_WP_TOTAL = ")); port->println(get(PARAM_WP_TOTAL),DEC);
//------- set cmdindex -------
} else if (strmatch(buf+4, PSTR("cmdindex "))) {
set(PARAM_WP_INDEX, atoi(buf+13));
decrement_WP_index();
next_command = get_wp_with_index(get(PARAM_WP_INDEX)+1);
port->print_P(PSTR("Command set to index ")); port->print(get(PARAM_WP_INDEX),DEC);
if (next_command.id >= 0x10 && next_command.id <= 0x1F) { //TODO: create a function the defines what type of command each command ID is
command_must_index = 0;
port->println_P(PSTR(" (must)"));
} else if (next_command.id >= 0x20 && next_command.id <= 0x2F) {
command_may_index = 0;
port->println_P(PSTR(" (may)"));
} else
port->println_P(PSTR(" (now)"));
next_command.id = CMD_BLANK;
if (get(PARAM_WP_INDEX) > get(PARAM_WP_TOTAL)) {
set(PARAM_WP_TOTAL, get(PARAM_WP_INDEX));
port->print_P(PSTR(" The total number of commands is now "));
port->println(get(PARAM_WP_TOTAL),DEC);
}
next_WP = current_loc;
update_commands();
//------- set cruise -------
} else if (strmatch(buf+4, PSTR("cruise "))) {
unsigned char j = 4+7;
#if AIRSPEED_SENSOR == 1
port->print_P(PSTR("airspeed_cruise changed from "));
port->print((float)get(PARAM_TRIM_AIRSPEED)/100,2);
port->print_P(PSTR(" to "));
set(PARAM_TRIM_AIRSPEED, (int)(readfloat(buf, &j)/100000));
set(PARAM_TRIM_AIRSPEED, constrain(get(PARAM_TRIM_AIRSPEED),0,9000)); //TODO: constrain minimum as stall speed, maximum as Vne
port->println(((float)get(PARAM_TRIM_AIRSPEED))/100,2);
#else
port->print_P(PSTR("throttle_cruise changed from "));
port->print(get(PARAM_TRIM_THROTTLE),DEC);
port->print_P(PSTR(" to "));
set(PARAM_TRIM_THROTTLE, constrain((int)(readfloat(buf, &j)/10000000),0,200));
port->println(get(PARAM_TRIM_THROTTLE),DEC);
#endif
//save_user_configs();
//------- set holdalt -------
} else if (strmatch(buf+4, PSTR("holdalt "))) {
int tempalt = atoi(buf+12)*100;
set(PARAM_ALT_HOLD_HOME, (float)tempalt); //save_alt_to_hold((int32_t)tempalt);
port->println_P(PSTR("Hold altitude above home set."));
//------- set imax {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} <X> -------
} else if (strmatch(buf+4, PSTR("imax "))) {
int i;
unsigned char j = 0;
if (get_pid_offset_name(buf+9, &i, &j)) {
buf[9+j] = 0;
port->print_P(PSTR("Integrator maximum for "));
port->print(buf+9);
port->print_P(PSTR(" changed from "));
port->print(pid_index[i]->imax(),DEC);
port->print_P(PSTR(" to "));
pid_index[i]->imax((float)atoi(buf+j));
pid_index[i]->save_gains();
port->println(pid_index[i]->imax(),DEC);
} else
print_error(ERR("ERROR: Did not recognize keyword; type set k -? for more information"));
//------- set kp {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} <X> -------
} else if (strmatch(buf+4, PSTR("kp "))) {
int i;
unsigned char j = 0;
if (get_pid_offset_name(buf+7, &i, &j)) {
buf[7+j] = 0;
port->print_P(PSTR("P gain for "));
port->print(buf+7);
port->print_P(PSTR(" changed from "));
port->print(pid_index[i]->kP(),DEC);
port->print_P(PSTR(" to "));
j += 7+1;
pid_index[i]->kP(((float)readfloat(buf, &j))/10000000);
pid_index[i]->save_gains();
port->println(pid_index[i]->kP(),DEC);
} else
print_gain_keyword_error();
//------- set ki {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} <X> -------
} else if (strmatch(buf+4, PSTR("ki "))) {
int i;
unsigned char j = 0;
if (get_pid_offset_name(buf+7, &i, &j)) {
buf[7+j] = 0;
port->print_P(PSTR("I gain for "));
port->print(buf+7);
port->print_P(PSTR(" changed from "));
port->print(pid_index[i]->kI(),DEC);
port->print_P(PSTR(" to "));
j += 7+1;
pid_index[i]->kI(((float)readfloat(buf, &j))/10000000);
pid_index[i]->save_gains();
port->println(pid_index[i]->kI(),DEC);
} else
print_gain_keyword_error();
//------- set kd {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} <X> -------
} else if (strmatch(buf+4, PSTR("kd "))) {
int i;
unsigned char j = 0;
if (get_pid_offset_name(buf+7, &i, &j)) {
buf[7+j] = 0;
port->print_P(PSTR("D gain for "));
port->print(buf+7);
port->print_P(PSTR(" changed from "));
port->print(pid_index[i]->kD(),DEC);
port->print_P(PSTR(" to "));
j += 7+1;
pid_index[i]->kD(((float)readfloat(buf, &j))/10000000);
pid_index[i]->save_gains();
port->println(pid_index[i]->kD(),DEC);
} else
print_gain_keyword_error();
//------- set kff {pitchcomp|ruddermix|pitchtothrottle} <X> -------
} else if (strmatch(buf+4, PSTR("kff "))) {
unsigned char j = 0;
if (strmatch(buf+8, PSTR("pitchcomp"))) {
buf[8+9] = 0;
port->print_P(PSTR("FF gain for "));
port->print(buf+8);
port->print_P(PSTR(" changed from "));
port->print(get(PARAM_KFF_PTCHCOMP),DEC);
port->print_P(PSTR(" to "));
j = 8+9+1;
set(PARAM_KFF_PTCHCOMP, ((float)readfloat(buf, &j))/10000000);
port->println(get(PARAM_KFF_PTCHCOMP),DEC);
} else if (strmatch(buf+8, PSTR("ruddermix"))) {
buf[8+9] = 0;
port->print_P(PSTR("FF gain for "));
port->print(buf+8);
port->print_P(PSTR(" changed from "));
port->print(get(PARAM_KFF_RDDRMIX),DEC);
port->print_P(PSTR(" to "));
j = 8+9+1;
set(PARAM_KFF_RDDRMIX, ((float)readfloat(buf, &j))/10000000);
port->println(get(PARAM_KFF_RDDRMIX),DEC);
} else if (strmatch(buf+8, PSTR("pitchtothrottle"))) {
buf[8+15] = 0;
port->print_P(PSTR("FF gain for "));
port->print(buf+8);
port->print_P(PSTR(" changed from "));
port->print(get(PARAM_KFF_PTCH2THR),DEC);
port->print_P(PSTR(" to "));
j = 8+15+1;
set(PARAM_KFF_PTCH2THR, ((float)readfloat(buf, &j))/10000000);
port->println(get(PARAM_KFF_PTCH2THR),DEC);
/*} else if (strmatch(buf+8, PSTR("throttletopitch"))) {
buf[8+15] = 0;
port->print_P(PSTR("FF gain for "));
port->print(buf+8);
port->print_P(PSTR(" changed from "));
port->print(kff[CASE_T_TO_P],DEC);
port->print_P(PSTR(" to "));
j = 8+15+1;
kff[CASE_T_TO_P] = ((float)readfloat(buf, &j))/10000000;
port->println(kff[CASE_T_TO_P],DEC);*/
} else
print_gain_keyword_error();
//------- set loiterradius -------
} else if (strmatch(buf+4, PSTR("loiterradius "))) {
set(PARAM_LOITER_RADIUS, atoi(buf+17));
port->print_P(PSTR("Set loiter radius to ")); port->print(get(PARAM_LOITER_RADIUS),DEC); port->println_P(PSTR(" meters"));
//------- set navrolllimit <X> -------
} else if (strmatch(buf+4, PSTR("navrolllimit "))) {
unsigned char j = 17;
port->print_P(PSTR("Nav roll limit changed from ")); port->print((float)get(PARAM_LIM_ROLL)/100,2);
port->print_P(PSTR(" to "));
set(PARAM_LIM_ROLL, readfloat(buf, &j)/100000);
port->println((float)get(PARAM_LIM_ROLL)/100,2);
//------- set pitchmax <X> -------
} else if (strmatch(buf+4, PSTR("pitchmax "))) {
unsigned char j = 13;
port->print_P(PSTR("Maximum pitch changed from ")); port->print((float)get(PARAM_LIM_PITCH_MAX)/100,2);
port->print_P(PSTR(" to "));
set(PARAM_LIM_PITCH_MAX, readfloat(buf, &j)/100000);
port->println((float)get(PARAM_LIM_PITCH_MAX)/100,2);
//------- set pitchmin <X> -------
} else if (strmatch(buf+4, PSTR("pitchmin "))) {
unsigned char j = 13;
port->print_P(PSTR("Minimum pitch changed from ")); port->print((float)get(PARAM_LIM_PITCH_MIN)/100,2);
port->print_P(PSTR(" to "));
set(PARAM_LIM_PITCH_MIN, readfloat(buf, &j)/100000);
port->println((float)get(PARAM_LIM_PITCH_MIN)/100,2);
//------- set pitchtarget <X> -------
} else if (strmatch(buf+4, PSTR("pitchtarget "))) {
unsigned char j = 16;
port->print_P(PSTR("Pitch target changed from ")); port->print((float)get(PARAM_TRIM_PITCH)/100,2);
port->print_P(PSTR(" to "));
set(PARAM_TRIM_PITCH, readfloat(buf, &j)/100000);
port->println((float)get(PARAM_TRIM_PITCH)/100,2);
//------- set rcin -------
} else if (strmatch(buf+4, PSTR("rcin"))) {
unsigned char index = buf[8]-'1';
if (index < 8) {
radio_in[index] = atoi(buf+9);
} else
print_error(ERR("USAGE: set rcin<N> <X>"));
//------- set rcout -------
} else if (strmatch(buf+4, PSTR("rcout"))) {
unsigned char index = buf[9]-'1';
if (index < 8) {
radio_out[index] = atoi(buf+10);
APM_RC.OutputCh(index, radio_out[index]);
} else
print_error(ERR("USAGE: set rcout<N> <X>"));
//------- set relay -------
} else if (strmatch(buf+4, PSTR("relay "))) {
unsigned char newvalue = atoi(buf+10);
if (newvalue == 0) {
relay_off();
port->println_P(PSTR("Relay turned off"));
} else if (newvalue == 1) {
relay_on();
port->println_P(PSTR("Relay turned on"));
} else {
relay_toggle();
port->println_P(PSTR("Relay toggled"));
}
//------- set throttlecruise <X> -------
} else if (strmatch(buf+4, PSTR("throttlecruise "))) {
port->print_P(PSTR("Throttle cruise changed from ")); port->print(get(PARAM_TRIM_THROTTLE),DEC); port->print_P(PSTR("% to "));
set(PARAM_TRIM_THROTTLE,atoi(buf+19));
port->print(get(PARAM_TRIM_THROTTLE),DEC); port->println_P(PSTR("%"));
//------- set throttlefailsafe <N> -------
} else if (strmatch(buf+4, PSTR("throttlefailsafe "))) {
set(PARAM_THR_FAILSAFE, atoi(buf+21));
/*if (get(PARAM_THROTTLE_FS) == 0)
throttle_FS_enabled = 0;
else
throttle_FS_enabled = 1;*/
//save_user_configs();
port->println_P(PSTR("Throttle failsafe adjusted."));
//------- set throttlefailsafeaction <N> -------
} else if (strmatch(buf+4, PSTR("throttlefailsafeaction "))) {
set(PARAM_THR_FS_ACTION, atoi(buf+27));
//save_user_configs();
//------- set throttlemax <X> -------
} else if (strmatch(buf+4, PSTR("throttlemax "))) {
port->print_P(PSTR("Throttle max changed from ")); port->print(get(PARAM_THR_MAX),DEC); port->print_P(PSTR("% to "));
set(PARAM_THR_MAX, atoi(buf+16));
port->print(get(PARAM_THR_MAX),DEC); port->println_P(PSTR("%"));
//save_user_configs();
//------- set throttlemin <X> -------
} else if (strmatch(buf+4, PSTR("throttlemin "))) {
port->print_P(PSTR("Throttle min changed from ")); port->print(get(PARAM_THR_MIN),DEC); port->print_P(PSTR("% to "));
set(PARAM_THR_MIN, atoi(buf+16));
port->print(get(PARAM_THR_MIN),DEC); port->println_P(PSTR("%"));
//save_user_configs();
//------- set wpradius -------
} else if (strmatch(buf+4, PSTR("wpradius "))) {
set(PARAM_WP_RADIUS, atoi(buf+13));
port->print_P(PSTR("Set waypoint radius to ")); port->print(get(PARAM_WP_RADIUS),DEC); port->println_P(PSTR(" meters"));
//------- set xtrackentryangle -------
} else if (strmatch(buf+4, PSTR("xtrackentryangle "))) {
unsigned char j = 21;
set(PARAM_XTRACK_ANGLE, readfloat(buf, &j)/100000);
port->print_P(PSTR("Crosstrack entry angle set to ")); port->println((float)get(PARAM_XTRACK_ANGLE)/100,2);
//------- set xtrackgain -------
} else if (strmatch(buf+4, PSTR("xtrackgain "))) {
unsigned char j = 15;
set(PARAM_XTRACK_GAIN, ((float)readfloat(buf, &j))/10000000);
port->print_P(PSTR("Crosstrack gain set to ")); port->println(get(PARAM_XTRACK_GAIN),2);
} else
print_error(ERR("USAGE: set {cmd|cmdcount|cmdindex|cruise|holdalt|kp|ki|kd|kff|loiterradius|rcin|rcout|wpradius}"));
//*********** Process 'status' commands ***********
} else if (strmatch(buf, PSTR("status "))) {
//------- status altitude -------
if (strmatch(buf+7, PSTR("altitude"))) {
port->printf_P(PSTR("Altitude:\n"
" altitude_error = %.2fm\n"
" target_altitude = %.2fm\n"
" next_WP.alt = %.2fm\n"
" wp_distance = %ldm\n"
" wp_totalDistance = %ldm\n"
" offset_altitude = %.2fm\n"),
(float)altitude_error/100,
(float)target_altitude/100,
(float)next_WP.alt/100,
wp_distance,
wp_totalDistance,
(float)offset_altitude/100);
//------- status climb -------
} else if (strmatch(buf+7, PSTR("climb"))) {
print_climb_debug_info();
//------- status control -------
} else if (strmatch(buf+7, PSTR("control"))) {
port->printf_P(PSTR("Control status:\n"
" Roll: nav= %.2f, servo_out= %.2f\n" // XXX float?
" Pitch: nav= %.2f, servo_out= %.2f\n" // XXX float?
" Throttle: nav= %d, servo_out= %d\n"),
(float)nav_roll / 100, (float)servo_out[CH_ROLL] / 100,
(float)nav_pitch / 100, (float)servo_out[CH_PITCH] / 100,
get(PARAM_TRIM_THROTTLE), servo_out[CH_THROTTLE]);
//------- status gps -------
} else if (strmatch(buf+7, PSTR("gps"))) {
port->printf_P(PSTR("GPS status:\n"
" Fix: %S (%d)\n"
" Satellites: %d\n"
" Fix count: %d\n"),
(gps.fix ? PSTR("Valid") : PSTR("INVALID")),
(int)gps.fix,
(int)gps.num_sats,
gps_fix_count);
//------- status landing -------
} else if (strmatch(buf+7, PSTR("landing"))) {
port->printf_P(PSTR("Landing status:"
" land_complete = %d\n"
" landing_pitch = %d\n"
" nav_pitch = %ld\n"
" airspeed_cruise = %d\n"
" throttle_cruise = %d\n"
" hold_course = %ld\n"
" nav_bearing = %ld\n"
" bearing_error = %ld\n"),
(int)land_complete,
landing_pitch,
nav_pitch,
get(PARAM_TRIM_AIRSPEED),
get(PARAM_TRIM_THROTTLE),
hold_course,
nav_bearing,
bearing_error);
//------- status loiter -------
} else if (strmatch(buf+7, PSTR("loiter"))) {
port->printf_P(PSTR("Loiter status:"
" Loiter radius: %d\n"
" Progress: %d/%d\n" // XXX original had non-ASCII units char
" Time: %ldms/%dms\n"),
get(PARAM_LOITER_RADIUS),
loiter_sum, loiter_total,
loiter_time, loiter_time_max);
//------- status navigation -------
} else if (strmatch(buf+7, PSTR("navigation"))) {
port->printf_P(PSTR("Navigation status:\n"
" From <%.6f, %.6f, %.1f>: "),
(float)prev_WP.lat/10000000.0,
(float)prev_WP.lng/10000000.0,
(float)prev_WP.alt/100.0);
print_rlocation(&prev_WP);
port->printf_P(PSTR(" At <%.6f, %.6f, %.1f>\n"),
(float)current_loc.lat/10000000.0,
(float)current_loc.lng/10000000.0,
(float)current_loc.alt/100.0);
port->printf_P(PSTR(" To <%.6f, %.6f, %.1f>: "),
(float)next_WP.lat/10000000.0,
(float)next_WP.lng/10000000.0,
(float)next_WP.alt/100.0);
print_rlocation(&next_WP);
port->printf_P(PSTR(" Distance: %.1f%% %ldm / %ldm; %.1f vertically\n"),
100.0*(float)(wp_totalDistance-wp_distance)/(float)wp_totalDistance,
wp_totalDistance-wp_distance,
wp_totalDistance,
(float)altitude_error/100.0);
port->printf_P(PSTR(" Nav bearing: %.2f; error = %.2f\n"),
(float)nav_bearing/100.0,
(float)bearing_error/100.0);
port->printf_P(PSTR(" Ground course: %.1f (current), %.1f (target)\n"),
(float)gps.ground_course/100.0,
(float)target_bearing/100.0);
if (hold_course >= 0) {
port->print_P(PSTR(" HOLD COURSE: ")); port->println(hold_course/100.0,2);
}
//------- status navpitch -------
} else if (strmatch(buf+7, PSTR("navpitch"))) {
#if AIRSPEED_SENSOR == ENABLED
port->printf_P(PSTR(">>> nav_pitch = PID[airspeed_error (%.2f) = airspeed_cruise (%.2f) - airspeed (%.2f)]\n"),
(float)airspeed_error / 100,
(float)get(PARAM_TRIM_AIRSPEED) / 100,
(float)airspeed / 100);
#else
port->printf_P(PSTR(">>> nav_pitch = PID[altitude_error (%ld) = target_altitude (%ld) - current_loc.alt (%ld)]\n"),
altitude_error,
target_altitude,
current_loc.alt);
#endif
port->printf_P(PSTR("nav_pitch (%.2f) - pitch_sensor (%.2f) + pitch_comp (%.2f) = %.2f\n"),
(float)nav_pitch / 100,
(float)dcm.pitch_sensor / 100,
2013-01-10 14:42:24 -04:00
fabsf(dcm.roll_sensor * get(PARAM_KFF_PTCHCOMP)) / 100,
(float)(nav_pitch-dcm.pitch_sensor + fabsf(dcm.roll_sensor * get(PARAM_KFF_PTCHCOMP))) / 100);
2011-09-08 22:31:32 -03:00
port->printf_P(PSTR("servo_out[CH_PITCH] (%.2f) = PID[nav_pitch + pitch_comp - pitch_sensor]"),
(float)servo_out[CH_PITCH] / 100);
//------- status navroll -------
} else if (strmatch(buf+7, PSTR("navroll"))) {
print_rlocation(&next_WP);
port->printf_P(PSTR("bearing_error (%ld) = nav_bearing (%ld) - yaw_sensor (%ld)\n"
"nav_roll (%ld) - roll_sensor (%ld) = %ld\n"
"servo_out[CH_ROLL] = %d\n"),
bearing_error, nav_bearing, dcm.yaw_sensor,
nav_roll, dcm.roll_sensor, nav_roll - dcm.roll_sensor,
servo_out[CH_ROLL]);
//------- status pid {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} -------
} else if (strmatch(buf+7, PSTR("pid "))) {
int i;
unsigned char j;
if (get_pid_offset_name(buf+11, &i, &j)) {
port->print(buf+11);
port->print_P(PSTR(": "));
display_PID = i; //The next time a PID calculation is performed on this channel, the print_PID routine in this GCS will be called
} else
print_error(ERR("ERROR: Did not recognize keyword"));
//------- status rcinputch -------
} else if (strmatch(buf+7, PSTR("rcinputch"))) {
unsigned char i;
port->println_P(PSTR("RC hardware input:"));
for (i=0; i<8; i++) {
port->print_P(PSTR(" Ch"));
port->print(i+1,DEC);
port->print_P(PSTR(": "));
port->println(APM_RC.InputCh(i));
}
//------- status rcin -------
} else if (strmatch(buf+7, PSTR("rcin"))) {
unsigned char i;
port->println_P(PSTR("RC software input:"));
for (i=0; i<8; i++) {
port->print_P(PSTR(" Ch"));
port->print(i+1,DEC);
port->print_P(PSTR(": "));
port->println(radio_in[i]);
}
//------- status rcout -------
} else if (strmatch(buf+7, PSTR("rcout"))) {
unsigned char i;
port->println_P(PSTR("RC software output:"));
for (i=0; i<8; i++) {
port->print_P(PSTR(" Ch"));
port->print(i+1,DEC);
port->print_P(PSTR(": "));
port->println(radio_out[i]);
}
//------- status rcpwm -------
} else if (strmatch(buf+7, PSTR("rcpwm"))) {
unsigned char i;
port->println_P(PSTR("RC hardware output:"));
for (i=0; i<8; i++) {
port->print_P(PSTR(" Ch"));
port->print(i+1,DEC);
port->print_P(PSTR(": "));
port->println(readOutputCh(i));
}
//------- status rctrim -------
} else if (strmatch(buf+7, PSTR("rctrim"))) {
unsigned char i;
port->println_P(PSTR("RC trim:"));
for (i=0; i<8; i++) {
port->print_P(PSTR(" Ch"));
port->print(i+1,DEC);
port->print_P(PSTR(": "));
port->println(radio_trim(i));
}
port->print_P(PSTR(" elevon1_trim = ")); port->println(elevon1_trim,DEC);
port->print_P(PSTR(" elevon2_trim = ")); port->println(elevon2_trim,DEC);
//------- status rc -------
} else if (strmatch(buf+7, PSTR("rc"))) {
unsigned char i;
port->println_P(PSTR("RC:\tCh\tHWin\tSWtrim\tSWin\tServo\tSWout\tHWout\t"));
for (i=0; i<8; i++) {
// XXX might benefit from field widths, since some terminals tab badly
port->printf_P(PSTR("\t%u\t%u\t%u\t%u\t%.2f\t%d\t%d\n"),
(unsigned int)(i + 1),
APM_RC.InputCh(i),
radio_trim(i),
radio_in[i],
(float)servo_out[i] / 100,
radio_out[i],
readOutputCh(i));
}
//------- status system -------
} else if (strmatch(buf+7, PSTR("system"))) {
port->printf_P(PSTR("System status:"
" Ground start: %S (%d)\n"
" Home: %SSET (%d)\n"),
(ground_start ? PSTR("YES") : PSTR("NO")), (int)ground_start,
(home_is_set ? PSTR("") : PSTR("NOT ")), (int)home_is_set);
//------- status takeoff -------
/*} else if (strmatch(buf+7, PSTR("takeoff"))) {
port->println_P(PSTR("Takeoff status:"));
port->print_P(PSTR(" takeoff in progress: "));
if (takeoff_in_progress)
port->println_P(PSTR("YES"));
else {
port->print_P(PSTR("NO; trigger = "));
port->print(toss_trigger,DEC);
port->print_P(PSTR(", current = "));
Vector3f temp = ardupilotDCM.get_accels();
port->println(abs(temp.y),1);
}
port->printf_P(PSTR(" takeoff_pitch = %.2f\n"
" scaler = %.3f\n"
" nav_pitch = %.2f\n"
" throttle = %d\n"
" hold_course = %.2f\n"
" nav_bearing = %ld\n"
" bearing_error = %ld\n"
" current_loc.alt = %ld\n"
" home.alt + takeoff_altitude = %ld"),
(float)takeoff_pitch / 100,
(float)airspeed / (float)airspeed_cruise,
(float)nav_pitch / 100,
servo_out[CH_THROTTLE],
(float)hold_course / 100,
nav_bearing,
bearing_error,
current_loc.alt,
home.alt + takeoff_altitude);*/
//------- status telemetry -------
} else if (strmatch(buf+7, PSTR("telemetry"))) {
port->printf_P(PSTR("Telemetry status:\n"
" %S heartbeat\n"
" %S location\n"
" %S attitude\n"
" %S command\n"
" Severity report level %d\n"),
(report_heartbeat ? PSTR("Show") : PSTR("Hide")),
(report_location ? PSTR("Show") : PSTR("Hide")),
(report_attitude ? PSTR("Show") : PSTR("Hide")),
(report_command ? PSTR("Show") : PSTR("Hide")),
(int)report_severity);
//------- status throttle -------
} else if (strmatch(buf+7, PSTR("throttle"))) {
#if AIRSPEED_SENSOR == ENABLED
port->printf_P(PSTR(">>> airspeed_energy_error (%ld) = 0.5 * (airspeed_cruise (%.2f)^2 - airspeed (%.2f)^2)\n"
"energy_error (%ld) = airspeed_energy_error (%ld) + altitude_error*0.098 (%ld)\n"
"servo_out[CH_THROTTLE] (%d) = PID[energy_error]\n"),
airspeed_energy_error, (float)get(PARAM_TRIM_AIRSPEED) / 100, (float)airspeed / 100,
energy_error, airspeed_energy_error, (long)((float)altitude_error * 0.098),
servo_out[CH_THROTTLE]);
#else
port->printf_P(PSTR("altitude_error (%ld) = target_altitude (%ld) - current_loc.alt (%ld)\n"
"servo_out[CH_THROTTLE] (%d) = PID[altitude_error]\n"),
altitude_error, target_altitude, current_loc.alt,
servo_out[CH_THROTTLE]);
#endif
//------- status waypoints -------
} else if (strmatch(buf+7, PSTR("waypoints"))) {
port->printf_P(PSTR("Waypoints status:\n"
" Distance: %ld/%ld\n"
" Index: %d/%d\n"
" Next: %d\n"),
wp_distance, wp_totalDistance,
(int)get(PARAM_WP_INDEX), (int)get(PARAM_WP_TOTAL),
(int)next_wp_index);
} else if (strmatch(buf+7, PSTR("james"))) {
port->println_P(PSTR("James is a monkey"));
} else {
print_error(ERR("USAGE: status {control|gps|landing|loiter|mixing|navigation|navpitch|navroll|rc|rcinputch|rcin|rcout|rcpwm|rctrim|system|takeoff|telemetry|throttle|waypoints}"));
}
} else {
print_error(ERR("USAGE: {echo <text>|groundstart|inithome|show|hide|print|status|set|reset|rtl}"));
print_error(ERR("Type <command> -? for specific usage guidance"));
}
}
/* strmatch compares two strings and returns 1 if they match and 0 if they don't
s2 must be stored in program memory (via PSTR) rather than RAM (like standard strings)
s1 must be at least as long as s2 for a valid match
if s1 is longer than s2, then only the beginning of s1 (the length of s2) must match s2 for a valid match */
int
GCS_DEBUGTERMINAL::strmatch(char* s1, const char* s2)
{
int i = 0;
char c1 = s1[0], c2 = pgm_read_byte(s2);
while (c1 != 0 && c2 != 0) {
if (c1 < c2)
return 0;
if (c1 > c2)
return 0;
i++;
c1 = s1[i];
c2 = pgm_read_byte(s2+i);
}
if (c2==0)
return 1;
else
return 0;
}
/* readfloat parses a string written as a float starting at the offset in *i and updates *i as it parses
numbers are multiplied by 10,000,000 and decimals beyond 7 places are discarded
parsing is terminated with either a space or a null, other characters are ignored */
long
GCS_DEBUGTERMINAL::readfloat(char* s, unsigned char* i)
{
long result = 0, multiplier = 0;
char c, neg = 0;
for (;;(*i)++) {
c = s[*i];
if (c == ' ')
break;
else if (c == 0)
break;
else if (c == '-')
neg = 1-neg;
else if (c == '.') {
result *= 10000000;
multiplier = 1000000;
} else if (c >= '0' && c <= '9') {
if (multiplier == 0)
result = (result*10) + c-'0';
else {
result += (c-'0')*multiplier;
multiplier /= 10;
}
}
}
if (multiplier == 0)
result *= 10000000;
if (neg)
return -result;
else
return result;
}
/* process_set_cmd processing the parameters of a 'set cmd' command and takes the appropriate action
*buffer is the buffer containing the parameters of the command; it should start after the space after 'set cmd'
bufferlen is the length of the buffer; the routine will stop looking for parameters after the offset index reaches this value
*/
#define SETPARAM_NONE (0)
#define SETPARAM_ID (1)
#define SETPARAM_P1 (2)
#define SETPARAM_LAT (3)
#define SETPARAM_LNG (4)
#define SETPARAM_ALT (5)
#define SETPARAM_P2 (6)
#define SETPARAM_P3 (7)
#define SETPARAM_P4 (8)
void
GCS_DEBUGTERMINAL::process_set_cmd(char *buffer, int bufferlen)
{
BetterStream *port = _port;
unsigned char i, j, err=1, setparam=SETPARAM_NONE;
unsigned char cmdindex=0, p1=0, cmdid;
long lat, lng, alt;
Location temp;
//Parse the command index
for (i=0; i<bufferlen; i++)
if (buffer[i] >= '0' && buffer[i] <= '9')
cmdindex = (cmdindex*10) + buffer[i]-'0';
else
break;
if (buffer[i] == ' ') {
//Find the end of the command-type string
i++;
for (j=i; j<bufferlen; j++) {
if (buffer[j] == ' ' || buffer[j] == 0)
break;
}
if (buffer[j] == ' ') {
//Process the command-type string
buffer[j] = 0; //Null-terminate the command-type string so strmatch can figure out when to stop comparing, and so atoi can work
if (strmatch(buffer+i, PSTR("waypoint")))
cmdid = CMD_WAYPOINT;
else if (strmatch(buffer+i, PSTR("takeoff")))
cmdid = CMD_TAKEOFF;
else if (strmatch(buffer+i, PSTR("landoptions")))
cmdid = CMD_LAND_OPTIONS;
else if (strmatch(buffer+i, PSTR("land")))
cmdid = CMD_LAND;
else if (strmatch(buffer+i, PSTR("loiternturns")))
cmdid = CMD_LOITER_N_TURNS;
else if (strmatch(buffer+i, PSTR("loitertime")))
cmdid = CMD_LOITER_TIME;
else if (strmatch(buffer+i, PSTR("loiter")))
cmdid = CMD_LOITER;
else if (strmatch(buffer+i, PSTR("delay")))
cmdid = CMD_DELAY;
else if (strmatch(buffer+i, PSTR("resetindex")))
cmdid = CMD_RESET_INDEX;
else if (strmatch(buffer+i, PSTR("throttlecruise")))
cmdid = CMD_THROTTLE_CRUISE;
else if (strmatch(buffer+i, PSTR("resethome")))
cmdid = CMD_RESET_HOME;
else if (strmatch(buffer+i, PSTR("index")))
cmdid = CMD_INDEX;
else if (strmatch(buffer+i, PSTR("rtl")))
cmdid = CMD_RTL;
else if (strmatch(buffer+i, PSTR("relay")))
cmdid = CMD_RELAY;
else if (strmatch(buffer+i, PSTR("servo")))
cmdid = CMD_SERVO;
else if (strmatch(buffer+i, PSTR("id")))
setparam = SETPARAM_ID;
else if (strmatch(buffer+i, PSTR("p1")))
setparam = SETPARAM_P1;
else if (strmatch(buffer+i, PSTR("alt")))
setparam = SETPARAM_ALT;
else if (strmatch(buffer+i, PSTR("lat")))
setparam = SETPARAM_LAT;
else if (strmatch(buffer+i, PSTR("lng")))
setparam = SETPARAM_LNG;
else if (strmatch(buffer+i, PSTR("p2")))
setparam = SETPARAM_P2;
else if (strmatch(buffer+i, PSTR("p3")))
setparam = SETPARAM_P3;
else if (strmatch(buffer+i, PSTR("p4")))
setparam = SETPARAM_P4;
else
cmdid = atoi(buffer+i);
if (setparam > SETPARAM_NONE) {
//Process new parameter value
i = j+1;
if (setparam == SETPARAM_ALT || setparam == SETPARAM_LAT || setparam == SETPARAM_LNG) {
lat = readfloat(buffer, &i);
} else {
unsigned char k;
for (k=i; i<sizeof(gcsinputbuffer); k++)
if (buffer[k] != '-' && !(buffer[k] >= '0' && buffer[k] <= '9'))
break;
buffer[k] = 0;
lat = atol(buffer+i);
i = k;
}
temp = get_wp_with_index(cmdindex);
if (setparam == SETPARAM_ID)
temp.id = lat;
else if (setparam == SETPARAM_P1)
temp.p1 = lat;
else if (setparam == SETPARAM_ALT)
temp.alt = lat/100000;
else if (setparam == SETPARAM_LAT)
temp.lat = lat;
else if (setparam == SETPARAM_LNG)
temp.lng = lat;
else if (setparam == SETPARAM_P2)
temp.alt = lat;
else if (setparam == SETPARAM_P3)
temp.lat = lat;
else if (setparam == SETPARAM_P4)
temp.lng = lat;
cmdid = temp.id;
p1 = temp.p1;
lat = temp.lat;
lng = temp.lng;
alt = temp.alt;
err = 0;
} else {
//Process param 1
for (i=j+1; i<bufferlen; i++) {
if (buffer[i] >= '0' && buffer[i] <= '9')
p1 = (p1*10) + buffer[i]-'0';
else
break;
}
if (buffer[i] == ' ') {
//Process altitude
i++;
if (strmatch(buffer+i, PSTR("here"))) {
lat = gps.latitude;
lng = gps.longitude;
alt = get_altitude_above_home(); //GPS.altitude;
err = 0;
} else {
alt = readfloat(buffer, &i)/100000;
if (buffer[i] == ' ') {
//Process latitude
i++;
lat = readfloat(buffer, &i);
if (strmatch(buffer+i, PSTR("here"))) {
lat = gps.latitude;
lng = gps.longitude;
err = 0;
} else {
if (buffer[i] == ' ') {
//Process longitude
i++;
lng = readfloat(buffer, &i);
//TODO: add other command special cases here
if (cmdid == CMD_LAND_OPTIONS) {
temp.p1 = p1;
temp.alt = alt;
temp.lat = lat / 10000000;
temp.lng = lng / 10000000;
}
err = 0;
} else
print_error(ERR("Error processing set cmd: Could not find longitude parameter"));
}
} else
print_error(ERR("Error processing set cmd: Could not find latitude parameter"));
}
} else
print_error(ERR("Error processing set cmd: Could not find altitude parameter"));
}
} else
print_error(ERR("Error processing set cmd: Could not find command type"));
} else
print_error(ERR("Error processing set cmd: Could not find command index"));
if (err == 0) {
temp.id = cmdid;
temp.p1 = p1;
temp.lat = lat;
temp.lng = lng;
temp.alt = alt;
if (cmdindex >= get(PARAM_WP_TOTAL)) {
set(PARAM_WP_TOTAL, cmdindex+1);
}
set_wp_with_index(temp, cmdindex);
port->printf_P(PSTR("Set command %d to %d with p1=%d, lat=%ld, lng=%ld, alt=%ld\n"),
(int)cmdindex, (int)temp.id, (int)temp.p1, temp.lat, temp.lng, temp.alt);
}
}
/* get_pid_offset_name matches a string expressed in *buffer with a pid keyword and returns the k-array
gain offset in *offset, and the length of that string expression in *length. If a good keyword
match is found, 1 is returned; 0 otherwise
*/
int
GCS_DEBUGTERMINAL::get_pid_offset_name(char *buffer, int *offset, unsigned char *length)
{
if (strmatch(buffer, PSTR("servoroll"))) {
*length += 9; *offset = CASE_SERVO_ROLL;
} else if (strmatch(buffer, PSTR("servopitch"))) {
*length += 10; *offset = CASE_SERVO_PITCH;
} else if (strmatch(buffer, PSTR("servorudder"))) {
*length += 11; *offset = CASE_SERVO_RUDDER;
} else if (strmatch(buffer, PSTR("navroll"))) {
*length += 7; *offset = CASE_NAV_ROLL;
} else if (strmatch(buffer, PSTR("navpitchasp"))) {
*length += 11; *offset = CASE_NAV_PITCH_ASP;
} else if (strmatch(buffer, PSTR("navpitchalt"))) {
*length += 11; *offset = CASE_NAV_PITCH_ALT;
} else if (strmatch(buffer, PSTR("throttlete"))) {
*length += 10; *offset = CASE_TE_THROTTLE;
} else if (strmatch(buffer, PSTR("throttlealt"))) {
*length += 11; *offset = CASE_ALT_THROTTLE;
} else {
return 0;
}
return 1;
}
/* print_rlocation prints the relative location of the specified waypoint from the plane in easy-to-read cartesian format
*/
void
GCS_DEBUGTERMINAL::print_rlocation(Location *wp)
{
//float x, y;
2013-01-10 14:42:24 -04:00
//y = (float)(wp->lat - current_loc.lat) * 0.0111194927f;
//x = (float)(wp->lng - current_loc.lng) * cosf(radians(current_loc.lat)) * 0.0111194927f;
2011-09-08 22:31:32 -03:00
BetterStream *port = _port;
int x, y;
2013-01-10 14:42:24 -04:00
y = (wp->lat - current_loc.lat) * 0.0111194927f;
x = (wp->lng - current_loc.lng) * cosf(radians(current_loc.lat)) * 0.0111194927f;
2011-09-08 22:31:32 -03:00
port->printf_P(PSTR("dP = <%d%c, %d%c, %.1f> meters\n"),
abs(y), (y >= 0 ? 'N' : 'S'),
abs(x), (x >= 0 ? 'E' : 'W'),
(float)(wp->alt - current_loc.alt) / 100);
}
/* print_error prints an error message if the user sends an invalid command
*/
void
GCS_DEBUGTERMINAL::print_error(const char *msg)
{
BetterStream *port = _port;
if (msg == 0)
port->println_P(PSTR("ERROR: Invalid command"));
else
port->println_P(msg);
}
void
GCS_DEBUGTERMINAL::send_text(uint8_t severity, const char *str)
{
BetterStream *port = _port;
if(severity >= DEBUG_LEVEL){
port->print_P(PSTR("MSG: "));
port->println(str);
}
}
void
GCS_DEBUGTERMINAL::send_message(uint8_t id, uint32_t param)
{
switch(id) {
case MSG_MODE_CHANGE:
print_control_mode();
break;
case MSG_HEARTBEAT:
if (report_heartbeat)
print_control_mode();
break;
case MSG_ATTITUDE:
if (report_attitude)
print_attitude();
break;
case MSG_LOCATION:
if (report_location)
print_position();
if (first_location == 0) {
send_text(0,"First location received");
first_location = 1;
}
break;
case MSG_CPU_LOAD:
if (report_cpu_load) {
_port->printf_P(PSTR("MSG: load %ld%%\n"), param);
}
break;
case MSG_COMMAND_LIST:
struct Location cmd = get_wp_with_index(param);
print_command(&cmd, param);
break;
}
}
void
GCS_DEBUGTERMINAL::print_current_waypoints()
{
_port->printf_P(PSTR("Current waypoints:"
" Prev:\t%ld,\t%ld\t%ld\n"
" Cur: \t%ld,\t%ld\t%ld\n"
" Next:%d\t%ld,\t%ld\t%ld\n"),
prev_WP.lat, prev_WP.lng, prev_WP.alt,
current_loc.lat, current_loc.lng, current_loc.alt,
(int)get(PARAM_WP_INDEX), next_WP.lat, next_WP.lng, next_WP.alt);
}
void
GCS_DEBUGTERMINAL::print_position(void)
{
recalc_climb_rate();
_port->printf_P(PSTR("Pos: %ld, %ld, %ldcm, %ldcm/s GS, %d cm/s AS, %d cm above home, %d? climb, %ldm from wp, %d%% throttle, %ld alt err\n"),
current_loc.lat, current_loc.lng, current_loc.alt,
gps.ground_speed,
airspeed,
get_altitude_above_home(),
climb_rate,
wp_distance,
get(PARAM_TRIM_THROTTLE),
altitude_error);
}
void
GCS_DEBUGTERMINAL::print_attitude(void)
{
_port->printf_P(PSTR("Att: %u roll_in, %u pitch in, %u throttle_in, "
"%ld roll_sensor, %ld pitch_sensor, "
"%ld ground_course, %ld target_bearing, "
"%ld nav_roll, %d loiter_sum, "
"%d roll servo_out, %d pitch_servo_out\n"),
radio_in[CH_ROLL], radio_in[CH_PITCH], radio_in[CH_THROTTLE],
dcm.roll_sensor, dcm.pitch_sensor,
gps.ground_course, target_bearing,
nav_roll, loiter_sum,
servo_out[CH_ROLL], servo_out[CH_PITCH]);
}
// required by Groundstation to plot lateral tracking course
void
GCS_DEBUGTERMINAL::print_new_wp_info()
{
_port->printf_P(PSTR("New waypt (%d): %ld, %ld, %ldm -> "
"%ld, %ld, %ldm; "
"%ldm dist, %u roll trim, %u pitch trim\n"),
(int)get(PARAM_WP_INDEX),
prev_WP.lat, prev_WP.lng, prev_WP.alt,
next_WP.lat, next_WP.lng, next_WP.alt,
wp_totalDistance,
radio_trim(CH_ROLL), radio_trim(CH_PITCH));
}
void
GCS_DEBUGTERMINAL::print_control_mode(void)
{
BetterStream *port = _port;
switch (control_mode){
case MANUAL:
port->println_P(PSTR("##MANUAL"));
break;
case STABILIZE:
port->println_P(PSTR("##STABILIZE"));
break;
case FLY_BY_WIRE_A:
port->println_P(PSTR("##FBW A"));
break;
case FLY_BY_WIRE_B:
port->println_P(PSTR("##FBW B"));
break;
case AUTO:
port->println_P(PSTR("##AUTO"));
break;
case RTL:
port->println_P(PSTR("##RTL"));
break;
case LOITER:
port->println_P(PSTR("##LOITER"));
break;
case 98:
port->println_P(PSTR("##Air Start Complete"));
break;
case 99:
port->println_P(PSTR("##Ground Start Complete"));
break;
}
}
void
GCS_DEBUGTERMINAL::print_tuning(void)
{
_port->printf_P(PSTR("TUN:%d, %ld, %ld, %d, %ld, %ld\n"),
servo_out[CH_ROLL], nav_roll / 100, dcm.roll_sensor / 100,
servo_out[CH_PITCH], nav_pitch / 100, dcm.pitch_sensor / 100);
}
void
GCS_DEBUGTERMINAL::print_command_id(byte id)
{
BetterStream *port = _port;
switch (id) {
// Command IDs - Must
case CMD_BLANK: port->print_P(PSTR("CMD_BLANK")); break;
case CMD_WAYPOINT: port->print_P(PSTR("waypoint")); break;
case CMD_LOITER: port->print_P(PSTR("loiter")); break;
case CMD_LOITER_N_TURNS: port->print_P(PSTR("loiternturns")); break;
case CMD_LOITER_TIME: port->print_P(PSTR("loitertime")); break;
case CMD_RTL: port->print_P(PSTR("rtl")); break;
case CMD_LAND: port->print_P(PSTR("land")); break;
case CMD_TAKEOFF: port->print_P(PSTR("takeoff")); break;
// Command IDs - May
case CMD_DELAY: port->print_P(PSTR("delay")); break;
case CMD_CLIMB: port->print_P(PSTR("climb")); break;
case CMD_LAND_OPTIONS: port->print_P(PSTR("landoptions")); break;
// Command IDs - Now
case CMD_RESET_INDEX: port->print_P(PSTR("resetindex")); break;
case CMD_GOTO_INDEX: port->print_P(PSTR("CMD_GOTO_INDEX")); break;
case CMD_GETVAR_INDEX: port->print_P(PSTR("CMD_GETVAR_INDEX")); break;
case CMD_SENDVAR_INDEX: port->print_P(PSTR("CMD_SENDVAR_INDEX")); break;
case CMD_TELEMETRY: port->print_P(PSTR("CMD_TELEMETRY")); break;
case CMD_THROTTLE_CRUISE: port->print_P(PSTR("throttlecruise")); break;
case CMD_AIRSPEED_CRUISE: port->print_P(PSTR("CMD_AIRSPEED_CRUISE")); break;
case CMD_RESET_HOME: port->print_P(PSTR("resethome")); break;
case CMD_KP_GAIN: port->print_P(PSTR("CMD_KP_GAIN")); break;
case CMD_KI_GAIN: port->print_P(PSTR("CMD_KI_GAIN")); break;
case CMD_KD_GAIN: port->print_P(PSTR("CMD_KD_GAIN")); break;
case CMD_KI_MAX: port->print_P(PSTR("CMD_KI_MAX")); break;
case CMD_KFF_GAIN: port->print_P(PSTR("CMD_KFF_GAIN")); break;
case CMD_RADIO_TRIM: port->print_P(PSTR("CMD_RADIO_TRIM")); break;
case CMD_RADIO_MAX: port->print_P(PSTR("CMD_RADIO_MAX")); break;
case CMD_RADIO_MIN: port->print_P(PSTR("CMD_RADIO_MIN")); break;
case CMD_ELEVON_TRIM: port->print_P(PSTR("CMD_ELEVON_TRIM")); break;
case CMD_INDEX: port->print_P(PSTR("index")); break;
case CMD_REPEAT: port->print_P(PSTR("CMD_REPEAT")); break;
case CMD_RELAY: port->print_P(PSTR("relay")); break;
case CMD_SERVO: port->print_P(PSTR("servo")); break;
default: port->print(id,DEC);
}
}
void
GCS_DEBUGTERMINAL::print_command(struct Location *cmd,byte index)
{
BetterStream *port = _port;
port->print_P(PSTR(" command #: "));
port->print(index, DEC);
port->print_P(PSTR(" id: "));
print_command_id(cmd->id);
port->print_P(PSTR(" p1: "));
port->print(cmd->p1,DEC);
port->print_P(PSTR(" p2/alt: "));
port->print(cmd->alt,DEC);
port->print_P(PSTR(" p3/lat: "));
port->print(cmd->lat,DEC);
port->print_P(PSTR(" p4/lng: "));
port->println(cmd->lng,DEC);
}
void
GCS_DEBUGTERMINAL::print_commands()
{
print_commands(1, get(PARAM_WP_TOTAL));
}
void
GCS_DEBUGTERMINAL::print_commands(unsigned char i1, unsigned char i2)
{
BetterStream *port = _port;
port->println_P(PSTR("Commands in memory:"));
port->print_P(PSTR(" "));
port->print(get(PARAM_WP_TOTAL), DEC);
port->println_P(PSTR(" commands total"));
// create a location struct to hold the temp Waypoints for printing
//Location tmp;
port->println_P(PSTR(" Home: "));
struct Location cmd = get_wp_with_index(0);
print_command(&cmd, 0);
port->println_P(PSTR(" Commands: "));
for (int i=i1; i<=i2; i++){
cmd = get_wp_with_index(i);
print_command(&cmd, i);
delay(10);
}
}
void
GCS_DEBUGTERMINAL::print_gains()
{
BetterStream *port = _port;
unsigned char i;
port->println_P(PSTR("PID gains \tP \tI \tD \tIMax)"));
port->print_P(PSTR("Servo roll \t")); print_gain(CASE_SERVO_ROLL);
port->print_P(PSTR("Servo pitch \t")); print_gain(CASE_SERVO_PITCH);
port->print_P(PSTR("Servo yaw \t")); print_gain(CASE_SERVO_RUDDER);
port->print_P(PSTR("Nav roll \t")); print_gain(CASE_NAV_ROLL);
port->print_P(PSTR("Nav pitch \t"));
if (AIRSPEED_SENSOR)
print_gain(CASE_NAV_PITCH_ASP);
else
print_gain(CASE_NAV_PITCH_ALT);
port->print_P(PSTR("Nav throttle\t"));
if (AIRSPEED_SENSOR)
print_gain(CASE_TE_THROTTLE);
else
print_gain(CASE_ALT_THROTTLE);
port->println_P(PSTR("Feed-forward gains"));
port->print_P(PSTR("Pitch compensation\t")); port->println(get(PARAM_KFF_PTCHCOMP),3);
port->print_P(PSTR("Rudder mix \t")); port->println(get(PARAM_KFF_RDDRMIX),3);
port->print_P(PSTR("Pitch to throttle \t")); port->println(get(PARAM_KFF_PTCH2THR),3);
//port->print_P(PSTR("Throttle to pitch \t")); port->println(kff[CASE_T_TO_P],3);
}
void
GCS_DEBUGTERMINAL::print_gain(unsigned char g)
{
BetterStream *port = _port;
port->print(pid_index[g]->kP(),3);
port->print_P(PSTR("\t"));
port->print(pid_index[g]->kI(),3);
port->print_P(PSTR("\t"));
port->print(pid_index[g]->kD(),3);
port->print_P(PSTR("\t"));
port->println(pid_index[g]->imax(),DEC);
}
void
GCS_DEBUGTERMINAL::print_gain_keyword_error()
{
print_error(ERR("ERROR: Did not recognize keyword; type k -? for more information"));
}
void
GCS_DEBUGTERMINAL::print_PID(long PID_error, long dt, float scaler, float derivative, float integrator, float last_error)
{
BetterStream *port = _port;
port->print_P(PSTR("P = ")); port->print(pid_index[display_PID]->kP() * scaler * (float)PID_error,2);
port->print_P(PSTR(",\tI = ")); port->print(integrator,2);
port->print_P(PSTR(",\tD = ")); port->print(pid_index[display_PID]->kD() * scaler * derivative,2);
port->print_P(PSTR("\terrors = {")); port->print(PID_error,DEC);
port->print_P(PSTR(", ")); port->print(last_error,DEC);
port->print_P(PSTR("} with dt = ")); port->println(dt,DEC);
display_PID = -1;
}
#endif // GCS_PROTOCOL_DEBUGTERMINAL
