Skip to content

Commit

Permalink
Plane: log attitude target in degrees
Browse files Browse the repository at this point in the history
  • Loading branch information
andyp1per committed Sep 9, 2024
1 parent 53e10a8 commit 8432912
Showing 1 changed file with 6 additions and 6 deletions.
12 changes: 6 additions & 6 deletions ArduPlane/Log.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -5,10 +5,11 @@
// Write an attitude packet
void Plane::Log_Write_Attitude(void)
{
Vector3f targets; // Package up the targets into a vector for commonality with Copter usage of Log_Wrote_Attitude
targets.x = nav_roll_cd;
targets.y = nav_pitch_cd;
targets.z = 0; //Plane does not have the concept of navyaw. This is a placeholder.
Vector3f targets { // Package up the targets into a vector for commonality with Copter usage of Log_Wrote_Attitude
nav_roll_cd * 0.01f,
nav_pitch_cd * 0.01f,
0 //Plane does not have the concept of navyaw. This is a placeholder.
};

#if HAL_QUADPLANE_ENABLED
if (quadplane.show_vtol_view()) {
Expand All @@ -18,8 +19,7 @@ void Plane::Log_Write_Attitude(void)
// since Euler angles are not used and it is a waste of cpu to compute them at the loop rate.
// Get them from the quaternion instead:
quadplane.attitude_control->get_attitude_target_quat().to_euler(targets.x, targets.y, targets.z);
targets *= degrees(100.0f);
quadplane.ahrs_view->Write_AttitudeView(targets);
quadplane.ahrs_view->Write_AttitudeView(targets * RAD_TO_DEG);
} else
#endif
{
Expand Down

0 comments on commit 8432912

Please sign in to comment.