void handle_command(struct vehicle_command_s *cmd) { /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; /* request to set different system mode */ switch (cmd->command) { case VEHICLE_CMD_PREFLIGHT_STORAGE: if (((int)(cmd->param3)) == 1) { /* enable logging */ mavlink_log_info(mavlink_fd, "[log] file:"); mavlink_log_info(mavlink_fd, "logdir"); logging_enabled = true; } if (((int)(cmd->param3)) == 0) { /* disable logging */ mavlink_log_info(mavlink_fd, "[log] stopped."); logging_enabled = false; } break; default: /* silently ignore */ break; } }
bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error) { /* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */ for (size_t i = 0; i < nMissionItems; i++) { static struct mission_item_s missionitem; const ssize_t len = sizeof(struct mission_item_s); if (dm_read(dm_current, i, &missionitem, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ if (throw_error) { return false; } else { return true; } } if (home_alt > missionitem.altitude) { if (throw_error) { mavlink_log_info(_mavlink_fd, "Waypoint %d below home", i); return false; } else { mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i); return true; } } } return true; }
void Navigator::on_mission_item_reached() { if (myState == NAV_STATE_MISSION) { _mission.report_mission_item_reached(); if (_do_takeoff) { /* takeoff completed */ _do_takeoff = false; mavlink_log_info(_mavlink_fd, "#audio: takeoff completed"); } else { /* advance by one mission item */ _mission.move_to_next(); } if (_mission.current_mission_available()) { set_mission_item(); } else { /* if no more mission items available then finish mission */ /* loiter at last waypoint */ _reset_loiter_pos = false; mavlink_log_info(_mavlink_fd, "[navigator] mission completed"); request_loiter_or_ready(); } } else if (myState == NAV_STATE_RTL) { /* RTL completed */ if (_rtl_state == RTL_STATE_DESCEND) { /* hovering above home position, land if needed or loiter */ mavlink_log_info(_mavlink_fd, "[navigator] RTL completed"); if (_mission_item.autocontinue) { dispatch(EVENT_LAND_REQUESTED); } else { _reset_loiter_pos = false; dispatch(EVENT_LOITER_REQUESTED); } } else { /* next RTL step */ _rtl_state = (RTLState)(_rtl_state + 1); set_rtl_item(); } } else if (myState == NAV_STATE_LAND) { /* landing completed */ mavlink_log_info(_mavlink_fd, "[navigator] landing completed"); dispatch(EVENT_READY_REQUESTED); } _mission.publish_mission_result(); }
int do_accel_calibration(int mavlink_fd) { /* announce change */ mavlink_log_info(mavlink_fd, "accel calibration started"); mavlink_log_info(mavlink_fd, "accel cal progress <0> percent"); /* measure and calculate offsets & scales */ float accel_offs[3]; float accel_scale[3]; int res = do_accel_calibration_measurements(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"); return OK; } else { /* measurements error */ mavlink_log_info(mavlink_fd, "accel calibration aborted"); return ERROR; } /* exit accel calibration mode */ }
static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_status) { bool success = true; // start with a pass and change to a fail if any test fails float test_limit = 1.0f; // pass limit re-used for each test // Get sensor_preflight data if available and exit with a fail recorded if not int sensors_sub = orb_subscribe(ORB_ID(sensor_preflight)); sensor_preflight_s sensors = {}; if (orb_copy(ORB_ID(sensor_preflight), sensors_sub, &sensors) != PX4_OK) { goto out; } // Use the difference between IMU's to detect a bad calibration. // If a single IMU is fitted, the value being checked will be zero so this check will always pass. param_get(param_find("COM_ARM_IMU_ACC"), &test_limit); if (sensors.accel_inconsistency_m_s_s > test_limit) { if (report_status) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCELS INCONSISTENT - CHECK CAL"); } success = false; goto out; } else if (sensors.accel_inconsistency_m_s_s > test_limit * 0.8f) { if (report_status) { mavlink_log_info(mavlink_log_pub, "PREFLIGHT ADVICE: ACCELS INCONSISTENT - CHECK CAL"); } } // Fail if gyro difference greater than 5 deg/sec and notify if greater than 2.5 deg/sec param_get(param_find("COM_ARM_IMU_GYR"), &test_limit); if (sensors.gyro_inconsistency_rad_s > test_limit) { if (report_status) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYROS INCONSISTENT - CHECK CAL"); } success = false; goto out; } else if (sensors.gyro_inconsistency_rad_s > test_limit * 0.5f) { if (report_status) { mavlink_log_info(mavlink_log_pub, "PREFLIGHT ADVICE: GYROS INCONSISTENT - CHECK CAL"); } } out: orb_unsubscribe(sensors_sub); return success; }
void RunwayTakeoff::update(float airspeed, float alt_agl, double current_lat, double current_lon, orb_advert_t *mavlink_log_pub) { switch (_state) { case RunwayTakeoffState::THROTTLE_RAMP: if (hrt_elapsed_time(&_initialized_time) > _throttle_ramp_time) { _state = RunwayTakeoffState::CLAMPED_TO_RUNWAY; } break; case RunwayTakeoffState::CLAMPED_TO_RUNWAY: if (airspeed > _airspeed_min.get() * _min_airspeed_scaling.get()) { _state = RunwayTakeoffState::TAKEOFF; mavlink_log_info(mavlink_log_pub, "#Takeoff airspeed reached"); } break; case RunwayTakeoffState::TAKEOFF: if (alt_agl > _nav_alt.get()) { _state = RunwayTakeoffState::CLIMBOUT; /* * If we started in heading hold mode, move the navigation start WP to the current location now. * The navigator will take this as starting point to navigate towards the takeoff WP. */ if (_heading_mode.get() == 0) { _start_wp(0) = (float)current_lat; _start_wp(1) = (float)current_lon; } mavlink_log_info(mavlink_log_pub, "#Climbout"); } break; case RunwayTakeoffState::CLIMBOUT: if (alt_agl > _climbout_diff.get()) { _climbout = false; _state = RunwayTakeoffState::FLY; mavlink_log_info(mavlink_log_pub, "#Navigating to waypoint"); } break; default: break; } }
int open_log_file() { /* string to hold the path to the log */ char log_file_name[16] = ""; char log_file_path[48] = ""; if (log_name_timestamp && gps_time != 0) { /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ time_t gps_time_sec = gps_time / 1000000; struct tm t; gmtime_r(&gps_time_sec, &t); strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.bin", &t); snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); } else { uint16_t file_number = 1; // start with file log001 /* look for the next file that does not exist */ while (file_number <= MAX_NO_LOGFILE) { /* format log file path: e.g. /fs/microsd/sess001/log001.bin */ snprintf(log_file_name, sizeof(log_file_name), "log%03u.bin", file_number); snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); if (!file_exist(log_file_path)) { break; } file_number++; } if (file_number > MAX_NO_LOGFILE) { /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ warnx("all %d possible files exist already", MAX_NO_LOGFILE); return -1; } } int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC); if (fd < 0) { warn("failed opening log: %s", log_file_name); mavlink_log_info(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); } else { warnx("log file: %s", log_file_name); mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name); } return fd; }
void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp, double lat, double lon, float gps_alt, float baro_alt) { // Reference altitude if (isfinite(_ekf->states[9])) { _filter_ref_offset = _ekf->states[9]; } else if (isfinite(-_ekf->hgtMea)) { _filter_ref_offset = -_ekf->hgtMea; } else { _filter_ref_offset = -_baro.altitude; } // init filtered gps and baro altitudes _gps_alt_filt = gps_alt; _baro_alt_filt = baro_alt; // Initialize projection _local_pos.ref_lat = lat; _local_pos.ref_lon = lon; _local_pos.ref_alt = gps_alt; _local_pos.ref_timestamp = timestamp; map_projection_init(&_pos_ref, lat, lon); mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); }
int stop_swarm( struct quad_mode_s *state, struct quad_mode_s *mode, orb_advert_t *mode_pub, int *state_sub ) { if ( state->current_state == (enum QUAD_STATE)QUAD_STATE_SWARMING ) { mode->cmd = (enum QUAD_CMD)QUAD_CMD_STOP_SWARM; orb_publish(ORB_ID(quad_mode), *mode_pub, mode); orb_copy(ORB_ID(quad_mode), *state_sub, state); struct pollfd fd_state; fd_state.fd = *state_sub; fd_state.events = POLLIN; int ret_state = poll(&fd_state, 1, time_out); if ( ret_state < 0 ) { warnx("poll cmd error"); } else if ( ret_state == 0 ) { return -1; } else if ( fd_state.revents & POLLIN ) { orb_copy(ORB_ID(quad_mode), *state_sub, state); if ( state->current_state == (enum QUAD_STATE)QUAD_STATE_HOVERING ) return 0; return -3; } else { mavlink_log_info(mavlink_fd, "[QC%i] something is very wrong", system_id); } } else { return -2; } }
bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued, float default_acceptance_rad, bool condition_landed) { bool failed = false; bool warned = false; /* Init if not done yet */ init(); _mavlink_fd = mavlink_fd; // first check if we have a valid position if (!home_valid /* can later use global / local pos for finer granularity */) { failed = true; warned = true; mavlink_log_info(_mavlink_fd, "Not yet ready for mission, no position lock."); } else { failed = failed || !check_dist_1wp(dm_current, nMissionItems, curr_lat, curr_lon, max_waypoint_distance, warning_issued); } // check if all mission item commands are supported failed = failed || !checkMissionItemValidity(dm_current, nMissionItems, condition_landed); failed = failed || !checkGeofence(dm_current, nMissionItems, geofence); failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned); if (isRotarywing) { failed = failed || !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid, default_acceptance_rad); } else { failed = failed || !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid); } return !failed; }
void RTL::on_activation() { /* decide where to enter the RTL procedure when we switch into it */ if (_rtl_state == RTL_STATE_NONE) { /* for safety reasons don't go into RTL if landed */ if (_navigator->get_vstatus()->condition_landed) { _rtl_state = RTL_STATE_LANDED; mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); /* if lower than return altitude, climb up first */ } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + _param_return_alt.get()) { _rtl_state = RTL_STATE_CLIMB; /* otherwise go straight to return */ } else { /* set altitude setpoint to current altitude */ _rtl_state = RTL_STATE_RETURN; _mission_item.altitude_is_relative = false; _mission_item.altitude = _navigator->get_global_position()->alt; } } set_rtl_item(); }
void sdlog2_stop_log() { warnx("stop logging"); mavlink_log_info(mavlink_fd, "[sdlog2] stop logging"); logging_enabled = false; /* wake up write thread one last time */ pthread_mutex_lock(&logbuffer_mutex); logwriter_should_exit = true; pthread_cond_signal(&logbuffer_cond); /* unlock, now the writer thread may return */ pthread_mutex_unlock(&logbuffer_mutex); /* wait for write thread to return */ int ret; if ((ret = pthread_join(logwriter_pthread, NULL)) != 0) { warnx("error joining logwriter thread: %i", ret); } logwriter_pthread = 0; pthread_attr_destroy(&logwriter_attr); sdlog2_status(); }
void sdlog2_start_log() { warnx("start logging."); mavlink_log_info(mavlink_fd, "[sdlog2] start logging"); /* initialize statistics counter */ log_bytes_written = 0; start_time = hrt_absolute_time(); log_msgs_written = 0; log_msgs_skipped = 0; /* initialize log buffer emptying thread */ pthread_attr_t receiveloop_attr; pthread_attr_init(&receiveloop_attr); struct sched_param param; /* low priority, as this is expensive disk I/O */ param.sched_priority = SCHED_PRIORITY_DEFAULT - 40; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); pthread_attr_setstacksize(&receiveloop_attr, 2048); logwriter_should_exit = false; /* start log buffer emptying thread */ if (0 != pthread_create(&logwriter_pthread, &receiveloop_attr, logwriter_thread, &lb)) { errx(1, "error creating logwriter thread"); } logging_enabled = true; // XXX we have to destroy the attr at some point }
int emergency_land( struct quad_mode_s *state, struct quad_mode_s *mode, orb_advert_t *mode_pub, int *state_sub, struct quad_swarm_cmd_s *swarm_cmd ) { mode->cmd = (enum QUAD_CMD)QUAD_CMD_LAND; orb_publish(ORB_ID(quad_mode), *mode_pub, mode); orb_copy(ORB_ID(quad_mode), *state_sub, state); struct pollfd fd_state; fd_state.fd = *state_sub; fd_state.events = POLLIN; int ret_state = poll(&fd_state, 1, time_out); if ( ret_state < 0 ) { warnx("poll cmd error"); } else if ( ret_state == 0 ) { return -1; } else if ( fd_state.revents & POLLIN ) { orb_copy(ORB_ID(quad_mode), *state_sub, state); if ( state->current_state == (enum QUAD_STATE)QUAD_STATE_GROUNDED ) return 0; return -3; } else { mavlink_log_info(mavlink_fd, "[QC%i] something is very wrong", system_id); } state->error = false; swarm_cmd->cmd_id = 0; }
int land( struct quad_mode_s *state, struct quad_mode_s *mode, orb_advert_t *mode_pub, int *state_sub, bool *transition_error ) { if ( (state->current_state == (enum QUAD_STATE)QUAD_STATE_HOVERING) || *transition_error ) { mode->cmd = (enum QUAD_CMD)QUAD_CMD_LAND; orb_publish(ORB_ID(quad_mode), *mode_pub, mode); orb_copy(ORB_ID(quad_mode), *state_sub, state); struct pollfd fd_state; fd_state.fd = *state_sub; fd_state.events = POLLIN; int ret_state = poll(&fd_state, 1, time_out); if ( ret_state < 0 ) { warnx("poll cmd error"); } else if ( ret_state == 0 ) { return -1; } else if ( fd_state.revents & POLLIN ) { orb_copy(ORB_ID(quad_mode), *state_sub, state); if ( state->current_state == (enum QUAD_STATE)QUAD_STATE_GROUNDED ) { return 0; *transition_error = false; } return -3; } else { mavlink_log_info(mavlink_fd, "[QC%i] something is very wrong", system_id); } } else { return -2; } }
void MulticopterPositionControl::reset_alt_sp() { if (_reset_alt_sp) { _reset_alt_sp = false; _pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2); mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2)); } }
void MulticopterPositionControl::reset_alt_sp() { if (_reset_alt_sp) { _reset_alt_sp = false; _pos_sp(2) = _pos(2); mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2)); } }
void sdlog2_status() { float kibibytes = log_bytes_written / 1024.0f; float mebibytes = kibibytes / 1024.0f; float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f; warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped); mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); }
int create_log_dir() { /* create dir on sdcard if needed */ uint16_t dir_number = 1; // start with dir sess001 int mkdir_ret; if (log_name_timestamp && gps_time != 0) { /* use GPS date for log dir naming: e.g. /fs/microsd/2014-01-19 */ time_t gps_time_sec = gps_time / 1000000; struct tm t; gmtime_r(&gps_time_sec, &t); int n = snprintf(log_dir, sizeof(log_dir), "%s/", log_root); strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &t); mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); if (mkdir_ret == OK) { warnx("log dir created: %s", log_dir); } else if (errno != EEXIST) { warn("failed creating new dir: %s", log_dir); return -1; } } else { /* look for the next dir that does not exist */ while (dir_number <= MAX_NO_LOGFOLDER) { /* format log dir: e.g. /fs/microsd/sess001 */ sprintf(log_dir, "%s/sess%03u", log_root, dir_number); mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); if (mkdir_ret == 0) { warnx("log dir created: %s", log_dir); break; } else if (errno != EEXIST) { warn("failed creating new dir: %s", log_dir); return -1; } /* dir exists already */ dir_number++; continue; } if (dir_number >= MAX_NO_LOGFOLDER) { /* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */ warnx("all %d possible dirs exist already", MAX_NO_LOGFOLDER); return -1; } } /* print logging path, important to find log file later */ warnx("log dir: %s", log_dir); mavlink_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir); return 0; }
void MulticopterPositionControl::reset_pos_sp() { if (_reset_pos_sp) { _reset_pos_sp = false; _pos_sp(0) = _pos(0); _pos_sp(1) = _pos(1); mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); } }
bool MissionFeasibilityChecker::checkDistanceToFirstWaypoint(const mission_s &mission, float max_distance) { if (max_distance <= 0.0f) { /* param not set, check is ok */ return true; } /* find first waypoint (with lat/lon) item in datamanager */ 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; } /* check distance from current position 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 < max_distance) { if (dist_to_1wp > ((max_distance * 3) / 2)) { /* allow at 2/3 distance, but warn */ mavlink_log_critical(_navigator->get_mavlink_log_pub(), "First waypoint far away: %d meters.", (int)dist_to_1wp); _navigator->get_mission_result()->warning = true; } return true; } else { /* item is too far from home */ mavlink_log_critical(_navigator->get_mavlink_log_pub(), "First waypoint too far away: %d meters, %d max.", (int)dist_to_1wp, (int)max_distance); _navigator->get_mission_result()->warning = true; return false; } } /* no waypoints found in mission, then we will not fly far away */ return true; }
void AttitudePositionEstimatorEKF::initializeGPS() { // GPS is in scaled integers, convert double lat = _gps.lat / 1.0e7; double lon = _gps.lon / 1.0e7; float gps_alt = _gps.alt / 1e3f; // Set up height correctly orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); _baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame // init filtered gps and baro altitudes _gps_alt_filt = gps_alt; _baro_alt_filt = _baro.altitude; _ekf->baroHgt = _baro.altitude; _ekf->hgtMea = _ekf->baroHgt; // Set up position variables correctly _ekf->GPSstatus = _gps.fix_type; _ekf->gpsLat = math::radians(lat); _ekf->gpsLon = math::radians(lon) - M_PI; _ekf->gpsHgt = gps_alt; // Look up mag declination based on current position float declination = math::radians(get_mag_declination(lat, lon)); float initVelNED[3]; initVelNED[0] = _gps.vel_n_m_s; initVelNED[1] = _gps.vel_e_m_s; initVelNED[2] = _gps.vel_d_m_s; _ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); // Initialize projection _local_pos.ref_lat = lat; _local_pos.ref_lon = lon; _local_pos.ref_alt = gps_alt; _local_pos.ref_timestamp = _gps.timestamp_position; map_projection_init(&_pos_ref, lat, lon); mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); #if 0 warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_ref_offset); warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination)); #endif _gps_initialized = true; }
bool MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, float max_distance_to_1st_waypoint, float max_distance_between_waypoints, bool land_start_req) { dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(mission.dataman_id); const size_t nMissionItems = mission.count; const bool isRotarywing = (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol); Geofence &geofence = _navigator->get_geofence(); fw_pos_ctrl_status_s *fw_pos_ctrl_status = _navigator->get_fw_pos_ctrl_status(); const float home_alt = _navigator->get_home_position()->alt; const bool home_valid = _navigator->home_position_valid(); const bool home_alt_valid = _navigator->home_alt_valid(); bool &warning_issued = _navigator->get_mission_result()->warning; const float default_acceptance_rad = _navigator->get_default_acceptance_radius(); const float default_altitude_acceptance_rad = _navigator->get_altitude_acceptance_radius(); const bool landed = _navigator->get_land_detected()->landed; bool failed = false; bool warned = false; // first check if we have a valid position if (!home_alt_valid) { failed = true; warned = true; mavlink_log_info(_navigator->get_mavlink_log_pub(), "Not yet ready for mission, no position lock."); } else { failed = failed || !checkDistanceToFirstWaypoint(dm_current, nMissionItems, max_distance_to_1st_waypoint, warning_issued); } // check if all mission item commands are supported failed = failed || !checkMissionItemValidity(dm_current, nMissionItems, landed); failed = failed || !checkDistancesBetweenWaypoints(dm_current, nMissionItems, max_distance_between_waypoints, warning_issued); failed = failed || !checkGeofence(dm_current, nMissionItems, geofence, home_alt, home_valid); failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_alt_valid, warned); if (isRotarywing) { failed = failed || !checkRotarywing(dm_current, nMissionItems, home_alt, home_alt_valid, default_altitude_acceptance_rad); } else { failed = failed || !checkFixedwing(dm_current, nMissionItems, fw_pos_ctrl_status, home_alt, home_alt_valid, default_acceptance_rad, land_start_req); } return !failed; }
void EngineFailure::advance_ef() { switch (_ef_state) { case EF_STATE_NONE: mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: Engine failure. Loitering down"); _ef_state = EF_STATE_LOITERDOWN; break; default: break; } }
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 = (double)NAN; double last_lon = (double)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) { /* 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; }
int check_if_batt_disconnected(orb_advert_t *mavlink_log_pub) { struct battery_status_s battery; memset(&battery,0,sizeof(battery)); int batt_sub = orb_subscribe(ORB_ID(battery_status)); orb_copy(ORB_ID(battery_status), batt_sub, &battery); if (battery.voltage_filtered_v > 3.0f && !(hrt_absolute_time() - battery.timestamp > 500000)) { mavlink_log_info(mavlink_log_pub, "Please disconnect battery and try again!"); return ERROR; } return OK; }
void error_msg( int error, bool *transition_error ) { if ( error == 0 ){ mavlink_log_info(mavlink_fd, "[QC%i] State transition succes!", system_id); } else if ( error == -1 ) { mavlink_log_info(mavlink_fd, "[QC%i] No state transition occured!", system_id); } else if ( error == -2 ) { mavlink_log_info(mavlink_fd, "[QC%i] Not in correct state!", system_id); } else if ( error == -3 ) { mavlink_log_info(mavlink_fd, "[QC%i] Not changed to the correct state!", system_id); } else { mavlink_log_info(mavlink_fd, "[QC%i] Unknown return value!", system_id); } if ( error == -1 ) *transition_error = true; }
void MulticopterPositionControl::reset_pos_sp() { if (_reset_pos_sp) { _reset_pos_sp = false; /* shift position setpoint to make attitude setpoint continuous */ _pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust / _params.vel_p(0) - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); _pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust / _params.vel_p(1) - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); } }
void Navigator::start_loiter() { reset_reached(); _do_takeoff = false; /* set loiter position if needed */ if (_reset_loiter_pos || !_pos_sp_triplet.current.valid) { _reset_loiter_pos = false; _pos_sp_triplet.current.lat = _global_pos.lat; _pos_sp_triplet.current.lon = _global_pos.lon; _pos_sp_triplet.current.yaw = NAN; // NAN means to use current yaw float min_alt_amsl = _parameters.min_altitude + _home_pos.alt; /* use current altitude if above min altitude set by parameter */ if (_global_pos.alt < min_alt_amsl && !_vstatus.is_rotary_wing) { _pos_sp_triplet.current.alt = min_alt_amsl; mavlink_log_info(_mavlink_fd, "#audio: loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); } else { _pos_sp_triplet.current.alt = _global_pos.alt; mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude"); } } _pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER; _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; _pos_sp_triplet.current.loiter_direction = 1; _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = true; _pos_sp_triplet.next.valid = false; _mission_item_valid = false; _pos_sp_triplet_updated = true; }
int do_rc_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, "trim calibration starting"); /* XXX fix this */ // if (current_status.rc_signal) { // mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); // return; // } int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); struct manual_control_setpoint_s sp; orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); /* set parameters */ float p = sp.roll; param_set(param_find("TRIM_ROLL"), &p); p = sp.pitch; param_set(param_find("TRIM_PITCH"), &p); p = sp.yaw; param_set(param_find("TRIM_YAW"), &p); /* store to permanent storage */ /* auto-save */ int save_ret = param_save_default(); if (save_ret != 0) { mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); close(sub_man); return ERROR; } mavlink_log_info(mavlink_fd, "trim calibration done"); close(sub_man); return OK; }