fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 00:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

MAVLink: Code style

Browse Source
This commit is contained in:
Lorenz Meier
2016-08-26 23:43:33 +02:00
parent e67a6bdfc2
commit 720c445f21
17 changed files with 922 additions and 627 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_parameters.cpp
+27 -2
View File
@@ -62,6 +62,7 @@ MavlinkParametersManager::~MavlinkParametersManager()
if (_uavcan_parameter_value_sub >= 0) {
orb_unsubscribe(_uavcan_parameter_value_sub);
}
if (_uavcan_parameter_request_pub) {
orb_unadvertise(_uavcan_parameter_request_pub);
}
@@ -92,6 +93,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
(req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) {
if (_send_all_index < 0) {
_send_all_index = PARAM_HASH;
} else {
/* a restart should skip the hash check on the ground */
_send_all_index = 0;
@@ -108,10 +110,12 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
if (_uavcan_parameter_request_pub == nullptr) {
_uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req);
} else {
orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
}
}
break;
}
@@ -160,9 +164,11 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
req.param_index = -1;
strncpy(req.param_id, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1);
req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
if (set.param_type == MAV_PARAM_TYPE_REAL32) {
req.param_type = MAV_PARAM_TYPE_REAL32;
req.real_value = set.param_value;
} else {
int32_t val;
memcpy(&val, &set.param_value, sizeof(int32_t));
@@ -172,10 +178,12 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
if (_uavcan_parameter_request_pub == nullptr) {
_uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req);
} else {
orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
}
}
break;
}
@@ -202,6 +210,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
param_value.param_type = MAV_PARAM_TYPE_UINT32;
memcpy(&param_value.param_value, &hash, sizeof(hash));
mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &param_value);
} else {
/* local name buffer to enforce null-terminated string */
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
@@ -211,6 +220,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
/* attempt to find parameter and send it */
send_param(param_find_no_notification(name));
}
} else {
/* when index is >= 0, send this parameter again */
int ret = send_param(param_for_used_index(req_read.param_index));
@@ -219,6 +229,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
sprintf(buf, "[pm] unknown param ID: %u", req_read.param_index);
_mavlink->send_statustext_info(buf);
} else if (ret == 2) {
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
sprintf(buf, "[pm] failed loading param from storage ID: %u", req_read.param_index);
@@ -228,7 +239,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
}
if (req_read.target_system == mavlink_system.sysid && req_read.target_component < 127 &&
(req_read.target_component != mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) {
(req_read.target_component != mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) {
// publish set request to UAVCAN driver via uORB.
uavcan_parameter_request_s req;
req.message_type = msg->msgid;
@@ -239,10 +250,12 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
if (_uavcan_parameter_request_pub == nullptr) {
_uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req);
} else {
orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
}
}
break;
}
@@ -258,18 +271,22 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
/* Copy values from msg to uorb using the parameter_rc_channel_index as index */
size_t i = map_rc.parameter_rc_channel_index;
_rc_param_map.param_index[i] = map_rc.param_index;
strncpy(&(_rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)]), map_rc.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
strncpy(&(_rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)]), map_rc.param_id,
MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* enforce null termination */
_rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1) + rc_parameter_map_s::PARAM_ID_LEN] = '\0';
_rc_param_map.scale[i] = map_rc.scale;
_rc_param_map.value0[i] = map_rc.param_value0;
_rc_param_map.value_min[i] = map_rc.param_value_min;
_rc_param_map.value_max[i] = map_rc.param_value_max;
if (map_rc.param_index == -2) { // -2 means unset map
_rc_param_map.valid[i] = false;
} else {
_rc_param_map.valid[i] = true;
}
_rc_param_map.timestamp = hrt_absolute_time();
if (_rc_param_map_pub == nullptr) {
@@ -280,6 +297,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
}
}
break;
}
@@ -300,6 +318,7 @@ MavlinkParametersManager::send(const hrt_abstime t)
bool param_value_ready;
orb_check(_uavcan_parameter_value_sub, &param_value_ready);
if (space_available && param_value_ready) {
struct uavcan_parameter_value_s value;
orb_copy(ORB_ID(uavcan_parameter_value), _uavcan_parameter_value_sub, &value);
@@ -308,16 +327,20 @@ MavlinkParametersManager::send(const hrt_abstime t)
msg.param_count = value.param_count;
msg.param_index = value.param_index;
strncpy(msg.param_id, value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
if (value.param_type == MAV_PARAM_TYPE_REAL32) {
msg.param_type = MAVLINK_TYPE_FLOAT;
msg.param_value = value.real_value;
} else {
int32_t val;
val = (int32_t)value.int_value;
memcpy(&msg.param_value, &val, sizeof(int32_t));
msg.param_type = MAVLINK_TYPE_INT32_T;
}
mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg);
} else if (_send_all_index >= 0 && _mavlink->boot_complete()) {
/* send all parameters if requested, but only after the system has booted */
@@ -351,6 +374,7 @@ MavlinkParametersManager::send(const hrt_abstime t)
/* look for the first parameter which is used */
param_t p;
do {
/* walk through all parameters, including unused ones */
p = param_for_index(_send_all_index);
@@ -364,6 +388,7 @@ MavlinkParametersManager::send(const hrt_abstime t)
if ((p == PARAM_INVALID) || (_send_all_index >= (int) param_count())) {
_send_all_index = -1;
}
} else if (_send_all_index == PARAM_HASH && hrt_absolute_time() > 20 * 1000 * 1000) {
/* the boot did not seem to ever complete, warn user and set boot complete */
_mavlink->send_statustext_critical("WARNING: SYSTEM BOOT INCOMPLETE. CHECK CONFIG.");