From 9aca1701f4702d69ab43aadabb6f4a12e9dbeee9 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 18 Aug 2015 11:02:52 +0200 Subject: [PATCH] code-style fixes --- src/modules/mavlink/mavlink_messages.cpp | 18 ++++---- .../vtol_att_control_main.cpp | 45 +++++++++++-------- 2 files changed, 35 insertions(+), 28 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 9360ec23bf..67809ed7be 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -2374,7 +2374,7 @@ private: /* do not allow top copying this class */ MavlinkStreamVtolState(MavlinkStreamVtolState &); - MavlinkStreamVtolState& operator = (const MavlinkStreamVtolState &); + MavlinkStreamVtolState &operator = (const MavlinkStreamVtolState &); protected: explicit MavlinkStreamVtolState(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -2388,25 +2388,25 @@ protected: if (_status_sub->update(&status)) { mavlink_vtol_state_t msg; - if (status.is_vtol) - { - if (status.is_rotary_wing) - { + if (status.is_vtol) { + if (status.is_rotary_wing) { if (status.in_transition_mode) { msg.state = MAV_VTOL_STATE_TRANSITION_TO_FW; + } else { msg.state = MAV_VTOL_STATE_MC; } - } - else { + + } else { if (status.in_transition_mode) { msg.state = MAV_VTOL_STATE_TRANSITION_TO_MC; + } else { msg.state = MAV_VTOL_STATE_FW; } } - } - else { + + } else { msg.state = MAV_VTOL_STATE_UNDEFINED; } diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 785ff7aab5..744f31d5b4 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -318,7 +318,8 @@ VtolAttitudeControl::vehicle_local_pos_poll() * Check for command updates. */ void -VtolAttitudeControl::vehicle_cmd_poll() { +VtolAttitudeControl::vehicle_cmd_poll() +{ bool updated; orb_check(_vehicle_cmd_sub, &updated); @@ -332,7 +333,8 @@ VtolAttitudeControl::vehicle_cmd_poll() { * Check received command */ void -VtolAttitudeControl::handle_command() { +VtolAttitudeControl::handle_command() +{ // update transition command if necessary if (_vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) { _transition_command = int(_vehicle_cmd.param1 + 0.5f); @@ -347,10 +349,12 @@ bool VtolAttitudeControl::is_fixed_wing_requested() { bool to_fw = _manual_control_sp.aux1 > 0.0f; - if (_v_control_mode.flag_control_offboard_enabled) { - to_fw = _transition_command == vehicle_status_s::VEHICLE_VTOL_STATE_FW; - } - return to_fw; + + if (_v_control_mode.flag_control_offboard_enabled) { + to_fw = _transition_command == vehicle_status_s::VEHICLE_VTOL_STATE_FW; + } + + return to_fw; } /** @@ -531,12 +535,11 @@ void VtolAttitudeControl::task_main() _vtol_type->update_vtol_state(); // reset transition command if not in offboard control - if (!_v_control_mode.flag_control_offboard_enabled) - { + if (!_v_control_mode.flag_control_offboard_enabled) { if (_vtol_type->get_mode() == ROTARY_WING) { _transition_command = vehicle_status_s::VEHICLE_VTOL_STATE_MC; - } - else if (_vtol_type->get_mode() == FIXED_WING) { + + } else if (_vtol_type->get_mode() == FIXED_WING) { _transition_command = vehicle_status_s::VEHICLE_VTOL_STATE_FW; } } @@ -555,6 +558,7 @@ void VtolAttitudeControl::task_main() fill_mc_att_rates_sp(); } + } else if (_vtol_type->get_mode() == FIXED_WING) { // vehicle is in fw mode _vtol_vehicle_status.vtol_in_rw_mode = false; @@ -569,11 +573,13 @@ void VtolAttitudeControl::task_main() fill_fw_att_rates_sp(); } + } else if (_vtol_type->get_mode() == TRANSITION) { // vehicle is doing a transition _vtol_vehicle_status.vtol_in_trans_mode = true; bool got_new_data = false; + if (fds[0].revents & POLLIN) { orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in); got_new_data = true; @@ -598,29 +604,30 @@ void VtolAttitudeControl::task_main() _vtol_type->fill_actuator_outputs(); /* Only publish if the proper mode(s) are enabled */ - if(_v_control_mode.flag_control_attitude_enabled || - _v_control_mode.flag_control_rates_enabled || - _v_control_mode.flag_control_manual_enabled) - { + if (_v_control_mode.flag_control_attitude_enabled || + _v_control_mode.flag_control_rates_enabled || + _v_control_mode.flag_control_manual_enabled) { if (_actuators_0_pub != nullptr) { orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); + } else { _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); } if (_actuators_1_pub != nullptr) { orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); + } else { _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); } } // publish the attitude rates setpoint - if(_v_rates_sp_pub != nullptr) { - orb_publish(ORB_ID(vehicle_rates_setpoint),_v_rates_sp_pub,&_v_rates_sp); - } - else { - _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint),&_v_rates_sp); + if (_v_rates_sp_pub != nullptr) { + orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); + + } else { + _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); } }