int px4_prctl(int option, const char *arg2, unsigned pid) { int rv; switch (option) { case PR_SET_NAME: // set the threads name - Not supported // rv = pthread_setname_np(pthread_self(), arg2); rv = -1; break; default: rv = -1; PX4_WARN("FAILED SETTING TASK NAME"); break; } return rv; }
int Ekf2::start() { ASSERT(_control_task == -1); /* start the task */ _control_task = px4_task_spawn_cmd("ekf2", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 9000, (px4_main_t)&Ekf2::task_main_trampoline, nullptr); if (_control_task < 0) { PX4_WARN("task start failed"); return -errno; } return OK; }
/** * Start the driver. * * This function only returns if the sensor is up and running * or could not be detected successfully. */ void start(int i2c_bus) { int fd; if (g_dev != nullptr) { PX4_ERR("already started"); } /* create the driver */ g_dev = new ETSAirspeed(i2c_bus); if (g_dev == nullptr) { goto fail; } if (OK != g_dev->Airspeed::init()) { goto fail; } /* set the poll rate to default, starts automatic data collection */ fd = px4_open(AIRSPEED0_DEVICE_PATH, O_RDONLY); if (fd < 0) { goto fail; } if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { goto fail; } return; fail: if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } PX4_WARN("no ETS airspeed sensor connected"); }
int AirspeedSim::init() { int ret = ERROR; /* init base class */ if (CDev::init() != OK) { DEVICE_DEBUG("CDev init failed"); goto out; } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(differential_pressure_s)); if (_reports == nullptr) { goto out; } /* register alternate interfaces if we have to */ _class_instance = register_class_devname(AIRSPEED_BASE_DEVICE_PATH); /* publication init */ if (_class_instance == CLASS_DEVICE_PRIMARY) { /* advertise sensor topic, measure manually to initialize valid report */ struct differential_pressure_s arp; measure(); _reports->get(&arp); /* measurement will have generated a report, publish */ _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp); if (_airspeed_pub == nullptr) { PX4_WARN("uORB started?"); } } ret = OK; out: return ret; }
/** * Start the driver. */ void start(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num) { if (g_dev[gps_num - 1] != nullptr) { PX4_WARN("GPS %i already started", gps_num); return; } /* create the driver */ g_dev[gps_num - 1] = new GPS(uart_path, fake_gps, enable_sat_info, gps_num); if (!g_dev[gps_num - 1] || OK != g_dev[gps_num - 1]->init()) { if (g_dev[gps_num - 1] != nullptr) { delete g_dev[gps_num - 1]; g_dev[gps_num - 1] = nullptr; } PX4_ERR("start of GPS %i failed", gps_num); } }
int RcInput::start() { int result = 0; result = navio_rc_init(); if (result != 0) { PX4_WARN("error: RC initialization failed"); return -1; } _isRunning = true; result = work_queue(HPWORK, &_work, (worker_t)&RcInput::cycle_trampoline, this, 0); if (result == -1) { _isRunning = false; } return result; }
Device::Device(const char *name) : // public // protected _name(name), _debug_enabled(false) { int ret = px4_sem_init(&_lock, 0, 1); if (ret != 0) { PX4_WARN("SEM INIT FAIL: ret %d, %s", ret, strerror(errno)); } /* setup a default device ID. When bus_type is UNKNOWN the other fields are invalid */ _device_id.devid = 0; _device_id.devid_s.bus_type = DeviceBusType_UNKNOWN; _device_id.devid_s.bus = 0; _device_id.devid_s.address = 0; _device_id.devid_s.devtype = 0; }
int VtolAttitudeControl::start() { ASSERT(_control_task == -1); /* start the task */ _control_task = px4_task_spawn_cmd("vtol_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 2048, (px4_main_t)&VtolAttitudeControl::task_main_trampoline, nullptr); if (_control_task < 0) { PX4_WARN("task start failed"); return -errno; } return OK; }
int TAP_ESC::send_packet(EscPacket &packet, int responder) { if (responder >= 0) { if (responder > _channels_count) { return -EINVAL; } select_responder(responder); } int packet_len = crc_packet(packet); int ret = ::write(_uart_fd, &packet.head, packet_len); if (ret != packet_len) { PX4_WARN("TX ERROR: ret: %d, errno: %d", ret, errno); } return ret; }
/** * Automatic scale calibration. * * Basic idea: * * output = (ext field +- 1.1 Ga self-test) * scale factor * * and consequently: * * 1.1 Ga = (excited - normal) * scale factor * scale factor = (excited - normal) / 1.1 Ga * * sxy = (excited - normal) / 766 | for conf reg. B set to 0x60 / Gain = 3 * sz = (excited - normal) / 713 | for conf reg. B set to 0x60 / Gain = 3 * * By subtracting the non-excited measurement the pure 1.1 Ga reading * can be extracted and the sensitivity of all axes can be matched. * * SELF TEST OPERATION * To check the IST8310L for proper operation, a self test feature in incorporated * in which the sensor will change the polarity on all 3 axis. The values with and * with and without selftest on shoult be compared and if the absolete value are equal * the IC is functional. */ int calibrate(enum IST8310_BUS busid) { int ret; struct ist8310_bus_option &bus = find_bus(busid); const char *path = bus.devpath; int fd = open(path, O_RDONLY); if (fd < 0) { err(1, "%s open failed (try 'ist8310 start' if the driver is not running", path); } if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) { PX4_WARN("failed to enable sensor calibration mode"); } close(fd); return ret; }
int micrortps_client_main(int argc, char *argv[]) { if (argc < 2) { usage(argv[0]); return -1; } if (!strcmp(argv[1], "start")) { PX4_WARN("PX4 built without RTPS bridge support, EXITING...\n"); return -1; } if (!strcmp(argv[1], "stop")) { PX4_INFO("Not running"); return -1; } usage(argv[0]); return -1; }
/** * Start the driver. */ void start(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, bool fake_gps, bool enable_sat_info, int gps_num) { if (g_dev[gps_num - 1] != nullptr) { PX4_WARN("GPS %i already started", gps_num); return; } /* create the driver */ g_dev[gps_num - 1] = new GPS(path, mode, interface, fake_gps, enable_sat_info, gps_num); if (!g_dev[gps_num - 1] || OK != g_dev[gps_num - 1]->init()) { if (g_dev[gps_num - 1] != nullptr) { delete g_dev[gps_num - 1]; g_dev[gps_num - 1] = nullptr; } PX4_ERR("start of GPS %i failed", gps_num); } }
int px4_prctl(int option, const char *arg2, px4_task_t pid) { int rv; switch (option) { case PR_SET_NAME: // set the threads name #ifdef __PX4_DARWIN rv = pthread_setname_np(arg2); #else rv = pthread_setname_np(pthread_self(), arg2); #endif break; default: rv = -1; PX4_WARN("FAILED SETTING TASK NAME"); break; } return rv; }
int initialise_uart() { // open uart _uart_fd = open(_device, O_RDWR | O_NOCTTY | O_NONBLOCK); int termios_state = -1; if (_uart_fd < 0) { PX4_ERR("failed to open uart device!"); return -1; } // set baud rate int speed = B250000; struct termios uart_config; tcgetattr(_uart_fd, &uart_config); // clear ONLCR flag (which appends a CR for every LF) uart_config.c_oflag &= ~ONLCR; // set baud rate if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { PX4_ERR("failed to set baudrate for %s: %d\n", _device, termios_state); close(_uart_fd); return -1; } if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { PX4_ERR("tcsetattr failed for %s\n", _device); close(_uart_fd); return -1; } // setup output flow control if (enable_flow_control(false)) { PX4_WARN("hardware flow disable failed"); } return _uart_fd; }
void TAP_ESC:: send_esc_outputs(const float *pwm, const unsigned num_pwm) { uint16_t rpm[TAP_ESC_MAX_MOTOR_NUM]; memset(rpm, 0, sizeof(rpm)); uint8_t motor_cnt = num_pwm; static uint8_t which_to_respone = 0; for (uint8_t i = 0; i < motor_cnt; i++) { rpm[i] = pwm[i]; if (rpm[i] > RPMMAX) { rpm[i] = RPMMAX; } else if (rpm[i] < RPMSTOPPED) { rpm[i] = RPMSTOPPED; } } rpm[which_to_respone] |= (RUN_FEEDBACK_ENABLE_MASK | RUN_BLUE_LED_ON_MASK); EscPacket packet = {0xfe, _channels_count, ESCBUS_MSG_ID_RUN}; packet.len *= sizeof(packet.d.reqRun.rpm_flags[0]); for (uint8_t i = 0; i < _channels_count; i++) { packet.d.reqRun.rpm_flags[i] = rpm[i]; } int ret = send_packet(packet, which_to_respone); if (++which_to_respone == _channels_count) { which_to_respone = 0; } if (ret < 1) { PX4_WARN("TX ERROR: ret: %d, errno: %d", ret, errno); } }
void PrecLand::on_activation() { // We need to subscribe here and not in the constructor because constructor is called before the navigator task is spawned if (!_targetPoseSub) { _targetPoseSub = orb_subscribe(ORB_ID(landing_target_pose)); } _state = PrecLandState::Start; _search_cnt = 0; _last_slewrate_time = 0; vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position(); if (!map_projection_initialized(&_map_ref)) { map_projection_init(&_map_ref, vehicle_local_position->ref_lat, vehicle_local_position->ref_lon); } position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); pos_sp_triplet->next.valid = false; // Check that the current position setpoint is valid, otherwise land at current position if (!pos_sp_triplet->current.valid) { PX4_WARN("Resetting landing position to current position"); pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; pos_sp_triplet->current.alt = _navigator->get_global_position()->alt; pos_sp_triplet->current.valid = true; } _sp_pev = matrix::Vector2f(0, 0); _sp_pev_prev = matrix::Vector2f(0, 0); _last_slewrate_time = 0; switch_to_state_start(); }
int px4_task_delete(px4_task_t id) { int rv = 0; pthread_t pid; PX4_WARN("Called px4_task_delete"); if (id < PX4_MAX_TASKS && taskmap[id].isused) pid = taskmap[id].pid; else return -EINVAL; // If current thread then exit, otherwise cancel if (pthread_self() == pid) { taskmap[id].isused = false; pthread_exit(0); } else { rv = pthread_cancel(pid); } taskmap[id].isused = false; return rv; }
void PrecLand::run_state_descend_above_target() { position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); // check if target visible if (!check_state_conditions(PrecLandState::DescendAboveTarget)) { if (!switch_to_state_final_approach()) { PX4_WARN("Lost landing target while landing (descending)."); // Stay at current position for searching for the target pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; pos_sp_triplet->current.alt = _navigator->get_global_position()->alt; if (!switch_to_state_start()) { if (!switch_to_state_fallback()) { PX4_ERR("Can't switch to fallback landing"); } } } return; } // XXX need to transform to GPS coords because mc_pos_control only looks at that double lat, lon; map_projection_reproject(&_map_ref, _target_pose.x_abs, _target_pose.y_abs, &lat, &lon); pos_sp_triplet->current.lat = lat; pos_sp_triplet->current.lon = lon; pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND; _navigator->set_position_setpoint_triplet_updated(); }
/** * Main entry point for this module **/ int land_detector_main(int argc, char *argv[]) { if (argc < 2) { goto exiterr; } if (argc >= 2 && !strcmp(argv[1], "start")) { if (land_detector_start(argv[2]) != 0) { PX4_WARN("land_detector start failed"); return 1; } return 0; } if (!strcmp(argv[1], "stop")) { land_detector_stop(); return 0; } if (!strcmp(argv[1], "status")) { if (land_detector_task) { if (land_detector_task->isRunning()) { PX4_WARN("running (%s): %s", _currentMode, (land_detector_task->isLanded()) ? "LANDED" : "IN AIR"); } else { PX4_WARN("exists, but not running (%s)", _currentMode); } return 0; } else { PX4_WARN("not running"); return 1; } } exiterr: PX4_WARN("usage: land_detector {start|stop|status} [mode]"); PX4_WARN("mode can either be 'fixedwing' or 'multicopter'"); return 1; }
int MPU9250_SPI::probe() { uint8_t whoami = 0; int ret = read(MPUREG_WHOAMI, &whoami, 1); if (ret != OK) { return -EIO; } switch (whoami) { case MPU_WHOAMI_9250: case MPU_WHOAMI_6500: ret = 0; break; default: PX4_WARN("probe failed! %u", whoami); ret = -EIO; } return ret; }
void usage() { PX4_WARN("Usage: df_isl_wrapper 'start', 'info', 'stop', 'calib', 'probe'"); }
void PrecLand::run_state_horizontal_approach() { position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); // check if target visible, if not go to start if (!check_state_conditions(PrecLandState::HorizontalApproach)) { PX4_WARN("Lost landing target while landig (horizontal approach)."); // Stay at current position for searching for the landing target pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; pos_sp_triplet->current.alt = _navigator->get_global_position()->alt; if (!switch_to_state_start()) { if (!switch_to_state_fallback()) { PX4_ERR("Can't switch to fallback landing"); } } return; } if (check_state_conditions(PrecLandState::DescendAboveTarget)) { if (!_point_reached_time) { _point_reached_time = hrt_absolute_time(); } if (hrt_absolute_time() - _point_reached_time > 2000000) { // if close enough for descent above target go to descend above target if (switch_to_state_descend_above_target()) { return; } } } if (hrt_absolute_time() - _state_start_time > STATE_TIMEOUT) { PX4_ERR("Precision landing took too long during horizontal approach phase."); if (switch_to_state_fallback()) { return; } PX4_ERR("Can't switch to fallback landing"); } float x = _target_pose.x_abs; float y = _target_pose.y_abs; slewrate(x, y); // XXX need to transform to GPS coords because mc_pos_control only looks at that double lat, lon; map_projection_reproject(&_map_ref, x, y, &lat, &lon); pos_sp_triplet->current.lat = lat; pos_sp_triplet->current.lon = lon; pos_sp_triplet->current.alt = _approach_alt; pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; _navigator->set_position_setpoint_triplet_updated(); }
void LogWriter::run() { while (!_exit_thread) { // Outer endless loop // Wait for _should_run flag while (!_exit_thread) { bool start = false; pthread_mutex_lock(&_mtx); pthread_cond_wait(&_cv, &_mtx); start = _should_run; pthread_mutex_unlock(&_mtx); if (start) { break; } } int poll_count = 0; int written = 0; while (true) { size_t available = 0; void *read_ptr = nullptr; bool is_part = false; /* lock _buffer * wait for sufficient data, cycle on notify() */ pthread_mutex_lock(&_mtx); while (true) { available = get_read_ptr(&read_ptr, &is_part); /* if sufficient data available or partial read or terminating, exit this wait loop */ if ((available >= _min_write_chunk) || is_part || !_should_run) { /* GOTO end of block */ break; } /* wait for a call to notify() * this call unlocks the mutex while waiting, and returns with the mutex locked */ pthread_cond_wait(&_cv, &_mtx); } pthread_mutex_unlock(&_mtx); written = 0; if (available > 0) { perf_begin(_perf_write); written = ::write(_fd, read_ptr, available); perf_end(_perf_write); /* call fsync periodically to minimize potential loss of data */ if (++poll_count >= 100) { perf_begin(_perf_fsync); ::fsync(_fd); perf_end(_perf_fsync); poll_count = 0; } if (written < 0) { PX4_WARN("error writing log file"); _should_run = false; /* GOTO end of block */ break; } pthread_mutex_lock(&_mtx); /* subtract bytes written from number in _buffer (_count -= written) */ mark_read(written); pthread_mutex_unlock(&_mtx); _total_written += written; } if (!_should_run && written == static_cast<int>(available) && !is_part) { // Stop only when all data written _running = false; _head = 0; _count = 0; if (_fd >= 0) { int res = ::close(_fd); _fd = -1; if (res) { PX4_WARN("error closing log file"); } else { PX4_INFO("closed logfile: %s, bytes written: %zu", _filename, _total_written); } } break; } } } }
int16_t uORB::FastRpcChannel::get_bulk_data ( uint8_t *buffer, int32_t max_buffer_in_bytes, int32_t *returned_bytes, int32_t *topic_count ) { int16_t rc = 0; // wait for data availability static hrt_abstime check_time = 0; hrt_abstime t1 = hrt_absolute_time(); _DataAvailableSemaphore.wait(); hrt_abstime t2 = hrt_absolute_time(); _QueueMutex.lock(); int32_t bytes_copied = 0; int32_t copy_result = 0; *returned_bytes = 0; *topic_count = 0; int32_t topic_count_to_return = 0; if (DataQSize() != 0) { //PX4_DEBUG( "get_bulk_data: QSize: %d", DataQSize() ); topic_count_to_return = DataQSize(); while (DataQSize() != 0) { // this is a hack as we are using a counting semaphore. Should be re-implemented with cond_variable and wait. //_DataAvailableSemaphore.wait(); if (get_data_msg_size_at(_DataQOutIndex) < (max_buffer_in_bytes - bytes_copied)) { // there is enough space in the buffer, copy the data. //PX4_DEBUG( "Coping Data to buffer..." ); copy_result = copy_data_to_buffer(_DataQOutIndex, buffer, bytes_copied, max_buffer_in_bytes); if (copy_result == -1) { if (bytes_copied == 0) { rc = -1; } break; } else { //PX4_DEBUG( "[%d] %02x %02x %02x %02x", *topic_count,\ // buffer[bytes_copied], \ // buffer[bytes_copied+1], \ // buffer[bytes_copied+2], \ // buffer[bytes_copied+3] ); bytes_copied += copy_result; (*topic_count)++; *returned_bytes = bytes_copied; _DataQOutIndex++; if (_DataQOutIndex == _MAX_MSG_QUEUE_SIZE) { _DataQOutIndex = 0; } } } else { if (bytes_copied == 0) { rc = -1; PX4_WARN("ERROR: Insufficent space in data buffer, no topics returned"); } else { PX4_DEBUG("Exiting out of the while loop..."); } break; } } } else { PX4_ERR("[get_data_bulk] Error: Semaphore is up when there is no data on the control/data queues"); rc = -1; } if (topic_count_to_return != *topic_count) { PX4_WARN("Not sending all topics: topics_to_return:[%ld] topics_returning:[%ld]", topic_count_to_return, *topic_count); } _QueueMutex.unlock(); hrt_abstime t3 = hrt_absolute_time(); if ((unsigned long)(t3 - t1) > _get_bulk_max) { _get_bulk_max = (unsigned long)(t3 - t1); } if ((unsigned long)(t3 - t1) < _get_bulk_min) { _get_bulk_min = (unsigned long)(t3 - t1); } if ((unsigned long)(*topic_count) > _bulk_topic_count_max) { _bulk_topic_count_max = (unsigned long)(*topic_count); } if ((unsigned long)(*topic_count) < _bulk_topic_count_min) { _bulk_topic_count_min = (unsigned long)(*topic_count); } if ((unsigned long)(t3 - check_time) > 10000000) { //PX4_DEBUG("GetData: t1: %lu t2: %lu t3: %lu", (unsigned long)t1, (unsigned long)t2, (unsigned long)t3); //PX4_DEBUG(".... dt1: %7lu dt2: %7lu Q: %d", (unsigned long)(t2 - t1), (unsigned long)(t3 - t2), DataQSize()); //PX4_DEBUG("ADSP RPC Stats: _get_bulk_min: %lu _get_bulk_max: %lu _dropped_pkts: %lu", _get_bulk_min, _get_bulk_max, // _dropped_pkts); //PX4_DEBUG(" .... topic_count_min: %lu topic_count_max: %lu", _bulk_topic_count_min, _bulk_topic_count_max); _get_bulk_max = 0; _get_bulk_min = 0xFFFFFF; _bulk_topic_count_min = 0xFFFFFF; _bulk_topic_count_max = 0; check_time = t3; } //PX4_DEBUG( "Returning topics: %d bytes_returned: %d", *topic_count, *returned_bytes ); 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) { warnx("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[2] = {}; /* Setup of loop */ fds[0].fd = _global_pos_sub; fds[0].events = POLLIN; fds[1].fd = _vehicle_command_sub; fds[1].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("navigator: 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_WARN("nav: poll error %d, %d", pret, errno); continue; } else { /* success, global pos was available */ 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_PAUSE_CONTINUE) { warnx("navigator: got pause/continue command"); } } /* 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 && (_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_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: _navigation_mode = nullptr; _can_loiter_at_sp = false; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: if (_fw_pos_ctrl_status.abort_landing) { // pos controller aborted landing, requests loiter // above landing waypoint _navigation_mode = &_loiter; _pos_sp_triplet_published_invalid_once = false; } else { _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; 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; return; }
void Ekf2Replay::task_main() { // formats const int _k_max_data_size = 1024; // 16x16 bytes uint8_t data[_k_max_data_size] = {}; const char param_file[] = "./rootfs/replay_params.txt"; // Open log file from which we read data // TODO Check if file exists int fd = ::open(_file_name, O_RDONLY); // create path to write a replay file char *replay_log_name; replay_log_name = strtok(_file_name, "."); char tmp[] = "_replayed.px4log"; char *path_to_replay_log = (char *) malloc(1 + strlen(tmp) + strlen(replay_log_name)); strcpy(path_to_replay_log, "."); strcat(path_to_replay_log, replay_log_name); strcat(path_to_replay_log, tmp); // create path which tells user location of replay file char tmp2[] = "./build_posix_sitl_replay/src/firmware/posix"; char *replay_file_location = (char *) malloc(1 + strlen(tmp) + strlen(tmp2) + strlen(replay_log_name)); strcat(replay_file_location, tmp2); strcat(replay_file_location, replay_log_name); strcat(replay_file_location, tmp); // open logfile to write _write_fd = ::open(path_to_replay_log, O_WRONLY | O_CREAT, S_IRWXU); std::ifstream tmp_file; tmp_file.open(param_file); std::string line; bool set_default_params_in_file = false; if (tmp_file.is_open() && ! tmp_file.eof()) { getline(tmp_file, line); if (line.empty()) { // the parameter file is empty so write the default values to it set_default_params_in_file = true; } } tmp_file.close(); std::ofstream myfile(param_file, std::ios::app); // subscribe to estimator topics _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _estimator_status_sub = orb_subscribe(ORB_ID(estimator_status)); _innov_sub = orb_subscribe(ORB_ID(ekf2_innovations)); _lpos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _control_state_sub = orb_subscribe(ORB_ID(control_state)); // we use attitude updates from the estimator for synchronisation _fds[0].fd = _att_sub; _fds[0].events = POLLIN; bool read_first_header = false; bool set_user_params = false; PX4_INFO("Replay in progress... \n"); PX4_INFO("Log data will be written to %s\n", replay_file_location); while (!_task_should_exit) { _message_counter++; uint8_t header[3] = {}; if (::read(fd, header, 3) != 3) { if (!read_first_header) { PX4_WARN("error reading log file, is the path printed above correct?"); } else { PX4_INFO("Done!"); } _task_should_exit = true; continue; } read_first_header = true; if ((header[0] != HEAD_BYTE1 || header[1] != HEAD_BYTE2)) { // we assume that the log file is finished here PX4_WARN("Done!"); _task_should_exit = true; continue; } // write header but only for messages which are not generated by the estimator if (needToSaveMessage(header[2])) { writeMessage(_write_fd, &header[0], 3); } if (header[2] == LOG_FORMAT_MSG) { // format message struct log_format_s f; if (::read(fd, &f.type, sizeof(f)) != sizeof(f)) { PRINT_READ_ERROR; _task_should_exit = true; continue; } writeMessage(_write_fd, &f.type, sizeof(log_format_s)); memcpy(&_formats[f.type], &f, sizeof(f)); } else if (header[2] == LOG_PARM_MSG) { // parameter message if (::read(fd, &data[0], sizeof(log_PARM_s)) != sizeof(log_PARM_s)) { PRINT_READ_ERROR; _task_should_exit = true; continue; } writeMessage(_write_fd, &data[0], sizeof(log_PARM_s)); // apply the parameters char param_name[16]; for (unsigned i = 0; i < 16; i++) { param_name[i] = data[i]; if (data[i] == '\0') { break; } } float param_data = 0; memcpy(¶m_data, &data[16], sizeof(float)); param_t handle = param_find(param_name); param_type_t param_format = param_type(handle); if (param_format == PARAM_TYPE_INT32) { int32_t value = 0; value = (int32_t)param_data; param_set(handle, (const void *)&value); } else if (param_format == PARAM_TYPE_FLOAT) { param_set(handle, (const void *)¶m_data); } // if the user param file was empty then we fill it with the ekf2 parameter values from // the log file if (set_default_params_in_file) { if (strncmp(param_name, "EKF2", 4) == 0) { std::ostringstream os; double value = (double)param_data; os << std::string(param_name) << " "; os << value << "\n"; myfile << os.str(); } } } else if (header[2] == LOG_VER_MSG) { // version message if (::read(fd, &data[0], sizeof(log_VER_s)) != sizeof(log_VER_s)) { PRINT_READ_ERROR; _task_should_exit = true; continue; } writeMessage(_write_fd, &data[0], sizeof(log_VER_s)); } else if (header[2] == LOG_TIME_MSG) { // time message if (::read(fd, &data[0], sizeof(log_TIME_s)) != sizeof(log_TIME_s)) { // assume that this is because we have reached the end of the file PX4_INFO("Done!"); _task_should_exit = true; continue; } writeMessage(_write_fd, &data[0], sizeof(log_TIME_s)); } else { // the first time we arrive here we should apply the parameters specified in the user file // this makes sure they are applied after the parameter values of the log file if (!set_user_params) { myfile.close(); setUserParams(param_file); set_user_params = true; } // data message if (::read(fd, &data[0], _formats[header[2]].length - 3) != _formats[header[2]].length - 3) { PX4_INFO("Done!"); _task_should_exit = true; continue; } // all messages which we are not getting from the estimator are written // back into the replay log file if (needToSaveMessage(header[2])) { writeMessage(_write_fd, &data[0], _formats[header[2]].length - 3); } if (header[2] == LOG_RPL1_MSG && _part1_counter_ref > 0) { // we have found another imu replay message while we still have one waiting to be published. // so publish that now publishAndWaitForEstimator(); } // set estimator input data setEstimatorInput(&data[0], header[2]); // we have read the imu replay message (part 1) and have waited 3 more cycles for other replay message parts // e.g. flow, gps or range. we know that in case they were written to the log file they should come right after // the first replay message, therefore, we can kick the estimator now if (_part1_counter_ref > 0 && _part1_counter_ref < _message_counter - 3) { publishAndWaitForEstimator(); } } } ::close(_write_fd); ::close(fd); delete ekf2_replay::instance; ekf2_replay::instance = nullptr; }
void Ekf2Replay::writeMessage(int &fd, void *data, size_t size) { if (size != ::write(fd, data, size)) { PX4_WARN("error writing to file"); } }
int ekf2_replay_main(int argc, char *argv[]) { if (argc < 1) { PX4_WARN("usage: ekf2_replay {start|stop|status}"); return 1; } if (!strcmp(argv[1], "start")) { if (ekf2_replay::instance != nullptr) { PX4_WARN("already running"); return 1; } ekf2_replay::instance = new Ekf2Replay(argv[2]); if (ekf2_replay::instance == nullptr) { PX4_WARN("alloc failed"); return 1; } if (OK != ekf2_replay::instance->start()) { delete ekf2_replay::instance; ekf2_replay::instance = nullptr; PX4_WARN("start failed"); return 1; } return 0; } if (!strcmp(argv[1], "stop")) { if (ekf2_replay::instance == nullptr) { PX4_WARN("not running"); return 1; } ekf2_replay::instance->exit(); // wait for the destruction of the instance while (ekf2_replay::instance != nullptr) { usleep(50000); } return 0; } if (!strcmp(argv[1], "status")) { if (ekf2_replay::instance) { PX4_WARN("running"); return 0; } else { PX4_WARN("not running"); return 1; } } PX4_WARN("unrecognized command"); return 1; }
int BAROSIM::collect() { int ret; #pragma pack(push, 1) struct { float pressure; float altitude; float temperature; } baro_report; #pragma pack(pop) perf_begin(_sample_perf); struct baro_report report; /* this should be fairly close to the end of the conversion, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); /* read the most recent measurement - read offset/size are hardcoded in the interface */ ret = _interface->dev_read(0, (void *)&baro_report, sizeof(baro_report)); if (ret < 0) { perf_count(_comms_errors); perf_end(_sample_perf); return ret; } /* handle a measurement */ if (_measure_phase == 0) { report.pressure = baro_report.pressure; report.altitude = baro_report.altitude; report.temperature = baro_report.temperature; report.timestamp = hrt_absolute_time(); } else { report.pressure = baro_report.pressure; report.altitude = baro_report.altitude; report.temperature = baro_report.temperature; report.timestamp = hrt_absolute_time(); /* publish it */ if (!(_pub_blocked)) { if (_baro_topic != nullptr) { /* publish it */ orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); } else { PX4_WARN("BAROSIM::collect _baro_topic not initialized"); } } if (_reports->force(&report)) { perf_count(_buffer_overflows); } /* notify anyone waiting for data */ poll_notify(POLLIN); } /* update the measurement state machine */ INCREMENT(_measure_phase, BAROSIM_MEASUREMENT_RATIO + 1); perf_end(_sample_perf); return OK; }
int IST8310::calibrate(struct file *filp, unsigned enable) { struct mag_report report; ssize_t sz; int ret = 1; float total_x = 0.0f; float total_y = 0.0f; float total_z = 0.0f; // XXX do something smarter here int fd = (int)enable; struct mag_calibration_s mscale_previous; struct mag_calibration_s mscale_null; mscale_null.x_offset = 0.0f; mscale_null.x_scale = 1.0f; mscale_null.y_offset = 0.0f; mscale_null.y_scale = 1.0f; mscale_null.z_offset = 0.0f; mscale_null.z_scale = 1.0f; float sum_in_test[3] = {0.0f, 0.0f, 0.0f}; float sum_in_normal[3] = {0.0f, 0.0f, 0.0f}; float *sum = &sum_in_normal[0]; if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) { PX4_WARN("FAILED: MAGIOCGSCALE 1"); ret = 1; goto out; } if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { PX4_WARN("FAILED: MAGIOCSSCALE 1"); ret = 1; goto out; } /* start the sensor polling at 50 Hz */ if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) { PX4_WARN("FAILED: SENSORIOCSPOLLRATE 50Hz"); ret = 1; goto out; } // discard 10 samples to let the sensor settle /* read the sensor 50 times */ for (uint8_t p = 0; p < 2; p++) { if (p == 1) { /* start the Self test */ if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) { PX4_WARN("FAILED: MAGIOCEXSTRAP 1"); ret = 1; goto out; } sum = &sum_in_test[0]; } for (uint8_t i = 0; i < 30; i++) { struct pollfd fds; /* wait for data to be ready */ fds.fd = fd; fds.events = POLLIN; ret = ::poll(&fds, 1, 2000); if (ret != 1) { PX4_WARN("ERROR: TIMEOUT 2"); goto out; } /* now go get it */ sz = ::read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { PX4_WARN("ERROR: READ 2"); ret = -EIO; goto out; } if (i > 10) { sum[0] += report.x_raw; sum[1] += report.y_raw; sum[2] += report.z_raw; } } } total_x = fabsf(sum_in_test[0] - sum_in_normal[0]); total_y = fabsf(sum_in_test[1] - sum_in_normal[1]); total_z = fabsf(sum_in_test[2] - sum_in_normal[2]); ret = ((total_x + total_y + total_z) < (float)0.000001); out: if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) { PX4_WARN("FAILED: MAGIOCSSCALE 2"); } /* set back to normal mode */ if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) { PX4_WARN("FAILED: MAGIOCEXSTRAP 0"); } if (ret == OK) { if (check_scale()) { /* failed */ PX4_WARN("FAILED: SCALE"); ret = PX4_ERROR; } } else { PX4_ERR("FAILED: CALIBRATION SCALE %d, %d, %d", (int)total_x, (int)total_y, (int)total_z); } return ret; }