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

Plane: log TECS target alt 

log the target alt we pass into TECS to help debug a CRUISE height
issue
This commit is contained in:
Andrew Tridgell 2022-10-17 09:33:38 +11:00
parent a313a1db3d
commit 5aef5473de
3 changed files with 16 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduPlane/ArduPlane.cpp
View File

@ -547,16 +547,16 @@ void Plane::update_alt()
distance_beyond_land_wp = current_loc.get_distance(next_WP_loc);
}
float target_alt = relative_target_altitude_cm();
tecs_target_alt_cm = relative_target_altitude_cm();
if (control_mode == &mode_rtl && !rtl.done_climb && (g2.rtl_climb_min > 0 || (plane.g2.flight_options & FlightOptions::CLIMB_BEFORE_TURN))) {
// ensure we do the initial climb in RTL. We add an extra
// 10m in the demanded height to push TECS to climb
// quickly
target_alt = MAX(target_alt, prev_WP_loc.alt - home.alt) + (g2.rtl_climb_min+10)*100;
tecs_target_alt_cm = MAX(tecs_target_alt_cm, prev_WP_loc.alt - home.alt) + (g2.rtl_climb_min+10)*100;
}
TECS_controller.update_pitch_throttle(target_alt,
TECS_controller.update_pitch_throttle(tecs_target_alt_cm,
target_airspeed_cm,
flight_stage,
distance_beyond_land_wp,

17
ArduPlane/Log.cpp
View File

@ -147,7 +147,8 @@ struct PACKED log_Nav_Tuning {
float airspeed_error;
int32_t target_lat;
int32_t target_lng;
int32_t target_alt;
int32_t target_alt_wp;
int32_t target_alt_tecs;
int32_t target_airspeed;
};
@ -166,7 +167,8 @@ void Plane::Log_Write_Nav_Tuning()
airspeed_error : airspeed_error,
target_lat : next_WP_loc.lat,
target_lng : next_WP_loc.lng,
target_alt : next_WP_loc.alt,
target_alt_wp : next_WP_loc.alt,
target_alt_tecs : tecs_target_alt_cm,
target_airspeed : target_airspeed_cm,
};
logger.WriteBlock(&pkt, sizeof(pkt));
@ -306,16 +308,17 @@ const struct LogStructure Plane::log_structure[] = {
// @Field: Dist: distance to the current navigation waypoint
// @Field: TBrg: bearing to the current navigation waypoint
// @Field: NavBrg: the vehicle's desired heading
// @Field: AltErr: difference between current vehicle height and target height
// @Field: AltE: difference between current vehicle height and target height
// @Field: XT: the vehicle's current distance from the current travel segment
// @Field: XTi: integration of the vehicle's crosstrack error
// @Field: AspdE: difference between vehicle's airspeed and desired airspeed
// @Field: AsE: difference between vehicle's airspeed and desired airspeed
// @Field: TLat: target latitude
// @Field: TLng: target longitude
// @Field: TAlt: target altitude
// @Field: TAspd: target airspeed
// @Field: TAW: target altitude WP
// @Field: TAT: target altitude TECS
// @Field: TAsp: target airspeed
{ LOG_NTUN_MSG, sizeof(log_Nav_Tuning),
"NTUN", "QfcccfffLLii", "TimeUS,Dist,TBrg,NavBrg,AltErr,XT,XTi,AspdE,TLat,TLng,TAlt,TAspd", "smddmmmnDUmn", "F0BBB0B0GGBB" , true },
"NTUN", "QfcccfffLLeee", "TimeUS,Dist,TBrg,NavBrg,AltE,XT,XTi,AsE,TLat,TLng,TAW,TAT,TAsp", "smddmmmnDUmmn", "F0BBB0B0GG000" , true },
// @LoggerMessage: ATRP
// @Description: Plane AutoTune

3
ArduPlane/Plane.h
View File

@ -1204,6 +1204,9 @@ private:
// mode reason for entering previous mode
ModeReason previous_mode_reason = ModeReason::UNKNOWN;
// last target alt we passed to tecs
int32_t tecs_target_alt_cm;
public:
void failsafe_check(void);
#if AP_SCRIPTING_ENABLED