aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Rover: added new STEERING mode

Browse Source 
this makes it easier to tune for auto mode
This commit is contained in:
Andrew Tridgell 2013-03-01 22:32:57 +11:00
parent e228bbfebf
commit daa603552b
12 changed files with 83 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
APMrover2/APMrover2.pde
View File

@ -390,8 +390,7 @@ static int16_t throttle_last = 0, throttle = 500;
////////////////////////////////////////////////////////////////////////////////
// Difference between current bearing and desired bearing. in centi-degrees
static int32_t bearing_error_cd;
// Difference between current altitude and desired altitude. Centimeters
static int32_t altitude_error;
// Distance perpandicular to the course line that we are off trackline. Meters
static float crosstrack_error;
@ -422,13 +421,13 @@ static float current_total1;
////////////////////////////////////////////////////////////////////////////////
// Navigation control variables
////////////////////////////////////////////////////////////////////////////////
// The instantaneous desired bank angle. Hundredths of a degree
// The instantaneous desired steering angle. Hundredths of a degree
static int32_t nav_steer;
////////////////////////////////////////////////////////////////////////////////
// Waypoint distances
////////////////////////////////////////////////////////////////////////////////
// Distance between plane and next waypoint. Meters
// Distance between rover and next waypoint. Meters
static float wp_distance;
// Distance between previous and next waypoint. Meters
static int32_t wp_totalDistance;
@ -471,7 +470,7 @@ static struct Location home;
static bool home_is_set;
// The location of the previous waypoint. Used for track following and altitude ramp calculations
static struct Location prev_WP;
// The plane's current location
// The rover's current location
static struct Location current_loc;
// The location of the current/active waypoint. Used for track following
static struct Location next_WP;
@ -656,11 +655,6 @@ static void fast_loop()
// ---------------------------------------
update_current_mode();
// apply desired steering if in an auto mode
if (control_mode >= AUTO) {
g.channel_steer.servo_out = nav_steer;
}
// write out the servo PWM values
// ------------------------------
set_servos();
@ -847,12 +841,32 @@ static void update_current_mode(void)
case RTL:
case GUIDED:
calc_nav_steer();
calc_throttle();
calc_throttle(g.speed_cruise);
break;
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
*/
bearing_error_cd = g.channel_steer.pwm_to_angle();
calc_nav_steer();
/* we need to reset the I term or it will build up */
g.pidNavSteer.reset_I();
calc_throttle(g.channel_throttle.pwm_to_angle() * 0.01 * g.speed_cruise);
break;
case LEARNING:
case MANUAL:
nav_steer = 0;
/*
in both MANUAL and LEARNING we pass through the
controls. Setting servo_out here actually doesn't matter, as
we set the exact value in set_servos(), but it helps for
logging
*/
g.channel_throttle.servo_out = g.channel_throttle.radio_in;
g.channel_steer.servo_out = g.channel_steer.pwm_to_angle();
break;
@ -866,6 +880,7 @@ static void update_navigation()
switch (control_mode) {
case MANUAL:
case LEARNING:
case STEERING:
case INITIALISING:
break;

7
APMrover2/GCS_Mavlink.pde
View File

@ -42,9 +42,8 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
// ArduPlane documentation
switch (control_mode) {
case MANUAL:
base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case LEARNING:
case STEERING:
base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case AUTO:
@ -139,6 +138,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
break;
case LEARNING:
case STEERING:
control_sensors_enabled |= (1<<10); // 3D angular rate control
control_sensors_enabled |= (1<<11); // attitude stabilisation
break;
@ -244,7 +244,7 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
bearing,
target_bearing / 100,
wp_distance,
altitude_error / 1.0e2,
0,
groundspeed_error,
crosstrack_error);
}
@ -1082,6 +1082,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
switch (packet.custom_mode) {
case MANUAL:
case LEARNING:
case STEERING:
case AUTO:
case RTL:
set_mode((enum mode)packet.custom_mode);

11
APMrover2/Log.pde
View File

@ -420,10 +420,9 @@ static void Log_Read_Control_Tuning()
struct log_Nav_Tuning {
LOG_PACKET_HEADER;
uint16_t yaw;
uint32_t wp_distance;
float wp_distance;
uint16_t target_bearing_cd;
uint16_t nav_bearing_cd;
int16_t altitude_error_cm;
int16_t nav_gain_scheduler;
};
@ -433,10 +432,9 @@ static void Log_Write_Nav_Tuning()
struct log_Nav_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_NAV_TUNING_MSG),
yaw : (uint16_t)ahrs.yaw_sensor,
wp_distance : (uint32_t)wp_distance,
wp_distance : wp_distance,
target_bearing_cd : (uint16_t)target_bearing,
nav_bearing_cd : (uint16_t)nav_bearing,
altitude_error_cm : (int16_t)altitude_error,
nav_gain_scheduler : (int16_t)(nav_gain_scaler*1000)
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
@ -448,12 +446,11 @@ static void Log_Read_Nav_Tuning()
struct log_Nav_Tuning pkt;
DataFlash.ReadPacket(&pkt, sizeof(pkt));
cliSerial->printf_P(PSTR("NTUN, %4.4f, %lu, %4.4f, %4.4f, %4.4f, %4.4f\n"),
cliSerial->printf_P(PSTR("NTUN, %4.4f, %4.2f, %4.4f, %4.4f, %4.4f\n"),
(float)pkt.yaw/100.0f,
(unsigned long)pkt.wp_distance,
pkt.wp_distance,
(float)(pkt.target_bearing_cd/100.0f),
(float)(pkt.nav_bearing_cd/100.0f),
(float)(pkt.altitude_error_cm/100.0f),
(float)(pkt.nav_gain_scheduler/100.0f));
}

