forked from Archive/PX4-Autopilot
mavlink_receiver: Fit in 'flash' region
This commit is contained in:
parent
b29a6b8761
commit
0326a8ec76
|
@ -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
|
void
|
||||||
MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
|
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
|
if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
|
||||||
// Fill correct field by checking frametype
|
fill_thrust(att_sp.thrust_body, vehicle_status.vehicle_type, set_attitude_target.thrust);
|
||||||
// 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;
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Publish attitude setpoint
|
// 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
|
if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
|
||||||
switch (_mavlink->get_system_type()) {
|
fill_thrust(rates_sp.thrust_body, vehicle_status.vehicle_type, set_attitude_target.thrust);
|
||||||
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;
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
_rates_sp_pub.publish(rates_sp);
|
_rates_sp_pub.publish(rates_sp);
|
||||||
|
|
|
@ -197,6 +197,8 @@ private:
|
||||||
void send_flight_information();
|
void send_flight_information();
|
||||||
void send_storage_information(int storage_id);
|
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.
|
* @brief Updates the battery, optical flow, and flight ID subscribed parameters.
|
||||||
*/
|
*/
|
||||||
|
|
Loading…
Reference in New Issue