mirror of https://github.com/ArduPilot/ardupilot
Rover: added PIVOT_TURN_ANGLE parameter
this allows skid steering rovers to turn on the spot for sharp turns
This commit is contained in:
parent
bbf5c11c8a
commit
4e2d5043ba
|
@ -35,6 +35,7 @@ public:
|
||||||
k_param_scheduler,
|
k_param_scheduler,
|
||||||
k_param_relay,
|
k_param_relay,
|
||||||
k_param_BoardConfig,
|
k_param_BoardConfig,
|
||||||
|
k_param_pivot_turn_angle,
|
||||||
|
|
||||||
// IO pins
|
// IO pins
|
||||||
k_param_rssi_pin = 20,
|
k_param_rssi_pin = 20,
|
||||||
|
@ -207,6 +208,7 @@ public:
|
||||||
AP_Int8 auto_trigger_pin;
|
AP_Int8 auto_trigger_pin;
|
||||||
AP_Float auto_kickstart;
|
AP_Float auto_kickstart;
|
||||||
AP_Float turn_max_g;
|
AP_Float turn_max_g;
|
||||||
|
AP_Int16 pivot_turn_angle;
|
||||||
|
|
||||||
// RC channels
|
// RC channels
|
||||||
RC_Channel rc_1;
|
RC_Channel rc_1;
|
||||||
|
|
|
@ -145,6 +145,15 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||||
// @User: Standard
|
// @User: Standard
|
||||||
GSCALAR(speed_turn_dist, "SPEED_TURN_DIST", 2.0f),
|
GSCALAR(speed_turn_dist, "SPEED_TURN_DIST", 2.0f),
|
||||||
|
|
||||||
|
// @Param: PIVOT_TURN_ANGLE
|
||||||
|
// @DisplayName: Pivot turn angle
|
||||||
|
// @Description: Navigation angle threshold in degrees to switch to pivot steering when SKID_STEER_OUT is 1. This allows you to setup a skid steering rover to turn on the spot in auto mode when the angle it needs to turn it greater than this angle. An angle of zero means to disable pivot turning. Note that you will probably also want to set a low value for WP_RADIUS to get neat turns.
|
||||||
|
// @Units: degrees
|
||||||
|
// @Range: 0 360
|
||||||
|
// @Increment: 1
|
||||||
|
// @User: Standard
|
||||||
|
GSCALAR(pivot_turn_angle, "PIVOT_TURN_ANGLE", 30),
|
||||||
|
|
||||||
// @Param: CH7_OPTION
|
// @Param: CH7_OPTION
|
||||||
// @DisplayName: Channel 7 option
|
// @DisplayName: Channel 7 option
|
||||||
// @Description: What to do use channel 7 for
|
// @Description: What to do use channel 7 for
|
||||||
|
|
|
@ -64,6 +64,20 @@ static bool auto_check_trigger(void)
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
work out if we are going to use pivot steering
|
||||||
|
*/
|
||||||
|
static bool use_pivot_steering(void)
|
||||||
|
{
|
||||||
|
if (g.skid_steer_out && g.pivot_turn_angle != 0) {
|
||||||
|
int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100;
|
||||||
|
if (abs(bearing_error) > g.pivot_turn_angle) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
calculate the throtte for auto-throttle modes
|
calculate the throtte for auto-throttle modes
|
||||||
|
@ -110,6 +124,10 @@ static void calc_throttle(float target_speed)
|
||||||
} else {
|
} else {
|
||||||
channel_throttle->servo_out = constrain_int16(throttle, g.throttle_min, g.throttle_max);
|
channel_throttle->servo_out = constrain_int16(throttle, g.throttle_min, g.throttle_max);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (use_pivot_steering()) {
|
||||||
|
channel_throttle->servo_out = 0;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/*****************************************
|
/*****************************************
|
||||||
|
@ -137,6 +155,14 @@ static void calc_lateral_acceleration()
|
||||||
// negative error = left turn
|
// negative error = left turn
|
||||||
// positive error = right turn
|
// positive error = right turn
|
||||||
lateral_acceleration = nav_controller->lateral_acceleration();
|
lateral_acceleration = nav_controller->lateral_acceleration();
|
||||||
|
if (use_pivot_steering()) {
|
||||||
|
int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100;
|
||||||
|
if (bearing_error > 0) {
|
||||||
|
lateral_acceleration = g.turn_max_g*GRAVITY_MSS;
|
||||||
|
} else {
|
||||||
|
lateral_acceleration = -g.turn_max_g*GRAVITY_MSS;
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
|
Loading…
Reference in New Issue