14
APMrover2/Parameters.pde
View File

@ -268,7 +268,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @DisplayName: Sonar trigger angle
// @Description: The course deviation in degrees to apply while avoiding an obstacle detected with the sonar. A positive number means to turn right, and a negative angle means to turn left.
// @Units: centimeters
// @Range: -90 90
// @Range: -45 45
// @Increment: 1
// @User: Standard
GSCALAR(sonar_turn_angle, "SONAR_TURN_ANGLE", 45),
@ -290,7 +290,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: MODE1
// @DisplayName: Mode1
// @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided
// @Values: 0:Manual,2:LEARNING,3:STEERING,10:Auto,11:RTL,15:Guided
// @User: Standard
// @Description: Driving mode for switch position 1 (910 to 1230 and above 2049)
GSCALAR(mode1, "MODE1", MODE_1),
@ -298,35 +298,35 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: MODE2
// @DisplayName: Mode2
// @Description: Driving mode for switch position 2 (1231 to 1360)
// @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided
// @Values: 0:Manual,2:LEARNING,3:STEERING,10:Auto,11:RTL,15:Guided
// @User: Standard
GSCALAR(mode2, "MODE2", MODE_2),
// @Param: MODE3
// @DisplayName: Mode3
// @Description: Driving mode for switch position 3 (1361 to 1490)
// @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided
// @Values: 0:Manual,2:LEARNING,3:STEERING,10:Auto,11:RTL,15:Guided
// @User: Standard
GSCALAR(mode3, "MODE3", MODE_3),
// @Param: MODE4
// @DisplayName: Mode4
// @Description: Driving mode for switch position 4 (1491 to 1620)
// @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided
// @Values: 0:Manual,2:LEARNING,3:STEERING,10:Auto,11:RTL,15:Guided
// @User: Standard
GSCALAR(mode4, "MODE4", MODE_4),
// @Param: MODE5
// @DisplayName: Mode5
// @Description: Driving mode for switch position 5 (1621 to 1749)
// @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided
// @Values: 0:Manual,2:LEARNING,3:STEERING,10:Auto,11:RTL,15:Guided
// @User: Standard
GSCALAR(mode5, "MODE5", MODE_5),
// @Param: MODE6
// @DisplayName: Mode6
// @Description: Driving mode for switch position 6 (1750 to 2049)
// @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided
// @Values: 0:Manual,2:LEARNING,3:STEERING,10:Auto,11:RTL,15:Guided
// @User: Standard
GSCALAR(mode6, "MODE6", MODE_6),

42
APMrover2/Steering.pde
View File

