Navigation Menu

Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Correct OSD horizon for VTOL modes and TRIM_PITCH_CD in Fixed Wing #18131

Merged
merged 3 commits into from Aug 3, 2021
Merged

Correct OSD horizon for VTOL modes and TRIM_PITCH_CD in Fixed Wing #18131

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
e4dd8b3
AP_OSD: correct OSD horizon for VTOL modes and TRIM_PITCH_CD in FW modes
Hwurzburg Jul 28, 2021
fc89313
AP_Vehicle: correct OSD horizon for VTOL modes and TRIM_PITCH_CD in F…
Hwurzburg Jul 28, 2021
f00e21f
ArduPlane: Correct OSD horizon for VTOL modes and TRIM_PITCH_CD in Fi…
Hwurzburg Aug 1, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
15 changes: 15 additions & 0 deletions ArduPlane/ArduPlane.cpp
View file
Open in desktop
Expand Up @@ -691,4 +691,19 @@ bool Plane::get_target_location(Location& target_loc)
}
#endif // ENABLE_SCRIPTING

#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;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

these two could go inside the if.

   if (!quadplane.show_vtol_view()) {  // correct for TRIM_PITCH_CD
      pitch = ahrs.pitch - g.pitch_trim_cd * 0.01 * DEG_TO_RAD;
      roll = ahrs.roll;
    } else {

But its no big deal.

if (!quadplane.show_vtol_view()) { // correct for TRIM_PITCH_CD
pitch -= g.pitch_trim_cd * 0.01 * DEG_TO_RAD;
} else {
pitch = quadplane.ahrs_view->pitch;
roll = quadplane.ahrs_view->roll;
}
}
#endif

AP_HAL_MAIN_CALLBACKS(&plane);
3 changes: 3 additions & 0 deletions ArduPlane/Plane.h
View file
Open in desktop
Expand Up @@ -951,6 +951,9 @@ class Plane : public AP_Vehicle {

// 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,
Expand Down
7 changes: 5 additions & 2 deletions libraries/AP_OSD/AP_OSD_Screen.cpp
View file
Open in desktop
Expand Up @@ -40,6 +40,7 @@
#include <AP_VideoTX/AP_VideoTX.h>
#include <AP_Terrain/AP_Terrain.h>
#include <AP_RangeFinder/AP_RangeFinder.h>
#include <AP_Vehicle/AP_Vehicle.h>
#if APM_BUILD_TYPE(APM_BUILD_Rover)
#include <AP_WindVane/AP_WindVane.h>
#endif
Expand Down Expand Up @@ -1455,8 +1456,10 @@ void AP_OSD_Screen::draw_horizon(uint8_t x, uint8_t y)
{
AP_AHRS &ahrs = AP::ahrs();
WITH_SEMAPHORE(ahrs.get_semaphore());
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Were still taking the ahrs semaphore, we do use the ahrs in the new function. I wonder if we should take the vehicle scheduler-semaphore instead.

I'm not really sure what we should be doing....

float roll = ahrs.roll;
float pitch = -ahrs.pitch;
float roll;
float pitch;
AP::vehicle()->get_osd_roll_pitch_rad(roll,pitch);
pitch *= -1;

//inverted roll AH
if (check_option(AP_OSD::OPTION_INVERTED_AH_ROLL)) {
Expand Down
7 changes: 7 additions & 0 deletions libraries/AP_Vehicle/AP_Vehicle.cpp
View file
Open in desktop
Expand Up @@ -403,6 +403,13 @@ void AP_Vehicle::publish_osd_info()
nav_info.wp_number = mission->get_current_nav_index();
osd->set_nav_info(nav_info);
}

void AP_Vehicle::get_osd_roll_pitch_rad(float &roll, float &pitch) const
{
roll = ahrs.roll;
pitch = ahrs.pitch;
}

#endif

AP_Vehicle *AP_Vehicle::_singleton = nullptr;
Expand Down
5 changes: 5 additions & 0 deletions libraries/AP_Vehicle/AP_Vehicle.h
View file
Open in desktop
Expand Up @@ -252,6 +252,11 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks {
*/
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

protected:

virtual void init_ardupilot() = 0;
Expand Down