transition_result_t arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status const struct safety_s *safety, ///< current safety settings arming_state_t new_arming_state, ///< arming state requested struct actuator_armed_s *armed, ///< current armed status bool fRunPreArmChecks, ///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none { // Double check that our static arrays are still valid ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0); ASSERT(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1); transition_result_t ret = TRANSITION_DENIED; arming_state_t current_arming_state = status->arming_state; bool feedback_provided = false; /* only check transition if the new state is actually different from the current one */ if (new_arming_state == current_arming_state) { ret = TRANSITION_NOT_CHANGED; } else { /* * Get sensing state if necessary */ int prearm_ret = OK; /* only perform the pre-arm check if we have to */ if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { prearm_ret = preflight_check(status, mavlink_fd, true /* pre-arm */ ); } /* re-run the pre-flight check as long as sensors are failing */ if (!status->condition_system_sensors_initialized && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { prearm_ret = preflight_check(status, mavlink_fd, false /* pre-flight */); status->condition_system_sensors_initialized = !prearm_ret; } /* * Perform an atomic state update */ #ifdef __PX4_NUTTX irqstate_t flags = irqsave(); #endif /* enforce lockdown in HIL */ if (status->hil_state == vehicle_status_s::HIL_STATE_ON) { armed->lockdown = true; prearm_ret = OK; status->condition_system_sensors_initialized = true; /* recover from a prearm fail */ if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY; } } else { armed->lockdown = false; } // Check that we have a valid state transition bool valid_transition = arming_transitions[new_arming_state][status->arming_state]; if (valid_transition) { // We have a good transition. Now perform any secondary validation. if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { // Do not perform pre-arm checks if coming from in air restore // Allow if vehicle_status_s::HIL_STATE_ON if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { // Fail transition if pre-arm check fails if (prearm_ret) { /* the prearm check already prints the reject reason */ feedback_provided = true; valid_transition = false; // Fail transition if we need safety switch press } else if (safety->safety_switch_available && !safety->safety_off) { mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!"); feedback_provided = true; valid_transition = false; } // Perform power checks only if circuit breaker is not // engaged for these checks if (!status->circuit_breaker_engaged_power_check) { // Fail transition if power is not good if (!status->condition_power_input_valid) { mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Connect power module."); feedback_provided = true; valid_transition = false; } // Fail transition if power levels on the avionics rail // are measured but are insufficient if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) { // Check avionics rail voltages if (status->avionics_power_rail_voltage < 4.75f) { mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); feedback_provided = true; valid_transition = false; } else if (status->avionics_power_rail_voltage < 4.9f) { mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); feedback_provided = true; } else if (status->avionics_power_rail_voltage > 5.4f) { mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage); feedback_provided = true; } } } } } else if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) { new_arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; } } // HIL can always go to standby if (status->hil_state == vehicle_status_s::HIL_STATE_ON && new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { valid_transition = true; } // Check if we are trying to arm, checks look good but we are in STANDBY_ERROR if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { if (status->condition_system_sensors_initialized) { mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, reboot before arming"); } else { mavlink_and_console_log_critical(mavlink_fd, "Preflight check failed, refusing to arm"); } feedback_provided = true; } else if ((new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && status->condition_system_sensors_initialized) { mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, reboot to complete"); feedback_provided = true; } else { // Silent ignore feedback_provided = true; } // Sensors need to be initialized for STANDBY state, except for HIL } else if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) && (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) && (!status->condition_system_sensors_initialized)) { if ((!status->condition_system_prearm_error_reported && status->condition_system_hotplug_timeout) || (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection"); status->condition_system_prearm_error_reported = true; } feedback_provided = true; valid_transition = false; } // Finish up the state transition if (valid_transition) { armed->armed = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR; armed->ready_to_arm = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY; ret = TRANSITION_CHANGED; status->arming_state = new_arming_state; } /* reset feedback state */ if (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR && valid_transition) { status->condition_system_prearm_error_reported = false; } if(status->data_link_found_new == true) { status->condition_system_prearm_error_reported = false; } /* end of atomic state update */ #ifdef __PX4_NUTTX irqrestore(flags); #endif } if (ret == TRANSITION_DENIED) { /* print to MAVLink and console if we didn't provide any feedback yet */ if (!feedback_provided) { mavlink_and_console_log_critical(mavlink_fd, "INVAL: %s - %s", state_names[status->arming_state], state_names[new_arming_state]); } } return ret; }
void *ubx_watchdog_loop(void *args) { /* Set thread name */ prctl(PR_SET_NAME, "gps ubx watchdog", getpid()); /* Retrieve file descriptor and thread flag */ struct arg_struct *arguments = (struct arg_struct *)args; int *fd = arguments->fd_ptr; bool *thread_should_exit = arguments->thread_should_exit_ptr; /* GPS watchdog error message skip counter */ bool ubx_healthy = false; uint8_t ubx_fail_count = 0; uint8_t ubx_success_count = 0; bool once_ok = false; int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); //int err_skip_counter = 0; while (!(*thread_should_exit)) { /* if some values are to old reconfigure gps */ int i; pthread_mutex_lock(ubx_mutex); bool all_okay = true; uint64_t timestamp_now = hrt_absolute_time(); for (i = 0; i < UBX_NO_OF_MESSAGES; i++) { // printf("timestamp_now=%llu\n", timestamp_now); // printf("last_message_timestamps=%llu\n", ubx_state->last_message_timestamps[i]); if (timestamp_now - ubx_state->last_message_timestamps[i] > UBX_WATCHDOG_CRITICAL_TIME_MICROSECONDS) { //printf("Warning: GPS ubx message %d not received for a long time\n", i); all_okay = false; } } pthread_mutex_unlock(ubx_mutex); if (!all_okay) { /* gps error */ ubx_fail_count++; // if (err_skip_counter == 0) // { // printf("GPS Watchdog detected gps not running or having problems\n"); // err_skip_counter = 20; // } // err_skip_counter--; //printf("gps_mode_try_all =%u, ubx_fail_count=%u, ubx_healthy=%u, once_ok=%u\n", gps_mode_try_all, ubx_fail_count, ubx_healthy, once_ok); /* If we have too many failures and another mode or baud should be tried, exit... */ if ((gps_mode_try_all == true || gps_baud_try_all == true) && (ubx_fail_count >= UBX_HEALTH_FAIL_COUNTER_LIMIT) && (ubx_healthy == false) && once_ok == false) { if (gps_verbose) printf("[gps] Connection attempt failed, no UBX module found\r\n"); gps_mode_success = false; break; } if (ubx_healthy && ubx_fail_count == UBX_HEALTH_FAIL_COUNTER_LIMIT) { printf("[gps] ERROR: UBX GPS module stopped responding\r\n"); // global_data_send_subsystem_info(&ubx_present_enabled); mavlink_log_critical(mavlink_fd, "[gps] UBX module stopped responding\n"); ubx_healthy = false; ubx_success_count = 0; } /* trying to reconfigure the gps configuration */ configure_gps_ubx(fd); fflush(stdout); sleep(1); } else { /* gps healthy */ ubx_success_count++; if (!ubx_healthy && ubx_success_count == UBX_HEALTH_SUCCESS_COUNTER_LIMIT) { //printf("[gps] ublox UBX module status ok (baud=%d)\r\n", current_gps_speed); // global_data_send_subsystem_info(&ubx_present_enabled_healthy); mavlink_log_info(mavlink_fd, "[gps] UBX module found, status ok\n"); ubx_healthy = true; ubx_fail_count = 0; once_ok = true; } } usleep(UBX_WATCHDOG_WAIT_TIME_MICROSECONDS); } if(gps_verbose) printf("[gps] ubx loop is going to terminate\n"); close(mavlink_fd); return NULL; }
void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd) { /* announce change */ mavlink_log_info(mavlink_fd, "accel calibration started"); /* set to accel calibration mode */ status->flag_preflight_accel_calibration = true; state_machine_publish(status_pub, status, mavlink_fd); /* measure and calculate offsets & scales */ float accel_offs[3]; float accel_scale[3]; int res = do_accel_calibration_mesurements(mavlink_fd, accel_offs, accel_scale); if (res == OK) { /* measurements complete successfully, set parameters */ if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0])) || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1])) || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2])) || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0])) || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1])) || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) { mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed"); } int fd = open(ACCEL_DEVICE_PATH, 0); struct accel_scale ascale = { accel_offs[0], accel_scale[0], accel_offs[1], accel_scale[1], accel_offs[2], accel_scale[2], }; if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) warn("WARNING: failed to set scale / offsets for accel"); close(fd); /* auto-save to EEPROM */ int save_ret = param_save_default(); if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); } mavlink_log_info(mavlink_fd, "accel calibration done"); tune_confirm(); sleep(2); tune_confirm(); sleep(2); /* third beep by cal end routine */ } else { /* measurements error */ mavlink_log_info(mavlink_fd, "accel calibration aborted"); tune_error(); sleep(2); } /* exit accel calibration mode */ status->flag_preflight_accel_calibration = false; state_machine_publish(status_pub, status, mavlink_fd); }
/** * Transition from one hil state to another */ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { transition_result_t ret = TRANSITION_DENIED; if (current_status->hil_state == new_state) { ret = TRANSITION_NOT_CHANGED; } else { switch (new_state) { case vehicle_status_s::HIL_STATE_OFF: /* we're in HIL and unexpected things can happen if we disable HIL now */ mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)"); ret = TRANSITION_DENIED; break; case vehicle_status_s::HIL_STATE_ON: if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { #ifdef __PX4_NUTTX /* Disable publication of all attached sensors */ /* list directory */ DIR *d; d = opendir("/dev"); if (d) { struct dirent *direntry; char devname[24]; while ((direntry = readdir(d)) != NULL) { /* skip serial ports */ if (!strncmp("tty", direntry->d_name, 3)) { continue; } /* skip mtd devices */ if (!strncmp("mtd", direntry->d_name, 3)) { continue; } /* skip ram devices */ if (!strncmp("ram", direntry->d_name, 3)) { continue; } /* skip MMC devices */ if (!strncmp("mmc", direntry->d_name, 3)) { continue; } /* skip mavlink */ if (!strcmp("mavlink", direntry->d_name)) { continue; } /* skip console */ if (!strcmp("console", direntry->d_name)) { continue; } /* skip null */ if (!strcmp("null", direntry->d_name)) { continue; } snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name); int sensfd = ::open(devname, 0); if (sensfd < 0) { warn("failed opening device %s", devname); continue; } int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1); close(sensfd); printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); } closedir(d); ret = TRANSITION_CHANGED; mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); } else { /* failed opening dir */ mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY"); ret = TRANSITION_DENIED; } #else const char *devname; unsigned int handle = 0; for(;;) { devname = px4_get_device_names(&handle); if (devname == NULL) break; /* skip mavlink */ if (!strcmp("/dev/mavlink", devname)) { continue; } int sensfd = px4_open(devname, 0); if (sensfd < 0) { warn("failed opening device %s", devname); continue; } int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1); px4_close(sensfd); printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); } ret = TRANSITION_CHANGED; mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); #endif } else { mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed"); ret = TRANSITION_DENIED; } break; default: warnx("Unknown HIL state"); break; } } if (ret == TRANSITION_CHANGED) { current_status->hil_state = new_state; current_status->timestamp = hrt_absolute_time(); // XXX also set lockdown here orb_publish(ORB_ID(vehicle_status), status_pub, current_status); } return ret; }
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; }
int Geofence::loadFromFile(const char *filename) { FILE *fp; char line[120]; int pointCounter = 0; bool gotVertical = false; const char commentChar = '#'; int rc = ERROR; /* Make sure no data is left in the datamanager */ clearDm(); /* open the mixer definition file */ fp = fopen(GEOFENCE_FILENAME, "r"); if (fp == NULL) { return ERROR; } /* create geofence points from valid lines and store in DM */ for (;;) { /* get a line, bail on error/EOF */ if (fgets(line, sizeof(line), fp) == NULL) { break; } /* Trim leading whitespace */ size_t textStart = 0; while ((textStart < sizeof(line) / sizeof(char)) && isspace(line[textStart])) { textStart++; } /* if the line starts with #, skip */ if (line[textStart] == commentChar) { continue; } /* if there is only a linefeed, skip it */ if (line[0] == '\n') { continue; } if (gotVertical) { /* Parse the line as a geofence point */ struct fence_vertex_s vertex; /* if the line starts with DMS, this means that the coordinate is given as degree minute second instead of decimal degrees */ if (line[textStart] == 'D' && line[textStart + 1] == 'M' && line[textStart + 2] == 'S') { /* Handle degree minute second format */ float lat_d, lat_m, lat_s, lon_d, lon_m, lon_s; if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6) { warnx("Scanf to parse DMS geofence vertex failed."); goto error; } // warnx("Geofence DMS: %.5f %.5f %.5f ; %.5f %.5f %.5f", (double)lat_d, (double)lat_m, (double)lat_s, (double)lon_d, (double)lon_m, (double)lon_s); vertex.lat = lat_d + lat_m / 60.0f + lat_s / 3600.0f; vertex.lon = lon_d + lon_m / 60.0f + lon_s / 3600.0f; } else { /* Handle decimal degree format */ if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) { warnx("Scanf to parse geofence vertex failed."); goto error; } } if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex)) { goto error; } warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter, (double)vertex.lat, (double)vertex.lon); pointCounter++; } else { /* Parse the line as the vertical limits */ if (sscanf(line, "%f %f", &_altitude_min, &_altitude_max) != 2) { goto error; } warnx("Geofence: alt min: %.4f, alt_max: %.4f", (double)_altitude_min, (double)_altitude_max); gotVertical = true; } } /* Check if import was successful */ if (gotVertical && pointCounter > 0) { _verticesCount = pointCounter; warnx("Geofence: imported successfully"); mavlink_log_info(_mavlinkFd, "Geofence imported"); rc = OK; } else { warnx("Geofence: import error"); mavlink_log_critical(_mavlinkFd, "Geofence import error"); } error: fclose(fp); return rc; }
void Navigator::task_main() { bool have_geofence_position_data = false; /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file * else clear geofence data in datamanager */ struct stat buffer; if (stat(GEOFENCE_FILENAME, &buffer) == 0) { PX4_INFO("Try to load geofence.txt"); _geofence.loadFromFile(GEOFENCE_FILENAME); } else { if (_geofence.clearDm() != OK) { mavlink_log_critical(&_mavlink_log_pub, "failed clearing geofence"); } } /* do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _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)); /* copy all topics first time */ vehicle_status_update(); vehicle_land_detected_update(); vehicle_control_mode_update(); global_position_update(); gps_position_update(); sensor_combined_update(); home_position_update(true); fw_pos_ctrl_status_update(); params_update(); /* wakeup source(s) */ px4_pollfd_struct_t fds[1] = {}; /* Setup of loop */ fds[0].fd = _global_pos_sub; fds[0].events = POLLIN; bool global_pos_available_once = false; while (!_task_should_exit) { /* wait for up to 200ms for data */ int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); if (pret == 0) { /* timed out - periodic check for _task_should_exit, etc. */ if (global_pos_available_once) { global_pos_available_once = false; PX4_WARN("global position timeout"); } /* Let the loop run anyway, don't do `continue` here. */ } else if (pret < 0) { /* this is undesirable but not much we can do - might want to flag unhappy status */ PX4_ERR("nav: poll error %d, %d", pret, errno); usleep(10000); continue; } else { if (fds[0].revents & POLLIN) { /* success, global pos is available */ global_position_update(); if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { have_geofence_position_data = true; } global_pos_available_once = true; } } perf_begin(_loop_perf); bool updated; /* gps updated */ orb_check(_gps_pos_sub, &updated); if (updated) { gps_position_update(); if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) { have_geofence_position_data = true; } } /* sensors combined updated */ orb_check(_sensor_combined_sub, &updated); if (updated) { sensor_combined_update(); } /* parameters updated */ orb_check(_param_update_sub, &updated); if (updated) { params_update(); updateParams(); } /* vehicle control mode updated */ orb_check(_control_mode_sub, &updated); if (updated) { vehicle_control_mode_update(); } /* vehicle status updated */ orb_check(_vstatus_sub, &updated); if (updated) { vehicle_status_update(); } /* vehicle land detected updated */ orb_check(_land_detected_sub, &updated); if (updated) { vehicle_land_detected_update(); } /* navigation capabilities updated */ orb_check(_fw_pos_ctrl_status_sub, &updated); if (updated) { fw_pos_ctrl_status_update(); } /* home position updated */ orb_check(_home_pos_sub, &updated); if (updated) { home_position_update(); } orb_check(_vehicle_command_sub, &updated); if (updated) { vehicle_command_s cmd; orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) { struct position_setpoint_triplet_s *rep = get_reposition_triplet(); // store current position as previous position and goal as next rep->previous.yaw = get_global_position()->yaw; rep->previous.lat = get_global_position()->lat; rep->previous.lon = get_global_position()->lon; rep->previous.alt = get_global_position()->alt; rep->current.loiter_radius = get_loiter_radius(); rep->current.loiter_direction = 1; rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; // Go on and check which changes had been requested if (PX4_ISFINITE(cmd.param4)) { rep->current.yaw = cmd.param4; } else { rep->current.yaw = NAN; } if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7; rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7; } else { rep->current.lat = get_global_position()->lat; rep->current.lon = get_global_position()->lon; } if (PX4_ISFINITE(cmd.param7)) { rep->current.alt = cmd.param7; } else { rep->current.alt = get_global_position()->alt; } rep->previous.valid = true; rep->current.valid = true; rep->next.valid = false; } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) { struct position_setpoint_triplet_s *rep = get_takeoff_triplet(); // store current position as previous position and goal as next rep->previous.yaw = get_global_position()->yaw; rep->previous.lat = get_global_position()->lat; rep->previous.lon = get_global_position()->lon; rep->previous.alt = get_global_position()->alt; rep->current.loiter_radius = get_loiter_radius(); rep->current.loiter_direction = 1; rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; rep->current.yaw = cmd.param4; if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7; rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7; } else { // If one of them is non-finite, reset both rep->current.lat = NAN; rep->current.lon = NAN; } rep->current.alt = cmd.param7; rep->previous.valid = true; rep->current.valid = true; rep->next.valid = false; } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_LAND_START) { /* find NAV_CMD_DO_LAND_START in the mission and * use MAV_CMD_MISSION_START to start the mission there */ unsigned land_start = _mission.find_offboard_land_start(); if (land_start != -1) { vehicle_command_s vcmd = {}; vcmd.target_system = get_vstatus()->system_id; vcmd.target_component = get_vstatus()->component_id; vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START; vcmd.param1 = land_start; vcmd.param2 = 0; orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH); (void)orb_unadvertise(pub); } } 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)) { _mission.set_current_offboard_mission_index(cmd.param1); } } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_PAUSE_CONTINUE) { PX4_INFO("got pause/continue command"); } } /* Check geofence violation */ static hrt_abstime last_geofence_check = 0; if (have_geofence_position_data && (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && (hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) { bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, home_position_valid()); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; _geofence_result.geofence_action = _geofence.getGeofenceAction(); if (!inside) { /* inform other apps via the mission result */ _geofence_result.geofence_violated = true; publish_geofence_result(); /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { mavlink_log_critical(&_mavlink_log_pub, "Geofence violation"); _geofence_violation_warning_sent = true; } } else { /* inform other apps via the mission result */ _geofence_result.geofence_violated = false; publish_geofence_result(); /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } } /* Do stuff according to navigation state set by commander */ switch (_vstatus.nav_state) { case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_mission; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_loiter; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: _pos_sp_triplet_published_invalid_once = false; if (_param_rcloss_act.get() == 1) { _navigation_mode = &_loiter; } else if (_param_rcloss_act.get() == 3) { _navigation_mode = &_land; } else if (_param_rcloss_act.get() == 4) { _navigation_mode = &_rcLoss; } else { /* if == 2 or unknown, RTL */ _navigation_mode = &_rtl; } break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_rtl; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_takeoff; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_land; break; case vehicle_status_s::NAVIGATION_STATE_DESCEND: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_land; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ _pos_sp_triplet_published_invalid_once = false; if (_param_datalinkloss_act.get() == 1) { _navigation_mode = &_loiter; } else if (_param_datalinkloss_act.get() == 3) { _navigation_mode = &_land; } else if (_param_datalinkloss_act.get() == 4) { _navigation_mode = &_dataLinkLoss; } else { /* if == 2 or unknown, RTL */ _navigation_mode = &_rtl; } break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_engineFailure; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_gpsFailure; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_follow_target; break; case vehicle_status_s::NAVIGATION_STATE_MANUAL: case vehicle_status_s::NAVIGATION_STATE_ACRO: case vehicle_status_s::NAVIGATION_STATE_ALTCTL: case vehicle_status_s::NAVIGATION_STATE_POSCTL: case vehicle_status_s::NAVIGATION_STATE_TERMINATION: case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: case vehicle_status_s::NAVIGATION_STATE_STAB: default: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = nullptr; _can_loiter_at_sp = false; break; } /* iterate through navigation modes and set active/inactive for each */ for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); } /* if nothing is running, set position setpoint triplet invalid once */ if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) { _pos_sp_triplet_published_invalid_once = true; _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; _pos_sp_triplet_updated = true; } if (_pos_sp_triplet_updated) { _pos_sp_triplet.timestamp = hrt_absolute_time(); publish_position_setpoint_triplet(); _pos_sp_triplet_updated = false; } if (_mission_result_updated) { publish_mission_result(); _mission_result_updated = false; } perf_end(_loop_perf); } PX4_INFO("exiting"); _navigator_task = -1; return; }
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::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)) { const bool fw_status_valid = (_navigator->get_fw_pos_ctrl_status()->timestamp > 0); const float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat, missionitem_previous.lon, missionitem.lat, missionitem.lon); if (fw_status_valid && (wp_distance > _navigator->get_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) { const float horizontal_slope_displacement = _navigator->get_fw_pos_ctrl_status()->landing_horizontal_slope_displacement; const float slope_angle_rad = _navigator->get_fw_pos_ctrl_status()->landing_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) { /* 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."); 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: 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; }
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; }
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; }
void Mission::set_mission_items() { /* make sure param is up to date */ updateParams(); /* reset the altitude foh logic, if altitude foh is enabled (param) a new foh element starts now */ altitude_sp_foh_reset(); struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); /* the home dist check provides user feedback, so we initialize it to this */ bool user_feedback_done = false; /* mission item that comes after current if available */ struct mission_item_s mission_item_next_position; bool has_next_position_item = false; work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT; /* copy information about the previous mission item */ if (item_contains_position(&_mission_item) && pos_sp_triplet->current.valid) { /* Copy previous mission item altitude */ _mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item); } /* try setting onboard mission item */ if (_param_onboard_enabled.get() && prepare_mission_items(true, &_mission_item, &mission_item_next_position, &has_next_position_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_ONBOARD) { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "onboard mission now running"); user_feedback_done = true; } _mission_type = MISSION_TYPE_ONBOARD; /* try setting offboard mission item */ } else if (prepare_mission_items(false, &_mission_item, &mission_item_next_position, &has_next_position_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { mavlink_log_info(_navigator->get_mavlink_log_pub(), "offboard mission now running"); user_feedback_done = true; } _mission_type = MISSION_TYPE_OFFBOARD; } else { /* no mission available or mission finished, switch to loiter */ if (_mission_type != MISSION_TYPE_NONE) { /* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */ mavlink_log_critical(_navigator->get_mavlink_log_pub(), "mission finished, loitering"); user_feedback_done = true; /* use last setpoint for loiter */ _navigator->set_can_loiter_at_sp(true); } _mission_type = MISSION_TYPE_NONE; /* set loiter mission item and ensure that there is a minimum clearance from home */ set_loiter_item(&_mission_item, _param_takeoff_alt.get()); /* update position setpoint triplet */ pos_sp_triplet->previous.valid = false; 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 */ _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); set_mission_finished(); if (!user_feedback_done) { /* only tell users that we got no mission if there has not been any * better, more specific feedback yet * https://en.wikipedia.org/wiki/Loiter_(aeronautics) */ if (_navigator->get_land_detected()->landed) { /* landed, refusing to take off without a mission */ mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no valid mission available, refusing takeoff"); } else { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no valid mission available, loitering"); } user_feedback_done = true; } _navigator->set_position_setpoint_triplet_updated(); return; } /*********************************** handle mission item *********************************************/ /* handle position mission items */ if (item_contains_position(&_mission_item)) { /* force vtol land */ if(_mission_item.nav_cmd == NAV_CMD_LAND && _param_force_vtol.get() && !_navigator->get_vstatus()->is_rotary_wing){ _mission_item.nav_cmd = NAV_CMD_VTOL_LAND; } /* we have a new position item so set previous position setpoint to current */ set_previous_pos_setpoint(); /* do takeoff before going to setpoint if needed and not already in takeoff */ if (do_need_takeoff() && _work_item_type != WORK_ITEM_TYPE_TAKEOFF) { new_work_item_type = WORK_ITEM_TYPE_TAKEOFF; /* use current mission item as next position item */ memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s)); mission_item_next_position.nav_cmd = NAV_CMD_WAYPOINT; has_next_position_item = true; float takeoff_alt = calculate_takeoff_altitude(&_mission_item); mavlink_log_info(_navigator->get_mavlink_log_pub(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); _mission_item.nav_cmd = NAV_CMD_TAKEOFF; _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; /* ignore yaw for takeoff items */ _mission_item.yaw = NAN; _mission_item.altitude = takeoff_alt; _mission_item.altitude_is_relative = false; _mission_item.autocontinue = true; _mission_item.time_inside = 0; } /* if we just did a takeoff navigate to the actual waypoint now */ if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) { if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF && _navigator->get_vstatus()->is_rotary_wing && !_navigator->get_land_detected()->landed && has_next_position_item) { /* check if the vtol_takeoff command is on top of us */ if(do_need_move_to_takeoff()){ new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF; } else { new_work_item_type = WORK_ITEM_TYPE_DEFAULT; } _mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION; _mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW; _mission_item.yaw = _navigator->get_global_position()->yaw; } else { new_work_item_type = WORK_ITEM_TYPE_DEFAULT; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ _mission_item.yaw = NAN; } } /* takeoff completed and transitioned, move to takeoff wp as fixed wing */ if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF && _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF) { new_work_item_type = WORK_ITEM_TYPE_DEFAULT; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.autocontinue = true; _mission_item.time_inside = 0.0f; } /* move to land wp as fixed wing */ if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND && _work_item_type == WORK_ITEM_TYPE_DEFAULT && !_navigator->get_land_detected()->landed) { new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND; /* use current mission item as next position item */ memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s)); has_next_position_item = true; float altitude = _navigator->get_global_position()->alt; if (pos_sp_triplet->current.valid) { altitude = pos_sp_triplet->current.alt; } _mission_item.altitude = altitude; _mission_item.altitude_is_relative = false; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.autocontinue = true; _mission_item.time_inside = 0; } /* transition to MC */ if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND && _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND && !_navigator->get_vstatus()->is_rotary_wing && !_navigator->get_land_detected()->landed) { _mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION; _mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; _mission_item.autocontinue = true; new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION; } /* move to landing waypoint before descent if necessary */ if (do_need_move_to_land() && (_work_item_type == WORK_ITEM_TYPE_DEFAULT || _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) { new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND; /* use current mission item as next position item */ memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s)); has_next_position_item = true; /* * Ignoring waypoint altitude: * Set altitude to the same as we have now to prevent descending too fast into * the ground. Actual landing will descend anyway until it touches down. * XXX: We might want to change that at some point if it is clear to the user * what the altitude means on this waypoint type. */ float altitude = _navigator->get_global_position()->alt; _mission_item.altitude = altitude; _mission_item.altitude_is_relative = false; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.autocontinue = true; _mission_item.time_inside = 5; } /* we just moved to the landing waypoint, now descend */ if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND && _navigator->get_vstatus()->is_rotary_wing) { new_work_item_type = WORK_ITEM_TYPE_DEFAULT; } /* ignore yaw for landing items */ /* XXX: if specified heading for landing is desired we could add another step before the descent * that aligns the vehicle first */ if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND ) { _mission_item.yaw = NAN; } /* handle non-position mission items such as commands */ } else { /* turn towards next waypoint before MC to FW transition */ if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION && _work_item_type != WORK_ITEM_TYPE_ALIGN && _navigator->get_vstatus()->is_rotary_wing && !_navigator->get_land_detected()->landed && has_next_position_item) { new_work_item_type = WORK_ITEM_TYPE_ALIGN; set_align_mission_item(&_mission_item, &mission_item_next_position); } /* yaw is aligned now */ if (_work_item_type == WORK_ITEM_TYPE_ALIGN) { new_work_item_type = WORK_ITEM_TYPE_DEFAULT; } } /*********************************** set setpoints and check next *********************************************/ /* set current position setpoint from mission item (is protected agains non-position items) */ 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); /* set current work item type */ _work_item_type = new_work_item_type; /* require takeoff after landing or idle */ if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _need_takeoff = true; } _navigator->set_can_loiter_at_sp(false); reset_mission_item_reached(); if (_mission_type == MISSION_TYPE_OFFBOARD) { set_current_offboard_mission_item(); } // TODO: report onboard mission item somehow if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) { /* 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); } else { /* next mission item is not available */ pos_sp_triplet->next.valid = false; } } else { /* vehicle will be paused on current waypoint, don't set next item */ pos_sp_triplet->next.valid = false; } /* Save the distance between the current sp and the previous one */ if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) { _distance_current_previous = get_distance_to_next_waypoint( pos_sp_triplet->current.lat, pos_sp_triplet->current.lon, pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon); } _navigator->set_position_setpoint_triplet_updated(); }
int do_airspeed_calibration(int mavlink_fd) { /* give directions */ mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); const unsigned calibration_count = 2000; int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); struct differential_pressure_s diff_pres; float diff_pres_offset = 0.0f; /* Reset sensor parameters */ struct airspeed_scale airscale = { diff_pres_offset, 1.0f, }; bool paramreset_successful = false; int fd = open(AIRSPEED_DEVICE_PATH, 0); if (fd > 0) { if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { paramreset_successful = true; } else { mavlink_log_critical(mavlink_fd, "airspeed offset zero failed"); } close(fd); } if (!paramreset_successful) { /* only warn if analog scaling is zero */ float analog_scaling = 0.0f; param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)); if (fabsf(analog_scaling) < 0.1f) { mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]"); close(diff_pres_sub); return ERROR; } /* set scaling offset parameter */ if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); close(diff_pres_sub); return ERROR; } } unsigned calibration_counter = 0; mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind"); usleep(500 * 1000); while (calibration_counter < calibration_count) { /* wait blocking for new data */ struct pollfd fds[1]; fds[0].fd = diff_pres_sub; fds[0].events = POLLIN; int poll_ret = poll(fds, 1, 1000); if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); diff_pres_offset += diff_pres.differential_pressure_raw_pa; calibration_counter++; if (calibration_counter % (calibration_count / 20) == 0) { mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 80) / calibration_count); } } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); close(diff_pres_sub); return ERROR; } } diff_pres_offset = diff_pres_offset / calibration_count; if (isfinite(diff_pres_offset)) { int fd_scale = open(AIRSPEED_DEVICE_PATH, 0); airscale.offset_pa = diff_pres_offset; if (fd_scale > 0) { if (OK != ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { mavlink_log_critical(mavlink_fd, "airspeed offset update failed"); } close(fd_scale); } if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); close(diff_pres_sub); return ERROR; } /* auto-save to EEPROM */ int save_ret = param_save_default(); if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); close(diff_pres_sub); return ERROR; } } else { mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); close(diff_pres_sub); return ERROR; } mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset); /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); calibration_counter = 0; const unsigned maxcount = 3000; /* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */ while (calibration_counter < maxcount) { /* wait blocking for new data */ struct pollfd fds[1]; fds[0].fd = diff_pres_sub; fds[0].events = POLLIN; int poll_ret = poll(fds, 1, 1000); if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); calibration_counter++; if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { if (calibration_counter % 100 == 0) { mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)", (int)diff_pres.differential_pressure_raw_pa); } continue; } /* do not allow negative values */ if (diff_pres.differential_pressure_raw_pa < 0.0f) { mavlink_log_info(mavlink_fd, "negative pressure: ERROR (%d Pa)", (int)diff_pres.differential_pressure_raw_pa); mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ diff_pres_offset = 0.0f; if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); close(diff_pres_sub); return ERROR; } /* save */ mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 0); (void)param_save_default(); close(diff_pres_sub); mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); return ERROR; } else { mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)", (int)diff_pres.differential_pressure_raw_pa); break; } } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); close(diff_pres_sub); return ERROR; } } if (calibration_counter == maxcount) { mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); close(diff_pres_sub); return ERROR; } mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100); mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); tune_neutral(true); close(diff_pres_sub); return OK; }
int AttitudePositionEstimatorEKF::check_filter_state() { /* * CHECK IF THE INPUT DATA IS SANE */ struct ekf_status_report ekf_report; int check = _ekf->CheckAndBound(&ekf_report); const char *const feedback[] = { 0, "NaN in states, resetting", "stale sensor data, resetting", "got initial position lock", "excessive gyro offsets", "velocity diverted, check accel config", "excessive covariances", "unknown condition, resetting" }; // Print out error condition if (check) { unsigned warn_index = static_cast<unsigned>(check); unsigned max_warn_index = (sizeof(feedback) / sizeof(feedback[0])); if (max_warn_index < warn_index) { warn_index = max_warn_index; } // Do not warn about accel offset if we have no position updates if (!(warn_index == 5 && _ekf->staticMode)) { warnx("reset: %s", feedback[warn_index]); mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]); } } struct estimator_status_report rep; memset(&rep, 0, sizeof(rep)); // If error flag is set, we got a filter reset if (check && ekf_report.error) { // Count the reset condition perf_count(_perf_reset); } else if (_ekf_logging) { _ekf->GetFilterState(&ekf_report); } if (_ekf_logging || check) { rep.timestamp = hrt_absolute_time(); rep.nan_flags |= (((uint8_t)ekf_report.angNaN) << 0); rep.nan_flags |= (((uint8_t)ekf_report.summedDelVelNaN) << 1); rep.nan_flags |= (((uint8_t)ekf_report.KHNaN) << 2); rep.nan_flags |= (((uint8_t)ekf_report.KHPNaN) << 3); rep.nan_flags |= (((uint8_t)ekf_report.PNaN) << 4); rep.nan_flags |= (((uint8_t)ekf_report.covarianceNaN) << 5); rep.nan_flags |= (((uint8_t)ekf_report.kalmanGainsNaN) << 6); rep.nan_flags |= (((uint8_t)ekf_report.statesNaN) << 7); rep.health_flags |= (((uint8_t)ekf_report.velHealth) << 0); rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1); rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2); rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive) << 3); rep.health_flags |= (((uint8_t)ekf_report.onGround) << 4); rep.health_flags |= (((uint8_t)ekf_report.staticMode) << 5); rep.health_flags |= (((uint8_t)ekf_report.useCompass) << 6); rep.health_flags |= (((uint8_t)ekf_report.useAirspeed) << 7); rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0); rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1); rep.timeout_flags |= (((uint8_t)ekf_report.hgtTimeout) << 2); rep.timeout_flags |= (((uint8_t)ekf_report.imuTimeout) << 3); if (_debug > 10) { if (rep.health_flags < ((1 << 0) | (1 << 1) | (1 << 2) | (1 << 3))) { warnx("health: VEL:%s POS:%s HGT:%s OFFS:%s", ((rep.health_flags & (1 << 0)) ? "OK" : "ERR"), ((rep.health_flags & (1 << 1)) ? "OK" : "ERR"), ((rep.health_flags & (1 << 2)) ? "OK" : "ERR"), ((rep.health_flags & (1 << 3)) ? "OK" : "ERR")); } if (rep.timeout_flags) { warnx("timeout: %s%s%s%s", ((rep.timeout_flags & (1 << 0)) ? "VEL " : ""), ((rep.timeout_flags & (1 << 1)) ? "POS " : ""), ((rep.timeout_flags & (1 << 2)) ? "HGT " : ""), ((rep.timeout_flags & (1 << 3)) ? "IMU " : "")); } } // Copy all states or at least all that we can fit size_t ekf_n_states = ekf_report.n_states; size_t max_states = (sizeof(rep.states) / sizeof(rep.states[0])); rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states; for (size_t i = 0; i < rep.n_states; i++) { rep.states[i] = ekf_report.states[i]; } if (_estimator_status_pub > 0) { orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep); } else { _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep); } } return check; }
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, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad); float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.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 > _nav_caps.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_fd, "Landing: last waypoint too high/too close"); mavlink_log_critical(_mavlink_fd, "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_fd, "Landing waypoint above last nav waypoint"); return false; } } else { /* Last wp is in flare region */ //xxx give recommendations mavlink_log_critical(_mavlink_fd, "Warning: Landing: last waypoint in flare region"); return false; } } else { mavlink_log_critical(_mavlink_fd, "Warning: starting with land waypoint"); return false; } } } /* No landing waypoints or no waypoints */ return true; }
void Takeoff::set_takeoff_position() { struct position_setpoint_triplet_s *rep = _navigator->get_takeoff_triplet(); float abs_altitude = 0.0f; float min_abs_altitude; if (_navigator->home_position_valid()) { //only use home position if it is valid min_abs_altitude = _navigator->get_global_position()->alt + _param_min_alt.get(); } else { //e.g. flow min_abs_altitude = _param_min_alt.get(); } // Use altitude if it has been set. If home position is invalid use min_abs_altitude if (rep->current.valid && PX4_ISFINITE(rep->current.alt) && _navigator->home_position_valid()) { abs_altitude = rep->current.alt; // If the altitude suggestion is lower than home + minimum clearance, raise it and complain. if (abs_altitude < min_abs_altitude) { abs_altitude = min_abs_altitude; mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Using minimum takeoff altitude: %.2f m", (double)_param_min_alt.get()); } } else { // Use home + minimum clearance but only notify. abs_altitude = min_abs_altitude; mavlink_log_info(_navigator->get_mavlink_log_pub(), "Using minimum takeoff altitude: %.2f m", (double)_param_min_alt.get()); } if (abs_altitude < _navigator->get_global_position()->alt) { // If the suggestion is lower than our current alt, let's not go down. abs_altitude = _navigator->get_global_position()->alt; mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already higher than takeoff altitude"); } // set current mission item to takeoff set_takeoff_item(&_mission_item, abs_altitude); _navigator->get_mission_result()->reached = false; _navigator->get_mission_result()->finished = false; _navigator->set_mission_result_updated(); reset_mission_item_reached(); // 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); pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.yaw_valid = true; pos_sp_triplet->next.valid = false; if (rep->current.valid) { // Go on and check which changes had been requested if (PX4_ISFINITE(rep->current.yaw)) { pos_sp_triplet->current.yaw = rep->current.yaw; } if (PX4_ISFINITE(rep->current.lat) && PX4_ISFINITE(rep->current.lon)) { pos_sp_triplet->current.lat = rep->current.lat; pos_sp_triplet->current.lon = rep->current.lon; } // mark this as done memset(rep, 0, sizeof(*rep)); } _navigator->set_can_loiter_at_sp(true); _navigator->set_position_setpoint_triplet_updated(); }
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.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 (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_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; } }
void Mission::set_mission_items() { /* make sure param is up to date */ updateParams(); /* reset the altitude foh logic, if altitude foh is enabled (param) a new foh element starts now */ altitude_sp_foh_reset(); struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); /* set previous position setpoint to current */ set_previous_pos_setpoint(); /* Copy previous mission item altitude (can be extended to a copy of the full mission item if needed) */ if (pos_sp_triplet->previous.valid) { _mission_item_previous_alt = _mission_item.altitude; } /* get home distance state */ bool home_dist_ok = check_dist_1wp(); /* the home dist check provides user feedback, so we initialize it to this */ bool user_feedback_done = !home_dist_ok; /* try setting onboard mission item */ if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_ONBOARD) { mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running"); } _mission_type = MISSION_TYPE_ONBOARD; /* try setting offboard mission item */ } else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running"); } _mission_type = MISSION_TYPE_OFFBOARD; } else { /* no mission available or mission finished, switch to loiter */ if (_mission_type != MISSION_TYPE_NONE) { mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished"); /* use last setpoint for loiter */ _navigator->set_can_loiter_at_sp(true); } else if (!user_feedback_done) { /* only tell users that we got no mission if there has not been any * better, more specific feedback yet */ mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available"); } _mission_type = MISSION_TYPE_NONE; /* set loiter mission item */ set_loiter_item(&_mission_item); /* update position setpoint triplet */ pos_sp_triplet->previous.valid = false; 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 */ _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); set_mission_finished(); _navigator->set_position_setpoint_triplet_updated(); return; } if (pos_sp_triplet->current.valid) { _on_arrival_yaw = _mission_item.yaw; } /* do takeoff on first waypoint for rotary wing vehicles */ if (_navigator->get_vstatus()->is_rotary_wing) { /* force takeoff if landed (additional protection) */ if (!_takeoff && _navigator->get_vstatus()->condition_landed) { _need_takeoff = true; } /* new current mission item set, check if we need takeoff */ if (_need_takeoff && ( _mission_item.nav_cmd == NAV_CMD_TAKEOFF || _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_RETURN_TO_LAUNCH)) { _takeoff = true; _need_takeoff = false; } } if (_takeoff) { /* do takeoff before going to setpoint */ /* set mission item as next position setpoint */ mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->next); /* calculate takeoff altitude */ float takeoff_alt = _mission_item.altitude; if (_mission_item.altitude_is_relative) { takeoff_alt += _navigator->get_home_position()->alt; } /* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */ if (_navigator->get_vstatus()->condition_landed) { takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get()); } else { takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get()); } /* check if we already above takeoff altitude */ if (_navigator->get_global_position()->alt < takeoff_alt - _navigator->get_acceptance_radius()) { mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); _mission_item.nav_cmd = NAV_CMD_TAKEOFF; _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; _mission_item.yaw = NAN; _mission_item.altitude = takeoff_alt; _mission_item.altitude_is_relative = false; _mission_item.autocontinue = true; _mission_item.time_inside = 0; mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); _navigator->set_position_setpoint_triplet_updated(); return; } else { /* skip takeoff */ _takeoff = false; } } /* set current position setpoint from mission item */ mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); /* require takeoff after landing or idle */ if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _need_takeoff = true; } _navigator->set_can_loiter_at_sp(false); reset_mission_item_reached(); if (_mission_type == MISSION_TYPE_OFFBOARD) { set_current_offboard_mission_item(); } // TODO: report onboard mission item somehow if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) { /* try to read next mission item */ struct mission_item_s mission_item_next; if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) { /* got next mission item, update setpoint triplet */ mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); } else { /* next mission item is not available */ pos_sp_triplet->next.valid = false; } } else { /* vehicle will be paused on current waypoint, don't set next item */ pos_sp_triplet->next.valid = false; } /* Save the distance between the current sp and the previous one */ if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) { _distance_current_previous = get_distance_to_next_waypoint(pos_sp_triplet->current.lat, pos_sp_triplet->current.lon, pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon); } _navigator->set_position_setpoint_triplet_updated(); }
void *mtk_watchdog_loop(void *args) { // printf("in mtk watchdog loop\n"); fflush(stdout); /* Set thread name */ prctl(PR_SET_NAME, "gps mtk watchdog", getpid()); /* Retrieve file descriptor and thread flag */ struct arg_struct *arguments = (struct arg_struct *)args; int *fd = arguments->fd_ptr; bool *thread_should_exit = arguments->thread_should_exit_ptr; bool mtk_healthy = false; uint8_t mtk_fail_count = 0; uint8_t mtk_success_count = 0; bool once_ok = false; int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); while (!(*thread_should_exit)) { fflush(stdout); /* if we have no update for a long time reconfigure gps */ pthread_mutex_lock(mtk_mutex); uint64_t timestamp_now = hrt_absolute_time(); bool all_okay = true; if (timestamp_now - mtk_state->last_message_timestamp > MTK_WATCHDOG_CRITICAL_TIME_MICROSECONDS) { all_okay = false; } pthread_mutex_unlock(mtk_mutex); if (!all_okay) { // printf("mtk unhealthy\n"); mtk_fail_count++; /* gps error */ // if (err_skip_counter == 0) // { // printf("[gps] GPS module not connected or not responding..\n"); // err_skip_counter = 20; // } // err_skip_counter--; // printf("gps_mode_try_all =%u, mtk_fail_count=%u, mtk_healthy=%u, once_ok=%u\n", gps_mode_try_all, mtk_fail_count, mtk_healthy, once_ok); /* If we have too many failures and another mode or baud should be tried, exit... */ if ((gps_mode_try_all == true || gps_baud_try_all == true) && (mtk_fail_count >= MTK_HEALTH_FAIL_COUNTER_LIMIT) && (mtk_healthy == false) && once_ok == false) { if (gps_verbose) printf("[gps] Connection attempt failed, no MTK module found\r\n"); gps_mode_success = false; break; } if (mtk_healthy && mtk_fail_count >= MTK_HEALTH_FAIL_COUNTER_LIMIT) { printf("[gps] ERROR: MTK GPS module stopped responding\r\n"); // global_data_send_subsystem_info(&mtk_present_enabled); mavlink_log_critical(mavlink_fd, "[gps] MTK module stopped responding\n"); mtk_healthy = false; mtk_success_count = 0; } /* trying to reconfigure the gps configuration */ configure_gps_mtk(fd); fflush(stdout); } else { /* gps healthy */ mtk_success_count++; if (!mtk_healthy && mtk_success_count >= MTK_HEALTH_SUCCESS_COUNTER_LIMIT) { printf("[gps] MTK module found, status ok (baud=%d)\r\n", current_gps_speed); /* MTK never has sat info */ // XXX Check if lock makes sense here mtk_gps->satellite_info_available = 0; // global_data_send_subsystem_info(&mtk_present_enabled_healthy); mavlink_log_info(mavlink_fd, "[gps] MTK custom binary module found, status ok\n"); mtk_healthy = true; mtk_fail_count = 0; once_ok = true; } } usleep(MTK_WATCHDOG_WAIT_TIME_MICROSECONDS); } if(gps_verbose) printf("[gps] mtk watchdog is going to terminate\n"); close(mavlink_fd); return NULL; }
void Navigator::task_main() { _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); _geofence.setMavlinkFd(_mavlink_fd); /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file * else clear geofence data in datamanager */ struct stat buffer; if (stat(GEOFENCE_FILENAME, &buffer) == 0) { warnx("Try to load geofence.txt"); _geofence.loadFromFile(GEOFENCE_FILENAME); } else { mavlink_log_info(_mavlink_fd, "No geofence set"); if (_geofence.clearDm() > 0) warnx("Geofence cleared"); else warnx("Could not clear geofence"); } /* do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); /* copy all topics first time */ vehicle_status_update(); vehicle_control_mode_update(); global_position_update(); gps_position_update(); sensor_combined_update(); home_position_update(); navigation_capabilities_update(); params_update(); /* rate limit position and sensor updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); orb_set_interval(_sensor_combined_sub, 20); hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ struct pollfd fds[8]; /* Setup of loop */ fds[0].fd = _global_pos_sub; fds[0].events = POLLIN; fds[1].fd = _home_pos_sub; fds[1].events = POLLIN; fds[2].fd = _capabilities_sub; fds[2].events = POLLIN; fds[3].fd = _vstatus_sub; fds[3].events = POLLIN; fds[4].fd = _control_mode_sub; fds[4].events = POLLIN; fds[5].fd = _param_update_sub; fds[5].events = POLLIN; fds[6].fd = _sensor_combined_sub; fds[6].events = POLLIN; fds[7].fd = _gps_pos_sub; fds[7].events = POLLIN; while (!_task_should_exit) { /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); if (pret == 0) { /* timed out - periodic check for _task_should_exit, etc. */ continue; } else if (pret < 0) { /* this is undesirable but not much we can do - might want to flag unhappy status */ warn("poll error %d, %d", pret, errno); continue; } perf_begin(_loop_perf); if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time) { /* try to reopen the mavlink log device with specified interval */ mavlink_open_time = hrt_abstime() + mavlink_open_interval; _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } static bool have_geofence_position_data = false; /* gps updated */ if (fds[7].revents & POLLIN) { gps_position_update(); if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) { have_geofence_position_data = true; } } /* sensors combined updated */ if (fds[6].revents & POLLIN) { sensor_combined_update(); } /* parameters updated */ if (fds[5].revents & POLLIN) { params_update(); updateParams(); } /* vehicle control mode updated */ if (fds[4].revents & POLLIN) { vehicle_control_mode_update(); } /* vehicle status updated */ if (fds[3].revents & POLLIN) { vehicle_status_update(); } /* navigation capabilities updated */ if (fds[2].revents & POLLIN) { navigation_capabilities_update(); } /* home position updated */ if (fds[1].revents & POLLIN) { home_position_update(); } /* global position updated */ if (fds[0].revents & POLLIN) { global_position_update(); if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { have_geofence_position_data = true; } } /* Check geofence violation */ static hrt_abstime last_geofence_check = 0; if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) { bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, _home_position_set); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; if (!inside) { /* inform other apps via the mission result */ _geofence_result.geofence_violated = true; publish_geofence_result(); /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation"); _geofence_violation_warning_sent = true; } } else { /* inform other apps via the mission result */ _geofence_result.geofence_violated = false; publish_geofence_result(); /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } } /* Do stuff according to navigation state set by commander */ switch (_vstatus.nav_state) { case vehicle_status_s::NAVIGATION_STATE_MANUAL: case vehicle_status_s::NAVIGATION_STATE_ACRO: case vehicle_status_s::NAVIGATION_STATE_ALTCTL: case vehicle_status_s::NAVIGATION_STATE_POSCTL: case vehicle_status_s::NAVIGATION_STATE_LAND: case vehicle_status_s::NAVIGATION_STATE_TERMINATION: case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: _navigation_mode = nullptr; _can_loiter_at_sp = false; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_mission; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_loiter; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: _pos_sp_triplet_published_invalid_once = false; if (_param_rcloss_obc.get() != 0) { _navigation_mode = &_rcLoss; } else { _navigation_mode = &_rtl; } break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_rtl; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ _pos_sp_triplet_published_invalid_once = false; if (_param_datalinkloss_obc.get() != 0) { _navigation_mode = &_dataLinkLoss; } else { _navigation_mode = &_rtl; } break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_engineFailure; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_gpsFailure; break; default: _navigation_mode = nullptr; _can_loiter_at_sp = false; break; } /* iterate through navigation modes and set active/inactive for each */ for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); } /* if nothing is running, set position setpoint triplet invalid once */ if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) { _pos_sp_triplet_published_invalid_once = true; _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; _pos_sp_triplet_updated = true; } if (_pos_sp_triplet_updated) { publish_position_setpoint_triplet(); _pos_sp_triplet_updated = false; } if (_mission_result_updated) { publish_mission_result(); _mission_result_updated = false; } perf_end(_loop_perf); } warnx("exiting."); _navigator_task = -1; _exit(0); }
void RTL::set_rtl_item() { struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); /* make sure we have the latest params */ updateParams(); if (!_rtl_start_lock) { set_previous_pos_setpoint(); } _navigator->set_can_loiter_at_sp(false); switch (_rtl_state) { case RTL_STATE_CLIMB: { float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get(); _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; _mission_item.altitude_is_relative = false; _mission_item.altitude = climb_alt; _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)", (int)(climb_alt), (int)(climb_alt - _navigator->get_home_position()->alt)); break; } case RTL_STATE_RETURN: { _mission_item.lat = _navigator->get_home_position()->lat; _mission_item.lon = _navigator->get_home_position()->lon; // don't change altitude if (pos_sp_triplet->previous.valid) { /* if previous setpoint is valid then use it to calculate heading to home */ _mission_item.yaw = get_bearing_to_next_waypoint( pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon, _mission_item.lat, _mission_item.lon); } else { /* else use current position */ _mission_item.yaw = get_bearing_to_next_waypoint( _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, _mission_item.lat, _mission_item.lon); } _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)", (int)(_mission_item.altitude), (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); _rtl_start_lock = true; break; } case RTL_STATE_TRANSITION_TO_MC: { _mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION; _mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; break; } case RTL_STATE_DESCEND: { _mission_item.lat = _navigator->get_home_position()->lat; _mission_item.lon = _navigator->get_home_position()->lon; _mission_item.altitude_is_relative = false; _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); _mission_item.yaw = _navigator->get_home_position()->yaw; // except for vtol which might be still off here and should point towards this location float d_current = get_distance_to_next_waypoint( _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, _mission_item.lat, _mission_item.lon); if (_navigator->get_vstatus()->is_vtol && d_current > _navigator->get_acceptance_radius()) { _mission_item.yaw = get_bearing_to_next_waypoint( _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, _mission_item.lat, _mission_item.lon); } _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = false; _mission_item.origin = ORIGIN_ONBOARD; /* disable previous setpoint to prevent drift */ pos_sp_triplet->previous.valid = false; mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)", (int)(_mission_item.altitude), (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); break; } case RTL_STATE_LOITER: { bool autoland = _param_land_delay.get() > -DELAY_SIGMA; _mission_item.lat = _navigator->get_home_position()->lat; _mission_item.lon = _navigator->get_home_position()->lon; // don't change altitude _mission_item.yaw = NAN; // don't bother with yaw _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get(); _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = autoland; _mission_item.origin = ORIGIN_ONBOARD; _navigator->set_can_loiter_at_sp(true); if (autoland) { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs", (double)_mission_item.time_inside); } else { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter"); } break; } case RTL_STATE_LAND: { _mission_item.yaw = _navigator->get_home_position()->yaw; set_land_item(&_mission_item, false); mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: land at home"); break; } case RTL_STATE_LANDED: { set_idle_item(&_mission_item); mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: completed, landed"); break; } default: break; } reset_mission_item_reached(); /* execute command if set */ 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); pos_sp_triplet->next.valid = false; _navigator->set_position_setpoint_triplet_updated(); }