Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: brake moved in from AC_WPNav

This commit is contained in:
Rishabh 2019-06-12 15:42:55 +09:00 committed by Randy Mackay
parent c59dde78c6
commit 00f6dc9b05
2 changed files with 25 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/mode.h
View File

@ -532,6 +532,8 @@ protected:
private:
void init_target();
uint32_t _timeout_start;
uint32_t _timeout_ms;

27
ArduCopter/mode_brake.cpp
View File

@ -10,7 +10,7 @@
bool ModeBrake::init(bool ignore_checks)
{
// set target to current position
wp_nav->init_brake_target(BRAKE_MODE_DECEL_RATE);
init_target();
// initialize vertical speed and acceleration
pos_control->set_max_speed_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z);
@ -34,7 +34,7 @@ void ModeBrake::run()
// if not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) {
make_safe_spool_down();
wp_nav->init_brake_target(BRAKE_MODE_DECEL_RATE);
init_target();
return;
}
@ -46,8 +46,9 @@ void ModeBrake::run()
loiter_nav->soften_for_landing();
}
// run brake controller
wp_nav->update_brake();
// use position controller to stop
pos_control->set_desired_velocity_xy(0.0f, 0.0f);
pos_control->update_xy_controller();
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), 0.0f);
@ -74,4 +75,22 @@ void ModeBrake::timeout_to_loiter_ms(uint32_t timeout_ms)
_timeout_ms = timeout_ms;
}
void ModeBrake::init_target()
{
// initialise position controller
pos_control->set_desired_velocity_xy(0.0f,0.0f);
pos_control->set_desired_accel_xy(0.0f,0.0f);
pos_control->init_xy_controller();
// initialise pos controller speed and acceleration
pos_control->set_max_speed_xy(inertial_nav.get_velocity().length());
pos_control->set_max_accel_xy(BRAKE_MODE_DECEL_RATE);
pos_control->calc_leash_length_xy();
// set target position
Vector3f stopping_point;
pos_control->get_stopping_point_xy(stopping_point);
pos_control->set_xy_target(stopping_point.x, stopping_point.y);
}
#endif