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

ArduPlane: fixed roll and pitch for OSD VTOL view

This commit is contained in:
yaapu 2022-09-23 14:27:26 +02:00 committed by Andrew Tridgell
parent 06caabfa04
commit 2c9e2abcf4
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

@ -796,26 +796,21 @@ bool Plane::set_velocity_match(const Vector2f &velocity)
#endif // AP_SCRIPTING_ENABLED
#if OSD_ENABLED
// 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
{
pitch = ahrs.pitch;
roll = ahrs.roll;
#if HAL_QUADPLANE_ENABLED
if (quadplane.show_vtol_view()) {
return;
}
#endif
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;
if (quadplane.show_vtol_view()) {
pitch = quadplane.ahrs_view->pitch;
roll = quadplane.ahrs_view->roll;
return;
}
#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);

2
ArduPlane/Plane.h
View File

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

3
libraries/AP_Vehicle/AP_Vehicle.cpp
View File

@ -631,6 +631,7 @@ void AP_Vehicle::publish_osd_info()
nav_info.wp_number = mission->get_current_nav_index();
osd->set_nav_info(nav_info);
}
#endif
void AP_Vehicle::get_osd_roll_pitch_rad(float &roll, float &pitch) const
{
@ -638,8 +639,6 @@ void AP_Vehicle::get_osd_roll_pitch_rad(float &roll, float &pitch) const
pitch = ahrs.pitch;
}
#endif
#if HAL_INS_ACCELCAL_ENABLED
#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; }
#if OSD_ENABLED
// 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;
#endif
/*
get the target body-frame angular velocities in rad/s (Z-axis component used by some gimbals)