Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-08 08:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

ArduPlane: fixed roll and pitch for OSD VTOL view

Browse Source
This commit is contained in:
yaapu 2022-09-23 14:27:26 +02:00 committed by Andrew Tridgell
parent 98b65299f6
commit e958c313a1
4 changed files with 11 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
ArduPlane/ArduPlane.cpp
View File

@ -798,26 +798,21 @@ bool Plane::set_velocity_match(const Vector2f &velocity)
#endif // AP_SCRIPTING_ENABLED #endif // AP_SCRIPTING_ENABLED
#if OSD_ENABLED
// correct AHRS pitch for TRIM_PITCH_CD in non-VTOL modes, and return VTOL view in VTOL // correct AHRS pitch for TRIM_PITCH_CD in non-VTOL modes, and return VTOL view in VTOL
void Plane::get_osd_roll_pitch_rad(float &roll, float &pitch) const void Plane::get_osd_roll_pitch_rad(float &roll, float &pitch) const
{ {
pitch = ahrs.pitch;
roll = ahrs.roll;
#if HAL_QUADPLANE_ENABLED #if HAL_QUADPLANE_ENABLED
if (quadplane.show_vtol_view()) { if (quadplane.show_vtol_view()) {
return; pitch = quadplane.ahrs_view->pitch;
} roll = quadplane.ahrs_view->roll;
#endif return;
if (!(g2.flight_options & FlightOptions::OSD_REMOVE_TRIM_PITCH_CD)) { // correct for TRIM_PITCH_CD }
pitch -= g.pitch_trim_cd * 0.01 * DEG_TO_RAD;
return;
}
#if HAL_QUADPLANE_ENABLED
pitch = quadplane.ahrs_view->pitch;
roll = quadplane.ahrs_view->roll;
#endif #endif
pitch = ahrs.pitch;
roll = ahrs.roll;
if (!(g2.flight_options & FlightOptions::OSD_REMOVE_TRIM_PITCH_CD)) { // correct for TRIM_PITCH_CD
pitch -= g.pitch_trim_cd * 0.01 * DEG_TO_RAD;
}
} }
#endif
AP_HAL_MAIN_CALLBACKS(&plane); AP_HAL_MAIN_CALLBACKS(&plane);

2
ArduPlane/Plane.h
View File

@ -985,9 +985,7 @@ private:
// ArduPlane.cpp // ArduPlane.cpp
void disarm_if_autoland_complete(); void disarm_if_autoland_complete();
# if OSD_ENABLED
void get_osd_roll_pitch_rad(float &roll, float &pitch) const override; void get_osd_roll_pitch_rad(float &roll, float &pitch) const override;
#endif
float tecs_hgt_afe(void); float tecs_hgt_afe(void);
void efi_update(void); void efi_update(void);
void get_scheduler_tasks(const AP_Scheduler::Task *&tasks, void get_scheduler_tasks(const AP_Scheduler::Task *&tasks,

3
libraries/AP_Vehicle/AP_Vehicle.cpp
View File

@ -635,6 +635,7 @@ void AP_Vehicle::publish_osd_info()
nav_info.wp_number = mission->get_current_nav_index(); nav_info.wp_number = mission->get_current_nav_index();
osd->set_nav_info(nav_info); osd->set_nav_info(nav_info);
} }
#endif
void AP_Vehicle::get_osd_roll_pitch_rad(float &roll, float &pitch) const void AP_Vehicle::get_osd_roll_pitch_rad(float &roll, float &pitch) const
{ {
@ -642,8 +643,6 @@ void AP_Vehicle::get_osd_roll_pitch_rad(float &roll, float &pitch) const
pitch = ahrs.pitch; pitch = ahrs.pitch;
} }
#endif
#if HAL_INS_ACCELCAL_ENABLED #if HAL_INS_ACCELCAL_ENABLED
#ifndef HAL_CAL_ALWAYS_REBOOT #ifndef HAL_CAL_ALWAYS_REBOOT

2
libraries/AP_Vehicle/AP_Vehicle.h
View File

@ -292,10 +292,8 @@ public:
*/ */
virtual bool get_pan_tilt_norm(float &pan_norm, float &tilt_norm) const { return false; } virtual bool get_pan_tilt_norm(float &pan_norm, float &tilt_norm) const { return false; }
#if OSD_ENABLED
// Returns roll and pitch for OSD Horizon, Plane overrides to correct for VTOL view and fixed wing TRIM_PITCH_CD // Returns roll and pitch for OSD Horizon, Plane overrides to correct for VTOL view and fixed wing TRIM_PITCH_CD
virtual void get_osd_roll_pitch_rad(float &roll, float &pitch) const; virtual void get_osd_roll_pitch_rad(float &roll, float &pitch) const;
#endif
/* /*
get the target body-frame angular velocities in rad/s (Z-axis component used by some gimbals) get the target body-frame angular velocities in rad/s (Z-axis component used by some gimbals)