@ -17,11 +17,35 @@ static void throttle_slew_limit(int16_t last_throttle)
}
}
static void calc_throttle()
static void calc_throttle(float target_speed)
{
if (target_speed <= 0) {
// cope with zero requested speed
g.channel_throttle.servo_out = g.throttle_min.get();
return;
}
int throttle_target = g.throttle_cruise + throttle_nudge;
/*
reduce target speed in proportion to turning rate, up to the
SPEED_TURN_GAIN percentage.
*/
float steer_rate = fabsf(nav_steer / (float)SERVO_MAX);
steer_rate = constrain(steer_rate, 0.0, 1.0);
float reduction = 1.0 - steer_rate*(100 - g.speed_turn_gain)*0.01;
if (control_mode >= AUTO && wp_distance <= g.speed_turn_dist) {
// in auto-modes we reduce speed when approaching waypoints
float reduction2 = 1.0 - (100-g.speed_turn_gain)*0.01*((g.speed_turn_dist - wp_distance)/g.speed_turn_dist);
if (reduction2 < reduction) {
reduction = reduction2;
}
}
target_speed *= reduction;
groundspeed_error = g.speed_cruise - ground_speed;
groundspeed_error = target_speed - ground_speed;
throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100) / 100);
g.channel_throttle.servo_out = constrain_int16(throttle, g.throttle_min.get(), g.throttle_max.get());
@ -45,8 +69,10 @@ static void calc_nav_steer()
nav_steer = g.pidNavSteer.get_pid(bearing_error_cd, nav_gain_scaler);
if (obstacle) { // obstacle avoidance
nav_steer += g.sonar_turn_angle;
nav_steer += g.sonar_turn_angle*100;
}
g.channel_steer.servo_out = nav_steer;
}
/*****************************************
@ -56,23 +82,15 @@ static void set_servos(void)
{
int16_t last_throttle = g.channel_throttle.radio_out;
if ((control_mode == MANUAL) || (control_mode == LEARNING)) {
if (control_mode == MANUAL || control_mode == LEARNING) {
// do a direct pass through of radio values
g.channel_steer.radio_out = g.channel_steer.radio_in;
if (obstacle) // obstacle in front, turn right in Stabilize mode
g.channel_steer.radio_out -= 500;
g.channel_throttle.radio_out = g.channel_throttle.radio_in;
} else {
g.channel_steer.calc_pwm();
g.channel_throttle.radio_out = g.channel_throttle.radio_in;
g.channel_throttle.servo_out = constrain_int16(g.channel_throttle.servo_out,
g.throttle_min.get(),
g.throttle_max.get());
}
if (control_mode >= AUTO) {
// convert 0 to 100% into PWM
g.channel_throttle.calc_pwm();

2
APMrover2/control_modes.pde
View File

@ -79,7 +79,7 @@ static void read_trim_switch()
}
CH7_wp_index = 1;
return;
} else if (control_mode == LEARNING) {
} else if (control_mode == LEARNING || control_mode == STEERING) {
// if SW7 is ON in LEARNING = record the Wp
// set the next_WP (home is stored at 0)

1
APMrover2/defines.h
View File

@ -70,6 +70,7 @@ enum ch7_option {
enum mode {
MANUAL=0,
LEARNING=2,
STEERING=3,
AUTO=10,
RTL=11,
GUIDED=15,

1
APMrover2/events.pde
View File

@ -11,6 +11,7 @@ static void failsafe_long_on_event(int fstype)
{
case MANUAL:
case LEARNING:
case STEERING:
set_mode(RTL);
break;

2
APMrover2/navigation.pde
View File

@ -15,7 +15,7 @@ static void navigate()
return;
}
// waypoint distance from plane
// waypoint distance from rover
// ----------------------------
wp_distance = get_distance(&current_loc, &next_WP);

2
APMrover2/radio.pde
View File

@ -2,7 +2,7 @@
//Function that will read the radio data, limit servos and trigger a failsafe
// ----------------------------------------------------------------------------
static uint8_t failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling
static uint8_t failsafeCounter = 0; // we wait a second to take over the throttle and send the rover circling
static void init_rc_in()

1
APMrover2/setup.pde
View File

@ -231,6 +231,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
while (
mode != MANUAL &&
mode != LEARNING &&
mode != STEERING &&
mode != AUTO &&
mode != RTL)
{

6
APMrover2/system.pde
View File

@ -311,6 +311,7 @@ static void set_mode(enum mode mode)
{
case MANUAL:
case LEARNING:
case STEERING:
break;
case AUTO:
@ -362,7 +363,7 @@ static void startup_INS_ground(bool force_accel_level)
// Makes the servos wiggle twice - about to begin INS calibration - HOLD LEVEL AND STILL!!
// -----------------------
demo_servos(2);
gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Beginning INS calibration; do not move plane"));
gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Beginning INS calibration; do not move vehicle"));
mavlink_delay(1000);
ahrs.init();
@ -493,6 +494,9 @@ print_mode(uint8_t mode)
case LEARNING:
cliSerial->println_P(PSTR("Learning"));
break;
case STEERING:
cliSerial->println_P(PSTR("Stearing"));
break;
case AUTO:
cliSerial->println_P(PSTR("AUTO"));
break;