diff --git a/libraries/SITL/SIM_Gimbal.cpp b/libraries/SITL/SIM_Gimbal.cpp index 5aede11b3b40a3..649159da9fea03 100644 --- a/libraries/SITL/SIM_Gimbal.cpp +++ b/libraries/SITL/SIM_Gimbal.cpp @@ -237,7 +237,7 @@ void Gimbal::param_send(const struct gimbal_param *p) param_value.param_type = MAV_PARAM_TYPE_REAL32; uint16_t len = mavlink_msg_param_value_encode_status(vehicle_system_id, - vehicle_component_id, + gimbal_component_id, &mavlink.status, &msg, ¶m_value); @@ -364,7 +364,7 @@ void Gimbal::send_report(void) heartbeat.custom_mode = 0; len = mavlink_msg_heartbeat_encode_status(vehicle_system_id, - vehicle_component_id, + gimbal_component_id, &mavlink.status, &msg, &heartbeat); @@ -394,7 +394,7 @@ void Gimbal::send_report(void) gimbal_report.joint_az = joint_angles.z; len = mavlink_msg_gimbal_report_encode_status(vehicle_system_id, - vehicle_component_id, + gimbal_component_id, &mavlink.status, &msg, &gimbal_report); diff --git a/libraries/SITL/SIM_Gimbal.h b/libraries/SITL/SIM_Gimbal.h index 2cedd3a1cf9c3f..f77547af0105a9 100644 --- a/libraries/SITL/SIM_Gimbal.h +++ b/libraries/SITL/SIM_Gimbal.h @@ -113,6 +113,9 @@ class Gimbal { uint32_t param_send_last_ms; uint8_t param_send_idx; + // component ID we send from: + const uint8_t gimbal_component_id = 154; // MAV_COMP_ID_GIMBAL + void send_report(void); void param_send(const struct gimbal_param *p); struct gimbal_param *param_find(const char *name);