diff --git a/src/drivers/vmount/input.cpp b/src/drivers/vmount/input.cpp index 16011b5702..69ac551bce 100644 --- a/src/drivers/vmount/input.cpp +++ b/src/drivers/vmount/input.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2016 PX4 Development Team. All rights reserved. +* Copyright (c) 2016-2017 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -43,7 +43,7 @@ namespace vmount { -int InputBase::update(unsigned int timeout_ms, ControlData **control_data) +int InputBase::update(unsigned int timeout_ms, ControlData **control_data, bool already_active) { if (!_initialized) { int ret = initialize(); @@ -60,7 +60,7 @@ int InputBase::update(unsigned int timeout_ms, ControlData **control_data) return 0; } - return update_impl(timeout_ms, control_data); + return update_impl(timeout_ms, control_data, already_active); } void InputBase::control_data_set_lon_lat(double lon, double lat, float altitude, float roll_angle, diff --git a/src/drivers/vmount/input.h b/src/drivers/vmount/input.h index 5606a056b6..9c2b2603e4 100644 --- a/src/drivers/vmount/input.h +++ b/src/drivers/vmount/input.h @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2016 PX4 Development Team. All rights reserved. +* Copyright (c) 2016-2017 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -63,15 +63,17 @@ public: * subsequent calls to update() that return no new data * (in other words: if (some) control_data values change, * non-null will be returned). + * @param already_active true if the mode was already active last time, false if it was not and "major" + * change is necessary such as big stick movement for RC. * @return 0 on success, <0 otherwise */ - virtual int update(unsigned int timeout_ms, ControlData **control_data); + virtual int update(unsigned int timeout_ms, ControlData **control_data, bool already_active); /** report status to stdout */ virtual void print_status() = 0; protected: - virtual int update_impl(unsigned int timeout_ms, ControlData **control_data) = 0; + virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active) = 0; virtual int initialize() { return 0; } diff --git a/src/drivers/vmount/input_mavlink.cpp b/src/drivers/vmount/input_mavlink.cpp index 98736496dd..f941659e25 100644 --- a/src/drivers/vmount/input_mavlink.cpp +++ b/src/drivers/vmount/input_mavlink.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2016 PX4 Development Team. All rights reserved. +* Copyright (c) 2016-2017 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -55,8 +55,7 @@ namespace vmount { -InputMavlinkROI::InputMavlinkROI(InputRC *manual_override) - : _manual_control(manual_override) +InputMavlinkROI::InputMavlinkROI() { } @@ -85,34 +84,24 @@ int InputMavlinkROI::initialize() return -errno; } - if (_manual_control) { - int ret = _manual_control->initialize(); - - if (ret) { - PX4_ERR("failed to initialize rc input"); - return ret; - } - } - return 0; } -int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_data) +int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active) { - px4_pollfd_struct_t polls[3]; + // already_active is unused, we don't care what happened previously. + + // Default to no change, set if we receive anything. + *control_data = nullptr; + + const int num_poll = 2; + px4_pollfd_struct_t polls[num_poll]; polls[0].fd = _vehicle_roi_sub; polls[0].events = POLLIN; polls[1].fd = _position_setpoint_triplet_sub; polls[1].events = POLLIN; - int num_poll = 2; - - if (_manual_control && _allow_manual_control) { - polls[num_poll].fd = _manual_control->_get_subscription_fd(); - polls[num_poll].events = POLLIN; - ++num_poll; - } int ret = px4_poll(polls, num_poll, timeout_ms); @@ -121,7 +110,7 @@ int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_ } if (ret == 0) { - *control_data = nullptr; + // Timeout, _control_data is already null } else { if (polls[0].revents & POLLIN) { @@ -129,30 +118,20 @@ int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_ orb_copy(ORB_ID(vehicle_roi), _vehicle_roi_sub, &vehicle_roi); if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_NONE) { - _allow_manual_control = true; - - if (!_manual_control) { - _control_data.type = ControlData::Type::Neutral; - } } else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPNEXT) { - _allow_manual_control = false; _read_control_data_from_position_setpoint_sub(); _control_data.type_data.lonlat.roll_angle = 0.f; _control_data.type_data.lonlat.pitch_fixed_angle = -10.f; } else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPINDEX) { - _allow_manual_control = false; //TODO how to do this? } else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_LOCATION) { - _allow_manual_control = false; control_data_set_lon_lat(vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt); } else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_TARGET) { - _allow_manual_control = false; //TODO is this even suported? - } _cur_roi_mode = vehicle_roi.mode; @@ -163,9 +142,6 @@ int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_ } _control_data.gimbal_shutter_retract = false; - - } else if (num_poll > 2 && (polls[2].revents & POLLIN)) { - _manual_control->_read_control_data_from_subscription(_control_data); } // check whether the position setpoint got updated @@ -190,11 +166,10 @@ void InputMavlinkROI::_read_control_data_from_position_setpoint_sub() void InputMavlinkROI::print_status() { - PX4_INFO("Input: Mavlink (ROI)%s", _manual_control ? " (with manual)" : ""); + PX4_INFO("Input: Mavlink (ROI)"); } -InputMavlinkCmdMount::InputMavlinkCmdMount(InputRC *manual_override) - : _manual_control(manual_override) +InputMavlinkCmdMount::InputMavlinkCmdMount() { } @@ -211,31 +186,19 @@ int InputMavlinkCmdMount::initialize() return -errno; } - if (_manual_control) { - int ret = _manual_control->initialize(); - - if (ret) { - PX4_ERR("failed to initialize rc input"); - return ret; - } - } - return 0; } -int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **control_data) +int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active) { - px4_pollfd_struct_t polls[2]; + // Default to notify that there was no change. + *control_data = nullptr; + + const int num_poll = 1; + px4_pollfd_struct_t polls[num_poll]; polls[0].fd = _vehicle_command_sub; polls[0].events = POLLIN; - int num_poll = 1; - - if (_manual_control && _allow_manual_control) { - polls[num_poll].fd = _manual_control->_get_subscription_fd(); - polls[num_poll].events = POLLIN; - ++num_poll; - } int ret = px4_poll(polls, num_poll, timeout_ms); @@ -244,7 +207,7 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con } if (ret == 0) { - *control_data = nullptr; + // Timeout control_data already null. } else { *control_data = &_control_data; @@ -260,7 +223,6 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con _control_data.gimbal_shutter_retract = false; if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL) { - _allow_manual_control = false; switch ((int)vehicle_command.param7) { case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT: @@ -283,7 +245,6 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con break; case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING: - _allow_manual_control = true; *control_data = nullptr; break; @@ -304,9 +265,6 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con _ack_vehicle_command(vehicle_command.command); } - - } else if (num_poll > 1 && (polls[1].revents & POLLIN)) { - _manual_control->_read_control_data_from_subscription(_control_data); } } @@ -333,7 +291,7 @@ void InputMavlinkCmdMount::_ack_vehicle_command(uint16_t command) void InputMavlinkCmdMount::print_status() { - PX4_INFO("Input: Mavlink (CMD_MOUNT)%s", _manual_control ? " (with manual)" : ""); + PX4_INFO("Input: Mavlink (CMD_MOUNT)"); } diff --git a/src/drivers/vmount/input_mavlink.h b/src/drivers/vmount/input_mavlink.h index 8c243a73ea..30f2fb5b13 100644 --- a/src/drivers/vmount/input_mavlink.h +++ b/src/drivers/vmount/input_mavlink.h @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2016 PX4 Development Team. All rights reserved. +* Copyright (c) 2016-2017 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -54,18 +54,13 @@ namespace vmount class InputMavlinkROI : public InputBase { public: - - /** - * @param manual_control if non-null allow manual input as long as we have not received any mavlink - * command yet or ROI mode is set to NONE. - */ - InputMavlinkROI(InputRC *manual_control = nullptr); + InputMavlinkROI(); virtual ~InputMavlinkROI(); virtual void print_status(); protected: - virtual int update_impl(unsigned int timeout_ms, ControlData **control_data); + virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active); virtual int initialize(); private: @@ -73,8 +68,6 @@ private: int _vehicle_roi_sub = -1; int _position_setpoint_triplet_sub = -1; - bool _allow_manual_control = true; - InputRC *_manual_control; uint8_t _cur_roi_mode = vehicle_roi_s::VEHICLE_ROI_NONE; }; @@ -86,26 +79,19 @@ private: class InputMavlinkCmdMount : public InputBase { public: - - /** - * @param manual_control if non-null allow manual input as long as we have not received any mavlink - * command yet. - */ - InputMavlinkCmdMount(InputRC *manual_control = nullptr); + InputMavlinkCmdMount(); virtual ~InputMavlinkCmdMount(); virtual void print_status(); protected: - virtual int update_impl(unsigned int timeout_ms, ControlData **control_data); + virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active); virtual int initialize(); private: void _ack_vehicle_command(uint16_t command); int _vehicle_command_sub = -1; - bool _allow_manual_control = true; - InputRC *_manual_control; orb_advert_t _vehicle_command_ack_pub = nullptr; bool _stabilize[3] = { false, false, false }; }; diff --git a/src/drivers/vmount/input_rc.cpp b/src/drivers/vmount/input_rc.cpp index c0d61742cc..06bd923860 100644 --- a/src/drivers/vmount/input_rc.cpp +++ b/src/drivers/vmount/input_rc.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2016 PX4 Development Team. All rights reserved. +* Copyright (c) 2016-2017 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -73,8 +73,11 @@ int InputRC::initialize() return 0; } -int InputRC::update_impl(unsigned int timeout_ms, ControlData **control_data) +int InputRC::update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active) { + // Default to no change signalled by NULL. + *control_data = nullptr; + px4_pollfd_struct_t polls[1]; polls[0].fd = _manual_control_setpoint_sub; polls[0].events = POLLIN; @@ -86,32 +89,58 @@ int InputRC::update_impl(unsigned int timeout_ms, ControlData **control_data) } if (ret == 0) { - *control_data = nullptr; - + // Timeout, leave NULL } else { if (polls[0].revents & POLLIN) { - _read_control_data_from_subscription(_control_data); - *control_data = &_control_data; + // Only if there was a change, we update the control data, otherwise leave it NULL. + if (_read_control_data_from_subscription(_control_data, already_active)) { + *control_data = &_control_data; + } } } return 0; } -void InputRC::_read_control_data_from_subscription(ControlData &control_data) +bool InputRC::_read_control_data_from_subscription(ControlData &control_data, bool already_active) { manual_control_setpoint_s manual_control_setpoint; orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &manual_control_setpoint); control_data.type = ControlData::Type::Angle; - for (int i = 0; i < 3; ++i) { - control_data.type_data.angle.is_speed[i] = false; - control_data.type_data.angle.angles[i] = _get_aux_value(manual_control_setpoint, i) * M_PI_F; + float new_aux_values[3]; - control_data.stabilize_axis[i] = false; + for (int i = 0; i < 3; ++i) { + new_aux_values[i] = _get_aux_value(manual_control_setpoint, i); } - control_data.gimbal_shutter_retract = false; + // If we were already active previously, we just update normally. Otherwise, there needs to be + // a major stick movement to re-activate manual (or it's running for the very first time). + bool major_movement = false; + + // Detect a big stick movement + for (int i = 0; i < 3; ++i) { + if (fabs(_last_set_aux_values[i] - new_aux_values[i]) > 0.25) { + major_movement = true; + } + } + + if (already_active || major_movement || _first_time) { + + for (int i = 0; i < 3; ++i) { + control_data.type_data.angle.is_speed[i] = false; + control_data.type_data.angle.angles[i] = new_aux_values[i] * M_PI_F; + control_data.stabilize_axis[i] = false; + + _last_set_aux_values[i] = new_aux_values[i]; + } + + control_data.gimbal_shutter_retract = false; + return true; + + } else { + return false; + } } float InputRC::_get_aux_value(const manual_control_setpoint_s &manual_control_setpoint, int channel_idx) diff --git a/src/drivers/vmount/input_rc.h b/src/drivers/vmount/input_rc.h index 178768e3b6..48044a74dc 100644 --- a/src/drivers/vmount/input_rc.h +++ b/src/drivers/vmount/input_rc.h @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2016 PX4 Development Team. All rights reserved. +* Copyright (c) 2016-2017 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -67,10 +67,13 @@ public: virtual void print_status(); protected: - virtual int update_impl(unsigned int timeout_ms, ControlData **control_data); + virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active); virtual int initialize(); - virtual void _read_control_data_from_subscription(ControlData &control_data); + /** + * @return true if there was a change in control data + */ + virtual bool _read_control_data_from_subscription(ControlData &control_data, bool already_active); int _get_subscription_fd() const { return _manual_control_setpoint_sub; } @@ -79,9 +82,9 @@ private: int _aux_channels[3]; int _manual_control_setpoint_sub = -1; + bool _first_time = true; - friend class InputMavlinkROI; - friend class InputMavlinkCmdMount; + float _last_set_aux_values[3] = {}; }; diff --git a/src/drivers/vmount/input_test.cpp b/src/drivers/vmount/input_test.cpp index 8c1df28419..9f9d037cc1 100644 --- a/src/drivers/vmount/input_test.cpp +++ b/src/drivers/vmount/input_test.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2016 PX4 Development Team. All rights reserved. +* Copyright (c) 2016-2017 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -57,7 +57,7 @@ bool InputTest::finished() return true; /* only a single-shot test (for now) */ } -int InputTest::update(unsigned int timeout_ms, ControlData **control_data) +int InputTest::update(unsigned int timeout_ms, ControlData **control_data, bool already_active) { //we directly override the update() here, since we don't need the initialization from the base class diff --git a/src/drivers/vmount/input_test.h b/src/drivers/vmount/input_test.h index e6fba3a3d1..15e3addd4c 100644 --- a/src/drivers/vmount/input_test.h +++ b/src/drivers/vmount/input_test.h @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2016 PX4 Development Team. All rights reserved. +* Copyright (c) 2016-2017 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -61,10 +61,10 @@ public: /** check whether the test finished, and thus the main thread can quit */ bool finished(); - virtual int update(unsigned int timeout_ms, ControlData **control_data); + virtual int update(unsigned int timeout_ms, ControlData **control_data, bool already_active); protected: - virtual int update_impl(unsigned int timeout_ms, ControlData **control_data) { return 0; } //not needed + virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active) { return 0; } //not needed virtual int initialize(); diff --git a/src/drivers/vmount/vmount.cpp b/src/drivers/vmount/vmount.cpp index 302bea26e0..f08960698b 100644 --- a/src/drivers/vmount/vmount.cpp +++ b/src/drivers/vmount/vmount.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2017 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,9 +35,11 @@ * @file vmount.cpp * @author Leon Müller (thedevleon) * @author Beat Küng - * MAV_MOUNT driver for controlling mavlink gimbals, rc gimbals/servors and - * future kinds of mounts. + * @author Julian Oes * + * Driver for to control mounts such as gimbals or servos. + * Inputs for the mounts can RC and/or mavlink commands. + * Outputs to the mounts can be RC (PWM) output or mavlink. */ #include @@ -67,8 +69,11 @@ using namespace vmount; /* thread state */ static volatile bool thread_should_exit = false; static volatile bool thread_running = false; + +static constexpr unsigned input_objs_len = 3; + struct ThreadData { - InputBase *input_obj = nullptr; + InputBase *input_objs[input_objs_len] = {nullptr, nullptr, nullptr}; OutputBase *output_obj = nullptr; }; static volatile ThreadData *g_thread_data = nullptr; @@ -80,7 +85,6 @@ struct Parameters { int mnt_mav_compid; int mnt_ob_lock_mode; int mnt_ob_norm_mode; - int mnt_man_control; int mnt_man_pitch; int mnt_man_roll; int mnt_man_yaw; @@ -93,7 +97,6 @@ struct Parameters { mnt_mav_compid != p.mnt_mav_compid || mnt_ob_lock_mode != p.mnt_ob_lock_mode || mnt_ob_norm_mode != p.mnt_ob_norm_mode || - mnt_man_control != p.mnt_man_control || mnt_man_pitch != p.mnt_man_pitch || mnt_man_roll != p.mnt_man_roll || mnt_man_yaw != p.mnt_man_yaw; @@ -107,7 +110,6 @@ struct ParameterHandles { param_t mnt_mav_compid; param_t mnt_ob_lock_mode; param_t mnt_ob_norm_mode; - param_t mnt_man_control; param_t mnt_man_pitch; param_t mnt_man_roll; param_t mnt_man_yaw; @@ -193,49 +195,77 @@ static int vmount_thread_main(int argc, char *argv[]) int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); thread_running = true; ControlData *control_data = nullptr; - InputRC *manual_input = nullptr; g_thread_data = &thread_data; + int last_active = -1; + while (!thread_should_exit) { - if (!thread_data.input_obj && (params.mnt_mode_in != 0 || test_input)) { //need to initialize + if (!thread_data.input_objs[0] && (params.mnt_mode_in != 0 || test_input)) { //need to initialize output_config.gimbal_normal_mode_value = params.mnt_ob_norm_mode; output_config.gimbal_retracted_mode_value = params.mnt_ob_lock_mode; output_config.mavlink_sys_id = params.mnt_mav_sysid; output_config.mavlink_comp_id = params.mnt_mav_compid; + bool alloc_failed = false; + if (test_input) { - thread_data.input_obj = test_input; + thread_data.input_objs[0] = test_input; } else { - if (params.mnt_man_control) { - manual_input = new InputRC(params.mnt_man_roll, params.mnt_man_pitch, params.mnt_man_yaw); - - if (!manual_input) { - PX4_ERR("memory allocation failed"); - break; - } - } - switch (params.mnt_mode_in) { - case 1: //RC - if (manual_input) { - thread_data.input_obj = manual_input; - manual_input = nullptr; + case 0: - } else { - thread_data.input_obj = new InputRC(params.mnt_man_roll, params.mnt_man_pitch, params.mnt_man_yaw); + // Automatic + if (!thread_data.input_objs[0]) { + thread_data.input_objs[0] = new InputMavlinkCmdMount(); + + if (!thread_data.input_objs[0]) { alloc_failed = true; } + } + + if (!thread_data.input_objs[1]) { + thread_data.input_objs[1] = new InputMavlinkROI(); + + if (!thread_data.input_objs[1]) { alloc_failed = true; } + } + + // RC is on purpose last here so that if there are any mavlink + // messages, they will take precedence over RC. + // This logic is done further below while update() is called. + if (!thread_data.input_objs[2]) { + thread_data.input_objs[0] = new InputRC(params.mnt_man_roll, params.mnt_man_pitch, params.mnt_man_yaw); + + if (!thread_data.input_objs[2]) { alloc_failed = true; } + } + + break; + + case 1: //RC + if (!thread_data.input_objs[0]) { + thread_data.input_objs[0] = new InputRC(params.mnt_man_roll, params.mnt_man_pitch, params.mnt_man_yaw); + + if (!thread_data.input_objs[0]) { alloc_failed = true; } } break; case 2: //MAVLINK_ROI - thread_data.input_obj = new InputMavlinkROI(manual_input); + if (!thread_data.input_objs[0]) { + thread_data.input_objs[0] = new InputMavlinkROI(); + + if (!thread_data.input_objs[0]) { alloc_failed = true; } + } + break; case 3: //MAVLINK_DO_MOUNT - thread_data.input_obj = new InputMavlinkCmdMount(manual_input); + if (!thread_data.input_objs[0]) { + thread_data.input_objs[0] = new InputMavlinkCmdMount(); + + if (!thread_data.input_objs[0]) { alloc_failed = true; } + } + break; default: @@ -247,10 +277,16 @@ static int vmount_thread_main(int argc, char *argv[]) switch (params.mnt_mode_out) { case 0: //AUX thread_data.output_obj = new OutputRC(output_config); + + if (!thread_data.output_obj) { alloc_failed = true; } + break; case 1: //MAVLINK thread_data.output_obj = new OutputMavlink(output_config); + + if (!thread_data.output_obj) { alloc_failed = true; } + break; default: @@ -258,7 +294,7 @@ static int vmount_thread_main(int argc, char *argv[]) break; } - if (!thread_data.input_obj || !thread_data.output_obj) { + if (alloc_failed) { PX4_ERR("memory allocation failed"); thread_should_exit = true; break; @@ -273,19 +309,31 @@ static int vmount_thread_main(int argc, char *argv[]) } } - if (thread_data.input_obj) { + if (thread_data.input_objs[0]) { //get input: we cannot make the timeout too large, because the output needs to update //periodically for stabilization and angle updates. - int ret = thread_data.input_obj->update(50, &control_data); - if (ret) { - PX4_ERR("failed to read input (%i)", ret); - break; + for (int i = 0; i < input_objs_len; ++i) { + + bool already_active = (last_active == i); + + ControlData *control_data_to_check = nullptr; + int ret = thread_data.input_objs[i]->update(50, &control_data, already_active); + + if (ret) { + PX4_ERR("failed to read input %i (ret: %i)", i, ret); + continue; + } + + if (control_data_to_check != nullptr) { + control_data = control_data_to_check; + last_active = i; + } } //update output - ret = thread_data.output_obj->update(control_data); + int ret = thread_data.output_obj->update(control_data); if (ret) { PX4_ERR("failed to write output (%i)", ret); @@ -315,20 +363,19 @@ static int vmount_thread_main(int argc, char *argv[]) if (updated) { //re-init objects - if (thread_data.input_obj) { - delete (thread_data.input_obj); - thread_data.input_obj = nullptr; + for (int i = 0; i < input_objs_len; ++i) { + if (thread_data.input_objs[i]) { + delete (thread_data.input_objs[i]); + thread_data.input_objs[i] = nullptr; + } } + last_active = -1; + if (thread_data.output_obj) { delete (thread_data.output_obj); thread_data.output_obj = nullptr; } - - if (manual_input) { - delete (manual_input); - manual_input = nullptr; - } } } } @@ -337,9 +384,11 @@ static int vmount_thread_main(int argc, char *argv[]) orb_unsubscribe(parameter_update_sub); - if (thread_data.input_obj) { - delete (thread_data.input_obj); - thread_data.input_obj = nullptr; + for (int i = 0; i < input_objs_len; ++i) { + if (thread_data.input_objs[i]) { + delete (thread_data.input_objs[i]); + thread_data.input_objs[i] = nullptr; + } } if (thread_data.output_obj) { @@ -347,11 +396,6 @@ static int vmount_thread_main(int argc, char *argv[]) thread_data.output_obj = nullptr; } - if (manual_input) { - delete (manual_input); - manual_input = nullptr; - } - thread_running = false; return 0; } @@ -422,10 +466,16 @@ int vmount_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running && g_thread_data) { - if (g_thread_data->input_obj) { - g_thread_data->input_obj->print_status(); + bool found_input = false; - } else { + for (int i = 0; i < input_objs_len; ++i) { + if (g_thread_data->input_objs[i]) { + g_thread_data->input_objs[i]->print_status(); + found_input = true; + } + } + + if (!found_input) { PX4_INFO("Input: None"); } @@ -457,7 +507,6 @@ void update_params(ParameterHandles ¶m_handles, Parameters ¶ms, bool &go param_get(param_handles.mnt_mav_compid, ¶ms.mnt_mav_compid); param_get(param_handles.mnt_ob_lock_mode, ¶ms.mnt_ob_lock_mode); param_get(param_handles.mnt_ob_norm_mode, ¶ms.mnt_ob_norm_mode); - param_get(param_handles.mnt_man_control, ¶ms.mnt_man_control); param_get(param_handles.mnt_man_pitch, ¶ms.mnt_man_pitch); param_get(param_handles.mnt_man_roll, ¶ms.mnt_man_roll); param_get(param_handles.mnt_man_yaw, ¶ms.mnt_man_yaw); @@ -473,7 +522,6 @@ bool get_params(ParameterHandles ¶m_handles, Parameters ¶ms) param_handles.mnt_mav_compid = param_find("MNT_MAV_COMPID"); param_handles.mnt_ob_lock_mode = param_find("MNT_OB_LOCK_MODE"); param_handles.mnt_ob_norm_mode = param_find("MNT_OB_NORM_MODE"); - param_handles.mnt_man_control = param_find("MNT_MAN_CONTROL"); param_handles.mnt_man_pitch = param_find("MNT_MAN_PITCH"); param_handles.mnt_man_roll = param_find("MNT_MAN_ROLL"); param_handles.mnt_man_yaw = param_find("MNT_MAN_YAW"); @@ -484,7 +532,6 @@ bool get_params(ParameterHandles ¶m_handles, Parameters ¶ms) param_handles.mnt_mav_compid == PARAM_INVALID || param_handles.mnt_ob_lock_mode == PARAM_INVALID || param_handles.mnt_ob_norm_mode == PARAM_INVALID || - param_handles.mnt_man_control == PARAM_INVALID || param_handles.mnt_man_pitch == PARAM_INVALID || param_handles.mnt_man_roll == PARAM_INVALID || param_handles.mnt_man_yaw == PARAM_INVALID) { diff --git a/src/drivers/vmount/vmount_params.c b/src/drivers/vmount/vmount_params.c index 6cbb60e6cd..ef6ea9d607 100644 --- a/src/drivers/vmount/vmount_params.c +++ b/src/drivers/vmount/vmount_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2013-2016 PX4 Development Team. All rights reserved. +* Copyright (c) 2013-2017 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -42,7 +42,7 @@ * RC uses the AUX input channels (see MNT_MAN_* parameters), * MAVLINK_ROI uses the MAV_CMD_DO_SET_ROI Mavlink message, and MAVLINK_DO_MOUNT the * MAV_CMD_DO_MOUNT_CONFIGURE and MAV_CMD_DO_MOUNT_CONTROL messages to control a mount. -* @value 0 DISABLE +* @value 0 AUTO * @value 1 RC * @value 2 MAVLINK_ROI * @value 3 MAVLINK_DO_MOUNT @@ -99,18 +99,6 @@ PARAM_DEFINE_FLOAT(MNT_OB_NORM_MODE, -1.0f); */ PARAM_DEFINE_FLOAT(MNT_OB_LOCK_MODE, 0.0f); - -/** -* This enables the mount to be manually controlled when no ROI is set. -* -* If set to 1, the mount will be controlled by the AUX channels below -* when no ROI is set. -* -* @boolean -* @group Mount -*/ -PARAM_DEFINE_INT32(MNT_MAN_CONTROL, 0); - /** * Auxiliary channel to control roll (in AUX input or manual mode). *