From e6ffa16125d8b0bdc6b85fc5fc5e6b73acc2731a Mon Sep 17 00:00:00 2001 From: fpvaspassion Date: Fri, 7 Aug 2015 19:16:12 +0300 Subject: [PATCH] Implementation of 183 mission intem --- src/modules/mavlink/mavlink_mission.cpp | 70 +++++++++++++++++++ src/modules/mavlink/mavlink_mission.h | 16 +++++ src/modules/navigator/mission_block.cpp | 7 ++ .../navigator/mission_feasibility_checker.cpp | 3 +- src/modules/uORB/topics/mission.h | 8 ++- 5 files changed, 102 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 0f15cdf364..5f7557e664 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -52,6 +52,10 @@ #include #include +#include +#include + + /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -69,6 +73,9 @@ bool MavlinkMissionManager::_transfer_in_progress = false; (_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \ (_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), @@ -87,6 +94,8 @@ 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)); @@ -114,6 +123,34 @@ 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() { @@ -788,11 +825,39 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * case MAV_CMD_NAV_TAKEOFF: mission_item->pitch_min = mavlink_mission_item->param1; break; + case MAV_CMD_DO_JUMP: mission_item->do_jump_mission_index = mavlink_mission_item->param1; mission_item->do_jump_current_count = 0; mission_item->do_jump_repeat_count = mavlink_mission_item->param2; 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: mission_item->acceptance_radius = mavlink_mission_item->param2; mission_item->time_inside = mavlink_mission_item->param1; @@ -830,6 +895,11 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * mavlink_mission_item->param1 = mission_item->pitch_min; break; + case NAV_CMD_DO_SET_SERVO: + mavlink_mission_item->param1 = mission_item->actuator_num; + mavlink_mission_item->param2 = mission_item->actuator_value; + break; + case NAV_CMD_DO_JUMP: mavlink_mission_item->param1 = mission_item->do_jump_mission_index; mavlink_mission_item->param2 = mission_item->do_jump_repeat_count - mission_item->do_jump_current_count; diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index 54af7dc466..f1792020d9 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -48,6 +48,13 @@ #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, @@ -130,6 +137,9 @@ private: MavlinkRateLimiter _slow_rate_limiter; + actuator_controls_s _actuators; + orb_advert_t _actuator_pub; + bool _verbose; /* do not allow top copying this class */ @@ -195,6 +205,12 @@ 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 a4151ae6ad..2ea28345d5 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -71,6 +71,13 @@ MissionBlock::~MissionBlock() 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"); + return true; + } + if (_mission_item.nav_cmd == NAV_CMD_IDLE) { return false; } diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index c57a12aefb..532b52475c 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -209,7 +209,8 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s missionitem.nav_cmd != NAV_CMD_TAKEOFF && missionitem.nav_cmd != NAV_CMD_ROI && missionitem.nav_cmd != NAV_CMD_PATHPLANNING && - missionitem.nav_cmd != NAV_CMD_DO_JUMP) { + missionitem.nav_cmd != NAV_CMD_DO_JUMP && + missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO) { mavlink_log_critical(_mavlink_fd, "Rejecting mission item %i: unsupported action.", (int)(i+1)); return false; diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 226ea2e980..769710f27d 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -60,7 +60,10 @@ enum NAV_CMD { NAV_CMD_TAKEOFF = 22, NAV_CMD_ROI = 80, NAV_CMD_PATHPLANNING = 81, - NAV_CMD_DO_JUMP = 177 + NAV_CMD_DO_JUMP = 177, + NAV_CMD_DO_SET_SERVO=183, + NAV_CMD_DO_REPEAT_SERVO=184 + }; enum ORIGIN { @@ -96,6 +99,9 @@ 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 */ + }; /**