diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 5f7557e664..86a05ab4f5 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -52,10 +52,6 @@ #include #include -#include -#include - - /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -74,8 +70,6 @@ bool MavlinkMissionManager::_transfer_in_progress = false; (_msg.target_component == MAV_COMP_ID_ALL))) -int chan_id; - MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink), _state(MAVLINK_WPM_STATE_IDLE), _time_last_recv(0), @@ -94,8 +88,6 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(m _mission_result_sub(-1), _offboard_mission_pub(nullptr), _slow_rate_limiter(_interval / 10.0f), - _actuators{}, - _actuator_pub(nullptr), _verbose(false) { _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); @@ -123,34 +115,6 @@ MavlinkMissionManager::get_size() } } - -/** - * Set the actuators - */ - -int -MavlinkMissionManager::actuators_publish() -{ - _actuators.timestamp = hrt_absolute_time(); - - // lazily publish _actuators only once available - if (_actuator_pub == nullptr) { - _mavlink->send_statustext_critical("Actuators published 1!"); - return orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &_actuators); - - } else { - _actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators); - if (_actuator_pub == nullptr) { - _mavlink->send_statustext_critical("Actuators published 2!"); - return OK; - - } else { - return -1; - } - } -} - - void MavlinkMissionManager::init_offboard_mission() { @@ -833,29 +797,11 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * break; case MAV_CMD_DO_SET_SERVO: + mission_item->actuator_num = mavlink_mission_item->param1; mission_item->actuator_value = mavlink_mission_item->param2; mission_item->autocontinue = true; mission_item->time_inside=0.0f; - - chan_id = mavlink_mission_item->param1; - - - _actuators.control[0] = mavlink_mission_item->param2; - _actuators.control[1] = mavlink_mission_item->param2; - _actuators.control[2] = mavlink_mission_item->param2; - _actuators.control[3] = mavlink_mission_item->param2; - _actuators.control[4] = mavlink_mission_item->param2; - _actuators.control[5] = mavlink_mission_item->param2; - _actuators.control[6] = mavlink_mission_item->param2; - _actuators.control[7] = mavlink_mission_item->param2; - - actuators_publish(); - - /* for test purpose of function during mission upload */ - //up_pwm_servo_arm(true); - //up_pwm_servo_set(mission_item->actuator_num-1, mission_item->actuator_value); - break; default: diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index f1792020d9..54af7dc466 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -48,13 +48,6 @@ #include "mavlink_rate_limiter.h" #include "mavlink_stream.h" -#include -#include -#include -#include -#include - - enum MAVLINK_WPM_STATES { MAVLINK_WPM_STATE_IDLE = 0, MAVLINK_WPM_STATE_SENDLIST, @@ -137,9 +130,6 @@ private: MavlinkRateLimiter _slow_rate_limiter; - actuator_controls_s _actuators; - orb_advert_t _actuator_pub; - bool _verbose; /* do not allow top copying this class */ @@ -205,12 +195,6 @@ private: */ int format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); - /** - * publish actuators state - */ - int actuators_publish(); - - protected: explicit MavlinkMissionManager(Mavlink *mavlink); diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 2ea28345d5..61cec989d6 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -60,7 +60,9 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) : _mission_item({0}), _waypoint_position_reached(false), _waypoint_yaw_reached(false), - _time_first_inside_orbit(0) + _time_first_inside_orbit(0), + _actuators{}, + _actuator_pub(nullptr) { } @@ -72,9 +74,11 @@ bool MissionBlock::is_mission_item_reached() { if (_mission_item.nav_cmd == NAV_CMD_DO_SET_SERVO) { - //up_pwm_servo_arm(true); - //up_pwm_servo_set(_mission_item.actuator_num-1, _mission_item.actuator_value); - warnx("Set servo cmd executed"); + + _actuators.control[_mission_item.actuator_num] = 1.0f / 2000 * -_mission_item.actuator_value; + _actuators.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &_actuators); + warnx("Set servo cmd executed"); return true; } diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 4e6e99acb0..c850a0313c 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -46,6 +46,7 @@ #include #include #include +#include #include "navigator_mode.h" @@ -97,6 +98,10 @@ protected: bool _waypoint_position_reached; bool _waypoint_yaw_reached; hrt_abstime _time_first_inside_orbit; + + actuator_controls_s _actuators; + orb_advert_t _actuator_pub; + }; #endif diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 532b52475c..8d9cca71c9 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -303,9 +303,25 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI for (unsigned i = 0; i < nMissionItems; i++) { if (dm_read(dm_current, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) { + /* Check non navigation item */ + if (mission_item.nav_cmd == NAV_CMD_DO_SET_SERVO){ + //check actuator number + if (mission_item.actuator_num<1 || mission_item.actuator_num>6) { + mavlink_log_critical(_mavlink_fd, "Actuator number %d is out of bounds 1..6", (int)mission_item.actuator_num); + warning_issued = true; + return false; + } + //check actuator value + if (mission_item.actuator_value<900 || mission_item.actuator_value>2000) { + mavlink_log_critical(_mavlink_fd, "Actuator number %d is out of bounds 900..2000", (int)mission_item.actuator_value); + warning_issued = true; + return false; + } + + } /* check only items with valid lat/lon */ - if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT || + else if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT || mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 769710f27d..7363e7d968 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -99,9 +99,8 @@ struct mission_item_s { int do_jump_mission_index; /**< index where the do jump will go to */ unsigned do_jump_repeat_count; /**< how many times do jump needs to be done */ unsigned do_jump_current_count; /**< count how many times the jump has been done */ - int actuator_num; /**< actuator number to be set 1..6 */ - int actuator_value; /**< new value for selected actuator 900...2000 */ - + int actuator_num; /**< actuator number to be set 0..5 ( corresponds to AUX outputs 1..6 */ + int actuator_value; /**< new value for selected actuator in ms 900...2000 */ }; /**