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

Copter: tradheli replaces rotor_runup_complete with spool state

This commit is contained in:
bnsgeyer 2019-02-28 18:03:00 +09:00 committed by Randy Mackay
parent db5776d080
commit d949c80d54
13 changed files with 88 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopter/Copter.h
View File

@ -538,8 +538,7 @@ private:
// Tradheli flags
typedef struct {
uint8_t dynamic_flight : 1; // 0 // true if we are moving at a significant speed (used to turn on/off leaky I terms)
uint8_t init_targets_on_arming : 1; // 1 // true if we have been disarmed, and need to reset rate controller targets when we arm
uint8_t inverted_flight : 1; // 2 // true for inverted flight mode
uint8_t inverted_flight : 1; // 1 // true for inverted flight mode
} heli_flags_t;
heli_flags_t heli_flags;

2
ArduCopter/heli.cpp
View File

@ -23,7 +23,7 @@ void Copter::heli_init()
// should be called at 50hz
void Copter::check_dynamic_flight(void)
{
if (!motors->armed() || !motors->rotor_runup_complete() ||
if (motors->get_spool_mode() != AP_Motors::THROTTLE_UNLIMITED ||
control_mode == LAND || (control_mode==RTL && mode_rtl.state() == RTL_Land) || (control_mode == AUTO && mode_auto.mode() == Auto_Land)) {
heli_dynamic_flight_counter = 0;
heli_flags.dynamic_flight = false;

2
ArduCopter/land_detector.cpp
View File

@ -47,7 +47,7 @@ void Copter::update_land_detector()
} else if (ap.land_complete) {
#if FRAME_CONFIG == HELI_FRAME
// if rotor speed and collective pitch are high then clear landing flag
if (motors->get_throttle() > get_non_takeoff_throttle() && !motors->limit.throttle_lower && motors->rotor_runup_complete()) {
if (motors->get_throttle() > get_non_takeoff_throttle() && !motors->limit.throttle_lower && motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) {
#else
// if throttle output is high then clear landing flag
if (motors->get_throttle() > get_non_takeoff_throttle()) {

9
ArduCopter/mode.cpp
View File

@ -193,7 +193,7 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
#if FRAME_CONFIG == HELI_FRAME
// do not allow helis to enter a non-manual throttle mode if the
// rotor runup is not complete
if (!ignore_checks && !new_flightmode->has_manual_throttle() && !motors->rotor_runup_complete()){
if (!ignore_checks && !new_flightmode->has_manual_throttle() && (motors->get_spool_mode() == AP_Motors::SPOOL_UP || motors->get_spool_mode() == AP_Motors::SPOOL_DOWN)) {
gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed");
AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
return false;
@ -381,12 +381,11 @@ bool Copter::Mode::_TakeOff::triggered(const float target_climb_rate) const
// can't takeoff unless we want to go up...
return false;
}
#if FRAME_CONFIG == HELI_FRAME
if (!copter.motors->rotor_runup_complete()) {
// hold heli on the ground until rotor speed runup has finished
if (copter.motors->get_spool_mode() != AP_Motors::THROTTLE_UNLIMITED) {
// hold aircraft on the ground until rotor speed runup has finished
return false;
}
#endif
return true;
}

38
ArduCopter/mode_acro_heli.cpp
View File

@ -29,20 +29,34 @@ void Copter::ModeAcro_Heli::run()
float target_roll, target_pitch, target_yaw;
float pilot_throttle_scaled;
// Tradheli should not reset roll, pitch, yaw targets when motors are not runup, because
// we may be in autorotation flight. These should be reset only when transitioning from disarmed
// to armed, because the pilot will have placed the helicopter down on the landing pad. This is so
// that the servos move in a realistic fashion while disarmed for operational checks.
// Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero so the swash servos move
if (motors->init_targets_on_arming()) {
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_attitude_target_to_current_attitude();
// Tradheli should not reset roll, pitch, yaw targets when motors are not runup while flying, because
// we may be in autorotation flight. This is so that the servos move in a realistic fashion while disarmed
// for operational checks. Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero
// so the swash servos move.
if (!motors->armed()) {
// Motors should be Stopped
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
} else {
// heli will not let the spool state progress to THROTTLE_UNLIMITED until motor interlock is enabled
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
// clear landing flag above zero throttle
if (motors->armed() && motors->get_interlock() && motors->rotor_runup_complete() && !ap.throttle_zero) {
set_land_complete(false);
if (motors->get_spool_mode() == AP_Motors::SHUT_DOWN) {
// Motors Stopped
attitude_control->set_attitude_target_to_current_attitude();
attitude_control->reset_rate_controller_I_terms();
} else if (motors->get_spool_mode() == AP_Motors::GROUND_IDLE) {
// Landed
if (motors->init_targets_on_arming()) {
attitude_control->set_attitude_target_to_current_attitude();
attitude_control->reset_rate_controller_I_terms();
}
} else if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) {
// clear landing flag above zero throttle
if (!motors->limit.throttle_lower) {
set_land_complete(false);
}
}
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

7
ArduCopter/mode_brake.cpp
View File

@ -61,7 +61,12 @@ void Copter::ModeBrake::run()
// body-frame rate controller is run directly from 100hz loop
// update altitude target and call position controller
pos_control->set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
// protects heli's from inflight motor interlock disable
if (motors->get_desired_spool_state() == AP_Motors::DESIRED_GROUND_IDLE && !ap.land_complete) {
pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
} else {
pos_control->set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
}
pos_control->update_z_controller();
if (_timeout_ms != 0 && millis()-_timeout_start >= _timeout_ms) {

7
ArduCopter/mode_circle.cpp
View File

@ -85,7 +85,12 @@ void Copter::ModeCircle::run()
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
// update altitude target and call position controller
pos_control->set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
// protects heli's from inflight motor interlock disable
if (motors->get_desired_spool_state() == AP_Motors::DESIRED_GROUND_IDLE && !ap.land_complete) {
pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
} else {
pos_control->set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
}
pos_control->update_z_controller();
}

7
ArduCopter/mode_flowhold.cpp
View File

@ -84,13 +84,6 @@ Copter::ModeFlowHold::ModeFlowHold(void) : Mode()
// flowhold_init - initialise flowhold controller
bool Copter::ModeFlowHold::init(bool ignore_checks)
{
#if FRAME_CONFIG == HELI_FRAME
// do not allow helis to enter Flow Hold if the Rotor Runup is not complete
if (!ignore_checks && !motors->rotor_runup_complete()){
return false;
}
#endif
if (!copter.optflow.enabled() || !copter.optflow.healthy()) {
return false;
}

4
ArduCopter/mode_guided.cpp
View File

@ -395,8 +395,8 @@ void Copter::Mode::auto_takeoff_run()
}
#if FRAME_CONFIG == HELI_FRAME
// helicopters stay in landed state until rotor speed runup has finished
if (motors->rotor_runup_complete()) {
// aircraft stays in landed state until rotor speed runup has finished
if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) {
set_land_complete(false);
} else {
// initialise wpnav targets

45
ArduCopter/mode_stabilize_heli.cpp
View File

@ -22,22 +22,7 @@ void Copter::ModeStabilize_Heli::run()
float target_yaw_rate;
float pilot_throttle_scaled;
// Tradheli should not reset roll, pitch, yaw targets when motors are not runup, because
// we may be in autorotation flight. These should be reset only when transitioning from disarmed
// to armed, because the pilot will have placed the helicopter down on the landing pad. This is so
// that the servos move in a realistic fashion while disarmed for operational checks.
// Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero so the swash servos move
if (motors->init_targets_on_arming()) {
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading();
}
// clear landing flag above zero throttle
if (motors->armed() && motors->get_interlock() && motors->rotor_runup_complete() && !ap.throttle_zero) {
set_land_complete(false);
}
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// apply SIMPLE mode transform to pilot inputs
@ -52,6 +37,36 @@ void Copter::ModeStabilize_Heli::run()
// get pilot's desired throttle
pilot_throttle_scaled = copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in());
// Tradheli should not reset roll, pitch, yaw targets when motors are not runup while flying, because
// we may be in autorotation flight. This is so that the servos move in a realistic fashion while disarmed
// for operational checks. Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero
// so the swash servos move.
if (!motors->armed()) {
// Motors should be Stopped
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
} else {
// heli will not let the spool state progress to THROTTLE_UNLIMITED until motor interlock is enabled
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
if (motors->get_spool_mode() == AP_Motors::SHUT_DOWN) {
// Motors Stopped
attitude_control->set_yaw_target_to_current_heading();
attitude_control->reset_rate_controller_I_terms();
} else if (motors->get_spool_mode() == AP_Motors::GROUND_IDLE) {
// Landed
if (motors->init_targets_on_arming()) {
attitude_control->set_yaw_target_to_current_heading();
attitude_control->reset_rate_controller_I_terms();
}
} else if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) {
// clear landing flag above zero throttle
if (!motors->limit.throttle_lower) {
set_land_complete(false);
}
}
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);

4
ArduCopter/motors.cpp
View File

@ -93,13 +93,11 @@ void Copter::auto_disarm_check()
return;
}
#if FRAME_CONFIG == HELI_FRAME
// if the rotor is still spinning, don't initiate auto disarm
if (motors->rotor_speed_above_critical()) {
if (motors->get_spool_mode() != AP_Motors::GROUND_IDLE) {
auto_disarm_begin = tnow_ms;
return;
}
#endif
// always allow auto disarm if using interlock switch or motors are Emergency Stopped
if ((ap.using_interlock && !motors->get_interlock()) || SRV_Channels::get_emergency_stop()) {

20
ArduCopter/system.cpp
View File

@ -368,28 +368,26 @@ void Copter::update_auto_armed()
if(flightmode->has_manual_throttle() && ap.throttle_zero && !failsafe.radio) {
set_auto_armed(false);
}
#if FRAME_CONFIG == HELI_FRAME
// if helicopters are on the ground, and the motor is switched off, auto-armed should be false
// so that rotor runup is checked again before attempting to take-off
if(ap.land_complete && !motors->rotor_runup_complete()) {
if(ap.land_complete && motors->get_spool_mode() != AP_Motors::THROTTLE_UNLIMITED && ap.using_interlock) {
set_auto_armed(false);
}
#endif // HELI_FRAME
}else{
// arm checks
#if FRAME_CONFIG == HELI_FRAME
// for tradheli if motors are armed and throttle is above zero and the motor is started, auto_armed should be true
if(motors->armed() && !ap.throttle_zero && motors->rotor_runup_complete()) {
set_auto_armed(true);
}
#else
if(motors->armed() && ap.using_interlock) {
if(!ap.throttle_zero && motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) {
set_auto_armed(true);
}
// if motors are armed and throttle is above zero auto_armed should be true
// if motors are armed and we are in throw mode, then auto_armed should be true
if(motors->armed() && (!ap.throttle_zero || control_mode == THROW)) {
set_auto_armed(true);
} else if (motors->armed() && !ap.using_interlock) {
if(!ap.throttle_zero || control_mode == THROW) {
set_auto_armed(true);
}
}
#endif // HELI_FRAME
}
}

4
ArduCopter/takeoff.cpp
View File

@ -32,12 +32,10 @@ bool Copter::Mode::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
return false;
}
#if FRAME_CONFIG == HELI_FRAME
// Helicopters should return false if MAVlink takeoff command is received while the rotor is not spinning
if (!copter.motors->rotor_runup_complete()) {
if (motors->get_spool_mode() != AP_Motors::THROTTLE_UNLIMITED && ap.using_interlock) {
return false;
}
#endif
if (!do_user_takeoff_start(takeoff_alt_cm)) {
return false;