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