bool
MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission)
{
	// do not allow mission if we find unsupported item
	for (size_t i = 0; i < mission.count; i++) {
		struct mission_item_s missionitem;
		const ssize_t len = sizeof(struct mission_item_s);

		if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) {
			// not supposed to happen unless the datamanager can't access the SD card, etc.
			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Cannot access SD card");
			return false;
		}

		// check if we find unsupported items and reject mission if so
		if (missionitem.nav_cmd != NAV_CMD_IDLE &&
		    missionitem.nav_cmd != NAV_CMD_WAYPOINT &&
		    missionitem.nav_cmd != NAV_CMD_LOITER_UNLIMITED &&
		    missionitem.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT &&
		    missionitem.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH &&
		    missionitem.nav_cmd != NAV_CMD_LAND &&
		    missionitem.nav_cmd != NAV_CMD_TAKEOFF &&
		    missionitem.nav_cmd != NAV_CMD_LOITER_TO_ALT &&
		    missionitem.nav_cmd != NAV_CMD_VTOL_TAKEOFF &&
		    missionitem.nav_cmd != NAV_CMD_VTOL_LAND &&
		    missionitem.nav_cmd != NAV_CMD_DELAY &&
		    missionitem.nav_cmd != NAV_CMD_DO_JUMP &&
		    missionitem.nav_cmd != NAV_CMD_DO_CHANGE_SPEED &&
		    missionitem.nav_cmd != NAV_CMD_DO_SET_HOME &&
		    missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO &&
		    missionitem.nav_cmd != NAV_CMD_DO_LAND_START &&
		    missionitem.nav_cmd != NAV_CMD_DO_TRIGGER_CONTROL &&
		    missionitem.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL &&
		    missionitem.nav_cmd != NAV_CMD_IMAGE_START_CAPTURE &&
		    missionitem.nav_cmd != NAV_CMD_IMAGE_STOP_CAPTURE &&
		    missionitem.nav_cmd != NAV_CMD_VIDEO_START_CAPTURE &&
		    missionitem.nav_cmd != NAV_CMD_VIDEO_STOP_CAPTURE &&
		    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_DO_SET_ROI_LOCATION &&
		    missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET &&
		    missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_NONE &&
		    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 &&
		    missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) {

			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: item %i: unsupported cmd: %d", (int)(i + 1),
					     (int)missionitem.nav_cmd);
			return false;
		}

		/* Check non navigation item */
		if (missionitem.nav_cmd == NAV_CMD_DO_SET_SERVO) {

			/* check actuator number */
			if (missionitem.params[0] < 0 || missionitem.params[0] > 5) {
				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Actuator number %d is out of bounds 0..5",
						     (int)missionitem.params[0]);
				return false;
			}

			/* check actuator value */
			if (missionitem.params[1] < -PWM_DEFAULT_MAX || missionitem.params[1] > PWM_DEFAULT_MAX) {
				mavlink_log_critical(_navigator->get_mavlink_log_pub(),
						     "Actuator value %d is out of bounds -PWM_DEFAULT_MAX..PWM_DEFAULT_MAX", (int)missionitem.params[1]);
				return false;
			}
		}

		// check if the mission starts with a land command while the vehicle is landed
		if ((i == 0) && missionitem.nav_cmd == NAV_CMD_LAND && _navigator->get_land_detected()->landed) {

			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: starts with landing");
			return false;
		}
	}

	return true;
}
bool
MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool land_start_req)
{
	/* Go through all mission items and search for a landing waypoint
	 * if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */

	bool landing_valid = false;

	bool land_start_found = false;
	size_t do_land_start_index = 0;
	size_t landing_approach_index = 0;

	for (size_t i = 0; i < mission.count; i++) {
		struct mission_item_s missionitem;
		const ssize_t len = sizeof(missionitem);

		if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) {
			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
			return false;
		}

		// if DO_LAND_START found then require valid landing AFTER
		if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) {
			if (land_start_found) {
				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: more than one land start.");
				return false;

			} else {
				land_start_found = true;
				do_land_start_index = i;
			}
		}

		if (missionitem.nav_cmd == NAV_CMD_LAND) {
			mission_item_s missionitem_previous {};

			if (i > 0) {
				landing_approach_index = i - 1;

				if (dm_read((dm_item_t)mission.dataman_id, landing_approach_index, &missionitem_previous, len) != len) {
					/* not supposed to happen unless the datamanager can't access the SD card, etc. */
					return false;
				}

				if (MissionBlock::item_contains_position(missionitem_previous)) {

					uORB::Subscription<position_controller_landing_status_s> landing_status{ORB_ID(position_controller_landing_status)};
					landing_status.forcedUpdate();

					const bool landing_status_valid = (landing_status.get().timestamp > 0);
					const float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat, missionitem_previous.lon,
								  missionitem.lat, missionitem.lon);

					if (landing_status_valid && (wp_distance > landing_status.get().flare_length)) {
						/* Last wp is before flare region */

						const float delta_altitude = missionitem.altitude - missionitem_previous.altitude;

						if (delta_altitude < 0) {

							const float horizontal_slope_displacement = landing_status.get().horizontal_slope_displacement;
							const float slope_angle_rad = landing_status.get().slope_angle_rad;
							const float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude,
										    horizontal_slope_displacement, slope_angle_rad);

							if (missionitem_previous.altitude > slope_alt_req + 1.0f) {
								/* Landing waypoint is above altitude of slope at the given waypoint distance (with small tolerance for floating point discrepancies) */
								mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: adjust landing approach.");

								const float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude,
											      missionitem.altitude, horizontal_slope_displacement, slope_angle_rad);

								mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Move down %d m or move further away by %d m.",
										     (int)ceilf(slope_alt_req - missionitem_previous.altitude),
										     (int)ceilf(wp_distance_req - wp_distance));

								return false;
							}

						} else {
							/* Landing waypoint is above last waypoint */
							mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing above last waypoint.");
							return false;
						}

					} else {
						/* Last wp is in flare region */
						mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: waypoint within landing flare.");
						return false;
					}

					landing_valid = true;

				} else {
					// mission item before land doesn't have a position
					mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: need landing approach.");
					return false;
				}

			} else {
				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: starts with land waypoint.");
				return false;
			}
		}
	}

	if (land_start_req && !land_start_found) {
		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing pattern required.");
		return false;
	}

	if (land_start_found && (!landing_valid || (do_land_start_index > landing_approach_index))) {
		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: invalid land start.");
		return false;
	}

	/* No landing waypoints or no waypoints */
	return true;
}
Ejemplo n.º 3
0
bool
Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item)
{
	/* select onboard/offboard mission */
	int *mission_index_ptr;
	struct mission_s *mission;
	dm_item_t dm_item;
	int mission_index_next;

	if (onboard) {
		/* onboard mission */
		mission_index_next = _current_onboard_mission_index + 1;
		mission_index_ptr = is_current ? &_current_onboard_mission_index : &mission_index_next;

		mission = &_onboard_mission;

		dm_item = DM_KEY_WAYPOINTS_ONBOARD;

	} else {
		/* offboard mission */
		mission_index_next = _current_offboard_mission_index + 1;
		mission_index_ptr = is_current ? &_current_offboard_mission_index : &mission_index_next;

		mission = &_offboard_mission;

		dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
	}

	/* Repeat this several times in case there are several DO JUMPS that we need to follow along, however, after
	 * 10 iterations we have to assume that the DO JUMPS are probably cycling and give up. */
	for (int i = 0; i < 10; i++) {

		if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
			/* mission item index out of bounds */
			return false;
		}

		const ssize_t len = sizeof(struct mission_item_s);

		/* read mission item to temp storage first to not overwrite current mission item if data damaged */
		struct mission_item_s mission_item_tmp;

		/* read mission item from datamanager */
		if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) {
			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
			mavlink_log_critical(_navigator->get_mavlink_fd(),
			                     "ERROR waypoint could not be read");
			return false;
		}

		/* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */
		if (mission_item_tmp.nav_cmd == NAV_CMD_DO_JUMP) {

			/* do DO_JUMP as many times as requested */
			if (mission_item_tmp.do_jump_current_count < mission_item_tmp.do_jump_repeat_count) {

				/* only raise the repeat count if this is for the current mission item
				* but not for the next mission item */
				if (is_current) {
					(mission_item_tmp.do_jump_current_count)++;
					/* save repeat count */
					if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET,
					    &mission_item_tmp, len) != len) {
						/* not supposed to happen unless the datamanager can't access the
						 * dataman */
						mavlink_log_critical(_navigator->get_mavlink_fd(),
								     "ERROR DO JUMP waypoint could not be written");
						return false;
					}
					report_do_jump_mission_changed(*mission_index_ptr,
								       mission_item_tmp.do_jump_repeat_count);
				}
				/* set new mission item index and repeat
				* we don't have to validate here, if it's invalid, we should realize this later .*/
				*mission_index_ptr = mission_item_tmp.do_jump_mission_index;

			} else {
				if (is_current) {
					mavlink_log_critical(_navigator->get_mavlink_fd(),
							     "DO JUMP repetitions completed");
				}
				/* no more DO_JUMPS, therefore just try to continue with next mission item */
				(*mission_index_ptr)++;
			}

		} else {
			/* if it's not a DO_JUMP, then we were successful */
			memcpy(mission_item, &mission_item_tmp, sizeof(struct mission_item_s));
			return true;
		}
	}

	/* we have given up, we don't want to cycle forever */
	mavlink_log_critical(_navigator->get_mavlink_fd(),
			     "ERROR DO JUMP is cycling, giving up");
	return false;
}
static int dm_read_reg(struct usbnet *dev, u8 reg, u8 *value)
{
	return dm_read(dev, reg, 1, value);
}
bool
MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, bool &warning_issued)
{

	/* check if first waypoint is not too far from home */
	if (dist_first_wp > 0.0f) {
		struct mission_item_s mission_item;

		/* find first waypoint (with lat/lon) item in datamanager */
		for (unsigned i = 0; i < nMissionItems; i++) {
			if (dm_read(dm_current, i,
					&mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) {
				/* Check non navigation item */
				if (mission_item.nav_cmd == NAV_CMD_DO_SET_SERVO){

					/* check actuator number */
					if (mission_item.params[0] < 0 || mission_item.params[0] > 5) {
						mavlink_log_critical(_mavlink_log_pub, "Actuator number %d is out of bounds 0..5", (int)mission_item.params[0]);
						warning_issued = true;
						return false;
					}
					/* check actuator value */
					if (mission_item.params[1] < -2000 || mission_item.params[1] > 2000) {
						mavlink_log_critical(_mavlink_log_pub, "Actuator value %d is out of bounds -2000..2000", (int)mission_item.params[1]);
						warning_issued = true;
						return false;
					}
				}
				/* check only items with valid lat/lon */
				else if (isPositionCommand(mission_item.nav_cmd)) {

					/* check distance from current position to item */
					float dist_to_1wp = get_distance_to_next_waypoint(
							mission_item.lat, mission_item.lon, curr_lat, curr_lon);

					if (dist_to_1wp < dist_first_wp) {
						_dist_1wp_ok = true;
						if (dist_to_1wp > ((dist_first_wp * 3) / 2)) {
							/* allow at 2/3 distance, but warn */
							mavlink_log_critical(_mavlink_log_pub, "Warning: First waypoint very far: %d m", (int)dist_to_1wp);
							warning_issued = true;
						}
						return true;

					} else {
						/* item is too far from home */
						mavlink_log_critical(_mavlink_log_pub, "First waypoint too far: %d m > %d, refusing mission",
								     (int)dist_to_1wp, (int)dist_first_wp);
						warning_issued = true;
						return false;
					}
				}

			} else {
				/* error reading, mission is invalid */
				mavlink_log_info(_mavlink_log_pub, "error reading offboard mission");
				return false;
			}
		}

		/* no waypoints found in mission, then we will not fly far away */
		_dist_1wp_ok = true;
		return true;

	} else {
		return true;
	}
}
Ejemplo n.º 6
0
bool
Mission::check_dist_1wp()
{
	if (_dist_1wp_ok) {
		/* always return true after at least one successful check */
		return true;
	}

	/* check if first waypoint is not too far from home */
	if (_param_dist_1wp.get() > 0.0f) {
		if (_navigator->get_vstatus()->condition_home_position_valid) {
			struct mission_item_s mission_item;

			/* find first waypoint (with lat/lon) item in datamanager */
			for (unsigned i = 0; i < _offboard_mission.count; i++) {
				if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id), i,
						&mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) {

					/* check only items with valid lat/lon */
					if (	mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
							mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
							mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
							mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
							mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
							mission_item.nav_cmd == NAV_CMD_PATHPLANNING) {

						/* check distance from home to item */
						float dist_to_1wp = get_distance_to_next_waypoint(
								mission_item.lat, mission_item.lon,
								_navigator->get_home_position()->lat, _navigator->get_home_position()->lon);

						if (dist_to_1wp < _param_dist_1wp.get()) {
							_dist_1wp_ok = true;
							if (dist_to_1wp > ((_param_dist_1wp.get() * 3) / 2)) {
								/* allow at 2/3 distance, but warn */
								mavlink_log_critical(_navigator->get_mavlink_fd(), "Warning: First waypoint very far: %d m", (int)dist_to_1wp);
							}
							return true;

						} else {
							/* item is too far from home */
							mavlink_log_critical(_navigator->get_mavlink_fd(), "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)_param_dist_1wp.get());
							return false;
						}
					}

				} else {
					/* error reading, mission is invalid */
					mavlink_log_info(_navigator->get_mavlink_fd(), "error reading offboard mission");
					return false;
				}
			}

			/* no waypoints found in mission, then we will not fly far away */
			_dist_1wp_ok = true;
			return true;

		} else {
			mavlink_log_info(_navigator->get_mavlink_fd(), "no home position");
			return false;
		}

	} else {
		return true;
	}
}
bool
MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, bool &warning_issued)
{
	if (_dist_1wp_ok) {
		/* always return true after at least one successful check */
		return true;
	}

	/* check if first waypoint is not too far from home */
	if (dist_first_wp > 0.0f) {
		struct mission_item_s mission_item;

		/* find first waypoint (with lat/lon) item in datamanager */
		for (unsigned i = 0; i < nMissionItems; i++) {
			if (dm_read(dm_current, i,
					&mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) {
				/* Check non navigation item */
				if (mission_item.nav_cmd == NAV_CMD_DO_SET_SERVO){

					/* check actuator number */
					if (mission_item.actuator_num < 0 || mission_item.actuator_num > 5) {
						mavlink_log_critical(_mavlink_fd, "Actuator number %d is out of bounds 0..5", (int)mission_item.actuator_num);
						warning_issued = true;
						return false;
					}
					/* check actuator value */
					if (mission_item.actuator_value < -2000 || mission_item.actuator_value > 2000) {
						mavlink_log_critical(_mavlink_fd, "Actuator value %d is out of bounds -2000..2000", (int)mission_item.actuator_value);
						warning_issued = true;
						return false;
					}
				}
				/* check only items with valid lat/lon */
				else if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
						mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
						mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
						mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
						mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
						mission_item.nav_cmd == NAV_CMD_PATHPLANNING) {

					/* check distance from current position to item */
					float dist_to_1wp = get_distance_to_next_waypoint(
							mission_item.lat, mission_item.lon, curr_lat, curr_lon);

					if (dist_to_1wp < dist_first_wp) {
						_dist_1wp_ok = true;
						if (dist_to_1wp > ((dist_first_wp * 3) / 2)) {
							/* allow at 2/3 distance, but warn */
							mavlink_log_critical(_mavlink_fd, "Warning: First waypoint very far: %d m", (int)dist_to_1wp);
							warning_issued = true;
						}
						return true;

					} else {
						/* item is too far from home */
						mavlink_log_critical(_mavlink_fd, "First waypoint too far: %d m,refusing mission", (int)dist_to_1wp, (int)dist_first_wp);
						warning_issued = true;
						return false;
					}
				}

			} else {
				/* error reading, mission is invalid */
				mavlink_log_info(_mavlink_fd, "error reading offboard mission");
				return false;
			}
		}

		/* no waypoints found in mission, then we will not fly far away */
		_dist_1wp_ok = true;
		return true;

	} else {
		return true;
	}
}
bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems)
{
	/* Go through all mission items and search for a landing waypoint
	 * if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */

	for (size_t i = 0; i < nMissionItems; i++) {
		struct mission_item_s missionitem;
		const ssize_t len = sizeof(missionitem);
		if (dm_read(dm_current, i, &missionitem, len) != len) {
			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
			return false;
		}

		if (missionitem.nav_cmd == NAV_CMD_LAND) {
			struct mission_item_s missionitem_previous;
			if (i != 0) {
				if (dm_read(dm_current, i-1, &missionitem_previous, len) != len) {
					/* not supposed to happen unless the datamanager can't access the SD card, etc. */
					return false;
				}

				float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat , missionitem_previous.lon, missionitem.lat, missionitem.lon);
				float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, _fw_pos_ctrl_status.landing_horizontal_slope_displacement, _fw_pos_ctrl_status.landing_slope_angle_rad);
				float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, _fw_pos_ctrl_status.landing_horizontal_slope_displacement, _fw_pos_ctrl_status.landing_slope_angle_rad);
				float delta_altitude = missionitem.altitude - missionitem_previous.altitude;
//				warnx("wp_distance %.2f, delta_altitude %.2f, missionitem_previous.altitude %.2f, missionitem.altitude %.2f, slope_alt_req %.2f, wp_distance_req %.2f",
//						wp_distance, delta_altitude, missionitem_previous.altitude, missionitem.altitude, slope_alt_req, wp_distance_req);
//				warnx("_nav_caps.landing_horizontal_slope_displacement %.4f, _nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_flare_length %.4f",
//						_nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad, _nav_caps.landing_flare_length);

				if (wp_distance > _fw_pos_ctrl_status.landing_flare_length) {
					/* Last wp is before flare region */

					if (delta_altitude < 0) {
						if (missionitem_previous.altitude <= slope_alt_req) {
							/* Landing waypoint is at or below altitude of slope at the given waypoint distance: this is ok, aircraft will intersect the slope */
							return true;
						} else {
							/* Landing waypoint is above altitude of slope at the given waypoint distance */
							mavlink_log_critical(_mavlink_log_pub, "Landing: last waypoint too high/too close");
							mavlink_log_critical(_mavlink_log_pub, "Move down to %.1fm or move further away by %.1fm",
									(double)(slope_alt_req),
									(double)(wp_distance_req - wp_distance));
							return false;
						}
					} else {
						/* Landing waypoint is above last waypoint */
						mavlink_log_critical(_mavlink_log_pub, "Landing waypoint above last nav waypoint");
						return false;
					}
				} else {
					/* Last wp is in flare region */
					//xxx give recommendations
					mavlink_log_critical(_mavlink_log_pub, "Last waypoint too close to landing waypoint");
					return false;
				}
			} else {
				mavlink_log_critical(_mavlink_log_pub, "Invalid mission: starts with land waypoint");
				return false;
			}
		}
	}

	/* No landing waypoints or no waypoints */
	return true;
}
Ejemplo n.º 9
0
void ap_uio_init_kep_map(void)
{
#ifndef WIN32 
  u8 i = 0 ,j =0;
  u8 cfg_tmp[128] = {0};
  u8 rpt_key[IRDA_MAX_USER + 1][8] = {{0}};
  
  u16 ir_head_len = 0;
  u16 ir_usr_code [IRDA_MAX_USER] = {0};

  u32 off_set = 0;
  u32 read_len = 0;
  u32 ir_max = IRDA_MAX_USER;

  uio_device_t *dev = dev_find_identifier(NULL, DEV_IDT_TYPE,
                            SYS_DEV_TYPE_UIO);

  read_len = dm_read(class_get_handle_by_id(DM_CLASS_ID),
                              IR_BLOCK_ID, 0, off_set, 
                               sizeof(ir_max),
                              (u8 *)cfg_tmp);
  off_set =  sizeof(ir_max);
  if(read_len > 0)
  {
    ir_max = cfg_tmp[0];
  }

  
  ir_head_len = sizeof(ir_block_info_t) * ir_max;

  read_len = dm_read(class_get_handle_by_id(DM_CLASS_ID),
                              IR_BLOCK_ID, 0, off_set, 
                               ir_head_len,
                              (u8 *)cfg_tmp);
  off_set += read_len; 

  if(read_len > 0)
  {
     for(i = 0;i < ir_head_len;i++)
    {
        *((u8 *)ir_info + i) = cfg_tmp[i];
    }
  }

  for(i = 0;i < ir_max;i++)
  {        
  
       read_len = dm_read(class_get_handle_by_id(DM_CLASS_ID),
                  IR_BLOCK_ID, 0, off_set, 
                  ir_info[i].ir_key_num,
                              (u8 *)cfg_tmp);

      ir_info[i].ir_key_num = ir_info[i].ir_key_num - 2;  //the last two for usr_high and usr_low

      off_set += read_len;
      if(read_len == 0)
      {
          continue;
      }
      if(ir_info[i].active == 1)
      {
          if(read_len > 0)
          {
              for(j = 0; j < ir_info[i].ir_key_num; j++)
              {
               ir_keymap[i][j].h_key = cfg_tmp[j];
               if(i == 2)
               {
                if(j == 36)
                {
                 ir_keymap[i][j].h_key = 0x01;
                }
                if(j == 45)
                {
                 ir_keymap[i][j].h_key = 0x08;
                }
                if(j == 46)
               {
                 ir_keymap[i][j].h_key = 0x10;
               }
               }
              }
              ir_usr_code[i] = (cfg_tmp[j] << 8) | cfg_tmp[j + 1];
              rpt_key[i][0] = get_key_value(i, V_KEY_DOWN);
              rpt_key[i][1] = get_key_value(i, V_KEY_UP);
              rpt_key[i][2] = get_key_value(i, V_KEY_RIGHT);
              rpt_key[i][3] = get_key_value(i, V_KEY_LEFT);
              if(i == 1)
              {
              rpt_key[i][4] = get_key_value(i, V_KEY_VDOWN);
              rpt_key[i][5] = get_key_value(i, V_KEY_VUP);
              }
              uio_set_rpt_key(dev, UIO_IRDA, rpt_key[i], 6, i);
          }
       }
  }
  
  read_len = dm_read(class_get_handle_by_id(DM_CLASS_ID),
                              FPKEY_BLOCK_ID, 0, 0, 
                              128,
                              (u8 *)cfg_tmp);
  if(read_len > 0)
  {
    for(i = 0; i < sizeof(fp_keymap) / sizeof(key_map_t); i++)
    {
      fp_keymap[i].h_key = cfg_tmp[i];
    }
  }

  rpt_key[ir_max][0] = get_key_value(ir_max, V_KEY_DOWN);
  rpt_key[ir_max][1] = get_key_value(ir_max, V_KEY_UP);
  rpt_key[ir_max][2] = get_key_value(ir_max, V_KEY_RIGHT);
  rpt_key[ir_max][3] = get_key_value(ir_max, V_KEY_LEFT);
  uio_set_rpt_key(dev, UIO_FRONTPANEL, rpt_key[ir_max], 4, ir_max);

  uio_set_user_code(dev, ir_max, ir_usr_code);
  dev_io_ctrl(dev, UIO_IR_SET_HW_USERCODE, 0);
#endif

  init_magic_keylist();
#ifdef PATCH_ENABLE
  register_magic_keylist(enter_ucas_key_list, 
  GET_MAGIC_LIST_CNT(enter_ucas_key_list), V_KEY_UCAS);
#endif
  register_magic_keylist(enter_twin_port_ui, 
    GET_MAGIC_LIST_CNT(enter_twin_port_ui), V_KEY_BISS_KEY);

  register_magic_keylist(enter_recall_list_key, 
  GET_MAGIC_LIST_CNT(enter_recall_list_key), V_KEY_UCAS);

  register_magic_keylist(enter_factory_test_key, 
  GET_MAGIC_LIST_CNT(enter_factory_test_key), V_KEY_GBOX);  
}
Ejemplo n.º 10
0
static int
task_main(int argc, char *argv[])
{
	char buffer[DM_MAX_DATA_SIZE];
	hrt_abstime wstart, wend, rstart, rend;

	warnx("Starting dataman test task %s", argv[1]);
	/* try to read an invalid item */
	int my_id = atoi(argv[1]);

	/* try to read an invalid item */
	if (dm_read(DM_KEY_NUM_KEYS, 0, buffer, sizeof(buffer)) >= 0) {
		warnx("%d read an invalid item failed", my_id);
		goto fail;
	}

	/* try to read an invalid index */
	if (dm_read(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, buffer, sizeof(buffer)) >= 0) {
		warnx("%d read an invalid index failed", my_id);
		goto fail;
	}

	srand(hrt_absolute_time() ^ my_id);
	unsigned hit = 0, miss = 0;
	wstart = hrt_absolute_time();

	for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
		memset(buffer, my_id, sizeof(buffer));
		buffer[1] = i;
		unsigned hash = i ^ my_id;
		unsigned len = (hash % (DM_MAX_DATA_SIZE / 2)) + 2;

		int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len);
		warnx("ret: %d", ret);

		if (ret != len) {
			warnx("%d write failed, index %d, length %d", my_id, hash, len);
			goto fail;
		}

		usleep(rand() & ((64 * 1024) - 1));
	}

	rstart = hrt_absolute_time();
	wend = rstart;

	for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
		unsigned hash = i ^ my_id;
		unsigned len2;
		unsigned len = (hash % (DM_MAX_DATA_SIZE / 2)) + 2;
		;

		if ((len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, buffer, sizeof(buffer))) < 2) {
			warnx("%d read failed length test, index %d", my_id, hash);
			goto fail;
		}

		if (buffer[0] == my_id) {
			hit++;

			if (len2 != len) {
				warnx("%d read failed length test, index %d, wanted %d, got %d", my_id, hash, len, len2);
				goto fail;
			}

			if (buffer[1] != i) {
				warnx("%d data verification failed, index %d, wanted %d, got %d", my_id, hash, my_id, buffer[1]);
				goto fail;
			}

		} else {
			miss++;
		}
	}

	rend = hrt_absolute_time();
	warnx("Test %d pass, hit %d, miss %d, io time read %llums. write %llums.",
	      my_id, hit, miss, (rend - rstart) / NUM_MISSIONS_SUPPORTED / 1000, (wend - wstart) / NUM_MISSIONS_SUPPORTED / 1000);
	px4_sem_post(sems + my_id);
	return 0;
