diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index 0c5b8412bc..49f30232c7 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -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 diff --git a/msg/vehicle_roi.msg b/msg/vehicle_roi.msg index 715dc46b87..ed2fabe8b9 100644 --- a/msg/vehicle_roi.msg +++ b/msg/vehicle_roi.msg @@ -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 diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index e823b935fa..69eb7b1b64 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -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 diff --git a/src/drivers/vmount/input_mavlink.cpp b/src/drivers/vmount/input_mavlink.cpp index 9724f29914..d34174d6f3 100644 --- a/src/drivers/vmount/input_mavlink.cpp +++ b/src/drivers/vmount/input_mavlink.cpp @@ -49,15 +49,9 @@ #include #include - 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; diff --git a/src/drivers/vmount/input_mavlink.h b/src/drivers/vmount/input_mavlink.h index 0b72b63970..bd9588223a 100644 --- a/src/drivers/vmount/input_mavlink.h +++ b/src/drivers/vmount/input_mavlink.h @@ -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; }; diff --git a/src/modules/commander/arm_auth.cpp b/src/modules/commander/arm_auth.cpp index dea65d75d2..a12fa8b3bb 100644 --- a/src/modules/commander/arm_auth.cpp +++ b/src/modules/commander/arm_auth.cpp @@ -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"); diff --git a/src/modules/commander/arm_auth.h b/src/modules/commander/arm_auth.h index 1d43bbaea1..b66c888b78 100644 --- a/src/modules/commander/arm_auth.h +++ b/src/modules/commander/arm_auth.h @@ -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(); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a761fc83f6..0313585f04 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -106,7 +106,6 @@ #include #include #include -#include #include #include #include @@ -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; } } diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 7d9562a70a..0189096f66 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -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: diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 5457f6b1f8..cfea39158d 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -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(); diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp index 56fcbffdfc..5ff5385671 100644 --- a/src/modules/navigator/enginefailure.cpp +++ b/src/modules/navigator/enginefailure.cpp @@ -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(); diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp index 27f0b7a86c..116144c2aa 100644 --- a/src/modules/navigator/follow_target.cpp +++ b/src/modules/navigator/follow_target.cpp @@ -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; diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index 144cf27a0a..d440c84406 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -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(); } } diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index 2844ce61c3..facea22d63 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -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); diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 6961781dfb..4e1f0187f3 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -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; } diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 9b46e78e13..54bbe2580e 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -54,8 +54,6 @@ #include #include - - 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 */ diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 41dce6837f..111ae9d9a3 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -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); diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index b513efcda3..d743995eca 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -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 */ diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 80bf69b003..40d00f5ac4 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -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, diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 44d6c9d344..05e2f7a15e 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -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 diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 1345ae93e9..910a6e6836 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -64,6 +64,7 @@ #include #include #include +#include #include #include @@ -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); + } +} diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 409f9e329b..fcc21b992a 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -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(); diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 148b3b1524..c1d2566ce8 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -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(); diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index e624b5c67b..d662fb9c7c 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -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;