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

AP_Landing: Deepstall implement log(), reset all integrators

Browse Source
This commit is contained in:
Michael du Breuil 2017-10-12 15:28:25 -07:00 committed by Tom Pittenger
parent 2909f77235
commit d166832de4
3 changed files with 31 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Landing/AP_Landing.cpp
View File

@ -580,6 +580,8 @@ void AP_Landing::log(void) const
type_slope_log(); type_slope_log();
break; break;
case TYPE_DEEPSTALL: case TYPE_DEEPSTALL:
deepstall.log();
break;
default: default:
break; break;
} }

30
libraries/AP_Landing/AP_Landing_Deepstall.cpp
View File

@ -153,7 +153,8 @@ const AP_Param::GroupInfo AP_Landing_Deepstall::var_info[] = {
void AP_Landing_Deepstall::do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude) void AP_Landing_Deepstall::do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude)
{ {
stage = DEEPSTALL_STAGE_FLY_TO_LANDING; stage = DEEPSTALL_STAGE_FLY_TO_LANDING;
ds_PID.reset_I(); ds_PID.reset();
L1_xtrack_i = 0.0f;
hold_level = false; // come out of yaw lock hold_level = false; // come out of yaw lock
// load the landing point in, the rest of path building is deferred for a better wind estimate // load the landing point in, the rest of path building is deferred for a better wind estimate
@ -408,12 +409,37 @@ const DataFlash_Class::PID_Info& AP_Landing_Deepstall::get_pid_info(void) const
return ds_PID.get_pid_info(); return ds_PID.get_pid_info();
} }
void AP_Landing_Deepstall::log(void) const {
const DataFlash_Class::PID_Info& pid_info = ds_PID.get_pid_info();
struct log_DSTL pkt = {
LOG_PACKET_HEADER_INIT(LOG_DSTL_MSG),
time_us : AP_HAL::micros64(),
stage : (uint8_t)stage,
target_heading : target_heading_deg,
target_lat : landing_point.lat,
target_lng : landing_point.lng,
target_alt : landing_point.alt,
crosstrack_error : (int16_t)(stage >= DEEPSTALL_STAGE_LAND ?
constrain_float(crosstrack_error * 1e2f, (float)INT16_MIN, (float)INT16_MAX) : 0),
travel_distance : (int16_t)(stage >= DEEPSTALL_STAGE_LAND ?
constrain_float(predicted_travel_distance * 1e2f, (float)INT16_MIN, (float)INT16_MAX) : 0),
l1_i : stage >= DEEPSTALL_STAGE_LAND ? L1_xtrack_i : 0.0f,
loiter_sum_cd : stage >= DEEPSTALL_STAGE_ESTIMATE_WIND ? loiter_sum_cd : 0,
desired : pid_info.desired,
P : pid_info.P,
I : pid_info.I,
D : pid_info.D,
};
DataFlash_Class::instance()->WriteBlock(&pkt, sizeof(pkt));
}
// termination handling, expected to set the servo outputs // termination handling, expected to set the servo outputs
bool AP_Landing_Deepstall::terminate(void) { bool AP_Landing_Deepstall::terminate(void) {
// if we were not in a deepstall, mark us as being in one // if we were not in a deepstall, mark us as being in one
if(!landing.flags.in_progress || stage != DEEPSTALL_STAGE_LAND) { if(!landing.flags.in_progress || stage != DEEPSTALL_STAGE_LAND) {
stall_entry_time = AP_HAL::millis(); stall_entry_time = AP_HAL::millis();
ds_PID.reset_I(); ds_PID.reset();
L1_xtrack_i = 0.0f;
landing.flags.in_progress = true; landing.flags.in_progress = true;
stage = DEEPSTALL_STAGE_LAND; stage = DEEPSTALL_STAGE_LAND;

1
libraries/AP_Landing/AP_Landing_Deepstall.h
View File

@ -103,6 +103,7 @@ private:
bool is_throttle_suppressed(void) const; bool is_throttle_suppressed(void) const;
bool is_flying_forward(void) const; bool is_flying_forward(void) const;
bool terminate(void); bool terminate(void);
void log(void) const;
bool send_deepstall_message(mavlink_channel_t chan) const; bool send_deepstall_message(mavlink_channel_t chan) const;