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

Plane: log TECS target alt

Browse Source 
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 da0ee9a8cb
commit 1c043bde9a
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

@ -549,16 +549,16 @@ void Plane::update_alt()
distance_beyond_land_wp = current_loc.get_distance(next_WP_loc); 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))) { 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 // ensure we do the initial climb in RTL. We add an extra
// 10m in the demanded height to push TECS to climb // 10m in the demanded height to push TECS to climb
// quickly // 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, target_airspeed_cm,
flight_stage, flight_stage,
distance_beyond_land_wp, distance_beyond_land_wp,

17
ArduPlane/Log.cpp
View File

@ -153,7 +153,8 @@ struct PACKED log_Nav_Tuning {
float airspeed_error; float airspeed_error;
int32_t target_lat; int32_t target_lat;
int32_t target_lng; int32_t target_lng;
int32_t target_alt; int32_t target_alt_wp;
int32_t target_alt_tecs;
int32_t target_airspeed; int32_t target_airspeed;
}; };
@ -172,7 +173,8 @@ void Plane::Log_Write_Nav_Tuning()
airspeed_error : airspeed_error, airspeed_error : airspeed_error,
target_lat : next_WP_loc.lat, target_lat : next_WP_loc.lat,
target_lng : next_WP_loc.lng, 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, target_airspeed : target_airspeed_cm,
}; };
logger.WriteBlock(&pkt, sizeof(pkt)); logger.WriteBlock(&pkt, sizeof(pkt));
@ -312,16 +314,17 @@ const struct LogStructure Plane::log_structure[] = {
// @Field: Dist: distance to the current navigation waypoint // @Field: Dist: distance to the current navigation waypoint
// @Field: TBrg: bearing to the current navigation waypoint // @Field: TBrg: bearing to the current navigation waypoint
// @Field: NavBrg: the vehicle's desired heading // @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: XT: the vehicle's current distance from the current travel segment
// @Field: XTi: integration of the vehicle's crosstrack error // @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: TLat: target latitude
// @Field: TLng: target longitude // @Field: TLng: target longitude
// @Field: TAlt: target altitude // @Field: TAW: target altitude WP
// @Field: TAspd: target airspeed // @Field: TAT: target altitude TECS
// @Field: TAsp: target airspeed
{ LOG_NTUN_MSG, sizeof(log_Nav_Tuning), { 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 // @LoggerMessage: ATRP
// @Description: Plane AutoTune // @Description: Plane AutoTune

3
ArduPlane/Plane.h
View File

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