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

Copter: consolidate throw mode state into structure

Browse Source
This commit is contained in:
Randy Mackay 2016-08-02 17:19:10 +09:00
parent 381397c7bd
commit 595aea236a
2 changed files with 13 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
ArduCopter/Copter.h
View File

@ -274,14 +274,6 @@ private:
uint32_t start_ms;
} takeoff_state;
// throw mode state
struct {
ThrowModeStage stage;
ThrowModeStage prev_stage;
uint32_t last_log_ms;
bool nextmode_attempted;
} throw_state = {Throw_Disarmed, Throw_Disarmed, 0, false};
uint32_t precland_last_update_ms;
// altitude below which we do no navigation in auto takeoff
@ -410,9 +402,15 @@ private:
// Flip
Vector3f flip_orig_attitude; // original copter attitude before flip
// Throw
uint32_t throw_free_fall_start_ms = 0; // system time free fall was detected
float throw_free_fall_start_velz = 0.0f;// vertical velocity when free fall was detected
// throw mode state
struct {
ThrowModeStage stage;
ThrowModeStage prev_stage;
uint32_t last_log_ms;
bool nextmode_attempted;
uint32_t free_fall_start_ms; // system time free fall was detected
float free_fall_start_velz; // vertical velocity when free fall was detected
} throw_state = {Throw_Disarmed, Throw_Disarmed, 0, false, 0, 0.0f};
// Battery Sensors
AP_BattMonitor battery;

8
ArduCopter/control_throw.cpp
View File

@ -234,13 +234,13 @@ bool Copter::throw_detected()
bool possible_throw_detected = (free_falling || high_speed) && changing_height && no_throw_action;
// Record time and vertical velocity when we detect the possible throw
if (possible_throw_detected && ((AP_HAL::millis() - throw_free_fall_start_ms) > 500)) {
throw_free_fall_start_ms = AP_HAL::millis();
throw_free_fall_start_velz = inertial_nav.get_velocity().z;
if (possible_throw_detected && ((AP_HAL::millis() - throw_state.free_fall_start_ms) > 500)) {
throw_state.free_fall_start_ms = AP_HAL::millis();
throw_state.free_fall_start_velz = inertial_nav.get_velocity().z;
}
// Once a possible throw condition has been detected, we check for 2.5 m/s of downwards velocity change in less than 0.5 seconds to confirm
bool throw_condition_confirmed = ((AP_HAL::millis() - throw_free_fall_start_ms < 500) && ((inertial_nav.get_velocity().z - throw_free_fall_start_velz) < -250.0f));
bool throw_condition_confirmed = ((AP_HAL::millis() - throw_state.free_fall_start_ms < 500) && ((inertial_nav.get_velocity().z - throw_state.free_fall_start_velz) < -250.0f));
// start motors and enter the control mode if we are in continuous freefall
if (throw_condition_confirmed) {