Skip to content

Commit

Permalink
Expose Attitude Target for a Quadplane - Review Fixes no. 1
Browse files Browse the repository at this point in the history
Fixes:
- The message packet gets transmitted iff. the platform is in the quadplane mode. Not sure if this solution is sufficient, but I could not have found appropriate flag to solve that issue. I'd appreciate some further suggestions.

- Cleared unrelated changes (gazebo-quadplane files for SITL)
- Left one logger fix in libraries/AP_Logger/LogStructure.h  -> as waf pointed out that out-of-order variable initialization is not supported at this time (this type of error got thrown at one point during the test compilation)
-

Co-Authored-By: Peter Hall <[email protected]>
  • Loading branch information
miodine and IamPete1 committed Oct 13, 2023
1 parent 4627912 commit 7f13f54
Show file tree
Hide file tree
Showing 3 changed files with 4 additions and 45 deletions.
14 changes: 4 additions & 10 deletions ArduPlane/GCS_Mavlink.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -165,24 +165,16 @@ void GCS_MAVLINK_Plane::send_attitude_target() {
float throttle = 0.0f;
Quaternion quat(0.0f,0.0f,0.0f,0.0f);

#if HAL_QUADPLANE_ENABLED
throttle = plane.quadplane.attitude_control->get_throttle_in();

if (!(plane.g2.flight_options & FlightOptions::GCS_REMOVE_TRIM_PITCH_CD)) {
target_p -= radians(plane.g.pitch_trim_cd * 0.01f);
}

if (plane.quadplane.show_vtol_view()) {
const Vector3f rate_targets = plane.quadplane.attitude_control->rate_bf_targets();

target_r = rate_targets.x;
target_p = rate_targets.y;
target_y = rate_targets.z;
throttle = plane.quadplane.attitude_control->get_throttle_in();
quat = plane.quadplane.attitude_control->get_attitude_target_quat();
}

quat = plane.quadplane.attitude_control->get_attitude_target_quat();
#endif

const float quat_out[4] = {quat.q1, quat.q2, quat.q3, quat.q4};
const uint16_t typemask = 0;

Expand Down Expand Up @@ -655,7 +647,9 @@ static const ap_message STREAM_RC_CHANNELS_msgs[] = {
};
static const ap_message STREAM_EXTRA1_msgs[] = {
MSG_ATTITUDE,
#if HAL_QUADPLANE_ENABLED
MSG_ATTITUDE_TARGET,
#endif
MSG_SIMSTATE,
MSG_AHRS2,
#if AP_RPM_ENABLED
Expand Down
30 changes: 0 additions & 30 deletions Tools/autotest/default_params/gazebo_quadplane.parm

This file was deleted.

5 changes: 0 additions & 5 deletions Tools/autotest/pysim/vehicleinfo.py
Original file line number Diff line number Diff line change
Expand Up @@ -299,11 +299,6 @@ def __init__(self):
"default_params_filename": "default_params/gazebo-zephyr.parm",
"external": True,
},
"gazebo-quadplane": {
"waf_target": "bin/arduplane",
"default_params_filename": "default_params/gazebo_quadplane.parm",
},

"last_letter": {
"waf_target": "bin/arduplane",
"default_params_filename": "models/plane.parm",
Expand Down

0 comments on commit 7f13f54

Please sign in to comment.