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.

Sign up for GitHub

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

Check system type for SET_ATTITUDE_TARGET attitude publising #13141

Merged
merged 10 commits into from
Oct 26, 2019
Merged

Check system type for SET_ATTITUDE_TARGET attitude publising #13141

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
3e81960
Check system type for SET_ATTITUDE_TARGET attitude publising
irsdkv Oct 9, 2019
dc7728b
VTOL state check for attitude setpoint publish
irsdkv Oct 9, 2019
535bcf1
Removed _is_vtol in MavlinkReceiver class
irsdkv Oct 9, 2019
0f6264e
mavlink_receiver: vtol_vehicle_status changed to vehicle_status subsc…
irsdkv Oct 9, 2019
ab5ee98
Apply suggestions from code review
irsdkv Oct 10, 2019
bc80b79
set_attitude_target: set rate thrust according to VTOL mode
irsdkv Oct 14, 2019
4dda73a
set_attitude_target: get vehicle_status every time.
irsdkv Oct 14, 2019
84e6a4c
Merge branch 'master' into pr-check_vtol_in_set_attitude_target
TSC21 Oct 26, 2019
66d5786
mavlink_receiver: Fit in 'flash' region
irsdkv Oct 26, 2019
1d6af06
Merge branch 'master' into pr-check_vtol_in_set_attitude_target
TSC21 Oct 26, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
109 changes: 65 additions & 44 deletions src/modules/mavlink/mavlink_receiver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1289,6 +1289,57 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
}
}

void MavlinkReceiver::fill_thrust(float *thrust_body_array, uint8_t vehicle_type, float thrust)
{
// Fill correct field by checking frametype
// TODO: add as needed
switch (_mavlink->get_system_type()) {
case MAV_TYPE_GENERIC:
break;

case MAV_TYPE_FIXED_WING:
thrust_body_array[0] = thrust;
break;

case MAV_TYPE_QUADROTOR:
case MAV_TYPE_HEXAROTOR:
case MAV_TYPE_OCTOROTOR:
case MAV_TYPE_TRICOPTER:
case MAV_TYPE_HELICOPTER:
thrust_body_array[2] = -thrust;
break;

case MAV_TYPE_GROUND_ROVER:
thrust_body_array[0] = thrust;
break;

case MAV_TYPE_VTOL_DUOROTOR:
case MAV_TYPE_VTOL_QUADROTOR:
case MAV_TYPE_VTOL_TILTROTOR:
case MAV_TYPE_VTOL_RESERVED2:
case MAV_TYPE_VTOL_RESERVED3:
case MAV_TYPE_VTOL_RESERVED4:
case MAV_TYPE_VTOL_RESERVED5:
switch (vehicle_type) {
case vehicle_status_s::VEHICLE_TYPE_FIXED_WING:
thrust_body_array[0] = thrust;

break;

case vehicle_status_s::VEHICLE_TYPE_ROTARY_WING:
thrust_body_array[2] = -thrust;

break;

default:
// This should never happen
break;
}

break;
}
}

void
MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
{
Expand Down Expand Up @@ -1368,6 +1419,8 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
_control_mode_sub.copy(&control_mode);

if (control_mode.flag_control_offboard_enabled) {
vehicle_status_s vehicle_status{};
_vehicle_status_sub.copy(&vehicle_status);

/* Publish attitude setpoint if attitude and thrust ignore bits are not set */
if (!(offboard_control_mode.ignore_attitude)) {
Expand All @@ -1387,31 +1440,19 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
}

if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
// Fill correct field by checking frametype
// TODO: add as needed
switch (_mavlink->get_system_type()) {
case MAV_TYPE_GENERIC:
break;

case MAV_TYPE_FIXED_WING:
att_sp.thrust_body[0] = set_attitude_target.thrust;
break;

case MAV_TYPE_QUADROTOR:
case MAV_TYPE_HEXAROTOR:
case MAV_TYPE_OCTOROTOR:
case MAV_TYPE_TRICOPTER:
case MAV_TYPE_HELICOPTER:
att_sp.thrust_body[2] = -set_attitude_target.thrust;
break;

case MAV_TYPE_GROUND_ROVER:
att_sp.thrust_body[0] = set_attitude_target.thrust;
break;
}
fill_thrust(att_sp.thrust_body, vehicle_status.vehicle_type, set_attitude_target.thrust);
}

_att_sp_pub.publish(att_sp);
// Publish attitude setpoint
if (vehicle_status.is_vtol && (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)) {
_mc_virtual_att_sp_pub.publish(att_sp);

} else if (vehicle_status.is_vtol && (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) {
_fw_virtual_att_sp_pub.publish(att_sp);

} else {
_att_sp_pub.publish(att_sp);
}
}

/* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */
Expand All @@ -1437,27 +1478,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
}

if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
switch (_mavlink->get_system_type()) {
case MAV_TYPE_GENERIC:
break;

case MAV_TYPE_FIXED_WING:
rates_sp.thrust_body[0] = set_attitude_target.thrust;
break;

case MAV_TYPE_QUADROTOR:
case MAV_TYPE_HEXAROTOR:
case MAV_TYPE_OCTOROTOR:
case MAV_TYPE_TRICOPTER:
case MAV_TYPE_HELICOPTER:
rates_sp.thrust_body[2] = -set_attitude_target.thrust;
break;

case MAV_TYPE_GROUND_ROVER:
rates_sp.thrust_body[0] = set_attitude_target.thrust;
break;
}

fill_thrust(rates_sp.thrust_body, vehicle_status.vehicle_type, set_attitude_target.thrust);
}

_rates_sp_pub.publish(rates_sp);
Expand Down
5 changes: 5 additions & 0 deletions src/modules/mavlink/mavlink_receiver.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -197,6 +197,8 @@ class MavlinkReceiver : public ModuleParams
void send_flight_information();
void send_storage_information(int storage_id);

void fill_thrust(float *thrust_body_array, uint8_t vehicle_type, float thrust);

/**
* @brief Updates the battery, optical flow, and flight ID subscribed parameters.
*/
Expand Down Expand Up @@ -229,6 +231,8 @@ class MavlinkReceiver : public ModuleParams
uORB::Publication<position_setpoint_triplet_s> _pos_sp_triplet_pub{ORB_ID(position_setpoint_triplet)};
uORB::Publication<vehicle_attitude_s> _attitude_pub{ORB_ID(vehicle_attitude)};
uORB::Publication<vehicle_attitude_setpoint_s> _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Publication<vehicle_attitude_setpoint_s> _mc_virtual_att_sp_pub{ORB_ID(mc_virtual_attitude_setpoint)};
uORB::Publication<vehicle_attitude_setpoint_s> _fw_virtual_att_sp_pub{ORB_ID(fw_virtual_attitude_setpoint)};
uORB::Publication<vehicle_global_position_s> _global_pos_pub{ORB_ID(vehicle_global_position)};
uORB::Publication<vehicle_gps_position_s> _gps_pub{ORB_ID(vehicle_gps_position)};
uORB::Publication<vehicle_land_detected_s> _land_detector_pub{ORB_ID(vehicle_land_detected)};
Expand Down Expand Up @@ -262,6 +266,7 @@ class MavlinkReceiver : public ModuleParams
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};

static constexpr unsigned int MOM_SWITCH_COUNT{8};
uint8_t _mom_switch_pos[MOM_SWITCH_COUNT] {};
Expand Down