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

mavlink: code style fixed

This commit is contained in:
Anton Babushkin 2014-03-21 20:06:34 +04:00
parent b1e59f37fe
commit 1c49d768fb
2 changed files with 68 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
src/modules/mavlink/mavlink_main.cpp
View File

@ -151,8 +151,9 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
}
/* no valid instance, bail */
if (!instance)
if (!instance) {
return;
}
int uart = instance->get_uart_fd();
@ -165,12 +166,12 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
int buf_free = 0;
if (instance->get_flow_control_enabled()
&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
if (buf_free == 0) {
if (last_write_times[(unsigned)channel] != 0 &&
hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500*1000UL) {
hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) {
warnx("DISABLING HARDWARE FLOW CONTROL");
instance->enable_flow_control(false);
@ -225,30 +226,37 @@ Mavlink::Mavlink() :
case 0:
_channel = MAVLINK_COMM_0;
break;
case 1:
_channel = MAVLINK_COMM_1;
break;
case 2:
_channel = MAVLINK_COMM_2;
break;
case 3:
_channel = MAVLINK_COMM_3;
break;
#ifdef MAVLINK_COMM_4
case 4:
_channel = MAVLINK_COMM_4;
break;
#endif
#ifdef MAVLINK_COMM_5
case 5:
case 5:
_channel = MAVLINK_COMM_5;
break;
#endif
#ifdef MAVLINK_COMM_6
case 6:
_channel = MAVLINK_COMM_6;
break;
#endif
default:
errx(1, "instance ID is out of range");
break;
@ -279,6 +287,7 @@ Mavlink::~Mavlink()
}
} while (_task_running);
}
LL_DELETE(_mavlink_instances, this);
}
@ -604,12 +613,16 @@ Mavlink::enable_flow_control(bool enabled)
}
struct termios uart_config;
int ret = tcgetattr(_uart_fd, &uart_config);
if (enabled) {
uart_config.c_cflag |= CRTSCTS;
} else {
uart_config.c_cflag &= ~CRTSCTS;
}
ret = tcsetattr(_uart_fd, TCSANOW, &uart_config);
if (!ret) {
@ -1652,6 +1665,7 @@ Mavlink::task_main(int argc, char *argv[])
case 'm':
if (strcmp(optarg, "custom") == 0) {
_mode = MAVLINK_MODE_CUSTOM;
} else if (strcmp(optarg, "camera") == 0) {
_mode = MAVLINK_MODE_CAMERA;
}
@ -1973,6 +1987,7 @@ Mavlink::start(int argc, char *argv[])
const unsigned limit = 100 * 1000 / sleeptime;
unsigned count = 0;
while (ic == Mavlink::instance_count() && count < limit) {
::usleep(sleeptime);
count++;
@ -2003,20 +2018,26 @@ Mavlink::stream(int argc, char *argv[])
bool err_flag = false;
int i = 0;
while (i < argc) {
if (0 == strcmp(argv[i], "-r") && i < argc - 1 ) {
rate = strtod(argv[i+1], nullptr);
if (0 == strcmp(argv[i], "-r") && i < argc - 1) {
rate = strtod(argv[i + 1], nullptr);
if (rate < 0.0f) {
err_flag = true;
}
i++;
} else if (0 == strcmp(argv[i], "-d") && i < argc - 1 ) {
device_name = argv[i+1];
} else if (0 == strcmp(argv[i], "-d") && i < argc - 1) {
device_name = argv[i + 1];
i++;
} else if (0 == strcmp(argv[i], "-s") && i < argc - 1 ) {
stream_name = argv[i+1];
} else if (0 == strcmp(argv[i], "-s") && i < argc - 1) {
stream_name = argv[i + 1];
i++;
} else {
err_flag = true;
}

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

@ -262,7 +262,6 @@ protected:
void send(const hrt_abstime t)
{
if (status_sub->update(t)) {
mavlink_msg_sys_status_send(_channel,
status->onboard_control_sensors_present,
status->onboard_control_sensors_enabled,
@ -318,7 +317,6 @@ protected:
void send(const hrt_abstime t)
{
if (sensor_sub->update(t)) {
uint16_t fields_updated = 0;
if (accel_timestamp != sensor->accelerometer_timestamp) {
@ -385,7 +383,6 @@ protected:
void send(const hrt_abstime t)
{
if (att_sub->update(t)) {
mavlink_msg_attitude_send(_channel,
att->timestamp / 1000,
att->roll, att->pitch, att->yaw,
@ -422,7 +419,6 @@ protected:
void send(const hrt_abstime t)
{
if (att_sub->update(t)) {
mavlink_msg_attitude_quaternion_send(_channel,
att->timestamp / 1000,
att->q[0],
@ -494,7 +490,6 @@ protected:
updated |= airspeed_sub->update(t);
if (updated) {
float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e);
uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F;
float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f;
@ -538,7 +533,6 @@ protected:
void send(const hrt_abstime t)
{
if (gps_sub->update(t)) {
mavlink_msg_gps_raw_int_send(_channel,
gps->timestamp_position,
gps->fix_type,
@ -592,15 +586,15 @@ protected:
if (updated) {
mavlink_msg_global_position_int_send(_channel,
pos->timestamp / 1000,
pos->lat * 1e7,
pos->lon * 1e7,
pos->alt * 1000.0f,
(pos->alt - home->alt) * 1000.0f,
pos->vel_n * 100.0f,
pos->vel_e * 100.0f,
pos->vel_d * 100.0f,
_wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
pos->timestamp / 1000,
pos->lat * 1e7,
pos->lon * 1e7,
pos->alt * 1000.0f,
(pos->alt - home->alt) * 1000.0f,
pos->vel_n * 100.0f,
pos->vel_e * 100.0f,
pos->vel_d * 100.0f,
_wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
}
}
};
@ -633,7 +627,6 @@ protected:
void send(const hrt_abstime t)
{
if (pos_sub->update(t)) {
mavlink_msg_local_position_ned_send(_channel,
pos->timestamp / 1000,
pos->x,
@ -730,7 +723,6 @@ protected:
void send(const hrt_abstime t)
{
if (act_sub->update(t)) {
mavlink_msg_servo_output_raw_send(_channel,
act->timestamp / 1000,
_n,
@ -790,7 +782,6 @@ protected:
updated |= act_sub->update(t);
if (updated) {
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state;
uint8_t mavlink_base_mode;
@ -870,7 +861,6 @@ protected:
void send(const hrt_abstime t)
{
if (pos_sp_triplet_sub->update(t)) {
mavlink_msg_global_position_setpoint_int_send(_channel,
MAV_FRAME_GLOBAL,
(int32_t)(pos_sp_triplet->current.lat * 1e7),
@ -908,14 +898,14 @@ protected:
void send(const hrt_abstime t)
{
pos_sp_sub->update(t);
mavlink_msg_local_position_setpoint_send(_channel,
MAV_FRAME_LOCAL_NED,
pos_sp->x,
pos_sp->y,
pos_sp->z,
pos_sp->yaw);
if (pos_sp_sub->update(t)) {
mavlink_msg_local_position_setpoint_send(_channel,
MAV_FRAME_LOCAL_NED,
pos_sp->x,
pos_sp->y,
pos_sp->z,
pos_sp->yaw);
}
}
};
@ -947,7 +937,6 @@ protected:
void send(const hrt_abstime t)
{
if (att_sp_sub->update(t)) {
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel,
att_sp->timestamp / 1000,
att_sp->roll_body,
@ -986,7 +975,6 @@ protected:
void send(const hrt_abstime t)
{
if (att_rates_sp_sub->update(t)) {
mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel,
att_rates_sp->timestamp / 1000,
att_rates_sp->roll,
@ -1025,7 +1013,6 @@ protected:
void send(const hrt_abstime t)
{
if (rc_sub->update(t)) {
const unsigned port_width = 8;
for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) {
@ -1075,7 +1062,6 @@ protected:
void send(const hrt_abstime t)
{
if (manual_sub->update(t)) {
mavlink_msg_manual_control_send(_channel,
mavlink_system.sysid,
manual->roll * 1000,
@ -1115,7 +1101,6 @@ protected:
void send(const hrt_abstime t)
{
if (flow_sub->update(t)) {
mavlink_msg_optical_flow_send(_channel,
flow->timestamp,
flow->sensor_id,
@ -1154,24 +1139,23 @@ protected:
void send(const hrt_abstime t)
{
if (att_ctrl_sub->update(t)) {
// send, add spaces so that string buffer is at least 10 chars long
mavlink_msg_named_value_float_send(_channel,
att_ctrl->timestamp / 1000,
"rll ctrl ",
att_ctrl->control[0]);
mavlink_msg_named_value_float_send(_channel,
att_ctrl->timestamp / 1000,
"ptch ctrl ",
att_ctrl->control[1]);
mavlink_msg_named_value_float_send(_channel,
att_ctrl->timestamp / 1000,
"yaw ctrl ",
att_ctrl->control[2]);
mavlink_msg_named_value_float_send(_channel,
att_ctrl->timestamp / 1000,
"thr ctrl ",
att_ctrl->control[3]);
/* send, add spaces so that string buffer is at least 10 chars long */
mavlink_msg_named_value_float_send(_channel,
att_ctrl->timestamp / 1000,
"rll ctrl ",
att_ctrl->control[0]);
mavlink_msg_named_value_float_send(_channel,
att_ctrl->timestamp / 1000,
"ptch ctrl ",
att_ctrl->control[1]);
mavlink_msg_named_value_float_send(_channel,
att_ctrl->timestamp / 1000,
"yaw ctrl ",
att_ctrl->control[2]);
mavlink_msg_named_value_float_send(_channel,
att_ctrl->timestamp / 1000,
"thr ctrl ",
att_ctrl->control[3]);
}
}
};
@ -1203,8 +1187,7 @@ protected:
void send(const hrt_abstime t)
{
if (debug_sub->update(t)) {
// Enforce null termination
/* enforce null termination */
debug->key[sizeof(debug->key) - 1] = '\0';
mavlink_msg_named_value_float_send(_channel,
@ -1246,10 +1229,11 @@ protected:
(void)status_sub->update(t);
if (status->arming_state == ARMING_STATE_ARMED
|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
/* send camera capture on */
mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
} else {
/* send camera capture off */
mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0);