mirror of https://github.com/ArduPilot/ardupilot
Rover: use L1 controller for navigation
this uses the AP_L1_Control library for rover navigation
This commit is contained in:
Andrew Tridgell
parent
396c4f30d2
commit
97ed733ada
|
|
@ -90,6 +90,8 @@
|
|||
#include <SITL.h>
|
||||
#include <AP_Scheduler.h> // main loop scheduler
|
||||
#include <stdarg.h>
|
||||
#include <AP_Navigation.h>
|
||||
#include <AP_L1_Control.h>
|
||||
|
||||
#include <AP_HAL_AVR.h>
|
||||
#include <AP_HAL_AVR_SITL.h>
|
||||
|
|
@ -242,6 +244,11 @@ AP_InertialSensor_Oilpan ins( &adc );
|
|||
|
||||
AP_AHRS_DCM ahrs(&ins, g_gps);
|
||||
|
||||
static AP_L1_Control L1_controller(ahrs);
|
||||
|
||||
// selected navigation controller
|
||||
static AP_Navigation *nav_controller = &L1_controller;
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
|
||||
SITL sitl;
|
||||
#endif
|
||||
|
|
@ -372,17 +379,6 @@ const float radius_of_earth = 6378100; // meters
|
|||
// true if we have a position estimate from AHRS
|
||||
static bool have_position;
|
||||
|
||||
// This is the currently calculated direction to fly.
|
||||
// deg * 100 : 0 to 360
|
||||
static int32_t nav_bearing;
|
||||
// This is the direction to the next waypoint
|
||||
// deg * 100 : 0 to 360
|
||||
static int32_t target_bearing;
|
||||
//This is the direction from the last waypoint to the next waypoint
|
||||
// deg * 100 : 0 to 360
|
||||
static int32_t crosstrack_bearing;
|
||||
// A gain scaler to account for ground speed/headwind/tailwind
|
||||
static float nav_gain_scaler = 1.0f;
|
||||
static bool rtl_complete = false;
|
||||
|
||||
// There may be two active commands in Auto mode.
|
||||
|
|
@ -427,15 +423,6 @@ static bool auto_triggered;
|
|||
static float ground_speed = 0;
|
||||
static int16_t throttle_last = 0, throttle = 500;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Location Errors
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Difference between current bearing and desired bearing. in centi-degrees
|
||||
static int32_t bearing_error_cd;
|
||||
|
||||
// Distance perpandicular to the course line that we are off trackline. Meters
|
||||
static float crosstrack_error;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// CH7 control
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
|
@ -462,8 +449,8 @@ static float current_total1;
|
|||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Navigation control variables
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// The instantaneous desired steering angle. Hundredths of a degree
|
||||
static int32_t nav_steer_cd;
|
||||
// The instantaneous desired lateral acceleration in m/s/s
|
||||
static float lateral_acceleration;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Waypoint distances
|
||||
|
|
@ -663,9 +650,6 @@ static void fast_loop()
|
|||
|
||||
read_sonars();
|
||||
|
||||
// uses the yaw from the DCM to give more accurate turns
|
||||
calc_bearing_error();
|
||||
|
||||
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
|
||||
Log_Write_Attitude();
|
||||
|
||||
|
|
@ -865,23 +849,22 @@ static void update_current_mode(void)
|
|||
case AUTO:
|
||||
case RTL:
|
||||
case GUIDED:
|
||||
calc_lateral_acceleration();
|
||||
calc_nav_steer();
|
||||
calc_throttle(g.speed_cruise);
|
||||
break;
|
||||
|
||||
case STEERING:
|
||||
case STEERING: {
|
||||
/*
|
||||
in steering mode we control the bearing error, which gives
|
||||
the same type of steering control as auto mode. The throttle
|
||||
controls the target speed, in proportion to the throttle
|
||||
in steering mode we control lateral acceleration directly
|
||||
*/
|
||||
bearing_error_cd = channel_steer->pwm_to_angle();
|
||||
lateral_acceleration = g.turn_max_g * GRAVITY_MSS * (channel_steer->pwm_to_angle()/4500.0f);
|
||||
calc_nav_steer();
|
||||
|
||||
/* we need to reset the I term or it will build up */
|
||||
g.pidNavSteer.reset_I();
|
||||
// and throttle gives speed in proportion to cruise speed
|
||||
calc_throttle(channel_throttle->pwm_to_angle() * 0.01 * g.speed_cruise);
|
||||
break;
|
||||
}
|
||||
|
||||
case LEARNING:
|
||||
case MANUAL:
|
||||
|
|
@ -923,8 +906,8 @@ static void update_navigation()
|
|||
case RTL:
|
||||
case GUIDED:
|
||||
// no loitering around the wp with the rover, goes direct to the wp position
|
||||
calc_lateral_acceleration();
|
||||
calc_nav_steer();
|
||||
calc_bearing_error();
|
||||
if (verify_RTL()) {
|
||||
channel_throttle->servo_out = g.throttle_min.get();
|
||||
set_mode(HOLD);
|
||||
|
|
|
|||
|
|
@ -237,17 +237,16 @@ static void NOINLINE send_location(mavlink_channel_t chan)
|
|||
|
||||
static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
|
||||
{
|
||||
int16_t bearing = nav_bearing / 100;
|
||||
mavlink_msg_nav_controller_output_send(
|
||||
chan,
|
||||
nav_steer_cd / 1.0e2,
|
||||
lateral_acceleration,
|
||||
0,
|
||||
bearing,
|
||||
target_bearing / 100,
|
||||
nav_controller->nav_bearing_cd() * 0.01f,
|
||||
nav_controller->target_bearing_cd() * 0.01f,
|
||||
wp_distance,
|
||||
0,
|
||||
groundspeed_error,
|
||||
crosstrack_error);
|
||||
nav_controller->crosstrack_error());
|
||||
}
|
||||
|
||||
static void NOINLINE send_gps_raw(mavlink_channel_t chan)
|
||||
|
|
|
|||
|
|
@ -305,7 +305,6 @@ struct PACKED log_Nav_Tuning {
|
|||
float wp_distance;
|
||||
uint16_t target_bearing_cd;
|
||||
uint16_t nav_bearing_cd;
|
||||
int16_t nav_gain_scalar;
|
||||
int8_t throttle;
|
||||
};
|
||||
|
||||
|
|
@ -316,9 +315,8 @@ static void Log_Write_Nav_Tuning()
|
|||
LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG),
|
||||
yaw : (uint16_t)ahrs.yaw_sensor,
|
||||
wp_distance : wp_distance,
|
||||
target_bearing_cd : (uint16_t)target_bearing,
|
||||
nav_bearing_cd : (uint16_t)nav_bearing,
|
||||
nav_gain_scalar : (int16_t)(nav_gain_scaler*1000),
|
||||
target_bearing_cd : (uint16_t)nav_controller->target_bearing_cd(),
|
||||
nav_bearing_cd : (uint16_t)nav_controller->nav_bearing_cd(),
|
||||
throttle : (int8_t)(100 * channel_throttle->norm_output())
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
|
|
@ -364,7 +362,7 @@ static void Log_Write_Mode()
|
|||
|
||||
struct PACKED log_Sonar {
|
||||
LOG_PACKET_HEADER;
|
||||
int16_t nav_steer;
|
||||
float lateral_accel;
|
||||
uint16_t sonar1_distance;
|
||||
uint16_t sonar2_distance;
|
||||
uint16_t detected_count;
|
||||
|
|
@ -383,7 +381,7 @@ static void Log_Write_Sonar()
|
|||
}
|
||||
struct log_Sonar pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG),
|
||||
nav_steer : (int16_t)nav_steer_cd,
|
||||
lateral_accel : lateral_acceleration,
|
||||
sonar1_distance : (uint16_t)sonar.distance_cm(),
|
||||
sonar2_distance : (uint16_t)sonar2.distance_cm(),
|
||||
detected_count : obstacle.detected_count,
|
||||
|
|
@ -466,9 +464,9 @@ static const struct LogStructure log_structure[] PROGMEM = {
|
|||
{ LOG_CTUN_MSG, sizeof(log_Control_Tuning),
|
||||
"CTUN", "hcchf", "Steer,Roll,Pitch,ThrOut,AccY" },
|
||||
{ LOG_NTUN_MSG, sizeof(log_Nav_Tuning),
|
||||
"NTUN", "HfHHhb", "Yaw,WpDist,TargBrg,NavBrg,NavGain,Thr" },
|
||||
"NTUN", "HfHHb", "Yaw,WpDist,TargBrg,NavBrg,Thr" },
|
||||
{ LOG_SONAR_MSG, sizeof(log_Sonar),
|
||||
"SONR", "hHHHbHCb", "NavStr,S1Dist,S2Dist,DCnt,TAng,TTim,Spd,Thr" },
|
||||
"SONR", "fHHHbHCb", "LatAcc,S1Dist,S2Dist,DCnt,TAng,TTim,Spd,Thr" },
|
||||
{ LOG_CURRENT_MSG, sizeof(log_Current),
|
||||
"CURR", "hhhHf", "Thr,Volt,Curr,Vcc,CurrTot" },
|
||||
{ LOG_MODE_MSG, sizeof(log_Mode),
|
||||
|
|
|
|||
|
|
@ -74,6 +74,8 @@ public:
|
|||
k_param_ch7_option,
|
||||
k_param_auto_trigger_pin,
|
||||
k_param_auto_kickstart,
|
||||
k_param_turn_circle,
|
||||
k_param_turn_max_g,
|
||||
|
||||
//
|
||||
// 160: Radio settings
|
||||
|
|
@ -156,6 +158,7 @@ public:
|
|||
k_param_ins,
|
||||
k_param_compass,
|
||||
k_param_rcmap,
|
||||
k_param_L1_controller,
|
||||
|
||||
// 254,255: reserved
|
||||
};
|
||||
|
|
@ -194,14 +197,14 @@ public:
|
|||
|
||||
// navigation parameters
|
||||
//
|
||||
AP_Float crosstrack_gain;
|
||||
AP_Int16 crosstrack_entry_angle;
|
||||
AP_Float speed_cruise;
|
||||
AP_Int8 speed_turn_gain;
|
||||
AP_Float speed_turn_dist;
|
||||
AP_Int8 ch7_option;
|
||||
AP_Int8 auto_trigger_pin;
|
||||
AP_Float auto_kickstart;
|
||||
AP_Float turn_circle;
|
||||
AP_Float turn_max_g;
|
||||
|
||||
// RC channels
|
||||
RC_Channel rc_1;
|
||||
|
|
@ -265,7 +268,6 @@ public:
|
|||
|
||||
// PID controllers
|
||||
//
|
||||
PID pidNavSteer;
|
||||
PID pidServoSteer;
|
||||
PID pidSpeedThrottle;
|
||||
|
||||
|
|
@ -292,7 +294,6 @@ public:
|
|||
|
||||
// PID controller initial P initial I initial D initial imax
|
||||
//-----------------------------------------------------------------------------------
|
||||
pidNavSteer (0.7, 0.1, 0.2, 2000),
|
||||
pidServoSteer (0.5, 0.1, 0.2, 2000),
|
||||
pidSpeedThrottle (0.7, 0.2, 0.2, 4000)
|
||||
{}
|
||||
|
|
|
|||
|
|
@ -123,23 +123,6 @@ const AP_Param::Info var_info[] PROGMEM = {
|
|||
// @User: Standard
|
||||
GSCALAR(pack_capacity, "BATT_CAPACITY", HIGH_DISCHARGE),
|
||||
|
||||
// @Param: XTRK_GAIN_SC
|
||||
// @DisplayName: Crosstrack Gain
|
||||
// @Description: This controls how hard the Rover tries to follow the lines between waypoints, as opposed to driving directly to the next waypoint. The value is the scale between distance off the line and angle to meet the line (in Degrees * 100)
|
||||
// @Range: 0 2000
|
||||
// @Increment: 1
|
||||
// @User: Standard
|
||||
GSCALAR(crosstrack_gain, "XTRK_GAIN_SC", XTRACK_GAIN_SCALED),
|
||||
|
||||
// @Param: XTRK_ANGLE_CD
|
||||
// @DisplayName: Crosstrack Entry Angle
|
||||
// @Description: Maximum angle used to correct for track following.
|
||||
// @Units: centi-Degrees
|
||||
// @Range: 0 9000
|
||||
// @Increment: 1
|
||||
// @User: Standard
|
||||
GSCALAR(crosstrack_entry_angle, "XTRK_ANGLE_CD", XTRACK_ENTRY_ANGLE_CENTIDEGREE),
|
||||
|
||||
// @Param: AUTO_TRIGGER_PIN
|
||||
// @DisplayName: Auto mode trigger pin
|
||||
// @Description: pin number to use to enable the throttle in auto mode. If set to -1 then don't use a trigger, otherwise this is a pin number which if held low in auto mode will enable the motor to run. If the switch is released while in AUTO then the motor will stop again. This can be used in combination with INITIAL_MODE to give a 'press button to start' rover with no receiver.
|
||||
|
|
@ -431,7 +414,24 @@ const AP_Param::Info var_info[] PROGMEM = {
|
|||
// @User: Standard
|
||||
GSCALAR(waypoint_radius, "WP_RADIUS", 2.0f),
|
||||
|
||||
GGROUP(pidNavSteer, "HDNG2STEER_", PID),
|
||||
// @Param: TURN_CIRCLE
|
||||
// @DisplayName: Turning circle
|
||||
// @Description: The diameter of the turning circle in meters of the rover at low speed when wheels are turned to the maximum turn angle
|
||||
// @Units: meters
|
||||
// @Range: 0.5 50
|
||||
// @Increment: 0.1
|
||||
// @User: Standard
|
||||
GSCALAR(turn_circle, "TURN_CIRCLE", 1.5f),
|
||||
|
||||
// @Param: TURN_MAX_G
|
||||
// @DisplayName: Turning maximum G force
|
||||
// @Description: The maximum turning acceleration (in units of gravities) that the rover can handle while remaining stable. The navigation code will keep the lateral acceleration below this level to avoid rolling over or slipping the wheels in turns
|
||||
// @Units: gravities
|
||||
// @Range: 0.2 10
|
||||
// @Increment: 0.1
|
||||
// @User: Standard
|
||||
GSCALAR(turn_max_g, "TURN_MAX_G", 2.0f),
|
||||
|
||||
GGROUP(pidServoSteer, "STEER2SRV_", PID),
|
||||
GGROUP(pidSpeedThrottle, "SPEED2THR_", PID),
|
||||
|
||||
|
|
@ -456,6 +456,10 @@ const AP_Param::Info var_info[] PROGMEM = {
|
|||
GOBJECT(gcs0, "SR0_", GCS_MAVLINK),
|
||||
GOBJECT(gcs3, "SR3_", GCS_MAVLINK),
|
||||
|
||||
// @Group: NAVL1_
|
||||
// @Path: ../libraries/AP_L1_Control/AP_L1_Control.cpp
|
||||
GOBJECT(L1_controller, "NAVL1_", AP_L1_Control),
|
||||
|
||||
// @Group: SONAR_
|
||||
// @Path: ../libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp
|
||||
GOBJECT(sonar, "SONAR_", AP_RangeFinder_analog),
|
||||
|
|
|
|||
|
|
@ -87,7 +87,7 @@ static void calc_throttle(float target_speed)
|
|||
reduce target speed in proportion to turning rate, up to the
|
||||
SPEED_TURN_GAIN percentage.
|
||||
*/
|
||||
float steer_rate = fabsf((nav_steer_cd/nav_gain_scaler) / (float)SERVO_MAX);
|
||||
float steer_rate = fabsf(lateral_acceleration / (g.turn_max_g*GRAVITY_MSS));
|
||||
steer_rate = constrain_float(steer_rate, 0.0, 1.0);
|
||||
float reduction = 1.0 - steer_rate*(100 - g.speed_turn_gain)*0.01;
|
||||
|
||||
|
|
@ -117,27 +117,55 @@ static void calc_throttle(float target_speed)
|
|||
* Calculate desired turn angles (in medium freq loop)
|
||||
*****************************************/
|
||||
|
||||
static void calc_nav_steer()
|
||||
static void calc_lateral_acceleration()
|
||||
{
|
||||
// Adjust gain based on ground speed
|
||||
if (ground_speed < 0.01) {
|
||||
nav_gain_scaler = 1.4f;
|
||||
} else {
|
||||
nav_gain_scaler = g.speed_cruise / ground_speed;
|
||||
}
|
||||
nav_gain_scaler = constrain_float(nav_gain_scaler, 0.2f, 1.4f);
|
||||
switch (control_mode) {
|
||||
case AUTO:
|
||||
nav_controller->update_waypoint(prev_WP, next_WP);
|
||||
break;
|
||||
|
||||
// Calculate the required turn of the wheels rover
|
||||
// ----------------------------------------
|
||||
case RTL:
|
||||
case GUIDED:
|
||||
case STEERING:
|
||||
nav_controller->update_waypoint(current_loc, next_WP);
|
||||
break;
|
||||
default:
|
||||
return;
|
||||
}
|
||||
|
||||
// Calculate the required turn of the wheels
|
||||
|
||||
// negative error = left turn
|
||||
// positive error = right turn
|
||||
nav_steer_cd = g.pidNavSteer.get_pid_4500(bearing_error_cd, nav_gain_scaler);
|
||||
lateral_acceleration = nav_controller->lateral_acceleration();
|
||||
}
|
||||
|
||||
// avoid obstacles, if any
|
||||
nav_steer_cd += obstacle.turn_angle*100;
|
||||
/*
|
||||
calculate steering angle given lateral_acceleration
|
||||
*/
|
||||
static void calc_nav_steer()
|
||||
{
|
||||
float speed = g_gps->ground_speed_cm * 0.01f;
|
||||
float steer;
|
||||
|
||||
channel_steer->servo_out = nav_steer_cd;
|
||||
if (speed < 1.0f) {
|
||||
// gps speed isn't very accurate at low speed
|
||||
speed = 1.0f;
|
||||
}
|
||||
|
||||
// add in obstacle avoidance
|
||||
lateral_acceleration += (obstacle.turn_angle/45.0f) * g.turn_max_g;
|
||||
|
||||
// constrain to max G force
|
||||
lateral_acceleration = constrain_float(lateral_acceleration, -g.turn_max_g*GRAVITY_MSS, g.turn_max_g*GRAVITY_MSS);
|
||||
|
||||
// this is a linear approximation of the inverse steering
|
||||
// equation for a rover. It returns steering as a proportion
|
||||
// from -1 to 1
|
||||
steer = 0.5 * lateral_acceleration * g.turn_circle / (speed*speed);
|
||||
steer = constrain_float(steer, -1, 1);
|
||||
|
||||
channel_steer->servo_out = steer * 4500;
|
||||
}
|
||||
|
||||
/*****************************************
|
||||
|
|
|
|||
|
|
@ -123,12 +123,6 @@ static void set_next_WP(const struct Location *wp)
|
|||
// this is handy for the groundstation
|
||||
wp_totalDistance = get_distance(current_loc, next_WP);
|
||||
wp_distance = wp_totalDistance;
|
||||
target_bearing = get_bearing_cd(current_loc, next_WP);
|
||||
nav_bearing = target_bearing;
|
||||
|
||||
// set a new crosstrack bearing
|
||||
// ----------------------------
|
||||
reset_crosstrack();
|
||||
}
|
||||
|
||||
static void set_guided_WP(void)
|
||||
|
|
@ -144,11 +138,6 @@ static void set_guided_WP(void)
|
|||
// this is handy for the groundstation
|
||||
wp_totalDistance = get_distance(current_loc, next_WP);
|
||||
wp_distance = wp_totalDistance;
|
||||
target_bearing = get_bearing_cd(current_loc, next_WP);
|
||||
|
||||
// set a new crosstrack bearing
|
||||
// ----------------------------
|
||||
reset_crosstrack();
|
||||
}
|
||||
|
||||
// run this at setup on the ground
|
||||
|
|
@ -185,7 +174,6 @@ void init_home()
|
|||
|
||||
static void restart_nav()
|
||||
{
|
||||
g.pidNavSteer.reset_I();
|
||||
g.pidSpeedThrottle.reset_I();
|
||||
prev_WP = current_loc;
|
||||
nav_command_ID = NO_COMMAND;
|
||||
|
|
|
|||
|
|
@ -6,10 +6,6 @@
|
|||
static void
|
||||
handle_process_nav_cmd()
|
||||
{
|
||||
// reset navigation integrators
|
||||
// -------------------------
|
||||
g.pidNavSteer.reset_I();
|
||||
|
||||
gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nav_command.id);
|
||||
|
||||
switch(next_nav_command.id){
|
||||
|
|
@ -211,8 +207,6 @@ static bool verify_takeoff()
|
|||
|
||||
static bool verify_nav_wp()
|
||||
{
|
||||
update_crosstrack();
|
||||
|
||||
if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
|
||||
gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"),
|
||||
(unsigned)nav_command_index,
|
||||
|
|
|
|||
|
|
@ -24,9 +24,7 @@ static void read_control_switch()
|
|||
oldSwitchPosition = switchPosition;
|
||||
prev_WP = current_loc;
|
||||
|
||||
// reset navigation and speed integrators
|
||||
// -------------------------
|
||||
g.pidNavSteer.reset_I();
|
||||
// reset speed integrator
|
||||
g.pidSpeedThrottle.reset_I();
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -24,46 +24,12 @@ static void navigate()
|
|||
return;
|
||||
}
|
||||
|
||||
// target_bearing is where we should be heading
|
||||
// --------------------------------------------
|
||||
target_bearing = get_bearing_cd(current_loc, next_WP);
|
||||
|
||||
// nav_bearing will includes xtrac correction
|
||||
// ------------------------------------------
|
||||
nav_bearing = target_bearing;
|
||||
|
||||
// control mode specific updates to nav_bearing
|
||||
// --------------------------------------------
|
||||
update_navigation();
|
||||
}
|
||||
|
||||
|
||||
static void calc_bearing_error()
|
||||
{
|
||||
static butter10hz1_6 butter;
|
||||
|
||||
bearing_error_cd = wrap_180_cd(nav_bearing - ahrs.yaw_sensor);
|
||||
bearing_error_cd = butter.filter(bearing_error_cd);
|
||||
}
|
||||
|
||||
static void update_crosstrack(void)
|
||||
{
|
||||
// Crosstrack Error
|
||||
// ----------------
|
||||
|
||||
// If we are too far off or too close we don't do track following
|
||||
if (abs(wrap_180_cd(target_bearing - crosstrack_bearing)) < 4500 && wp_distance >= 3.0f) {
|
||||
crosstrack_error = sinf(radians((target_bearing - crosstrack_bearing) / (float)100)) * wp_distance; // Meters we are off track line
|
||||
nav_bearing += constrain_float(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
|
||||
nav_bearing = wrap_360_cd(nav_bearing);
|
||||
}
|
||||
}
|
||||
|
||||
static void reset_crosstrack()
|
||||
{
|
||||
crosstrack_bearing = get_bearing_cd(prev_WP, next_WP); // Used for track following
|
||||
}
|
||||
|
||||
void reached_waypoint()
|
||||
{
|
||||
|
||||
|
|
|
|||
|
|
@ -109,7 +109,6 @@ setup_show(uint8_t argc, const Menu::arg *argv)
|
|||
report_radio();
|
||||
report_batt_monitor();
|
||||
report_gains();
|
||||
report_xtrack();
|
||||
report_throttle();
|
||||
report_modes();
|
||||
report_compass();
|
||||
|
|
@ -510,28 +509,12 @@ static void report_gains()
|
|||
cliSerial->printf_P(PSTR("servo steer:\n"));
|
||||
print_PID(&g.pidServoSteer);
|
||||
|
||||
cliSerial->printf_P(PSTR("nav steer:\n"));
|
||||
print_PID(&g.pidNavSteer);
|
||||
|
||||
cliSerial->printf_P(PSTR("speed throttle:\n"));
|
||||
print_PID(&g.pidSpeedThrottle);
|
||||
|
||||
print_blanks(2);
|
||||
}
|
||||
|
||||
static void report_xtrack()
|
||||
{
|
||||
//print_blanks(2);
|
||||
cliSerial->printf_P(PSTR("Crosstrack\n"));
|
||||
print_divider();
|
||||
// radio
|
||||
cliSerial->printf_P(PSTR("XTRACK: %4.2f\n"
|
||||
"XTRACK angle: %d\n"),
|
||||
(float)g.crosstrack_gain,
|
||||
(int)g.crosstrack_entry_angle);
|
||||
print_blanks(2);
|
||||
}
|
||||
|
||||
static void report_throttle()
|
||||
{
|
||||
//print_blanks(2);
|
||||
|
|
|
|||
Loading…
Reference in New Issue