Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 06:14:14 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

ROI - move handling to navigator (#7939)

Browse Source
This commit is contained in:
Daniel Agar
2017-09-26 12:25:02 -04:00
committed by GitHub
parent e72769c924
commit 3c18be387c
24 changed files with 301 additions and 291 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/vehicle_command.msg
+20 -20
View File
@@ -52,7 +52,7 @@ uint16 VEHICLE_CMD_DO_GUIDED_MASTER=221 # set id of master controller |System
uint16 VEHICLE_CMD_DO_GUIDED_LIMITS=222 # set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty|
uint16 VEHICLE_CMD_DO_LAST = 240 # NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|
uint16 VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241 # Trigger calibration. This command will be only accepted if in pre-flight mode. See mavlink spec MAV_CMD_PREFLIGHT_CALIBRATION
uint32 PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION = 3 # param value for VEHICLE_CMD_PREFLIGHT_CALIBRATION to start temperature calibration
uint16 PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION = 3 # param value for VEHICLE_CMD_PREFLIGHT_CALIBRATION to start temperature calibration
uint16 VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS = 242 # Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units|
uint16 VEHICLE_CMD_PREFLIGHT_STORAGE = 245 # Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty|
uint16 VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246 # Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty|
@@ -69,27 +69,27 @@ uint16 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243 # UAVCAN configuration. If param 1 ==
uint16 VEHICLE_CMD_LOGGING_START = 2510 # start streaming ULog data
uint16 VEHICLE_CMD_LOGGING_STOP = 2511 # stop streaming ULog data
uint16 VEHICLE_CMD_RESULT_ACCEPTED = 0 # Command ACCEPTED and EXECUTED |
uint16 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1 # Command TEMPORARY REJECTED/DENIED |
uint16 VEHICLE_CMD_RESULT_DENIED = 2 # Command PERMANENTLY DENIED |
uint16 VEHICLE_CMD_RESULT_UNSUPPORTED = 3 # Command UNKNOWN/UNSUPPORTED |
uint16 VEHICLE_CMD_RESULT_FAILED = 4 # Command executed, but failed |
uint16 VEHICLE_CMD_RESULT_IN_PROGRESS = 5 # Command being executed |
uint16 VEHICLE_CMD_RESULT_ENUM_END = 6 #
uint8 VEHICLE_CMD_RESULT_ACCEPTED = 0 # Command ACCEPTED and EXECUTED |
uint8 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1 # Command TEMPORARY REJECTED/DENIED |
uint8 VEHICLE_CMD_RESULT_DENIED = 2 # Command PERMANENTLY DENIED |
uint8 VEHICLE_CMD_RESULT_UNSUPPORTED = 3 # Command UNKNOWN/UNSUPPORTED |
uint8 VEHICLE_CMD_RESULT_FAILED = 4 # Command executed, but failed |
uint8 VEHICLE_CMD_RESULT_IN_PROGRESS = 5 # Command being executed |
uint8 VEHICLE_CMD_RESULT_ENUM_END = 6 #
uint16 VEHICLE_MOUNT_MODE_RETRACT = 0 # Load and keep safe position (Roll,Pitch,Yaw) from permanent memory and stop stabilization |
uint16 VEHICLE_MOUNT_MODE_NEUTRAL = 1 # Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. |
uint16 VEHICLE_MOUNT_MODE_MAVLINK_TARGETING = 2 # Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization |
uint16 VEHICLE_MOUNT_MODE_RC_TARGETING = 3 # Load neutral position and start RC Roll,Pitch,Yaw control with stabilization |
uint16 VEHICLE_MOUNT_MODE_GPS_POINT = 4 # Load neutral position and start to point to Lat,Lon,Alt |
uint16 VEHICLE_MOUNT_MODE_ENUM_END = 5 #
uint8 VEHICLE_MOUNT_MODE_RETRACT = 0 # Load and keep safe position (Roll,Pitch,Yaw) from permanent memory and stop stabilization |
uint8 VEHICLE_MOUNT_MODE_NEUTRAL = 1 # Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. |
uint8 VEHICLE_MOUNT_MODE_MAVLINK_TARGETING = 2 # Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization |
uint8 VEHICLE_MOUNT_MODE_RC_TARGETING = 3 # Load neutral position and start RC Roll,Pitch,Yaw control with stabilization |
uint8 VEHICLE_MOUNT_MODE_GPS_POINT = 4 # Load neutral position and start to point to Lat,Lon,Alt |
uint8 VEHICLE_MOUNT_MODE_ENUM_END = 5 #
uint16 VEHICLE_ROI_NONE = 0 # No region of interest |
uint16 VEHICLE_ROI_WPNEXT = 1 # Point toward next MISSION |
uint16 VEHICLE_ROI_WPINDEX = 2 # Point toward given MISSION |
uint16 VEHICLE_ROI_LOCATION = 3 # Point toward fixed location |
uint16 VEHICLE_ROI_TARGET = 4 # Point toward target
uint16 VEHICLE_ROI_ENUM_END = 5
uint8 VEHICLE_ROI_NONE = 0 # No region of interest |
uint8 VEHICLE_ROI_WPNEXT = 1 # Point toward next MISSION |
uint8 VEHICLE_ROI_WPINDEX = 2 # Point toward given MISSION |
uint8 VEHICLE_ROI_LOCATION = 3 # Point toward fixed location |
uint8 VEHICLE_ROI_TARGET = 4 # Point toward target
uint8 VEHICLE_ROI_ENUM_END = 5
uint8 ORB_QUEUE_LENGTH = 3
msg/vehicle_roi.msg
+8 -6
View File
@@ -1,15 +1,17 @@
# Vehicle Region Of Interest (ROI)
uint8 VEHICLE_ROI_NONE = 0 # No region of interest |
uint8 VEHICLE_ROI_WPNEXT = 1 # Point toward next MISSION |
uint8 VEHICLE_ROI_WPINDEX = 2 # Point toward given MISSION |
uint8 VEHICLE_ROI_LOCATION = 3 # Point toward fixed location |
uint8 VEHICLE_ROI_TARGET = 4 # Point toward target
uint8 VEHICLE_ROI_ENUM_END = 5
uint8 ROI_NONE = 0 # No region of interest
uint8 ROI_WPNEXT = 1 # Point toward next MISSION
uint8 ROI_WPINDEX = 2 # Point toward given MISSION
uint8 ROI_LOCATION = 3 # Point toward fixed location
uint8 ROI_TARGET = 4 # Point toward target
uint8 ROI_ENUM_END = 5
uint8 mode # ROI mode (see above)
uint32 mission_seq # mission sequence to point to
uint32 target_seq # target sequence to point to
float64 lat # Latitude to point to
float64 lon # Longitude to point to
float32 alt # Altitude to point to
msg/vehicle_status.msg
+2 -2
View File
@@ -48,8 +48,8 @@ uint8 hil_state # current hil state
bool failsafe # true if system is in failsafe state
uint8 system_type # system type, contains mavlink MAV_TYPE
uint32 system_id # system id, contains MAVLink's system ID field
uint32 component_id # subsystem / component id, contains MAVLink's component ID field
uint8 system_id # system id, contains MAVLink's system ID field
uint8 component_id # subsystem / component id, contains MAVLink's component ID field
bool is_rotary_wing # True if system is in rotary wing configuration, so for a VTOL this is only true while flying as a multicopter
bool is_vtol # True if the system is VTOL capable
src/drivers/vmount/input_mavlink.cpp
+6 -12
View File
@@ -49,15 +49,9 @@
#include <px4_posix.h>
#include <errno.h>
namespace vmount
{
InputMavlinkROI::InputMavlinkROI()
{
}
InputMavlinkROI::~InputMavlinkROI()
{
if (_vehicle_roi_sub >= 0) {
@@ -116,27 +110,27 @@ int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_
_control_data.gimbal_shutter_retract = false;
if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_NONE) {
if (vehicle_roi.mode == vehicle_roi_s::ROI_NONE) {
_control_data.type = ControlData::Type::Neutral;
*control_data = &_control_data;
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPNEXT) {
} else if (vehicle_roi.mode == vehicle_roi_s::ROI_WPNEXT) {
_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;
*control_data = &_control_data;
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPINDEX) {
} else if (vehicle_roi.mode == vehicle_roi_s::ROI_WPINDEX) {
//TODO how to do this?
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_LOCATION) {
} else if (vehicle_roi.mode == vehicle_roi_s::ROI_LOCATION) {
control_data_set_lon_lat(vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt);
*control_data = &_control_data;
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_TARGET) {
} else if (vehicle_roi.mode == vehicle_roi_s::ROI_TARGET) {
//TODO is this even suported?
}
@@ -150,7 +144,7 @@ int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_
// check whether the position setpoint got updated
if (polls[1].revents & POLLIN) {
if (_cur_roi_mode == vehicle_roi_s::VEHICLE_ROI_WPNEXT) {
if (_cur_roi_mode == vehicle_roi_s::ROI_WPNEXT) {
_read_control_data_from_position_setpoint_sub();
*control_data = &_control_data;
src/drivers/vmount/input_mavlink.h
+2 -2
View File
@@ -57,7 +57,7 @@ namespace vmount
class InputMavlinkROI : public InputBase
{
public:
InputMavlinkROI();
InputMavlinkROI() = default;
virtual ~InputMavlinkROI();
virtual void print_status();
@@ -71,7 +71,7 @@ private:
int _vehicle_roi_sub = -1;
int _position_setpoint_triplet_sub = -1;
uint8_t _cur_roi_mode = vehicle_roi_s::VEHICLE_ROI_NONE;
uint8_t _cur_roi_mode = vehicle_roi_s::ROI_NONE;
};
src/modules/commander/arm_auth.cpp
+2 -2
View File
@@ -66,7 +66,7 @@ struct packed_struct {
uint8_t authentication_method;
} arm_parameters;
static uint32_t *system_id;
static uint8_t *system_id;
static uint8_t _auth_method_arm_req_check();
static uint8_t _auth_method_two_arm_check();
@@ -260,7 +260,7 @@ void arm_auth_update(hrt_abstime now)
}
}
void arm_auth_init(orb_advert_t *mav_log_pub, uint32_t *sys_id)
void arm_auth_init(orb_advert_t *mav_log_pub, uint8_t *sys_id)
{
system_id = sys_id;
param_arm_parameters = param_find("COM_ARM_AUTH");
src/modules/commander/arm_auth.h
+1 -1
View File
@@ -42,7 +42,7 @@ enum arm_auth_methods {
ARM_AUTH_METHOD_LAST
};
void arm_auth_init(orb_advert_t *mav_log_pub, uint32_t *system_id);
void arm_auth_init(orb_advert_t *mav_log_pub, uint8_t *system_id);
void arm_auth_update(hrt_abstime now);
uint8_t arm_auth_check();
enum arm_auth_methods arm_auth_method_get();
src/modules/commander/commander.cpp
+19 -51
View File
@@ -106,7 +106,6 @@
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/power_button_state.h>
#include <uORB/topics/vehicle_roi.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/sensor_combined.h>
@@ -225,7 +224,6 @@ static float max_imu_gyr_diff = 0.09f;
static float min_stick_change = 0.25f;
static struct vehicle_status_s status = {};
static struct vehicle_roi_s _roi = {};
static struct battery_status_s battery = {};
static struct actuator_armed_s armed = {};
static struct safety_s safety = {};
@@ -286,8 +284,7 @@ void usage(const char *reason);
bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd,
struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos,
struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub,
orb_advert_t *command_ack_pub, struct vehicle_roi_s *roi,
orb_advert_t *roi_pub, bool *changed);
orb_advert_t *command_ack_pub, bool *changed);
/**
* Mainloop of commander.
@@ -514,8 +511,8 @@ int commander_main(int argc, char *argv[])
.param4 = NAN,
.param7 = NAN,
.command = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF,
.target_system = (uint8_t)status.system_id,
.target_component = (uint8_t)status.component_id
.target_system = status.system_id,
.target_component = status.component_id
};
orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
@@ -545,8 +542,8 @@ int commander_main(int argc, char *argv[])
.param4 = NAN,
.param7 = NAN,
.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND,
.target_system = (uint8_t)status.system_id,
.target_component = (uint8_t)status.component_id
.target_system = status.system_id,
.target_component = status.component_id
};
orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
@@ -568,8 +565,8 @@ int commander_main(int argc, char *argv[])
.param4 = NAN,
.param7 = NAN,
.command = vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION,
.target_system = (uint8_t)status.system_id,
.target_component = (uint8_t)status.component_id
.target_system = status.system_id,
.target_component = status.component_id
};
orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
@@ -637,8 +634,8 @@ int commander_main(int argc, char *argv[])
.param4 = 0.0f,
.param7 = 0.0f,
.command = vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION,
.target_system = (uint8_t)status.system_id,
.target_component = (uint8_t)status.component_id
.target_system = status.system_id,
.target_component = status.component_id
};
orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
@@ -764,8 +761,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local,
struct home_position_s *home, struct vehicle_global_position_s *global_pos,
struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub,
orb_advert_t *command_ack_pub, struct vehicle_roi_s *roi,
orb_advert_t *roi_pub, bool *changed)
orb_advert_t *command_ack_pub, bool *changed)
{
/* only handle commands that are meant to be handled by this system and component */
if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id)
@@ -1159,34 +1155,6 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
}
break;
case vehicle_command_s::VEHICLE_CMD_NAV_ROI:
case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI: {
roi->mode = cmd->param1;
if (roi->mode == vehicle_roi_s::VEHICLE_ROI_WPINDEX) {
roi->mission_seq = cmd->param2;
}
else if (roi->mode == vehicle_roi_s::VEHICLE_ROI_LOCATION) {
roi->lat = cmd->param5;
roi->lon = cmd->param6;
roi->alt = cmd->param7;
}
else if (roi->mode == vehicle_roi_s::VEHICLE_ROI_TARGET) {
roi->target_seq = cmd->param2;
}
if (*roi_pub != nullptr) {
orb_publish(ORB_ID(vehicle_roi), *roi_pub, roi);
} else {
*roi_pub = orb_advertise(ORB_ID(vehicle_roi), roi);
}
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
break;
}
case vehicle_command_s::VEHICLE_CMD_MISSION_START: {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
@@ -1529,10 +1497,6 @@ int commander_thread_main(int argc, char *argv[])
orb_advert_t home_pub = nullptr;
memset(&_home, 0, sizeof(_home));
/* region of interest */
orb_advert_t roi_pub = nullptr;
memset(&_roi, 0, sizeof(_roi));
/* command ack */
orb_advert_t command_ack_pub = nullptr;
@@ -1818,13 +1782,12 @@ int commander_thread_main(int argc, char *argv[])
pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, nullptr);
pthread_attr_destroy(&commander_low_prio_attr);
arm_auth_init(&mavlink_log_pub, &(status.system_id));
arm_auth_init(&mavlink_log_pub, &status.system_id);
while (!thread_should_exit) {
arming_ret = TRANSITION_NOT_CHANGED;
/* update parameters */
orb_check(param_changed_sub, &updated);
@@ -1852,8 +1815,13 @@ int commander_thread_main(int argc, char *argv[])
status.is_vtol = is_vtol(&status);
/* check and update system / component ID */
param_get(_param_system_id, &(status.system_id));
param_get(_param_component_id, &(status.component_id));
int32_t sys_id = 0;
param_get(_param_system_id, &sys_id);
status.system_id = sys_id;
int32_t comp_id = 0;
param_get(_param_component_id, &comp_id);
status.component_id = comp_id;
get_circuit_breaker_params();
@@ -3008,7 +2976,7 @@ int commander_thread_main(int argc, char *argv[])
/* handle it */
if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position,
&attitude, &home_pub, &command_ack_pub, &_roi, &roi_pub, &status_changed)) {
&attitude, &home_pub, &command_ack_pub, &status_changed)) {
status_changed = true;
}
}
src/modules/mavlink/mavlink_mission.cpp
+29 -3
View File
@@ -1333,6 +1333,22 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
mission_item->loiter_exit_xtrack = (mavlink_mission_item->param4 > 0);
break;
case MAV_CMD_NAV_ROI:
case MAV_CMD_DO_SET_ROI:
if ((int)mavlink_mission_item->param1 == MAV_ROI_LOCATION) {
mission_item->nav_cmd = NAV_CMD_DO_SET_ROI;
mission_item->params[0] = MAV_ROI_LOCATION;
mission_item->params[4] = mavlink_mission_item->x;
mission_item->params[5] = mavlink_mission_item->y;
mission_item->params[6] = mavlink_mission_item->z;
} else {
return MAV_MISSION_INVALID_PARAM1;
}
break;
case MAV_CMD_NAV_VTOL_TAKEOFF:
case MAV_CMD_NAV_VTOL_LAND:
mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
@@ -1391,6 +1407,19 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
mission_item->do_jump_repeat_count = mavlink_mission_item->param2;
break;
case MAV_CMD_NAV_ROI:
case MAV_CMD_DO_SET_ROI: {
const int roi_mode = mavlink_mission_item->param1;
if (roi_mode == MAV_ROI_NONE || roi_mode == MAV_ROI_WPNEXT || roi_mode == MAV_ROI_WPINDEX) {
mission_item->nav_cmd = NAV_CMD_DO_SET_ROI;
} else {
return MAV_MISSION_INVALID_PARAM1;
}
}
break;
case MAV_CMD_DO_CHANGE_SPEED:
case MAV_CMD_DO_SET_SERVO:
case MAV_CMD_DO_LAND_START:
@@ -1402,8 +1431,6 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
case MAV_CMD_IMAGE_STOP_CAPTURE:
case MAV_CMD_VIDEO_START_CAPTURE:
case MAV_CMD_VIDEO_STOP_CAPTURE:
case NAV_CMD_DO_SET_ROI:
case NAV_CMD_ROI:
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
case MAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
case MAV_CMD_SET_CAMERA_MODE:
@@ -1478,7 +1505,6 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
case NAV_CMD_DO_MOUNT_CONFIGURE:
case NAV_CMD_DO_MOUNT_CONTROL:
case NAV_CMD_DO_SET_ROI:
case NAV_CMD_ROI:
case NAV_CMD_DO_SET_CAM_TRIGG_DIST:
case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
case NAV_CMD_SET_CAMERA_MODE:
src/modules/navigator/datalinkloss.cpp
+1 -1
View File
@@ -168,7 +168,7 @@ DataLinkLoss::set_dll_item()
reset_mission_item_reached();
/* convert mission item to current position setpoint and make it valid */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
_navigator->set_position_setpoint_triplet_updated();
src/modules/navigator/enginefailure.cpp
+1 -1
View File
@@ -126,7 +126,7 @@ EngineFailure::set_ef_item()
reset_mission_item_reached();
/* convert mission item to current position setpoint and make it valid */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
_navigator->set_position_setpoint_triplet_updated();
src/modules/navigator/follow_target.cpp
+1 -1
View File
@@ -361,7 +361,7 @@ void FollowTarget::update_position_sp(bool use_velocity, bool use_position, floa
pos_sp_triplet->previous.valid = use_position;
pos_sp_triplet->previous = pos_sp_triplet->current;
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET;
pos_sp_triplet->current.position_valid = use_position;
pos_sp_triplet->current.velocity_valid = use_velocity;
src/modules/navigator/land.cpp
+2 -2
View File
@@ -82,7 +82,7 @@ Land::on_activation()
/* convert mission item to current setpoint */
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->previous.valid = false;
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
_navigator->set_can_loiter_at_sp(false);
@@ -99,7 +99,7 @@ Land::on_active()
set_idle_item(&_mission_item);
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
_navigator->set_position_setpoint_triplet_updated();
}
}
src/modules/navigator/loiter.cpp
+1 -1
View File
@@ -129,7 +129,7 @@ Loiter::set_loiter_position()
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->current.velocity_valid = false;
pos_sp_triplet->previous.valid = false;
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);
src/modules/navigator/mission.cpp
+17 -20
View File
@@ -145,13 +145,12 @@ void
Mission::on_inactivation()
{
// Disable camera trigger
vehicle_command_s cmd{};
vehicle_command_s cmd = {};
cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL;
// Pause trigger
cmd.param1 = -1.0f;
cmd.param3 = 1.0f;
cmd.timestamp = hrt_absolute_time();
_navigator->publish_vehicle_cmd(cmd);
_navigator->publish_vehicle_cmd(&cmd);
}
void
@@ -160,13 +159,12 @@ Mission::on_activation()
set_mission_items();
// unpause triggering if it was paused
vehicle_command_s cmd{};
vehicle_command_s cmd = {};
cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL;
// unpause trigger
cmd.param1 = -1.0f;
cmd.param3 = 0.0f;
cmd.timestamp = hrt_absolute_time();
_navigator->publish_vehicle_cmd(cmd);
_navigator->publish_vehicle_cmd(&cmd);
}
void
@@ -510,7 +508,7 @@ Mission::set_mission_items()
/* update position setpoint triplet */
pos_sp_triplet->previous.valid = false;
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
/* reuse setpoint for LOITER only if it's not IDLE */
@@ -542,7 +540,7 @@ Mission::set_mission_items()
/*********************************** handle mission item *********************************************/
/* handle position mission items */
if (item_contains_position(&_mission_item)) {
if (item_contains_position(_mission_item)) {
/* force vtol land */
if (_mission_item.nav_cmd == NAV_CMD_LAND && _param_force_vtol.get() == 1
@@ -766,7 +764,7 @@ Mission::set_mission_items()
set_align_mission_item(&_mission_item, &mission_item_next_position);
/* set position setpoint to target during the transition */
mission_item_to_position_setpoint(&mission_item_next_position, &pos_sp_triplet->current);
mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->current);
}
/* yaw is aligned now */
@@ -808,10 +806,10 @@ Mission::set_mission_items()
/*********************************** set setpoints and check next *********************************************/
/* set current position setpoint from mission item (is protected against non-position items) */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
/* issue command if ready (will do nothing for position mission items) */
issue_command(&_mission_item);
issue_command(_mission_item);
/* set current work item type */
_work_item_type = new_work_item_type;
@@ -834,7 +832,7 @@ Mission::set_mission_items()
/* try to process next mission item */
if (has_next_position_item) {
/* got next mission item, update setpoint triplet */
mission_item_to_position_setpoint(&mission_item_next_position, &pos_sp_triplet->next);
mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->next);
} else {
/* next mission item is not available */
@@ -1025,10 +1023,10 @@ Mission::heading_sp_update()
point_to_latlon[1] = _navigator->get_home_position()->lon;
} else if (_param_yawmode.get() == MISSION_YAWMODE_TO_ROI
&& _navigator->get_vroi()->mode == vehicle_roi_s::VEHICLE_ROI_LOCATION) {
&& _navigator->get_vroi().mode == vehicle_roi_s::ROI_LOCATION) {
/* target location is ROI */
point_to_latlon[0] = _navigator->get_vroi()->lat;
point_to_latlon[1] = _navigator->get_vroi()->lon;
point_to_latlon[0] = _navigator->get_vroi().lat;
point_to_latlon[1] = _navigator->get_vroi().lon;
} else {
/* target location is next (current) waypoint */
@@ -1185,7 +1183,7 @@ Mission::do_abort_landing()
_mission_item.origin = ORIGIN_ONBOARD;
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
_navigator->set_position_setpoint_triplet_updated();
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "Holding at %dm above landing.",
@@ -1204,8 +1202,7 @@ Mission::do_abort_landing()
// send reposition cmd to get out of mission
vehicle_command_s vcmd = {};
mission_item_to_vehicle_command(&_mission_item, &vcmd);
vcmd.timestamp = hrt_absolute_time();
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION;
vcmd.param1 = -1;
vcmd.param2 = 1;
@@ -1213,7 +1210,7 @@ Mission::do_abort_landing()
vcmd.param6 = _mission_item.lon;
vcmd.param7 = alt_sp;
_navigator->publish_vehicle_cmd(vcmd);
_navigator->publish_vehicle_cmd(&vcmd);
}
bool
@@ -1230,7 +1227,7 @@ Mission::prepare_mission_items(bool onboard, struct mission_item_s *mission_item
/* trying to find next position mission item */
while (read_mission_item(onboard, offset, next_position_mission_item)) {
if (item_contains_position(next_position_mission_item)) {
if (item_contains_position(*next_position_mission_item)) {
*has_next_position_item = true;
break;
}
src/modules/navigator/mission_block.cpp
+43 -85
View File
@@ -54,8 +54,6 @@
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vtol_vehicle_status.h>
MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
NavigatorMode(navigator, name),
_param_loiter_min_alt(this, "MIS_LTRMIN_ALT", false),
@@ -97,7 +95,6 @@ MissionBlock::is_mission_item_reached()
case NAV_CMD_DO_MOUNT_CONFIGURE:
case NAV_CMD_DO_MOUNT_CONTROL:
case NAV_CMD_DO_SET_ROI:
case NAV_CMD_ROI:
case NAV_CMD_DO_SET_CAM_TRIGG_DIST:
case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
case NAV_CMD_SET_CAMERA_MODE:
@@ -413,61 +410,24 @@ MissionBlock::reset_mission_item_reached()
}
void
MissionBlock::mission_item_to_vehicle_command(const struct mission_item_s *item, struct vehicle_command_s *cmd)
MissionBlock::issue_command(const mission_item_s &item)
{
// we're expecting a mission command item here so assign the "raw" inputs to the command
// (MAV_FRAME_MISSION mission item)
cmd->param1 = item->params[0];
cmd->param2 = item->params[1];
cmd->param3 = item->params[2];
cmd->param4 = item->params[3];
cmd->param5 = item->params[4];
cmd->param6 = item->params[5];
cmd->param7 = item->params[6];
cmd->command = item->nav_cmd;
cmd->target_system = _navigator->get_vstatus()->system_id;
// The camera commands are not processed on the autopilot but will be
// sent to the mavlink links to other components.
switch (item->nav_cmd) {
case NAV_CMD_IMAGE_START_CAPTURE:
case NAV_CMD_IMAGE_STOP_CAPTURE:
case NAV_CMD_VIDEO_START_CAPTURE:
case NAV_CMD_VIDEO_STOP_CAPTURE:
cmd->target_component = 100; // MAV_COMP_ID_CAMERA
break;
default:
cmd->target_component = _navigator->get_vstatus()->component_id;
break;
}
cmd->source_system = _navigator->get_vstatus()->system_id;
cmd->source_component = _navigator->get_vstatus()->component_id;
cmd->confirmation = false;
}
void
MissionBlock::issue_command(const struct mission_item_s *item)
{
if (item_contains_position(item)) {
return;
}
// NAV_CMD_DO_LAND_START is only a marker
if (item->nav_cmd == NAV_CMD_DO_LAND_START) {
if (item.nav_cmd == NAV_CMD_DO_LAND_START) {
return;
}
if (item->nav_cmd == NAV_CMD_DO_SET_SERVO) {
if (item.nav_cmd == NAV_CMD_DO_SET_SERVO) {
PX4_INFO("do_set_servo command");
// XXX: we should issue a vehicle command and handle this somewhere else
_actuators = {};
// params[0] actuator number to be set 0..5 (corresponds to AUX outputs 1..6)
// params[1] new value for selected actuator in ms 900...2000
_actuators.control[(int)item->params[0]] = 1.0f / 2000 * -item->params[1];
_actuators.control[(int)item.params[0]] = 1.0f / 2000 * -item.params[1];
_actuators.timestamp = hrt_absolute_time();
if (_actuator_pub != nullptr) {
@@ -478,16 +438,23 @@ MissionBlock::issue_command(const struct mission_item_s *item)
}
} else {
const hrt_abstime now = hrt_absolute_time();
_action_start = hrt_absolute_time();
struct vehicle_command_s cmd = {
.timestamp = now
};
// mission_item -> vehicle_command
mission_item_to_vehicle_command(item, &cmd);
_action_start = now;
// we're expecting a mission command item here so assign the "raw" inputs to the command
// (MAV_FRAME_MISSION mission item)
vehicle_command_s vcmd = {};
vcmd.command = item.nav_cmd;
vcmd.param1 = item.params[0];
vcmd.param2 = item.params[1];
vcmd.param3 = item.params[2];
vcmd.param4 = item.params[3];
vcmd.param5 = item.params[4];
vcmd.param6 = item.params[5];
vcmd.param7 = item.params[6];
_navigator->publish_vehicle_cmd(cmd);
_navigator->publish_vehicle_cmd(&vcmd);
}
}
@@ -502,41 +469,41 @@ MissionBlock::get_time_inside(const struct mission_item_s &item)
}
bool
MissionBlock::item_contains_position(const struct mission_item_s *item)
MissionBlock::item_contains_position(const mission_item_s &item)
{
return item->nav_cmd == NAV_CMD_WAYPOINT ||
item->nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
item->nav_cmd == NAV_CMD_LAND ||
item->nav_cmd == NAV_CMD_TAKEOFF ||
item->nav_cmd == NAV_CMD_LOITER_TO_ALT ||
item->nav_cmd == NAV_CMD_VTOL_TAKEOFF ||
item->nav_cmd == NAV_CMD_VTOL_LAND;
return item.nav_cmd == NAV_CMD_WAYPOINT ||
item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
item.nav_cmd == NAV_CMD_LAND ||
item.nav_cmd == NAV_CMD_TAKEOFF ||
item.nav_cmd == NAV_CMD_LOITER_TO_ALT ||
item.nav_cmd == NAV_CMD_VTOL_TAKEOFF ||
item.nav_cmd == NAV_CMD_VTOL_LAND;
}
bool
MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp)
MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, position_setpoint_s *sp)
{
/* don't change the setpoint for non-position items */
if (!item_contains_position(item)) {
return false;
}
sp->lat = item->lat;
sp->lon = item->lon;
sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude;
sp->yaw = item->yaw;
sp->yaw_valid = PX4_ISFINITE(item->yaw);
sp->loiter_radius = (fabsf(item->loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(item->loiter_radius) :
sp->lat = item.lat;
sp->lon = item.lon;
sp->alt = item.altitude_is_relative ? item.altitude + _navigator->get_home_position()->alt : item.altitude;
sp->yaw = item.yaw;
sp->yaw_valid = PX4_ISFINITE(item.yaw);
sp->loiter_radius = (fabsf(item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(item.loiter_radius) :
_navigator->get_loiter_radius();
sp->loiter_direction = (item->loiter_radius > 0) ? 1 : -1;
sp->acceptance_radius = item->acceptance_radius;
sp->disable_mc_yaw_control = item->disable_mc_yaw;
sp->loiter_direction = (item.loiter_radius > 0) ? 1 : -1;
sp->acceptance_radius = item.acceptance_radius;
sp->disable_mc_yaw_control = item.disable_mc_yaw;
sp->cruising_speed = _navigator->get_cruising_speed();
sp->cruising_throttle = _navigator->get_cruising_throttle();
switch (item->nav_cmd) {
switch (item.nav_cmd) {
case NAV_CMD_IDLE:
sp->type = position_setpoint_s::SETPOINT_TYPE_IDLE;
break;
@@ -553,7 +520,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
// set pitch and ensure that the hold time is zero
sp->pitch_min = item->pitch_min;
sp->pitch_min = item.pitch_min;
}
break;
@@ -720,19 +687,10 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio
!_navigator->get_vstatus()->is_rotary_wing &&
_param_force_vtol.get() == 1) {
struct vehicle_command_s cmd = {
.timestamp = hrt_absolute_time(),
.param5 = 0.0f,
.param6 = 0.0f,
.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC,
.param2 = 0.0f,
.param3 = 0.0f,
.param4 = 0.0f,
.param7 = 0.0f,
.command = NAV_CMD_DO_VTOL_TRANSITION
};
_navigator->publish_vehicle_cmd(cmd);
vehicle_command_s cmd = {};
cmd.command = NAV_CMD_DO_VTOL_TRANSITION;
cmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
_navigator->publish_vehicle_cmd(&cmd);
}
/* set the land item */
src/modules/navigator/mission_block.h
+3 -8
View File
@@ -66,7 +66,7 @@ public:
MissionBlock(const MissionBlock &) = delete;
MissionBlock &operator=(const MissionBlock &) = delete;
static bool item_contains_position(const struct mission_item_s *item);
static bool item_contains_position(const mission_item_s &item);
protected:
/**
@@ -86,7 +86,7 @@ protected:
* @param the mission item to convert
* @param the position setpoint that needs to be set
*/
bool mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
bool mission_item_to_position_setpoint(const mission_item_s &item, position_setpoint_s *sp);
/**
* Set previous position setpoint to current setpoint
@@ -120,12 +120,7 @@ protected:
*/
void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target, float yaw);
/**
* Convert a mission item to a command
*/
void mission_item_to_vehicle_command(const struct mission_item_s *item, struct vehicle_command_s *cmd);
void issue_command(const struct mission_item_s *item);
void issue_command(const mission_item_s &item);
float get_time_inside(const struct mission_item_s &item);
src/modules/navigator/mission_feasibility_checker.cpp
+5 -6
View File
@@ -184,7 +184,7 @@ MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionIt
// Geofence function checks against home altitude amsl
missionitem.altitude = missionitem.altitude_is_relative ? missionitem.altitude + home_alt : missionitem.altitude;
if (MissionBlock::item_contains_position(&missionitem) &&
if (MissionBlock::item_contains_position(missionitem) &&
!geofence.check(missionitem)) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence violation for waypoint %d", i + 1);
@@ -212,7 +212,7 @@ MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_
}
/* reject relative alt without home set */
if (missionitem.altitude_is_relative && !home_valid && MissionBlock::item_contains_position(&missionitem)) {
if (missionitem.altitude_is_relative && !home_valid && MissionBlock::item_contains_position(missionitem)) {
warning_issued = true;
@@ -229,7 +229,7 @@ MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_
/* calculate the global waypoint altitude */
float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude;
if ((home_alt > wp_alt) && MissionBlock::item_contains_position(&missionitem)) {
if ((home_alt > wp_alt) && MissionBlock::item_contains_position(missionitem)) {
warning_issued = true;
@@ -286,7 +286,6 @@ MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t
missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE &&
missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL &&
missionitem.nav_cmd != NAV_CMD_DO_SET_ROI &&
missionitem.nav_cmd != NAV_CMD_ROI &&
missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST &&
missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL &&
missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE &&
@@ -411,7 +410,7 @@ MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nM
return false;
}
if (MissionBlock::item_contains_position(&missionitem_previous)) {
if (MissionBlock::item_contains_position(missionitem_previous)) {
float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat, missionitem_previous.lon, missionitem.lat,
missionitem.lon);
@@ -489,7 +488,7 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
/* find first waypoint (with lat/lon) item in datamanager */
for (size_t i = 0; i < nMissionItems; i++) {
if (dm_read(dm_current, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) {
if (MissionBlock::item_contains_position(&mission_item)) {
if (MissionBlock::item_contains_position(mission_item)) {
/* check only items with valid lat/lon */
/* check distance from current position to item */
src/modules/navigator/navigation.h
-1
View File
@@ -66,7 +66,6 @@ enum NAV_CMD {
NAV_CMD_TAKEOFF = 22,
NAV_CMD_LOITER_TO_ALT = 31,
NAV_CMD_DO_FOLLOW_REPOSITION = 33,
NAV_CMD_ROI = 80,
NAV_CMD_VTOL_TAKEOFF = 84,
NAV_CMD_VTOL_LAND = 85,
NAV_CMD_DELAY = 93,
src/modules/navigator/navigator.h
+12 -14
View File
@@ -106,13 +106,7 @@ public:
*/
void publish_geofence_result();
/**
* Publish the attitude sp, only to be used in very special modes when position control is deactivated
* Example: mode that is triggered on gps failure
*/
void publish_att_sp();
void publish_vehicle_cmd(const struct vehicle_command_s &vcmd);
void publish_vehicle_cmd(vehicle_command_s *vcmd);
/**
* Setters
@@ -131,17 +125,19 @@ public:
struct position_setpoint_triplet_s *get_reposition_triplet() { return &_reposition_triplet; }
struct position_setpoint_triplet_s *get_takeoff_triplet() { return &_takeoff_triplet; }
struct vehicle_global_position_s *get_global_position() { return &_global_pos; }
struct vehicle_gps_position_s *get_gps_position() { return &_gps_pos; }
struct vehicle_land_detected_s *get_land_detected() { return &_land_detected; }
struct vehicle_local_position_s *get_local_position() { return &_local_pos; }
struct vehicle_status_s *get_vstatus() { return &_vstatus; }
struct vehicle_roi_s *get_vroi() { return &_vroi; }
const vehicle_roi_s &get_vroi() { return _vroi; }
bool home_position_valid() { return (_home_pos.timestamp > 0); }
int get_onboard_mission_sub() { return _onboard_mission_sub; }
int get_offboard_mission_sub() { return _offboard_mission_sub; }
Geofence &get_geofence() { return _geofence; }
bool get_can_loiter_at_sp() { return _can_loiter_at_sp; }
float get_loiter_radius() { return _param_loiter_radius.get(); }
@@ -215,6 +211,7 @@ public:
* @return the distance at which the next waypoint should be used
*/
float get_acceptance_radius(float mission_item_radius);
orb_advert_t *get_mavlink_log_pub() { return &_mavlink_log_pub; }
void increment_mission_instance_count() { _mission_instance_count++; }
@@ -230,8 +227,6 @@ private:
bool _task_should_exit{false}; /**< if true, sensor task should exit */
int _navigator_task{-1}; /**< task handle for sensor task */
orb_advert_t _mavlink_log_pub{nullptr}; /**< the uORB advert to send messages over mavlink */
int _fw_pos_ctrl_status_sub{-1}; /**< notification of vehicle capabilities updates */
int _global_pos_sub{-1}; /**< global position subscription */
int _gps_pos_sub{-1}; /**< gps position subscription */
@@ -244,12 +239,14 @@ private:
int _sensor_combined_sub{-1}; /**< sensor combined subscription */
int _vehicle_command_sub{-1}; /**< vehicle commands (onboard and offboard) */
int _vstatus_sub{-1}; /**< vehicle status subscription */
int _vehicle_roi_sub{-1}; /**< vehicle ROI subscription */
orb_advert_t _geofence_result_pub{nullptr};
orb_advert_t _mavlink_log_pub{nullptr}; /**< the uORB advert to send messages over mavlink */
orb_advert_t _mission_result_pub{nullptr};
orb_advert_t _pos_sp_triplet_pub{nullptr};
orb_advert_t _vehicle_cmd_ack_pub{nullptr};
orb_advert_t _vehicle_cmd_pub{nullptr};
orb_advert_t _vehicle_roi_pub{nullptr};
fw_pos_ctrl_status_s _fw_pos_ctrl_status{}; /**< fixed wing navigation capabilities */
geofence_result_s _geofence_result{};
@@ -264,8 +261,8 @@ private:
vehicle_gps_position_s _gps_pos{}; /**< gps position */
vehicle_land_detected_s _land_detected{}; /**< vehicle land_detected */
vehicle_local_position_s _local_pos{}; /**< local vehicle position */
vehicle_status_s _vstatus{}; /**< vehicle status */
vehicle_roi_s _vroi{}; /**< vehicle ROI */
vehicle_status_s _vstatus{}; /**< vehicle status */
int _mission_instance_count{-1}; /**< instance count for the current mission */
@@ -313,7 +310,6 @@ private:
void sensor_combined_update();
void vehicle_land_detected_update();
void vehicle_status_update();
void vehicle_roi_update();
/**
* Shim for calling task_main from task_create.
@@ -339,5 +335,7 @@ private:
* Publish the mission result so commander and mavlink know what is going on
*/
void publish_mission_result();
void publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result);
};
#endif
src/modules/navigator/navigator_main.cpp
+119 -45
View File
@@ -64,6 +64,7 @@
#include <uORB/topics/home_position.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/uORB.h>
@@ -213,12 +214,6 @@ Navigator::params_update()
}
}
void
Navigator::vehicle_roi_update()
{
orb_copy(ORB_ID(vehicle_roi), _vehicle_roi_sub, &_vroi);
}
void
Navigator::task_main_trampoline(int argc, char *argv[])
{
@@ -252,7 +247,6 @@ Navigator::task_main()
_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
_vehicle_roi_sub = orb_subscribe(ORB_ID(vehicle_roi));
/* copy all topics first time */
vehicle_status_update();
@@ -264,7 +258,6 @@ Navigator::task_main()
home_position_update(true);
fw_pos_ctrl_status_update(true);
params_update();
vehicle_roi_update();
/* wakeup source(s) */
px4_pollfd_struct_t fds[1] = {};
@@ -365,13 +358,6 @@ Navigator::task_main()
home_position_update();
}
/* ROI updated */
orb_check(_vehicle_roi_sub, &updated);
if (updated) {
vehicle_roi_update();
}
/* vehicle_command updated */
orb_check(_vehicle_command_sub, &updated);
@@ -379,7 +365,13 @@ Navigator::task_main()
vehicle_command_s cmd;
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) {
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND) {
// DO_GO_AROUND is currently handled by the position controller (unacknowledged)
// TODO: move DO_GO_AROUND handling to navigator
publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) {
struct position_setpoint_triplet_s *rep = get_reposition_triplet();
struct position_setpoint_triplet_s *curr = get_position_setpoint_triplet();
@@ -435,6 +427,8 @@ Navigator::task_main()
rep->current.valid = true;
rep->next.valid = false;
// CMD_DO_REPOSITION is acknowledged by commander
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) {
struct position_setpoint_triplet_s *rep = get_takeoff_triplet();
@@ -472,6 +466,8 @@ Navigator::task_main()
rep->current.valid = true;
rep->next.valid = false;
// CMD_NAV_TAKEOFF is acknowledged by commander
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_LAND_START) {
/* find NAV_CMD_DO_LAND_START in the mission and
@@ -480,30 +476,17 @@ Navigator::task_main()
int land_start = _mission.find_offboard_land_start();
if (land_start != -1) {
struct vehicle_command_s vcmd = {
.timestamp = hrt_absolute_time(),
.param5 = 0.0f,
.param6 = 0.0f,
.param1 = (float)land_start,
.param2 = 0.0f,
.param3 = 0.0f,
.param4 = 0.0f,
.param7 = 0.0f,
.command = vehicle_command_s::VEHICLE_CMD_MISSION_START,
.target_system = (uint8_t)get_vstatus()->system_id,
.target_component = (uint8_t)get_vstatus()->component_id,
.source_system = (uint8_t)get_vstatus()->system_id,
.source_component = (uint8_t)get_vstatus()->component_id
};
publish_vehicle_cmd(vcmd);
vehicle_command_s vcmd = {};
vcmd.param1 = land_start;
publish_vehicle_cmd(&vcmd);
} else {
PX4_WARN("planned landing not available");
}
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) {
publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) {
if (get_mission_result()->valid &&
PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0) && (cmd.param1 < _mission_result.seq_total)) {
@@ -526,6 +509,51 @@ Navigator::task_main()
set_cruising_throttle();
}
}
// TODO: handle responses for supported DO_CHANGE_SPEED options?
publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI) {
_vroi = {};
_vroi.mode = cmd.param1;
switch (_vroi.mode) {
case vehicle_command_s::VEHICLE_ROI_NONE:
break;
case vehicle_command_s::VEHICLE_ROI_WPNEXT:
// TODO: implement point toward next MISSION
break;
case vehicle_command_s::VEHICLE_ROI_WPINDEX:
_vroi.mission_seq = cmd.param2;
break;
case vehicle_command_s::VEHICLE_ROI_LOCATION:
_vroi.lat = cmd.param5;
_vroi.lon = cmd.param6;
_vroi.alt = cmd.param7;
break;
case vehicle_command_s::VEHICLE_ROI_TARGET:
_vroi.target_seq = cmd.param2;
break;
default:
_vroi.mode = vehicle_command_s::VEHICLE_ROI_NONE;
break;
}
_vroi.timestamp = hrt_absolute_time();
if (_vehicle_roi_pub != nullptr) {
orb_publish(ORB_ID(vehicle_roi), _vehicle_roi_pub, &_vroi);
} else {
_vehicle_roi_pub = orb_advertise(ORB_ID(vehicle_roi), &_vroi);
}
publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
}
}
@@ -979,17 +1007,6 @@ Navigator::publish_geofence_result()
}
}
void
Navigator::publish_vehicle_cmd(const struct vehicle_command_s &vcmd)
{
if (_vehicle_cmd_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command), _vehicle_cmd_pub, &vcmd);
} else {
_vehicle_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
}
}
void
Navigator::set_mission_failure(const char *reason)
{
@@ -999,3 +1016,60 @@ Navigator::set_mission_failure(const char *reason)
mavlink_log_critical(&_mavlink_log_pub, "%s", reason);
}
}
void
Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd)
{
vcmd->timestamp = hrt_absolute_time();
vcmd->source_system = _vstatus.system_id;
vcmd->source_component = _vstatus.component_id;
vcmd->target_system = _vstatus.system_id;
vcmd->confirmation = false;
vcmd->from_external = false;
// The camera commands are not processed on the autopilot but will be
// sent to the mavlink links to other components.
switch (vcmd->command) {
case NAV_CMD_IMAGE_START_CAPTURE:
case NAV_CMD_IMAGE_STOP_CAPTURE:
case NAV_CMD_VIDEO_START_CAPTURE:
case NAV_CMD_VIDEO_STOP_CAPTURE:
vcmd->target_component = 100; // MAV_COMP_ID_CAMERA
break;
default:
vcmd->target_component = _vstatus.component_id;
break;
}
if (_vehicle_cmd_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command), _vehicle_cmd_pub, vcmd);
} else {
_vehicle_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
}
}
void
Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result)
{
vehicle_command_ack_s command_ack = {};
command_ack.timestamp = hrt_absolute_time();
command_ack.command = cmd.command;
command_ack.target_system = cmd.source_system;
command_ack.target_component = cmd.source_component;
command_ack.from_external = cmd.from_external;
command_ack.result = result;
command_ack.result_param1 = 0;
command_ack.result_param2 = 0;
if (_vehicle_cmd_ack_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command_ack), _vehicle_cmd_ack_pub, &command_ack);
} else {
_vehicle_cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
}
}
src/modules/navigator/rcloss.cpp
+1 -1
View File
@@ -140,7 +140,7 @@ RCLoss::set_rcl_item()
reset_mission_item_reached();
/* convert mission item to current position setpoint and make it valid */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
_navigator->set_position_setpoint_triplet_updated();
src/modules/navigator/rtl.cpp
+4 -4
View File
@@ -84,7 +84,7 @@ RTL::on_activation()
{
set_current_position_item(&_mission_item);
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->next.valid = false;
@@ -286,12 +286,12 @@ RTL::set_rtl_item()
reset_mission_item_reached();
/* execute command if set. This is required for commands like VTOL transition */
if (!item_contains_position(&_mission_item)) {
issue_command(&_mission_item);
if (!item_contains_position(_mission_item)) {
issue_command(_mission_item);
}
/* convert mission item to current position setpoint and make it valid */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
_navigator->set_position_setpoint_triplet_updated();
src/modules/navigator/takeoff.cpp
+2 -2
View File
@@ -92,7 +92,7 @@ Takeoff::on_active()
// set loiter item so position controllers stop doing takeoff logic
set_loiter_item(&_mission_item);
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
_navigator->set_position_setpoint_triplet_updated();
}
}
@@ -148,7 +148,7 @@ Takeoff::set_takeoff_position()
// convert mission item to current setpoint
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.yaw_valid = true;
pos_sp_triplet->next.valid = false;