dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mavlink_receiver: Fit in 'flash' region

This commit is contained in:
Ildar Sadykov 2019-10-26 15:53:27 +03:00 committed by Nuno Marques
parent b29a6b8761
commit 0326a8ec76
2 changed files with 55 additions and 97 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

150
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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)
{
@ -1389,57 +1440,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
// 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;
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_status.vehicle_type) {
case vehicle_status_s::VEHICLE_TYPE_FIXED_WING:
att_sp.thrust_body[0] = set_attitude_target.thrust;
break;
case vehicle_status_s::VEHICLE_TYPE_ROTARY_WING:
att_sp.thrust_body[2] = -set_attitude_target.thrust;
break;
default:
// This should never happen
break;
}
break;
}
fill_thrust(att_sp.thrust_body, vehicle_status.vehicle_type, set_attitude_target.thrust);
}
// Publish attitude setpoint
@ -1477,52 +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;
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_status.vehicle_type) {
case vehicle_status_s::VEHICLE_TYPE_FIXED_WING:
rates_sp.thrust_body[0] = set_attitude_target.thrust;
break;
case vehicle_status_s::VEHICLE_TYPE_ROTARY_WING:
rates_sp.thrust_body[2] = -set_attitude_target.thrust;
break;
default:
// This should never happen
break;
}
break;
}
fill_thrust(rates_sp.thrust_body, vehicle_status.vehicle_type, set_attitude_target.thrust);
}
_rates_sp_pub.publish(rates_sp);

2
src/modules/mavlink/mavlink_receiver.h
View File

@ -197,6 +197,8 @@ private:
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.
*/