Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-04 23:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: land, loiter, rtl provide dt to AC_WPNav::set_pilot_desired_acceleration

Browse Source 
Also fix accelerations/lean-angles for land and rtl-land
This commit is contained in:
Randy Mackay 2018-02-03 11:56:44 +09:00
parent 93de23e7c4
commit 1a0be015f9
3 changed files with 19 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
ArduCopter/mode_land.cpp
View File

@ -202,7 +202,8 @@ void Copter::land_run_vertical_control(bool pause_descent)
void Copter::land_run_horizontal_control() void Copter::land_run_horizontal_control()
{ {
int16_t roll_control = 0, pitch_control = 0; float target_roll = 0.0f;
float target_pitch = 0.0f;
float target_yaw_rate = 0; float target_yaw_rate = 0;
// relax loiter target if we might be landed // relax loiter target if we might be landed
@ -224,12 +225,11 @@ void Copter::land_run_horizontal_control()
// apply SIMPLE mode transform to pilot inputs // apply SIMPLE mode transform to pilot inputs
update_simple_mode(); update_simple_mode();
// process pilot's roll and pitch input // convert pilot input to lean angles
roll_control = channel_roll->get_control_in(); Mode::get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, wp_nav->get_loiter_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
pitch_control = channel_pitch->get_control_in();
// record if pilot has overriden roll or pitch // record if pilot has overriden roll or pitch
if (roll_control != 0 || pitch_control != 0) { if (!is_zero(target_roll) || !is_zero(target_pitch)) {
ap.land_repo_active = true; ap.land_repo_active = true;
} }
} }
@ -255,9 +255,9 @@ void Copter::land_run_horizontal_control()
pos_control->override_vehicle_velocity_xy(-target_vel_rel); pos_control->override_vehicle_velocity_xy(-target_vel_rel);
} }
#endif #endif
// process roll, pitch inputs // process roll, pitch inputs
wp_nav->set_pilot_desired_acceleration(roll_control, pitch_control); wp_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt);
// run loiter controller // run loiter controller
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);

3
ArduCopter/mode_loiter.cpp
View File

@ -85,11 +85,10 @@ void Copter::ModeLoiter::run()
update_simple_mode(); update_simple_mode();
// convert pilot input to lean angles // convert pilot input to lean angles
// ToDo: convert get_pilot_desired_lean_angles to return angles as floats
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, wp_nav->get_loiter_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, wp_nav->get_loiter_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
// process pilot's roll and pitch input // process pilot's roll and pitch input
wp_nav->set_pilot_desired_acceleration(target_roll, target_pitch); wp_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt);
// get pilot's desired yaw rate // get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());

17
ArduCopter/mode_rtl.cpp
View File

@ -257,8 +257,9 @@ void Copter::ModeRTL::descent_start()
// called by rtl_run at 100hz or more // called by rtl_run at 100hz or more
void Copter::ModeRTL::descent_run() void Copter::ModeRTL::descent_run()
{ {
int16_t roll_control = 0, pitch_control = 0; float target_roll = 0.0f;
float target_yaw_rate = 0; float target_pitch = 0.0f;
float target_yaw_rate = 0.0f;
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
@ -282,9 +283,13 @@ void Copter::ModeRTL::descent_run()
// apply SIMPLE mode transform to pilot inputs // apply SIMPLE mode transform to pilot inputs
update_simple_mode(); update_simple_mode();
// process pilot's roll and pitch input // convert pilot input to lean angles
roll_control = channel_roll->get_control_in(); get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, wp_nav->get_loiter_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
pitch_control = channel_pitch->get_control_in();
// record if pilot has overriden roll or pitch
if (!is_zero(target_roll) || !is_zero(target_pitch)) {
ap.land_repo_active = true;
}
} }
// get pilot's desired yaw rate // get pilot's desired yaw rate
@ -295,7 +300,7 @@ void Copter::ModeRTL::descent_run()
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// process roll, pitch inputs // process roll, pitch inputs
wp_nav->set_pilot_desired_acceleration(roll_control, pitch_control); wp_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt);
// run loiter controller // run loiter controller
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);