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

Rover: add object avoidance to steering and acro modes

Browse Source
This commit is contained in:
Randy Mackay 2017-12-13 18:17:42 +09:00
parent 2320bfb637
commit 509c7e2a8f
2 changed files with 10 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
APMrover2/mode_acro.cpp
View File

@ -17,7 +17,7 @@ void ModeAcro::update()
get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle); get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle);
// convert pilot throttle input to desired speed (up to twice the cruise speed) // convert pilot throttle input to desired speed (up to twice the cruise speed)
const float target_speed = desired_throttle * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f); float target_speed = desired_throttle * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);
// convert pilot steering input to desired turn rate in radians/sec // convert pilot steering input to desired turn rate in radians/sec
const float target_turn_rate = (desired_steering / 4500.0f) * radians(g2.acro_turn_rate); const float target_turn_rate = (desired_steering / 4500.0f) * radians(g2.acro_turn_rate);
@ -35,6 +35,9 @@ void ModeAcro::update()
const bool reversed = is_negative(target_speed); const bool reversed = is_negative(target_speed);
rover.set_reverse(reversed); rover.set_reverse(reversed);
// apply object avoidance to desired speed using half vehicle's maximum acceleration/deceleration
rover.g2.avoid.adjust_speed(0.0f, 0.5f * attitude_control.get_accel_max(), ahrs.yaw, target_speed, rover.G_Dt);
// run steering turn rate controller and throttle controller // run steering turn rate controller and throttle controller
const float steering_out = attitude_control.get_steering_out_rate(target_turn_rate, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, reversed); const float steering_out = attitude_control.get_steering_out_rate(target_turn_rate, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, reversed);
g2.motors.set_steering(steering_out * 4500.0f); g2.motors.set_steering(steering_out * 4500.0f);

7
APMrover2/mode_steering.cpp
View File

@ -7,7 +7,7 @@ void ModeSteering::update()
get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle); get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle);
// convert pilot throttle input to desired speed (up to twice the cruise speed) // convert pilot throttle input to desired speed (up to twice the cruise speed)
const float target_speed = desired_throttle * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f); float target_speed = desired_throttle * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);
// get speed forward // get speed forward
float speed; float speed;
@ -47,12 +47,17 @@ void ModeSteering::update()
// mark us as in_reverse when using a negative throttle // mark us as in_reverse when using a negative throttle
rover.set_reverse(reversed); rover.set_reverse(reversed);
// apply object avoidance to desired speed using half vehicle's maximum acceleration/deceleration
rover.g2.avoid.adjust_speed(0.0f, 0.5f * attitude_control.get_accel_max(), ahrs.yaw, target_speed, rover.G_Dt);
// run speed to throttle output controller // run speed to throttle output controller
if (is_zero(target_speed) && !is_pivot_turning) { if (is_zero(target_speed) && !is_pivot_turning) {
stop_vehicle(); stop_vehicle();
} else { } else {
// run lateral acceleration to steering controller // run lateral acceleration to steering controller
calc_steering_from_lateral_acceleration(desired_lat_accel, reversed); calc_steering_from_lateral_acceleration(desired_lat_accel, reversed);
// run speed to throttle controller
calc_throttle(target_speed, false); calc_throttle(target_speed, false);
} }
} }