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

Plane: tailsitter: start transtion from desired pitch not actual

Browse Source
This commit is contained in:
Iampete1 2021-11-06 00:12:47 +00:00 committed by Peter Hall
parent 51caf15f94
commit 773c48b30c
2 changed files with 30 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
ArduPlane/tailsitter.cpp
View File

@ -433,7 +433,7 @@ bool Tailsitter::transition_fw_complete(void)
gcs().send_text(MAV_SEVERITY_WARNING, "Transition FW done, roll error");
return true;
}
if (AP_HAL::millis() - transition->transition_start_ms > ((transition_angle_fw+(transition->transition_initial_pitch*0.01f))/transition_rate_fw)*1500) {
if (AP_HAL::millis() - transition->fw_transition_start_ms > ((transition_angle_fw+(transition->fw_transition_initial_pitch*0.01f))/transition_rate_fw)*1500) {
gcs().send_text(MAV_SEVERITY_WARNING, "Transition FW done, timeout");
return true;
}
@ -473,7 +473,7 @@ bool Tailsitter::transition_vtol_complete(void) const
gcs().send_text(MAV_SEVERITY_WARNING, "Transition VTOL done, roll error");
return true;
}
if (AP_HAL::millis() - transition->transition_start_ms > ((trans_angle-(transition->transition_initial_pitch*0.01f))/transition_rate_vtol)*1500) {
if (AP_HAL::millis() - transition->vtol_transition_start_ms > ((trans_angle-(transition->vtol_transition_initial_pitch*0.01f))/transition_rate_vtol)*1500) {
gcs().send_text(MAV_SEVERITY_WARNING, "Transition VTOL done, timeout");
return true;
}
@ -690,14 +690,13 @@ void Tailsitter_Transition::update()
case TRANSITION_ANGLE_WAIT_FW: {
if (tailsitter.transition_fw_complete()) {
transition_state = TRANSITION_DONE;
transition_start_ms = 0;
break;
}
quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
quadplane.assisted_flight = true;
uint32_t dt = now - transition_start_ms;
uint32_t dt = now - fw_transition_start_ms;
// multiply by 0.1 to convert (degrees/second * milliseconds) to centi degrees
plane.nav_pitch_cd = constrain_float(transition_initial_pitch - (quadplane.tailsitter.transition_rate_fw * dt) * 0.1f * (plane.fly_inverted()?-1.0f:1.0f), -8500, 8500);
plane.nav_pitch_cd = constrain_float(fw_transition_initial_pitch - (quadplane.tailsitter.transition_rate_fw * dt) * 0.1f * (plane.fly_inverted()?-1.0f:1.0f), -8500, 8500);
plane.nav_roll_cd = 0;
quadplane.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
plane.nav_pitch_cd,
@ -729,15 +728,13 @@ void Tailsitter_Transition::VTOL_update()
multicopter
*/
transition_state = TRANSITION_ANGLE_WAIT_VTOL;
transition_start_ms = now;
transition_initial_pitch = constrain_float(quadplane.ahrs.pitch_sensor,-8500,8500);
}
last_vtol_mode_ms = now;
if (transition_state == TRANSITION_ANGLE_WAIT_VTOL) {
float aspeed;
bool have_airspeed = quadplane.ahrs.airspeed_estimate(aspeed);
// provide assistance in forward flight portion of tailsitter transision
// provide assistance in forward flight portion of tailsitter transistion
quadplane.assisted_flight = quadplane.should_assist(aspeed, have_airspeed);
if (!quadplane.tailsitter.transition_vtol_complete()) {
return;
@ -770,11 +767,15 @@ void Tailsitter_Transition::set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& na
during transition to vtol in a tailsitter try to raise the
nose while keeping the wings level
*/
uint32_t dt = now - transition_start_ms;
uint32_t dt = now - vtol_transition_start_ms;
// multiply by 0.1 to convert (degrees/second * milliseconds) to centi degrees
nav_pitch_cd = constrain_float(transition_initial_pitch + (tailsitter.transition_rate_vtol * dt) * 0.1f, -8500, 8500);
nav_pitch_cd = constrain_float(vtol_transition_initial_pitch + (tailsitter.transition_rate_vtol * dt) * 0.1f, -8500, 8500);
nav_roll_cd = 0;
allow_stick_mixing = false;
} else if (transition_state == TRANSITION_DONE) {
// still in FW, reset transition starting point
force_transistion_complete();
}
}
@ -782,9 +783,17 @@ void Tailsitter_Transition::set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& na
void Tailsitter_Transition::restart()
{
transition_state = TRANSITION_ANGLE_WAIT_FW;
transition_start_ms = AP_HAL::millis();
transition_initial_pitch = constrain_float(quadplane.ahrs_view->pitch_sensor,-8500,8500);
};
fw_transition_start_ms = AP_HAL::millis();
fw_transition_initial_pitch = constrain_float(quadplane.attitude_control->get_attitude_target_quat().get_euler_pitch() * degrees(100.0),-8500,8500);
}
// force state to FW and setup for the transition back to VTOL
void Tailsitter_Transition::force_transistion_complete()
{
transition_state = TRANSITION_DONE;
vtol_transition_start_ms = AP_HAL::millis();
vtol_transition_initial_pitch = constrain_float(plane.nav_pitch_cd,-8500,8500);
}
MAV_VTOL_STATE Tailsitter_Transition::get_mav_vtol_state() const
{

15
ArduPlane/tailsitter.h
View File

@ -130,10 +130,7 @@ public:
void VTOL_update() override;
void force_transistion_complete() override {
transition_state = TRANSITION_DONE;
transition_start_ms = 0;
};
void force_transistion_complete() override;
bool complete() const override { return transition_state == TRANSITION_DONE; }
@ -158,9 +155,13 @@ private:
TRANSITION_DONE
} transition_state;
// timer start for transition
uint32_t transition_start_ms;
float transition_initial_pitch;
// for transition to VTOL flight
uint32_t vtol_transition_start_ms;
float vtol_transition_initial_pitch;
// for transition to FW flight
uint32_t fw_transition_start_ms;
float fw_transition_initial_pitch;
// time when we were last in a vtol control mode
uint32_t last_vtol_mode_ms;