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