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

mavlink fix code style

Browse Source
This commit is contained in:
Daniel Agar 2017-01-04 17:45:32 -05:00
parent c280358e32
commit c9956e25b4
9 changed files with 80 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
Tools/files_to_check_code_style.sh
View File

@ -9,6 +9,7 @@ then
fi
exec find src \
-path src/drivers/bootloaders -o \
-path src/examples/attitude_estimator_ekf -prune -o \
-path src/examples/ekf_att_pos_estimator -prune -o \
-path src/lib/DriverFramework -prune -o \
@ -16,9 +17,7 @@ exec find src \
-path src/lib/external_lgpl -prune -o \
-path src/lib/mathlib -prune -o \
-path src/lib/matrix -prune -o \
-path src/drivers/bootloaders -o \
-path src/modules/commander -prune -o \
-path src/modules/mavlink -prune -o \
-path src/modules/sdlog2 -prune -o \
-path src/modules/systemlib/uthash -prune -o \
-path src/modules/uavcan -prune -o \

11
src/modules/mavlink/mavlink_main.h
View File

@ -172,10 +172,11 @@ public:
BROADCAST_MODE_ON
};
static const char *mavlink_mode_str(enum MAVLINK_MODE mode) {
static const char *mavlink_mode_str(enum MAVLINK_MODE mode)
{
switch (mode) {
case MAVLINK_MODE_NORMAL:
return "Normal";
return "Normal";
case MAVLINK_MODE_CUSTOM:
return "Custom";
@ -441,12 +442,14 @@ public:
/** get ulog streaming if active, nullptr otherwise */
MavlinkULog *get_ulog_streaming() { return _mavlink_ulog; }
void try_start_ulog_streaming(uint8_t target_system, uint8_t target_component) {
void try_start_ulog_streaming(uint8_t target_system, uint8_t target_component)
{
if (_mavlink_ulog) { return; }
_mavlink_ulog = MavlinkULog::try_start(_datarate, 0.7f, target_system, target_component);
}
void request_stop_ulog_streaming() {
void request_stop_ulog_streaming()
{
if (_mavlink_ulog) { _mavlink_ulog_stop_requested = true; }
}

12
src/modules/mavlink/mavlink_messages.cpp
View File

@ -2656,8 +2656,10 @@ protected:
mavlink_attitude_target_t msg{};
msg.time_boot_ms = att_sp.timestamp / 1000;
if (att_sp.q_d_valid) {
memcpy(&msg.q[0], &att_sp.q_d[0], sizeof(msg.q));
} else {
mavlink_euler_to_quaternion(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, msg.q);
}
@ -3284,7 +3286,7 @@ protected:
if (!status.in_transition_mode && status.is_rotary_wing) {
_msg.vtol_state = MAV_VTOL_STATE_MC;
} else if (!status.in_transition_mode){
} else if (!status.in_transition_mode) {
_msg.vtol_state = MAV_VTOL_STATE_FW;
} else if (status.in_transition_mode && status.in_transition_to_fw) {
@ -3384,9 +3386,11 @@ protected:
{
struct vehicle_global_position_s global_pos;
updated |= _global_pos_sub->update(&_global_pos_time, &global_pos);
if (_global_pos_time != 0) {
msg.altitude_amsl = global_pos.alt;
global_alt = global_pos.alt;
} else {
msg.altitude_amsl = NAN;
}
@ -3680,7 +3684,7 @@ private:
/* do not allow top copying this class */
MavlinkStreamHighLatency(MavlinkStreamHighLatency &);
MavlinkStreamHighLatency& operator = (const MavlinkStreamHighLatency &);
MavlinkStreamHighLatency &operator = (const MavlinkStreamHighLatency &);
protected:
explicit MavlinkStreamHighLatency(Mavlink *mavlink) : MavlinkStream(mavlink),
@ -3769,6 +3773,7 @@ protected:
if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
msg.throttle = actuator.control[actuator_controls_s::INDEX_THROTTLE] * 100;
} else {
msg.throttle = 0;
}
@ -3835,7 +3840,7 @@ public:
unsigned get_size()
{
return (_att_time > 0 || _gpos_time > 0) ? MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN +
MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
}
private:
@ -3892,6 +3897,7 @@ protected:
msg.yacc = 0;
msg.zacc = 0;
}
mavlink_msg_hil_state_quaternion_send_struct(_mavlink->get_channel(), &msg);
}
}

15
src/modules/mavlink/mavlink_mission.cpp
View File

@ -309,6 +309,7 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1
if (_verbose) {
PX4_INFO("WPM: Send MISSION_REQUEST_INT seq %u to ID %u", wpr.seq, wpr.target_system);
}
} else {
mavlink_mission_request_t wpr;
@ -362,6 +363,7 @@ MavlinkMissionManager::send(const hrt_abstime now)
_time_last_reached = now;
_last_reached = mission_result.seq_reached;
send_mission_item_reached((uint16_t)mission_result.seq_reached);
} else {
_last_reached = -1;
}
@ -572,6 +574,7 @@ MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg)
if (_int_mode) {
_int_mode = false;
}
handle_mission_request_both(msg);
}
@ -582,6 +585,7 @@ MavlinkMissionManager::handle_mission_request_int(const mavlink_message_t *msg)
if (!_int_mode) {
_int_mode = true;
}
handle_mission_request_both(msg);
}
@ -879,7 +883,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
int
MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item,
struct mission_item_s *mission_item)
struct mission_item_s *mission_item)
{
if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL ||
mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) {
@ -890,12 +894,14 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
* alignment, so we can just swap float for int32_t. */
const mavlink_mission_item_int_t *item_int
= reinterpret_cast<const mavlink_mission_item_int_t *>(mavlink_mission_item);
mission_item->lat = ((double)item_int->x)*1e-7;
mission_item->lon = ((double)item_int->y)*1e-7;
mission_item->lat = ((double)item_int->x) * 1e-7;
mission_item->lon = ((double)item_int->y) * 1e-7;
} else {
mission_item->lat = (double)mavlink_mission_item->x;
mission_item->lon = (double)mavlink_mission_item->y;
}
mission_item->altitude = mavlink_mission_item->z;
if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL) {
@ -1027,7 +1033,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
int
MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *mission_item,
mavlink_mission_item_t *mavlink_mission_item)
mavlink_mission_item_t *mavlink_mission_item)
{
mavlink_mission_item->frame = mission_item->frame;
mavlink_mission_item->command = mission_item->nav_cmd;
@ -1089,6 +1095,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
mavlink_mission_item->x = (float)mission_item->lat;
mavlink_mission_item->y = (float)mission_item->lon;
}
mavlink_mission_item->z = mission_item->altitude;
if (mission_item->altitude_is_relative) {

5
src/modules/mavlink/mavlink_orb_subscription.cpp
View File

@ -171,12 +171,15 @@ MavlinkOrbSubscription::is_published()
_last_pub_check = now;
#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE)
// Snapdragon has currently no support for orb_exists, therefore
// we're not using it.
if (_fd < 0) {
_fd = orb_subscribe_multi(_topic, _instance);
}
#else
// We don't want to subscribe to anything that does not exist
// in order to save memory and file descriptors.
// However, for some topics like vehicle_command_ack, we want to subscribe
@ -188,6 +191,7 @@ MavlinkOrbSubscription::is_published()
if (_fd < 0) {
_fd = orb_subscribe_multi(_topic, _instance);
}
#endif
bool updated;
@ -199,6 +203,7 @@ MavlinkOrbSubscription::is_published()
// topic may have been last published before we subscribed
uint64_t time_topic = 0;
if (!_published && orb_stat(_fd, &time_topic) == PX4_OK) {
if (time_topic != 0) {
_published = true;

3
src/modules/mavlink/mavlink_parameters.cpp
View File

@ -351,7 +351,8 @@ MavlinkParametersManager::send(const hrt_abstime t)
// Re-pack the message with the UAVCAN node ID
mavlink_message_t mavlink_packet;
mavlink_msg_param_value_encode_chan(mavlink_system.sysid, value.node_id, _mavlink->get_channel(), &mavlink_packet, &msg);
mavlink_msg_param_value_encode_chan(mavlink_system.sysid, value.node_id, _mavlink->get_channel(), &mavlink_packet,
&msg);
_mavlink_resend_uart(_mavlink->get_channel(), &mavlink_packet);
} else if (_send_all_index >= 0 && _mavlink->boot_complete()) {

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

@ -90,7 +90,7 @@ protected:
void send(const hrt_abstime t);
int send_param(param_t param, int component_id=-1);
int send_param(param_t param, int component_id = -1);
orb_advert_t _rc_param_map_pub;
struct rc_parameter_map_s _rc_param_map;

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

@ -387,7 +387,7 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
} else if (cmd_mavlink.command == MAV_CMD_SET_MESSAGE_INTERVAL) {
ret = set_message_interval((int)(cmd_mavlink.param1 + 0.5f),
cmd_mavlink.param2, cmd_mavlink.param3);
cmd_mavlink.param2, cmd_mavlink.param3);
} else if (cmd_mavlink.command == MAV_CMD_GET_MESSAGE_INTERVAL) {
get_message_interval((int)cmd_mavlink.param1);
@ -456,14 +456,17 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
if (send_ack) {
vehicle_command_ack_s command_ack;
command_ack.command = cmd_mavlink.command;
if (ret == PX4_OK) {
command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
} else {
command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
}
if (_command_ack_pub == nullptr) {
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, vehicle_command_ack_s::ORB_QUEUE_LENGTH);
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &command_ack);
@ -556,14 +559,17 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
if (send_ack) {
vehicle_command_ack_s command_ack;
command_ack.command = cmd_mavlink.command;
if (ret == PX4_OK) {
command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
} else {
command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
}
if (_command_ack_pub == nullptr) {
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, vehicle_command_ack_s::ORB_QUEUE_LENGTH);
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &command_ack);
@ -903,6 +909,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
pos_sp_triplet.current.velocity_frame =
set_position_target_local_ned.coordinate_frame;
} else {
pos_sp_triplet.current.velocity_valid = false;
}
@ -2241,7 +2248,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
ctrl_state.z_acc = hil_state.zacc;
static float _acc_hor_filt = 0;
_acc_hor_filt = 0.95f * _acc_hor_filt + 0.05f * sqrtf(ctrl_state.x_acc * ctrl_state.x_acc + ctrl_state.y_acc * ctrl_state.y_acc);
_acc_hor_filt = 0.95f * _acc_hor_filt + 0.05f * sqrtf(ctrl_state.x_acc * ctrl_state.x_acc + ctrl_state.y_acc *
ctrl_state.y_acc);
ctrl_state.horz_acc_mag = _acc_hor_filt;
ctrl_state.airspeed_valid = false;

40
src/modules/mavlink/mavlink_ulog.cpp
View File

@ -51,16 +51,17 @@ const float MavlinkULog::_rate_calculation_delta_t = 0.1f;
MavlinkULog::MavlinkULog(int datarate, float max_rate_factor, uint8_t target_system, uint8_t target_component)
: _target_system(target_system), _target_component(target_component),
_max_rate_factor(max_rate_factor),
_max_num_messages(math::max(1, (int)ceilf(_rate_calculation_delta_t * _max_rate_factor * datarate /
(MAVLINK_MSG_ID_LOGGING_DATA_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)))),
_current_rate_factor(max_rate_factor)
_max_rate_factor(max_rate_factor),
_max_num_messages(math::max(1, (int)ceilf(_rate_calculation_delta_t *_max_rate_factor * datarate /
(MAVLINK_MSG_ID_LOGGING_DATA_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)))),
_current_rate_factor(max_rate_factor)
{
_ulog_stream_sub = orb_subscribe(ORB_ID(ulog_stream));
if (_ulog_stream_sub < 0) {
PX4_ERR("orb_subscribe failed (%i)", errno);
}
_waiting_for_initial_ack = true;
_last_sent_time = hrt_absolute_time(); //(ab)use this timestamp during initialization
_next_rate_check = _last_sent_time + _rate_calculation_delta_t * 1.e6f;
@ -71,6 +72,7 @@ MavlinkULog::~MavlinkULog()
if (_ulog_stream_ack_pub) {
orb_unadvertise(_ulog_stream_ack_pub);
}
if (_ulog_stream_sub >= 0) {
orb_unsubscribe(_ulog_stream_sub);
}
@ -87,28 +89,34 @@ void MavlinkULog::start_ack_received()
int MavlinkULog::handle_update(mavlink_channel_t channel)
{
static_assert(sizeof(ulog_stream_s::data) == MAVLINK_MSG_LOGGING_DATA_FIELD_DATA_LEN, "Invalid uorb ulog_stream.data length");
static_assert(sizeof(ulog_stream_s::data) == MAVLINK_MSG_LOGGING_DATA_ACKED_FIELD_DATA_LEN, "Invalid uorb ulog_stream.data length");
static_assert(sizeof(ulog_stream_s::data) == MAVLINK_MSG_LOGGING_DATA_FIELD_DATA_LEN,
"Invalid uorb ulog_stream.data length");
static_assert(sizeof(ulog_stream_s::data) == MAVLINK_MSG_LOGGING_DATA_ACKED_FIELD_DATA_LEN,
"Invalid uorb ulog_stream.data length");
if (_waiting_for_initial_ack) {
if (hrt_elapsed_time(&_last_sent_time) > 3e5) {
PX4_WARN("no ack from logger (is it running?)");
return -1;
}
return 0;
}
// check if we're waiting for an ACK
if (_last_sent_time) {
bool check_for_updates = false;
if (_ack_received) {
_last_sent_time = 0;
check_for_updates = true;
} else {
if (hrt_elapsed_time(&_last_sent_time) > ulog_stream_ack_s::ACK_TIMEOUT * 1000) {
if (++_sent_tries > ulog_stream_ack_s::ACK_MAX_TRIES) {
return -ETIMEDOUT;
} else {
PX4_DEBUG("re-sending ulog mavlink message (try=%i)", _sent_tries);
_last_sent_time = hrt_absolute_time();
@ -131,8 +139,10 @@ int MavlinkULog::handle_update(mavlink_channel_t channel)
bool updated = false;
int ret = orb_check(_ulog_stream_sub, &updated);
while (updated && !ret && _current_num_msgs < _max_num_messages) {
orb_copy(ORB_ID(ulog_stream), _ulog_stream_sub, &_ulog_data);
if (_ulog_data.flags & ulog_stream_s::FLAGS_NEED_ACK) {
_sent_tries = 1;
_last_sent_time = hrt_absolute_time();
@ -160,22 +170,26 @@ int MavlinkULog::handle_update(mavlink_channel_t channel)
memcpy(msg.data, _ulog_data.data, sizeof(msg.data));
mavlink_msg_logging_data_send_struct(channel, &msg);
}
++_current_num_msgs;
ret = orb_check(_ulog_stream_sub, &updated);
}
//need to update the rate?
hrt_abstime t = hrt_absolute_time();
if (t > _next_rate_check) {
if (_current_num_msgs < _max_num_messages) {
_current_rate_factor = _max_rate_factor * (float)_current_num_msgs / _max_num_messages;
} else {
_current_rate_factor = _max_rate_factor;
}
_current_num_msgs = 0;
_next_rate_check = t + _rate_calculation_delta_t * 1.e6f;
PX4_DEBUG("current rate=%.3f (max=%i msgs in %.3fs)", (double)_current_rate_factor, _max_num_messages,
(double)_rate_calculation_delta_t);
(double)_rate_calculation_delta_t);
}
return 0;
@ -186,48 +200,58 @@ void MavlinkULog::initialize()
if (_init) {
return;
}
px4_sem_init(&_lock, 1, 1);
_init = true;
}
MavlinkULog* MavlinkULog::try_start(int datarate, float max_rate_factor, uint8_t target_system, uint8_t target_component)
MavlinkULog *MavlinkULog::try_start(int datarate, float max_rate_factor, uint8_t target_system,
uint8_t target_component)
{
MavlinkULog *ret = nullptr;
bool failed = false;
lock();
if (!_instance) {
ret = _instance = new MavlinkULog(datarate, max_rate_factor, target_system, target_component);
if (!_instance) {
failed = true;
}
}
unlock();
if (failed) {
PX4_ERR("alloc failed");
}
return ret;
}
void MavlinkULog::stop()
{
lock();
if (_instance) {
delete _instance;
_instance = nullptr;
}
unlock();
}
void MavlinkULog::handle_ack(mavlink_logging_ack_t ack)
{
lock();
if (_instance) { // make sure stop() was not called right before
if (_wait_for_ack_sequence == ack.sequence) {
_ack_received = true;
publish_ack(ack.sequence);
}
}
unlock();
}