mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-04 11:30:35 +08:00
Implementation of 183 mission intem
This commit is contained in:
fpvaspassion
committed by
Lorenz Meier
Lorenz Meier
parent
384047787a
commit
e6ffa16125
@@ -52,6 +52,10 @@
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/mission_result.h>
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
|
||||
|
||||
/* 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;
|
||||
|
||||
@@ -48,6 +48,13 @@
|
||||
#include "mavlink_rate_limiter.h"
|
||||
#include "mavlink_stream.h"
|
||||
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_controls_0.h>
|
||||
#include <uORB/topics/actuator_controls_1.h>
|
||||
#include <uORB/topics/actuator_controls_2.h>
|
||||
#include <uORB/topics/actuator_controls_3.h>
|
||||
|
||||
|
||||
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);
|
||||
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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 */
|
||||
|
||||
};
|
||||
|
||||
/**
|
||||
|
||||
Reference in New Issue
Block a user