diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index 4cf8122818..ed3ae61c9c 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -202,7 +202,8 @@ void Copter::land_run_vertical_control(bool pause_descent) 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; // 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 update_simple_mode(); - // process pilot's roll and pitch input - roll_control = channel_roll->get_control_in(); - pitch_control = channel_pitch->get_control_in(); + // convert pilot input to lean angles + 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()); // 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; } } @@ -255,9 +255,9 @@ void Copter::land_run_horizontal_control() pos_control->override_vehicle_velocity_xy(-target_vel_rel); } #endif - + // 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 wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index b60bd0ac44..a12fccc46a 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -85,11 +85,10 @@ void Copter::ModeLoiter::run() update_simple_mode(); // 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()); // 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 target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 1c721d3f05..dd16953ad1 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -257,8 +257,9 @@ void Copter::ModeRTL::descent_start() // called by rtl_run at 100hz or more void Copter::ModeRTL::descent_run() { - int16_t roll_control = 0, pitch_control = 0; - float target_yaw_rate = 0; + float target_roll = 0.0f; + 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 (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { @@ -282,9 +283,13 @@ void Copter::ModeRTL::descent_run() // apply SIMPLE mode transform to pilot inputs update_simple_mode(); - // process pilot's roll and pitch input - roll_control = channel_roll->get_control_in(); - pitch_control = channel_pitch->get_control_in(); + // convert pilot input to lean angles + 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()); + + // 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 @@ -295,7 +300,7 @@ void Copter::ModeRTL::descent_run() motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // 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 wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);