fail:
	warnx("Test %d fail, buffer %02x %02x %02x %02x %02x %02x",
	      my_id, buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5]);
	px4_sem_post(sems + my_id);
	return -1;
}
bool
MissionFeasibilityChecker::checkDistancesBetweenWaypoints(const mission_s &mission, float max_distance)
{
	if (max_distance <= 0.0f) {
		/* param not set, check is ok */
		return true;
	}

	double last_lat = NAN;
	double last_lon = NAN;

	/* Go through all waypoints */
	for (size_t i = 0; i < mission.count; i++) {

		struct mission_item_s mission_item {};

		if (!(dm_read((dm_item_t)mission.dataman_id, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s))) {
			/* error reading, mission is invalid */
			mavlink_log_info(_navigator->get_mavlink_log_pub(), "Error reading offboard mission.");
			return false;
		}

		/* check only items with valid lat/lon */
		if (!MissionBlock::item_contains_position(mission_item)) {
			continue;
		}

		/* Compare it to last waypoint if already available. */
		if (PX4_ISFINITE(last_lat) && PX4_ISFINITE(last_lon)) {

			/* check distance from current position to item */
			const float dist_between_waypoints = get_distance_to_next_waypoint(
					mission_item.lat, mission_item.lon,
					last_lat, last_lon);

			if (dist_between_waypoints < max_distance) {

				if (dist_between_waypoints > ((max_distance * 3) / 2)) {
					/* allow at 2/3 distance, but warn */
					mavlink_log_critical(_navigator->get_mavlink_log_pub(),
							     "Distance between waypoints very far: %d meters.", (int)dist_between_waypoints);

					_navigator->get_mission_result()->warning = true;
				}

			} else {
				/* item is too far from home */
				mavlink_log_critical(_navigator->get_mavlink_log_pub(),
						     "Distance between waypoints too far: %d meters, %d max.",
						     (int)dist_between_waypoints, (int)max_distance);

				_navigator->get_mission_result()->warning = true;
				return false;
			}
		}

		last_lat = mission_item.lat;
		last_lon = mission_item.lon;
	}

	/* We ran through all waypoints and have not found any distances between waypoints that are too far. */
	return true;
}
Ejemplo n.º 12
0
void
Mission::on_inactive()
{
	/* Without home a mission can't be valid yet anyway, let's wait. */
	if (!_navigator->home_position_valid()) {
		return;
	}

	if (_inited) {
		/* check if missions have changed so that feedback to ground station is given */
		bool onboard_updated = false;
		orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);

		if (onboard_updated) {
			update_onboard_mission();
		}

		bool offboard_updated = false;
		orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);

		if (offboard_updated) {
			update_offboard_mission();
		}

		/* reset the current offboard mission if needed */
		if (need_to_reset_mission(false)) {
			reset_offboard_mission(_offboard_mission);
			update_offboard_mission();
		}

	} else {

		/* load missions from storage */
		mission_s mission_state;

		dm_lock(DM_KEY_MISSION_STATE);

		/* read current state */
		int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));

		dm_unlock(DM_KEY_MISSION_STATE);

		if (read_res == sizeof(mission_s)) {
			_offboard_mission.dataman_id = mission_state.dataman_id;
			_offboard_mission.count = mission_state.count;
			_current_offboard_mission_index = mission_state.current_seq;
		}

		/* On init let's check the mission, maybe there is already one available. */
		check_mission_valid(false);

		_inited = true;
	}

	/* require takeoff after non-loiter or landing */
	if (!_navigator->get_can_loiter_at_sp() || _navigator->get_land_detected()->landed) {
		_need_takeoff = true;

		/* Reset work item type to default if auto take-off has been paused or aborted,
		   and we landed in manual mode. */
		if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) {
			_work_item_type = WORK_ITEM_TYPE_DEFAULT;
		}
	}
}
bool
MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems,
		fw_pos_ctrl_status_s *fw_pos_ctrl_status, bool land_start_req)
{
	/* Go through all mission items and search for a landing waypoint
	 * if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */

	bool landing_valid = false;

	bool land_start_found = false;
	size_t do_land_start_index = 0;
	size_t landing_approach_index = 0;

	for (size_t i = 0; i < nMissionItems; i++) {
		struct mission_item_s missionitem;
		const ssize_t len = sizeof(missionitem);

		if (dm_read(dm_current, i, &missionitem, len) != len) {
			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
			return false;
		}

		// if DO_LAND_START found then require valid landing AFTER
		if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) {
			if (land_start_found) {
				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: more than one land start.");
				return false;

			} else {
				land_start_found = true;
				do_land_start_index = i;
			}
		}

		if (missionitem.nav_cmd == NAV_CMD_LAND) {
			mission_item_s missionitem_previous {};

			if (i > 0) {
				landing_approach_index = i - 1;

				if (dm_read(dm_current, landing_approach_index, &missionitem_previous, len) != len) {
					/* not supposed to happen unless the datamanager can't access the SD card, etc. */
					return false;
				}

				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);

					float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude,
							      fw_pos_ctrl_status->landing_horizontal_slope_displacement, fw_pos_ctrl_status->landing_slope_angle_rad);

					float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude,
								fw_pos_ctrl_status->landing_horizontal_slope_displacement, fw_pos_ctrl_status->landing_slope_angle_rad);

					if (wp_distance > fw_pos_ctrl_status->landing_flare_length) {
						/* Last wp is before flare region */

						const float delta_altitude = missionitem.altitude - missionitem_previous.altitude;

						if (delta_altitude < 0) {
							if (missionitem_previous.altitude > slope_alt_req) {
								/* Landing waypoint is above altitude of slope at the given waypoint distance */
								mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: adjust landing approach.");
								mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Move down %.1fm or move further away by %.1fm.",
										     (double)(slope_alt_req - missionitem_previous.altitude),
										     (double)(wp_distance_req - wp_distance));

								return false;
							}

						} else {
							/* Landing waypoint is above last waypoint */
							mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing above last waypoint.");
							return false;
						}

					} else {
						/* Last wp is in flare region */
						mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: waypoint within landing flare.");
						return false;
					}

					landing_valid = true;

				} else {
					// mission item before land doesn't have a position
					mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: need landing approach.");
					return false;
				}

			} else {
				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: starts with land waypoint.");
				return false;
			}
		}
	}

	if (land_start_req && !land_start_found) {
		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: land start required.");
		return false;
	}

	if (land_start_found && (!landing_valid || (do_land_start_index > landing_approach_index))) {
		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: invalid land start.");
		return false;
	}

	/* No landing waypoints or no waypoints */
	return true;
}