void GPS::task_main() { /* open the serial port */ _serial_fd = ::open(_port, O_RDWR); if (_serial_fd < 0) { DEVICE_LOG("failed to open serial port: %s err: %d", _port, errno); /* tell the dtor that we are exiting, set error code */ _task = -1; _exit(1); } uint64_t last_rate_measurement = hrt_absolute_time(); unsigned last_rate_count = 0; /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { if (_fake_gps) { _report_gps_pos.timestamp_position = hrt_absolute_time(); _report_gps_pos.lat = (int32_t)47.378301e7f; _report_gps_pos.lon = (int32_t)8.538777e7f; _report_gps_pos.alt = (int32_t)1200e3f; _report_gps_pos.timestamp_variance = hrt_absolute_time(); _report_gps_pos.s_variance_m_s = 10.0f; _report_gps_pos.c_variance_rad = 0.1f; _report_gps_pos.fix_type = 3; _report_gps_pos.eph = 0.9f; _report_gps_pos.epv = 1.8f; _report_gps_pos.timestamp_velocity = hrt_absolute_time(); _report_gps_pos.vel_n_m_s = 0.0f; _report_gps_pos.vel_e_m_s = 0.0f; _report_gps_pos.vel_d_m_s = 0.0f; _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); _report_gps_pos.cog_rad = 0.0f; _report_gps_pos.vel_ned_valid = true; /* no time and satellite information simulated */ if (!(_pub_blocked)) { if (_report_gps_pos_pub != nullptr) { orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); } else { _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); } } usleep(2e5); } else { if (_Helper != nullptr) { delete(_Helper); /* set to zero to ensure parser is not used while not instantiated */ _Helper = nullptr; } switch (_mode) { case GPS_DRIVER_MODE_UBX: _Helper = new UBX(_serial_fd, &_report_gps_pos, _p_report_sat_info); break; case GPS_DRIVER_MODE_MTK: _Helper = new MTK(_serial_fd, &_report_gps_pos); break; case GPS_DRIVER_MODE_ASHTECH: _Helper = new ASHTECH(_serial_fd, &_report_gps_pos, _p_report_sat_info); break; default: break; } unlock(); /* the Ashtech driver lies about successful configuration and the * MTK driver is not well tested, so we really only trust the UBX * driver for an advance publication */ if (_Helper->configure(_baudrate) == 0) { unlock(); /* reset report */ memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); if (_mode == GPS_DRIVER_MODE_UBX) { /* Publish initial report that we have access to a GPS, * but set all critical state fields to indicate we have * no valid position lock */ _report_gps_pos.timestamp_time = hrt_absolute_time(); /* reset the timestamps for data, because we have no data yet */ _report_gps_pos.timestamp_position = 0; _report_gps_pos.timestamp_variance = 0; _report_gps_pos.timestamp_velocity = 0; /* set a massive variance */ _report_gps_pos.eph = 10000.0f; _report_gps_pos.epv = 10000.0f; _report_gps_pos.fix_type = 0; if (!(_pub_blocked)) { if (_report_gps_pos_pub != nullptr) { orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); } else { _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); } } /* GPS is obviously detected successfully, reset statistics */ _Helper->reset_update_rates(); } int helper_ret; while ((helper_ret = _Helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) { // lock(); /* opportunistic publishing - else invalid data would end up on the bus */ if (!(_pub_blocked)) { if (helper_ret & 1) { orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); } if (_p_report_sat_info && (helper_ret & 2)) { if (_report_sat_info_pub != nullptr) { orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info); } else { _report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info); } } } if (helper_ret & 1) { // consider only pos info updates for rate calculation */ last_rate_count++; } /* measure update rate every 5 seconds */ if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); last_rate_measurement = hrt_absolute_time(); last_rate_count = 0; _Helper->store_update_rates(); _Helper->reset_update_rates(); } if (!_healthy) { const char *mode_str = "unknown"; switch (_mode) { case GPS_DRIVER_MODE_UBX: mode_str = "UBX"; break; case GPS_DRIVER_MODE_MTK: mode_str = "MTK"; break; case GPS_DRIVER_MODE_ASHTECH: mode_str = "ASHTECH"; break; default: break; } warnx("module found: %s", mode_str); _healthy = true; } } if (_healthy) { warnx("module lost"); _healthy = false; _rate = 0.0f; } lock(); } lock(); /* select next mode */ switch (_mode) { case GPS_DRIVER_MODE_UBX: _mode = GPS_DRIVER_MODE_MTK; break; case GPS_DRIVER_MODE_MTK: _mode = GPS_DRIVER_MODE_ASHTECH; break; case GPS_DRIVER_MODE_ASHTECH: _mode = GPS_DRIVER_MODE_UBX; break; default: break; } } } warnx("exiting"); ::close(_serial_fd); /* tell the dtor that we are exiting */ _task = -1; _exit(0); }
int uORBTest::UnitTest::test_multi_reversed() { test_note("try multi-topic support subscribing before publishing"); /* For these tests 0 and 1 instances are taken from before, therefore continue with 2 and 3. */ /* Subscribe first and advertise afterwards. */ int sfd2 = orb_subscribe_multi(ORB_ID(orb_multitest), 2); if (sfd2 < 0) { return test_fail("sub. id2: ret: %d", sfd2); } struct orb_test t, u; t.val = 0; int instance2; _pfd[2] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance2, ORB_PRIO_MAX); int instance3; _pfd[3] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance3, ORB_PRIO_MIN); test_note("advertised"); if (instance2 != 2) { return test_fail("mult. id2: %d", instance2); } if (instance3 != 3) { return test_fail("mult. id3: %d", instance3); } t.val = 204; if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[2], &t)) { return test_fail("mult. pub0 fail"); } t.val = 304; if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[3], &t)) { return test_fail("mult. pub1 fail"); } test_note("published"); if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd2, &u)) { return test_fail("sub #2 copy failed: %d", errno); } if (u.val != 204) { return test_fail("sub #3 val. mismatch: %d", u.val); } int sfd3 = orb_subscribe_multi(ORB_ID(orb_multitest), 3); if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd3, &u)) { return test_fail("sub #3 copy failed: %d", errno); } if (u.val != 304) { return test_fail("sub #3 val. mismatch: %d", u.val); } return test_note("PASS multi-topic reversed"); }
int AttitudePositionEstimatorEKF::check_filter_state() { /* * CHECK IF THE INPUT DATA IS SANE */ struct ekf_status_report ekf_report; int check = _ekf->CheckAndBound(&ekf_report); const char *const feedback[] = { 0, "NaN in states, resetting", "stale sensor data, resetting", "got initial position lock", "excessive gyro offsets", "velocity diverted, check accel config", "excessive covariances", "unknown condition, resetting" }; // Print out error condition if (check) { unsigned warn_index = static_cast<unsigned>(check); unsigned max_warn_index = (sizeof(feedback) / sizeof(feedback[0])); if (max_warn_index < warn_index) { warn_index = max_warn_index; } // Do not warn about accel offset if we have no position updates if (!(warn_index == 5 && _ekf->staticMode)) { warnx("reset: %s", feedback[warn_index]); mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]); } } struct estimator_status_s rep; memset(&rep, 0, sizeof(rep)); // If error flag is set, we got a filter reset if (check && ekf_report.error) { // Count the reset condition perf_count(_perf_reset); // 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); initReferencePosition(_gps.timestamp_position, lat, lon, gps_alt, _baro.altitude); } else if (_ekf_logging) { _ekf->GetFilterState(&ekf_report); } if (_ekf_logging || check) { rep.timestamp = hrt_absolute_time(); rep.nan_flags |= (((uint8_t)ekf_report.angNaN) << 0); rep.nan_flags |= (((uint8_t)ekf_report.summedDelVelNaN) << 1); rep.nan_flags |= (((uint8_t)ekf_report.KHNaN) << 2); rep.nan_flags |= (((uint8_t)ekf_report.KHPNaN) << 3); rep.nan_flags |= (((uint8_t)ekf_report.PNaN) << 4); rep.nan_flags |= (((uint8_t)ekf_report.covarianceNaN) << 5); rep.nan_flags |= (((uint8_t)ekf_report.kalmanGainsNaN) << 6); rep.nan_flags |= (((uint8_t)ekf_report.statesNaN) << 7); rep.health_flags |= (((uint8_t)ekf_report.velHealth) << 0); rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1); rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2); rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive) << 3); rep.health_flags |= (((uint8_t)ekf_report.onGround) << 4); rep.health_flags |= (((uint8_t)ekf_report.staticMode) << 5); rep.health_flags |= (((uint8_t)ekf_report.useCompass) << 6); rep.health_flags |= (((uint8_t)ekf_report.useAirspeed) << 7); rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0); rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1); rep.timeout_flags |= (((uint8_t)ekf_report.hgtTimeout) << 2); rep.timeout_flags |= (((uint8_t)ekf_report.imuTimeout) << 3); if (_debug > 10) { if (rep.health_flags < ((1 << 0) | (1 << 1) | (1 << 2) | (1 << 3))) { warnx("health: VEL:%s POS:%s HGT:%s OFFS:%s", ((rep.health_flags & (1 << 0)) ? "OK" : "ERR"), ((rep.health_flags & (1 << 1)) ? "OK" : "ERR"), ((rep.health_flags & (1 << 2)) ? "OK" : "ERR"), ((rep.health_flags & (1 << 3)) ? "OK" : "ERR")); } if (rep.timeout_flags) { warnx("timeout: %s%s%s%s", ((rep.timeout_flags & (1 << 0)) ? "VEL " : ""), ((rep.timeout_flags & (1 << 1)) ? "POS " : ""), ((rep.timeout_flags & (1 << 2)) ? "HGT " : ""), ((rep.timeout_flags & (1 << 3)) ? "IMU " : "")); } } // Copy all states or at least all that we can fit size_t ekf_n_states = ekf_report.n_states; size_t max_states = (sizeof(rep.states) / sizeof(rep.states[0])); rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states; for (size_t i = 0; i < rep.n_states; i++) { rep.states[i] = ekf_report.states[i]; } if (_estimator_status_pub > 0) { orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep); } else { _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep); } } return check; }
void LSM303D::mag_measure() { /* status register and data as read back from the device */ #pragma pack(push, 1) struct { uint8_t cmd; uint8_t status; int16_t x; int16_t y; int16_t z; } raw_mag_report; #pragma pack(pop) mag_report *mag_report = &_mag_reports[_next_mag_report]; /* start the performance counter */ perf_begin(_mag_sample_perf); /* fetch data from the sensor */ raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report)); /* * 1) Scale raw value to SI units using scaling from datasheet. * 2) Subtract static offset (in SI units) * 3) Scale the statically calibrated values with a linear * dynamically obtained factor * * Note: the static sensor offset is the number the sensor outputs * at a nominally 'zero' input. Therefore the offset has to * be subtracted. * * Example: A gyro outputs a value of 74 at zero angular rate * the offset is 74 from the origin and subtracting * 74 from all measurements centers them around zero. */ mag_report->timestamp = hrt_absolute_time(); mag_report->x_raw = raw_mag_report.x; mag_report->y_raw = raw_mag_report.y; mag_report->z_raw = raw_mag_report.z; mag_report->x = ((mag_report->x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; mag_report->y = ((mag_report->y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; mag_report->z = ((mag_report->z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; mag_report->scaling = _mag_range_scale; mag_report->range_ga = (float)_mag_range_ga; /* post a report to the ring - note, not locked */ INCREMENT(_next_mag_report, _num_mag_reports); /* if we are running up against the oldest report, fix it */ if (_next_mag_report == _oldest_mag_report) INCREMENT(_oldest_mag_report, _num_mag_reports); /* XXX please check this poll_notify, is it the right one? */ /* notify anyone waiting for data */ poll_notify(POLLIN); /* publish for subscribers */ orb_publish(ORB_ID(sensor_mag), _mag_topic, mag_report); _mag_read++; /* stop the perf counter */ perf_end(_mag_sample_perf); }
int uORBTest::UnitTest::test_multi() { /* this routine tests the multi-topic support */ test_note("try multi-topic support"); struct orb_test t, u; t.val = 0; int instance0; _pfd[0] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX); test_note("advertised"); int instance1; _pfd[1] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN); if (instance0 != 0) { return test_fail("mult. id0: %d", instance0); } if (instance1 != 1) { return test_fail("mult. id1: %d", instance1); } t.val = 103; if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[0], &t)) { return test_fail("mult. pub0 fail"); } test_note("published"); t.val = 203; if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[1], &t)) { return test_fail("mult. pub1 fail"); } /* subscribe to both topics and ensure valid data is received */ int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0); if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd0, &u)) { return test_fail("sub #0 copy failed: %d", errno); } if (u.val != 103) { return test_fail("sub #0 val. mismatch: %d", u.val); } int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1); if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd1, &u)) { return test_fail("sub #1 copy failed: %d", errno); } if (u.val != 203) { return test_fail("sub #1 val. mismatch: %d", u.val); } /* test priorities */ int prio; if (PX4_OK != orb_priority(sfd0, &prio)) { return test_fail("prio #0"); } if (prio != ORB_PRIO_MAX) { return test_fail("prio: %d", prio); } if (PX4_OK != orb_priority(sfd1, &prio)) { return test_fail("prio #1"); } if (prio != ORB_PRIO_MIN) { return test_fail("prio: %d", prio); } if (PX4_OK != latency_test<struct orb_test>(ORB_ID(orb_test), false)) { return test_fail("latency test failed"); } orb_unsubscribe(sfd0); orb_unsubscribe(sfd1); return test_note("PASS multi-topic test"); }
void VtolAttitudeControl::task_main() { warnx("started"); fflush(stdout); /* do subscriptions */ _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint)); _fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint)); _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _battery_status_sub = orb_subscribe(ORB_ID(battery_status)); _actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc)); _actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw)); parameters_update(); // initialize parameter cache /* update vtol vehicle status*/ _vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false; // make sure we start with idle in mc mode set_idle_mc(); flag_idle_mc = true; /* wakeup source*/ struct pollfd fds[3]; /*input_mc, input_fw, parameters*/ fds[0].fd = _actuator_inputs_mc; fds[0].events = POLLIN; fds[1].fd = _actuator_inputs_fw; fds[1].events = POLLIN; fds[2].fd = _params_sub; fds[2].events = POLLIN; while (!_task_should_exit) { /*Advertise/Publish vtol vehicle status*/ if (_vtol_vehicle_status_pub > 0) { orb_publish(ORB_ID(vtol_vehicle_status), _vtol_vehicle_status_pub, &_vtol_vehicle_status); } else { _vtol_vehicle_status.timestamp = hrt_absolute_time(); _vtol_vehicle_status_pub = orb_advertise(ORB_ID(vtol_vehicle_status), &_vtol_vehicle_status); } /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit */ if (pret == 0) { continue; } /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { warn("poll error %d, %d", pret, errno); /* sleep a bit before next try */ usleep(100000); continue; } if (fds[2].revents & POLLIN) { //parameters were updated, read them now /* read from param to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), _params_sub, &update); /* update parameters from storage */ parameters_update(); } _vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false; vehicle_control_mode_poll(); //Check for changes in vehicle control mode. vehicle_manual_poll(); //Check for changes in manual inputs. arming_status_poll(); //Check for arming status updates. actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output vehicle_rates_sp_mc_poll(); vehicle_rates_sp_fw_poll(); parameters_update_poll(); vehicle_local_pos_poll(); // Check for new sensor values vehicle_airspeed_poll(); vehicle_battery_poll(); if (_manual_control_sp.aux1 <= 0.0f) { /* vehicle is in mc mode */ _vtol_vehicle_status.vtol_in_rw_mode = true; if (!flag_idle_mc) { /* we want to adjust idle speed for mc mode */ set_idle_mc(); flag_idle_mc = true; } /* got data from mc_att_controller */ if (fds[0].revents & POLLIN) { vehicle_manual_poll(); /* update remote input */ orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in); // scale pitch control with total airspeed scale_mc_output(); fill_mc_att_control_output(); fill_mc_att_rates_sp(); if (_actuators_0_pub > 0) { orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); } else { _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); } if (_actuators_1_pub > 0) { orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); } else { _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); } } } if (_manual_control_sp.aux1 >= 0.0f) { /* vehicle is in fw mode */ _vtol_vehicle_status.vtol_in_rw_mode = false; if (flag_idle_mc) { /* we want to adjust idle speed for fixed wing mode */ set_idle_fw(); flag_idle_mc = false; } if (fds[1].revents & POLLIN) { /* got data from fw_att_controller */ orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in); vehicle_manual_poll(); //update remote input fill_fw_att_control_output(); fill_fw_att_rates_sp(); if (_actuators_0_pub > 0) { orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); } else { _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); } if (_actuators_1_pub > 0) { orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); } else { _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); } } } // publish the attitude rates setpoint if(_v_rates_sp_pub > 0) { orb_publish(ORB_ID(vehicle_rates_setpoint),_v_rates_sp_pub,&_v_rates_sp); } else { _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint),&_v_rates_sp); } } warnx("exit"); _control_task = -1; _exit(0); }
void GPS::task_main() { /* open the serial port */ _serial_fd = ::open(_port, O_RDWR | O_NOCTTY); if (_serial_fd < 0) { PX4_ERR("GPS: failed to open serial port: %s err: %d", _port, errno); /* tell the dtor that we are exiting, set error code */ _task = -1; px4_task_exit(1); } #ifndef __PX4_QURT // TODO: this call is not supported on Snapdragon just yet. // However it seems to be nonblocking anyway and working. int flags = fcntl(_serial_fd, F_GETFL, 0); fcntl(_serial_fd, F_SETFL, flags | O_NONBLOCK); #endif for (int i = 0; i < _orb_inject_data_fd_count; ++i) { _orb_inject_data_fd[i] = orb_subscribe_multi(ORB_ID(gps_inject_data), i); } initializeCommunicationDump(); uint64_t last_rate_measurement = hrt_absolute_time(); unsigned last_rate_count = 0; /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { if (_fake_gps) { _report_gps_pos.timestamp_position = hrt_absolute_time(); _report_gps_pos.lat = (int32_t)47.378301e7f; _report_gps_pos.lon = (int32_t)8.538777e7f; _report_gps_pos.alt = (int32_t)1200e3f; _report_gps_pos.timestamp_variance = hrt_absolute_time(); _report_gps_pos.s_variance_m_s = 10.0f; _report_gps_pos.c_variance_rad = 0.1f; _report_gps_pos.fix_type = 3; _report_gps_pos.eph = 0.9f; _report_gps_pos.epv = 1.8f; _report_gps_pos.timestamp_velocity = hrt_absolute_time(); _report_gps_pos.vel_n_m_s = 0.0f; _report_gps_pos.vel_e_m_s = 0.0f; _report_gps_pos.vel_d_m_s = 0.0f; _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); _report_gps_pos.cog_rad = 0.0f; _report_gps_pos.vel_ned_valid = true; /* no time and satellite information simulated */ publish(); usleep(2e5); } else { if (_helper != nullptr) { delete(_helper); /* set to zero to ensure parser is not used while not instantiated */ _helper = nullptr; } switch (_mode) { case GPS_DRIVER_MODE_UBX: _helper = new GPSDriverUBX(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info); break; case GPS_DRIVER_MODE_MTK: _helper = new GPSDriverMTK(&GPS::callback, this, &_report_gps_pos); break; case GPS_DRIVER_MODE_ASHTECH: _helper = new GPSDriverAshtech(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info); break; default: break; } /* the Ashtech driver lies about successful configuration and the * MTK driver is not well tested, so we really only trust the UBX * driver for an advance publication */ if (_helper->configure(_baudrate, GPSHelper::OutputMode::GPS) == 0) { /* reset report */ memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); if (_mode == GPS_DRIVER_MODE_UBX) { /* Publish initial report that we have access to a GPS, * but set all critical state fields to indicate we have * no valid position lock */ _report_gps_pos.timestamp_time = hrt_absolute_time(); /* reset the timestamps for data, because we have no data yet */ _report_gps_pos.timestamp_position = 0; _report_gps_pos.timestamp_variance = 0; _report_gps_pos.timestamp_velocity = 0; /* set a massive variance */ _report_gps_pos.eph = 10000.0f; _report_gps_pos.epv = 10000.0f; _report_gps_pos.fix_type = 0; publish(); /* GPS is obviously detected successfully, reset statistics */ _helper->resetUpdateRates(); } int helper_ret; while ((helper_ret = _helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) { if (helper_ret & 1) { publish(); last_rate_count++; } if (_p_report_sat_info && (helper_ret & 2)) { publishSatelliteInfo(); } /* measure update rate every 5 seconds */ if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { float dt = (float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f; _rate = last_rate_count / dt; _rate_rtcm_injection = _last_rate_rtcm_injection_count / dt; last_rate_measurement = hrt_absolute_time(); last_rate_count = 0; _last_rate_rtcm_injection_count = 0; _helper->storeUpdateRates(); _helper->resetUpdateRates(); } if (!_healthy) { // Helpful for debugging, but too verbose for normal ops // const char *mode_str = "unknown"; // // switch (_mode) { // case GPS_DRIVER_MODE_UBX: // mode_str = "UBX"; // break; // // case GPS_DRIVER_MODE_MTK: // mode_str = "MTK"; // break; // // case GPS_DRIVER_MODE_ASHTECH: // mode_str = "ASHTECH"; // break; // // default: // break; // } // // PX4_WARN("module found: %s", mode_str); _healthy = true; } //check for disarming if (_vehicle_status_sub != -1 && _dump_from_gps_device_fd != -1) { bool updated; orb_check(_vehicle_status_sub, &updated); if (updated) { vehicle_status_s vehicle_status; orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &vehicle_status); bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) || (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR); if (armed) { _is_armed = true; } else if (_is_armed) { //disable communication dump when disarming close(_dump_from_gps_device_fd); _dump_from_gps_device_fd = -1; close(_dump_to_gps_device_fd); _dump_to_gps_device_fd = -1; _is_armed = false; } } } } if (_healthy) { PX4_WARN("GPS module lost"); _healthy = false; _rate = 0.0f; _rate_rtcm_injection = 0.0f; } } /* select next mode */ switch (_mode) { case GPS_DRIVER_MODE_UBX: _mode = GPS_DRIVER_MODE_MTK; break; case GPS_DRIVER_MODE_MTK: _mode = GPS_DRIVER_MODE_ASHTECH; break; case GPS_DRIVER_MODE_ASHTECH: _mode = GPS_DRIVER_MODE_UBX; break; default: break; } } } PX4_WARN("exiting"); for (size_t i = 0; i < _orb_inject_data_fd_count; ++i) { orb_unsubscribe(_orb_inject_data_fd[i]); _orb_inject_data_fd[i] = -1; } if (_dump_to_gps_device_fd != -1) { close(_dump_to_gps_device_fd); _dump_to_gps_device_fd = -1; } if (_dump_from_gps_device_fd != -1) { close(_dump_from_gps_device_fd); _dump_from_gps_device_fd = -1; } if (_vehicle_status_sub != -1) { orb_unsubscribe(_vehicle_status_sub); _vehicle_status_sub = -1; } ::close(_serial_fd); orb_unadvertise(_report_gps_pos_pub); /* tell the dtor that we are exiting */ _task = -1; px4_task_exit(0); }
CameraTrigger::CameraTrigger() : _engagecall {}, _disengagecall {}, _engage_turn_on_off_call {}, _disengage_turn_on_off_call {}, _keepalivecall_up {}, _keepalivecall_down {}, _activation_time(0.5f /* ms */), _interval(100.0f /* ms */), _distance(25.0f /* m */), _trigger_seq(0), _trigger_enabled(false), _trigger_paused(false), _one_shot(false), _test_shot(false), _turning_on(false), _last_shoot_position(0.0f, 0.0f), _valid_position(false), _command_sub(-1), _lpos_sub(-1), _trigger_pub(nullptr), _cmd_ack_pub(nullptr), _trigger_mode(TRIGGER_MODE_NONE), _camera_interface_mode(CAMERA_INTERFACE_MODE_GPIO), _camera_interface(nullptr) { // Initiate camera interface based on camera_interface_mode if (_camera_interface != nullptr) { delete (_camera_interface); // set to zero to ensure parser is not used while not instantiated _camera_interface = nullptr; } memset(&_work, 0, sizeof(_work)); // Parameters _p_interval = param_find("TRIG_INTERVAL"); _p_distance = param_find("TRIG_DISTANCE"); _p_activation_time = param_find("TRIG_ACT_TIME"); _p_mode = param_find("TRIG_MODE"); _p_interface = param_find("TRIG_INTERFACE"); param_get(_p_activation_time, &_activation_time); param_get(_p_interval, &_interval); param_get(_p_distance, &_distance); param_get(_p_mode, &_trigger_mode); param_get(_p_interface, &_camera_interface_mode); switch (_camera_interface_mode) { #ifdef __PX4_NUTTX case CAMERA_INTERFACE_MODE_GPIO: _camera_interface = new CameraInterfaceGPIO(); break; case CAMERA_INTERFACE_MODE_GENERIC_PWM: _camera_interface = new CameraInterfacePWM(); break; case CAMERA_INTERFACE_MODE_SEAGULL_MAP2_PWM: _camera_interface = new CameraInterfaceSeagull(); break; #endif case CAMERA_INTERFACE_MODE_MAVLINK: // start an interface that does nothing. Instead mavlink will listen to the camera_trigger uORB message _camera_interface = new CameraInterface(); break; default: PX4_ERR("unknown camera interface mode: %i", (int)_camera_interface_mode); break; } // Enforce a lower bound on the activation interval in PWM modes to not miss // engage calls in-between 50Hz PWM pulses. (see PX4 PR #6973) if ((_activation_time < 40.0f) && (_camera_interface_mode == CAMERA_INTERFACE_MODE_GENERIC_PWM || _camera_interface_mode == CAMERA_INTERFACE_MODE_SEAGULL_MAP2_PWM)) { _activation_time = 40.0f; PX4_WARN("Trigger interval too low for PWM interface, setting to 40 ms"); param_set(_p_activation_time, &(_activation_time)); } // Advertise critical publishers here, because we cannot advertise in interrupt context struct camera_trigger_s trigger = {}; _trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger); }
void CameraTrigger::cycle_trampoline(void *arg) { CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg); // default loop polling interval int poll_interval_usec = 5000; if (trig->_command_sub < 0) { trig->_command_sub = orb_subscribe(ORB_ID(vehicle_command)); } bool updated = false; orb_check(trig->_command_sub, &updated); struct vehicle_command_s cmd; unsigned cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; bool need_ack = false; // this flag is set when the polling loop is slowed down to allow the camera to power on trig->_turning_on = false; // these flags are used to detect state changes in the command loop bool main_state = trig->_trigger_enabled; bool pause_state = trig->_trigger_paused; // Command handling if (updated) { orb_copy(ORB_ID(vehicle_command), trig->_command_sub, &cmd); if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL) { need_ack = true; if (commandParamToInt(cmd.param7) == 1) { // test shots are not logged or forwarded to GCS for geotagging trig->_test_shot = true; } if (commandParamToInt((float)cmd.param5) == 1) { // Schedule shot trig->_one_shot = true; } cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) { need_ack = true; if (commandParamToInt(cmd.param3) == 1) { // pause triggger trig->_trigger_paused = true; } else if (commandParamToInt(cmd.param3) == 0) { trig->_trigger_paused = false; } if (commandParamToInt(cmd.param2) == 1) { // reset trigger sequence trig->_trigger_seq = 0; } if (commandParamToInt(cmd.param1) == 1) { trig->_trigger_enabled = true; } else if (commandParamToInt(cmd.param1) == 0) { trig->_trigger_enabled = false; } cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST) { need_ack = true; /* * TRANSITIONAL SUPPORT ADDED AS OF 11th MAY 2017 (v1.6 RELEASE) */ if (cmd.param1 > 0.0f) { trig->_distance = cmd.param1; param_set(trig->_p_distance, &(trig->_distance)); trig->_trigger_enabled = true; trig->_trigger_paused = false; } else if (commandParamToInt(cmd.param1) == 0) { trig->_trigger_paused = true; } else if (commandParamToInt(cmd.param1) == -1) { trig->_trigger_enabled = false; } // We can only control the shutter integration time of the camera in GPIO mode (for now) if (cmd.param2 > 0.0f) { if (trig->_camera_interface_mode == CAMERA_INTERFACE_MODE_GPIO) { trig->_activation_time = cmd.param2; param_set(trig->_p_activation_time, &(trig->_activation_time)); } } // Trigger once immediately if param is set if (cmd.param3 > 0.0f) { // Schedule shot trig->_one_shot = true; } cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL) { need_ack = true; if (cmd.param1 > 0.0f) { trig->_interval = cmd.param1; param_set(trig->_p_interval, &(trig->_interval)); } // We can only control the shutter integration time of the camera in GPIO mode if (cmd.param2 > 0.0f) { if (trig->_camera_interface_mode == CAMERA_INTERFACE_MODE_GPIO) { trig->_activation_time = cmd.param2; param_set(trig->_p_activation_time, &(trig->_activation_time)); } } cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } } // State change handling if ((main_state != trig->_trigger_enabled) || (pause_state != trig->_trigger_paused) || trig->_one_shot) { if (trig->_trigger_enabled || trig->_one_shot) { // Just got enabled via a command // If camera isn't already powered on, we enable power to it if (!trig->_camera_interface->is_powered_on() && trig->_camera_interface->has_power_control()) { trig->toggle_power(); trig->enable_keep_alive(true); // Give the camera time to turn on before starting to send trigger signals poll_interval_usec = 3000000; trig->_turning_on = true; } } else if (!trig->_trigger_enabled || trig->_trigger_paused) { // Just got disabled/paused via a command // Power off the camera if we are disabled if (trig->_camera_interface->is_powered_on() && trig->_camera_interface->has_power_control() && !trig->_trigger_enabled) { trig->enable_keep_alive(false); trig->toggle_power(); } // cancel all calls for both disabled and paused hrt_cancel(&trig->_engagecall); hrt_cancel(&trig->_disengagecall); // ensure that the pin is off hrt_call_after(&trig->_disengagecall, 0, (hrt_callout)&CameraTrigger::disengage, trig); // reset distance counter if needed if (trig->_trigger_mode == TRIGGER_MODE_DISTANCE_ON_CMD || trig->_trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON) { // this will force distance counter reinit on getting enabled/unpaused trig->_valid_position = false; } } // only run on state changes, not every loop iteration if (trig->_trigger_mode == TRIGGER_MODE_INTERVAL_ON_CMD) { // update intervalometer state and reset calls trig->update_intervalometer(); } } // run every loop iteration and trigger if needed if (trig->_trigger_mode == TRIGGER_MODE_DISTANCE_ON_CMD || trig->_trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON) { // update distance counter and trigger trig->update_distance(); } // One shot command-based capture handling if (trig->_one_shot && !trig->_turning_on) { // One-shot trigger trig->shoot_once(); trig->_one_shot = false; if (trig->_test_shot) { trig->_test_shot = false; } } // Command ACK handling if (updated && need_ack) { vehicle_command_ack_s command_ack = { .timestamp = 0, .result_param2 = 0, .command = cmd.command, .result = (uint8_t)cmd_result, .from_external = false, .result_param1 = 0, .target_system = cmd.source_system, .target_component = cmd.source_component }; if (trig->_cmd_ack_pub == nullptr) { trig->_cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, vehicle_command_ack_s::ORB_QUEUE_LENGTH); } else { orb_publish(ORB_ID(vehicle_command_ack), trig->_cmd_ack_pub, &command_ack); } } work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline, camera_trigger::g_camera_trigger, USEC2TICK(poll_interval_usec)); }
void BMI055_gyro::measure() { if (hrt_absolute_time() < _reset_wait) { // we're waiting for a reset to complete return; } struct BMI_GyroReport bmi_gyroreport; struct Report { int16_t temp; int16_t gyro_x; int16_t gyro_y; int16_t gyro_z; } report; /* start measuring */ perf_begin(_sample_perf); /* * Fetch the full set of measurements from the BMI055 gyro in one pass. */ bmi_gyroreport.cmd = BMI055_GYR_X_L | DIR_READ; if (OK != transfer((uint8_t *)&bmi_gyroreport, ((uint8_t *)&bmi_gyroreport), sizeof(bmi_gyroreport))) { return; } check_registers(); uint8_t temp = read_reg(BMI055_ACC_TEMP); report.temp = temp; report.gyro_x = bmi_gyroreport.gyro_x; report.gyro_y = bmi_gyroreport.gyro_y; report.gyro_z = bmi_gyroreport.gyro_z; if (report.temp == 0 && report.gyro_x == 0 && report.gyro_y == 0 && report.gyro_z == 0) { // all zero data - probably a SPI bus error perf_count(_bad_transfers); perf_end(_sample_perf); // note that we don't call reset() here as a reset() // costs 20ms with interrupts disabled. That means if // the bmi055 does go bad it would cause a FMU failure, // regardless of whether another sensor is available, return; } perf_count(_good_transfers); if (_register_wait != 0) { // we are waiting for some good transfers before using // the sensor again. We still increment // _good_transfers, but don't return any data yet _register_wait--; return; } /* * Report buffers. */ gyro_report grb; grb.timestamp = hrt_absolute_time(); // report the error count as the sum of the number of bad // transfers and bad register reads. This allows the higher // level code to decide if it should use this sensor based on // whether it has had failures grb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); /* * 1) Scale raw value to SI units using scaling from datasheet. * 2) Subtract static offset (in SI units) * 3) Scale the statically calibrated values with a linear * dynamically obtained factor * * Note: the static sensor offset is the number the sensor outputs * at a nominally 'zero' input. Therefore the offset has to * be subtracted. * * Example: A gyro outputs a value of 74 at zero angular rate * the offset is 74 from the origin and subtracting * 74 from all measurements centers them around zero. */ grb.x_raw = report.gyro_x; grb.y_raw = report.gyro_y; grb.z_raw = report.gyro_z; float xraw_f = report.gyro_x; float yraw_f = report.gyro_y; float zraw_f = report.gyro_z; // apply user specified rotation rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; grb.x = _gyro_filter_x.apply(x_gyro_in_new); grb.y = _gyro_filter_y.apply(y_gyro_in_new); grb.z = _gyro_filter_z.apply(z_gyro_in_new); matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); matrix::Vector3f gval_integrated; bool gyro_notify = _gyro_int.put(grb.timestamp, gval, gval_integrated, grb.integral_dt); grb.x_integral = gval_integrated(0); grb.y_integral = gval_integrated(1); grb.z_integral = gval_integrated(2); grb.scaling = _gyro_range_scale; grb.range_rad_s = _gyro_range_rad_s; grb.temperature_raw = report.temp; grb.temperature = _last_temperature; grb.device_id = _device_id.devid; _gyro_reports->force(&grb); /* notify anyone waiting for data */ if (gyro_notify) { poll_notify(POLLIN); } if (gyro_notify && !(_pub_blocked)) { /* publish it */ orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb); } /* stop measuring */ perf_end(_sample_perf); }
void task_main(int argc, char *argv[]) { _is_running = true; if (uart_initialize(_device) < 0) { PX4_ERR("Failed to initialize UART."); return; } // Subscribe for orb topics _controls_sub = orb_subscribe(ORB_ID(actuator_controls_0)); _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); // Start disarmed _armed.armed = false; _armed.prearmed = false; // Set up poll topic px4_pollfd_struct_t fds[1]; fds[0].fd = _controls_sub; fds[0].events = POLLIN; /* Don't limit poll intervall for now, 250 Hz should be fine. */ //orb_set_interval(_controls_sub, 10); // Set up mixer if (initialize_mixer(MIXER_FILENAME) < 0) { PX4_ERR("Mixer initialization failed."); return; } pwm_limit_init(&_pwm_limit); // TODO XXX: this is needed otherwise we crash in the callback context. _rc_pub = orb_advertise(ORB_ID(input_rc), &_rc); // Main loop while (!_task_should_exit) { int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 10); /* Timed out, do a periodic check for _task_should_exit. */ if (pret == 0) { continue; } /* This is undesirable but not much we can do. */ if (pret < 0) { PX4_WARN("poll error %d, %d", pret, errno); /* sleep a bit before next try */ usleep(100000); continue; } if (fds[0].revents & POLLIN) { orb_copy(ORB_ID(actuator_controls_0), _controls_sub, &_controls); _outputs.timestamp = _controls.timestamp; /* do mixing */ _outputs.noutputs = _mixer->mix(_outputs.output, 0 /* not used */, NULL); /* disable unused ports by setting their output to NaN */ for (size_t i = _outputs.noutputs; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) { _outputs.output[i] = NAN; } const uint16_t reverse_mask = 0; uint16_t disarmed_pwm[4]; uint16_t min_pwm[4]; uint16_t max_pwm[4]; uint16_t trim_pwm[4]; for (unsigned int i = 0; i < 4; i++) { disarmed_pwm[i] = _pwm_disarmed; min_pwm[i] = _pwm_min; max_pwm[i] = _pwm_max; trim_pwm[i] = 0; } uint16_t pwm[4]; // TODO FIXME: pre-armed seems broken pwm_limit_calc(_armed.armed, false/*_armed.prearmed*/, _outputs.noutputs, reverse_mask, disarmed_pwm, min_pwm, max_pwm, _outputs.output, pwm, &_pwm_limit); send_outputs_mavlink(pwm, 4); if (_outputs_pub != nullptr) { orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs); } else { _outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs); } } bool updated; orb_check(_armed_sub, &updated); if (updated) { orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); } } uart_deinitialize(); orb_unsubscribe(_controls_sub); orb_unsubscribe(_armed_sub); _is_running = false; }
int LIS3MDL::collect() { #pragma pack(push, 1) struct { uint8_t x[2]; uint8_t y[2]; uint8_t z[2]; } lis_report; struct { int16_t x; int16_t y; int16_t z; int16_t t; } report; #pragma pack(pop) int ret = 0; uint8_t buf_rx[2] = {0}; float xraw_f; float yraw_f; float zraw_f; struct mag_report new_mag_report; bool sensor_is_onboard = false; perf_begin(_sample_perf); new_mag_report.timestamp = hrt_absolute_time(); new_mag_report.error_count = perf_event_count(_comms_errors); new_mag_report.scaling = _range_scale; new_mag_report.device_id = _device_id.devid; ret = _interface->read(ADDR_OUT_X_L, (uint8_t *)&lis_report, sizeof(lis_report)); /** * Weird behavior: the X axis will be read instead of the temperature registers if you use a pointer to a packed struct...not sure why. * This works now, but further investigation to determine why this happens would be good (I am guessing a type error somewhere) */ ret = _interface->read(ADDR_OUT_T_L, (uint8_t *)&buf_rx, sizeof(buf_rx)); if (ret != OK) { perf_count(_comms_errors); PX4_WARN("Register read error."); return ret; } report.x = (int16_t)((lis_report.x[1] << 8) | lis_report.x[0]); report.y = (int16_t)((lis_report.y[1] << 8) | lis_report.y[0]); report.z = (int16_t)((lis_report.z[1] << 8) | lis_report.z[0]); report.t = (int16_t)((buf_rx[1] << 8) | buf_rx[0]); float temperature = report.t; new_mag_report.temperature = 25.0f + (temperature / 8.0f); // XXX revisit for SPI part, might require a bus type IOCTL unsigned dummy = 0; sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy); new_mag_report.is_external = !sensor_is_onboard; /** * RAW outputs */ new_mag_report.x_raw = report.x; new_mag_report.y_raw = report.y; new_mag_report.z_raw = report.z; xraw_f = report.x; yraw_f = report.y; zraw_f = report.z; // apply user specified rotation rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); new_mag_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale; /* flip axes and negate value for y */ new_mag_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale; /* z remains z */ new_mag_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale; if (!(_pub_blocked)) { if (_mag_topic != nullptr) { /* publish it */ orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_mag_report); } else { _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_mag_report, &_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX); if (_mag_topic == nullptr) { DEVICE_DEBUG("ADVERT FAIL"); } } } _last_report = new_mag_report; /* post a report to the ring */ _reports->force(&new_mag_report); /* notify anyone waiting for data */ poll_notify(POLLIN); ret = OK; perf_end(_sample_perf); return ret; }
int SF0X::collect() { int ret; perf_begin(_sample_perf); /* clear buffer if last read was too long ago */ uint64_t read_elapsed = hrt_elapsed_time(&_last_read); /* the buffer for read chars is buflen minus null termination */ char readbuf[sizeof(_linebuf)]; unsigned readlen = sizeof(readbuf) - 1; /* read from the sensor (uart buffer) */ ret = ::read(_fd, &readbuf[0], readlen); if (ret < 0) { debug("read err: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); /* only throw an error if we time out */ if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { return ret; } else { return -EAGAIN; } } else if (ret == 0) { return -EAGAIN; } _last_read = hrt_absolute_time(); float si_units; bool valid = false; for (int i = 0; i < ret; i++) { if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &si_units)) { valid = true; } } if (!valid) { return -EAGAIN; } debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO")); struct range_finder_report report; /* this should be fairly close to the end of the measurement, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); report.distance = si_units; report.minimum_distance = get_minimum_distance(); report.maximum_distance = get_maximum_distance(); report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0); /* publish it */ orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); if (_reports->force(&report)) { perf_count(_buffer_overflows); } /* notify anyone waiting for data */ poll_notify(POLLIN); ret = OK; perf_end(_sample_perf); return ret; }
/** * Transition from one hil state to another */ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { transition_result_t ret = TRANSITION_DENIED; if (current_status->hil_state == new_state) { ret = TRANSITION_NOT_CHANGED; } else { switch (new_state) { case vehicle_status_s::HIL_STATE_OFF: /* we're in HIL and unexpected things can happen if we disable HIL now */ mavlink_and_console_log_critical(mavlink_fd, "Not switching off HIL (safety)"); ret = TRANSITION_DENIED; break; case vehicle_status_s::HIL_STATE_ON: if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { #ifdef __PX4_NUTTX /* Disable publication of all attached sensors */ /* list directory */ DIR *d; d = opendir("/dev"); if (d) { struct dirent *direntry; char devname[24]; while ((direntry = readdir(d)) != NULL) { /* skip serial ports */ if (!strncmp("tty", direntry->d_name, 3)) { continue; } /* skip mtd devices */ if (!strncmp("mtd", direntry->d_name, 3)) { continue; } /* skip ram devices */ if (!strncmp("ram", direntry->d_name, 3)) { continue; } /* skip MMC devices */ if (!strncmp("mmc", direntry->d_name, 3)) { continue; } /* skip mavlink */ if (!strcmp("mavlink", direntry->d_name)) { continue; } /* skip console */ if (!strcmp("console", direntry->d_name)) { continue; } /* skip null */ if (!strcmp("null", direntry->d_name)) { continue; } snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name); int sensfd = ::open(devname, 0); if (sensfd < 0) { warn("failed opening device %s", devname); continue; } int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1); close(sensfd); printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); } closedir(d); ret = TRANSITION_CHANGED; mavlink_and_console_log_critical(mavlink_fd, "Switched to ON hil state"); } else { /* failed opening dir */ mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY"); ret = TRANSITION_DENIED; } #else const char *devname; unsigned int handle = 0; for(;;) { devname = px4_get_device_names(&handle); if (devname == NULL) break; /* skip mavlink */ if (!strcmp("/dev/mavlink", devname)) { continue; } int sensfd = px4_open(devname, 0); if (sensfd < 0) { warn("failed opening device %s", devname); continue; } int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1); px4_close(sensfd); printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); } ret = TRANSITION_CHANGED; mavlink_and_console_log_critical(mavlink_fd, "Switched to ON hil state"); #endif } else { mavlink_and_console_log_critical(mavlink_fd, "Not switching to HIL when armed"); ret = TRANSITION_DENIED; } break; default: warnx("Unknown HIL state"); break; } } if (ret == TRANSITION_CHANGED) { current_status->hil_state = new_state; current_status->timestamp = hrt_absolute_time(); // XXX also set lockdown here orb_publish(ORB_ID(vehicle_status), status_pub, current_status); } return ret; }
void Navigator::params_update() { parameter_update_s param_update; orb_copy(ORB_ID(parameter_update), _param_update_sub, ¶m_update); }
int systeminitialization(void){ //---------- sensor initialization ---------// // subscribe to sensor_combined topic sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined)); // set up the discretization time orb_set_interval(sensor_sub_fd, 30); // set up the poll structure fds[0].fd= sensor_sub_fd; fds[0].events= POLLIN; // initialize the counter missingDataCounter= 0; //---------- motor initialization ---------// warnx("DO NOT FORGET TO STOP THE COMMANDER APP!"); warnx("(run <commander stop> to do so)"); memset(&actuators, 0, sizeof(actuators)); actuator_pub_fd = orb_advertise(ORB_ID(actuator_controls_0), &actuators); memset(&arm, 0 , sizeof(arm)); arm.timestamp = hrt_absolute_time(); arm.ready_to_arm = true; arm.armed = true; arm_pub_fd = orb_advertise(ORB_ID(actuator_armed), &arm); orb_publish(ORB_ID(actuator_armed), arm_pub_fd, &arm); /* read back values to validate */ arm_sub_fd = orb_subscribe(ORB_ID(actuator_armed)); orb_copy(ORB_ID(actuator_armed), arm_sub_fd, &arm); if (arm.ready_to_arm && arm.armed) { warnx("Actuator armed"); } else { errx(1, "Arming actuators failed"); } /* float outESC= 1.0f; int count= 1; while(count < 100){ actuators.control[0] = outESC; actuators.control[1] = outESC; actuators.control[2] = outESC; actuators.control[3] = outESC; actuators.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(actuator_controls_0), actuator_pub_fd, &actuators); usleep(10); } */ return 0; }
void Navigator::task_main() { bool have_geofence_position_data = false; /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file * else clear geofence data in datamanager */ struct stat buffer; if (stat(GEOFENCE_FILENAME, &buffer) == 0) { PX4_INFO("Try to load geofence.txt"); _geofence.loadFromFile(GEOFENCE_FILENAME); } else { if (_geofence.clearDm() != OK) { mavlink_log_critical(&_mavlink_log_pub, "failed clearing geofence"); } } /* do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); _vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); /* copy all topics first time */ vehicle_status_update(); vehicle_land_detected_update(); vehicle_control_mode_update(); global_position_update(); gps_position_update(); sensor_combined_update(); home_position_update(true); fw_pos_ctrl_status_update(); params_update(); /* wakeup source(s) */ px4_pollfd_struct_t fds[1] = {}; /* Setup of loop */ fds[0].fd = _global_pos_sub; fds[0].events = POLLIN; bool global_pos_available_once = false; while (!_task_should_exit) { /* wait for up to 200ms for data */ int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); if (pret == 0) { /* timed out - periodic check for _task_should_exit, etc. */ if (global_pos_available_once) { global_pos_available_once = false; PX4_WARN("global position timeout"); } /* Let the loop run anyway, don't do `continue` here. */ } else if (pret < 0) { /* this is undesirable but not much we can do - might want to flag unhappy status */ PX4_ERR("nav: poll error %d, %d", pret, errno); usleep(10000); continue; } else { if (fds[0].revents & POLLIN) { /* success, global pos is available */ global_position_update(); if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { have_geofence_position_data = true; } global_pos_available_once = true; } } perf_begin(_loop_perf); bool updated; /* gps updated */ orb_check(_gps_pos_sub, &updated); if (updated) { gps_position_update(); if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) { have_geofence_position_data = true; } } /* sensors combined updated */ orb_check(_sensor_combined_sub, &updated); if (updated) { sensor_combined_update(); } /* parameters updated */ orb_check(_param_update_sub, &updated); if (updated) { params_update(); updateParams(); } /* vehicle control mode updated */ orb_check(_control_mode_sub, &updated); if (updated) { vehicle_control_mode_update(); } /* vehicle status updated */ orb_check(_vstatus_sub, &updated); if (updated) { vehicle_status_update(); } /* vehicle land detected updated */ orb_check(_land_detected_sub, &updated); if (updated) { vehicle_land_detected_update(); } /* navigation capabilities updated */ orb_check(_fw_pos_ctrl_status_sub, &updated); if (updated) { fw_pos_ctrl_status_update(); } /* home position updated */ orb_check(_home_pos_sub, &updated); if (updated) { home_position_update(); } orb_check(_vehicle_command_sub, &updated); if (updated) { vehicle_command_s cmd; orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) { struct position_setpoint_triplet_s *rep = get_reposition_triplet(); // store current position as previous position and goal as next rep->previous.yaw = get_global_position()->yaw; rep->previous.lat = get_global_position()->lat; rep->previous.lon = get_global_position()->lon; rep->previous.alt = get_global_position()->alt; rep->current.loiter_radius = get_loiter_radius(); rep->current.loiter_direction = 1; rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; // Go on and check which changes had been requested if (PX4_ISFINITE(cmd.param4)) { rep->current.yaw = cmd.param4; } else { rep->current.yaw = NAN; } if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7; rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7; } else { rep->current.lat = get_global_position()->lat; rep->current.lon = get_global_position()->lon; } if (PX4_ISFINITE(cmd.param7)) { rep->current.alt = cmd.param7; } else { rep->current.alt = get_global_position()->alt; } rep->previous.valid = true; rep->current.valid = true; rep->next.valid = false; } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) { struct position_setpoint_triplet_s *rep = get_takeoff_triplet(); // store current position as previous position and goal as next rep->previous.yaw = get_global_position()->yaw; rep->previous.lat = get_global_position()->lat; rep->previous.lon = get_global_position()->lon; rep->previous.alt = get_global_position()->alt; rep->current.loiter_radius = get_loiter_radius(); rep->current.loiter_direction = 1; rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; rep->current.yaw = cmd.param4; if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7; rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7; } else { // If one of them is non-finite, reset both rep->current.lat = NAN; rep->current.lon = NAN; } rep->current.alt = cmd.param7; rep->previous.valid = true; rep->current.valid = true; rep->next.valid = false; } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_LAND_START) { /* find NAV_CMD_DO_LAND_START in the mission and * use MAV_CMD_MISSION_START to start the mission there */ unsigned land_start = _mission.find_offboard_land_start(); if (land_start != -1) { vehicle_command_s vcmd = {}; vcmd.target_system = get_vstatus()->system_id; vcmd.target_component = get_vstatus()->component_id; vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START; vcmd.param1 = land_start; vcmd.param2 = 0; orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH); (void)orb_unadvertise(pub); } } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) { if (get_mission_result()->valid && PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0) && (cmd.param1 < _mission_result.seq_total)) { _mission.set_current_offboard_mission_index(cmd.param1); } } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_PAUSE_CONTINUE) { PX4_INFO("got pause/continue command"); } } /* Check geofence violation */ static hrt_abstime last_geofence_check = 0; if (have_geofence_position_data && (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && (hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) { bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, home_position_valid()); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; _geofence_result.geofence_action = _geofence.getGeofenceAction(); if (!inside) { /* inform other apps via the mission result */ _geofence_result.geofence_violated = true; publish_geofence_result(); /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { mavlink_log_critical(&_mavlink_log_pub, "Geofence violation"); _geofence_violation_warning_sent = true; } } else { /* inform other apps via the mission result */ _geofence_result.geofence_violated = false; publish_geofence_result(); /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } } /* Do stuff according to navigation state set by commander */ switch (_vstatus.nav_state) { case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_mission; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_loiter; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: _pos_sp_triplet_published_invalid_once = false; if (_param_rcloss_act.get() == 1) { _navigation_mode = &_loiter; } else if (_param_rcloss_act.get() == 3) { _navigation_mode = &_land; } else if (_param_rcloss_act.get() == 4) { _navigation_mode = &_rcLoss; } else { /* if == 2 or unknown, RTL */ _navigation_mode = &_rtl; } break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_rtl; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_takeoff; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_land; break; case vehicle_status_s::NAVIGATION_STATE_DESCEND: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_land; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ _pos_sp_triplet_published_invalid_once = false; if (_param_datalinkloss_act.get() == 1) { _navigation_mode = &_loiter; } else if (_param_datalinkloss_act.get() == 3) { _navigation_mode = &_land; } else if (_param_datalinkloss_act.get() == 4) { _navigation_mode = &_dataLinkLoss; } else { /* if == 2 or unknown, RTL */ _navigation_mode = &_rtl; } break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_engineFailure; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_gpsFailure; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_follow_target; break; case vehicle_status_s::NAVIGATION_STATE_MANUAL: case vehicle_status_s::NAVIGATION_STATE_ACRO: case vehicle_status_s::NAVIGATION_STATE_ALTCTL: case vehicle_status_s::NAVIGATION_STATE_POSCTL: case vehicle_status_s::NAVIGATION_STATE_TERMINATION: case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: case vehicle_status_s::NAVIGATION_STATE_STAB: default: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = nullptr; _can_loiter_at_sp = false; break; } /* iterate through navigation modes and set active/inactive for each */ for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); } /* if nothing is running, set position setpoint triplet invalid once */ if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) { _pos_sp_triplet_published_invalid_once = true; _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; _pos_sp_triplet_updated = true; } if (_pos_sp_triplet_updated) { _pos_sp_triplet.timestamp = hrt_absolute_time(); publish_position_setpoint_triplet(); _pos_sp_triplet_updated = false; } if (_mission_result_updated) { publish_mission_result(); _mission_result_updated = false; } perf_end(_loop_perf); } PX4_INFO("exiting"); _navigator_task = -1; return; }
int SF0X::collect() { int ret; perf_begin(_sample_perf); /* clear buffer if last read was too long ago */ uint64_t read_elapsed = hrt_elapsed_time(&_last_read); /* the buffer for read chars is buflen minus null termination */ char readbuf[sizeof(_linebuf)]; unsigned readlen = sizeof(readbuf) - 1; /* read from the sensor (uart buffer) */ ret = ::read(_fd, &readbuf[0], readlen); if (ret < 0) { DEVICE_DEBUG("read err: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); /* only throw an error if we time out */ if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { return ret; } else { return -EAGAIN; } } else if (ret == 0) { return -EAGAIN; } _last_read = hrt_absolute_time(); float distance_m = -1.0f; bool valid = false; for (int i = 0; i < ret; i++) { if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m)) { valid = true; } } if (!valid) { return -EAGAIN; } DEVICE_DEBUG("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((valid) ? "OK" : "NO")); struct distance_sensor_s report; report.timestamp = hrt_absolute_time(); report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; report.orientation = 8; report.current_distance = distance_m; report.min_distance = get_minimum_distance(); report.max_distance = get_maximum_distance(); report.covariance = 0.0f; /* TODO: set proper ID */ report.id = 0; /* publish it */ orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); if (_reports->force(&report)) { perf_count(_buffer_overflows); } /* notify anyone waiting for data */ poll_notify(POLLIN); ret = OK; perf_end(_sample_perf); return ret; }
void MulticopterAttitudeControl::task_main() { /* * do subscriptions */ _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); _motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits)); _quaternion_sub = orb_subscribe(ORB_ID(quaternion)); /* initialize parameters cache */ parameters_update(); /* wakeup source: vehicle attitude */ px4_pollfd_struct_t fds[1]; fds[0].fd = _ctrl_state_sub; fds[0].events = POLLIN; while (!_task_should_exit) { orb_copy(ORB_ID(quaternion), _quaternion_sub, &_quaternion); /* wait for up to 100ms for data */ int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit */ if (pret == 0) { continue; } /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { warn("mc att ctrl: poll error %d, %d", pret, errno); /* sleep a bit before next try */ usleep(100000); continue; } perf_begin(_loop_perf); /* run controller on attitude changes */ if (fds[0].revents & POLLIN) { static uint64_t last_run = 0; float dt = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); /* guard against too small (< 2ms) and too large (> 20ms) dt's */ if (dt < 0.002f) { dt = 0.002f; } else if (dt > 0.02f) { dt = 0.02f; } /* copy attitude and control state topics */ orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); /* check for updates in other topics */ parameter_update_poll(); vehicle_control_mode_poll(); arming_status_poll(); vehicle_manual_poll(); vehicle_status_poll(); vehicle_motor_limits_poll(); /* Check if we are in rattitude mode and the pilot is above the threshold on pitch * or roll (yaw can rotate 360 in normal att control). If both are true don't * even bother running the attitude controllers */ if (_vehicle_status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE) { if (fabsf(_manual_control_sp.y) > _params.rattitude_thres || fabsf(_manual_control_sp.x) > _params.rattitude_thres) { _v_control_mode.flag_control_attitude_enabled = false; } } if (_v_control_mode.flag_control_attitude_enabled) { if (_ts_opt_recovery == nullptr) { // the tailsitter recovery instance has not been created, thus, the vehicle // is not a tailsitter, do normal attitude control control_attitude(dt); } else { vehicle_attitude_setpoint_poll(); _thrust_sp = _v_att_sp.thrust; math::Quaternion q(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); math::Quaternion q_sp(&_v_att_sp.q_d[0]); _ts_opt_recovery->setAttGains(_params.att_p, _params.yaw_ff); _ts_opt_recovery->calcOptimalRates(q, q_sp, _v_att_sp.yaw_sp_move_rate, _rates_sp); /* limit rates */ for (int i = 0; i < 3; i++) { _rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i)); } } /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); _v_rates_sp.yaw = _rates_sp(2); _v_rates_sp.thrust = _thrust_sp; _v_rates_sp.timestamp = hrt_absolute_time(); if (_v_rates_sp_pub != nullptr) { orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); } else if (_rates_sp_id) { _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); } //} } else { /* attitude controller disabled, poll rates setpoint topic */ if (_v_control_mode.flag_control_manual_enabled) { /* manual rates control - ACRO mode */ _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max); _thrust_sp = math::min(_manual_control_sp.z, MANUAL_THROTTLE_MAX_MULTICOPTER); /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); _v_rates_sp.yaw = _rates_sp(2); _v_rates_sp.thrust = _thrust_sp; _v_rates_sp.timestamp = hrt_absolute_time(); if (_v_rates_sp_pub != nullptr) { orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); } else if (_rates_sp_id) { _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); } } else { /* attitude controller disabled, poll rates setpoint topic */ vehicle_rates_setpoint_poll(); _rates_sp(0) = _v_rates_sp.roll; _rates_sp(1) = _v_rates_sp.pitch; _rates_sp(2) = _v_rates_sp.yaw; _thrust_sp = _v_rates_sp.thrust; } } if (_v_control_mode.flag_control_rates_enabled) { control_attitude_rates(dt); /* publish actuator controls */ _actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f; _actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f; _actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f; _actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.timestamp = hrt_absolute_time(); _actuators.timestamp_sample = _ctrl_state.timestamp; _controller_status.roll_rate_integ = _rates_int(0); _controller_status.pitch_rate_integ = _rates_int(1); _controller_status.yaw_rate_integ = _rates_int(2); _controller_status.timestamp = hrt_absolute_time(); if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub != nullptr) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); perf_end(_controller_latency_perf); } else if (_actuators_id) { _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); } } /* publish controller status */ if (_controller_status_pub != nullptr) { orb_publish(ORB_ID(mc_att_ctrl_status), _controller_status_pub, &_controller_status); } else { _controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status); } } } perf_end(_loop_perf); } _control_task = -1; return; }
static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* welcome user */ warnx("started"); static int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[mpc] started"); /* structures */ struct vehicle_control_mode_s control_mode; memset(&control_mode, 0, sizeof(control_mode)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_attitude_setpoint_s att_sp; memset(&att_sp, 0, sizeof(att_sp)); struct manual_control_setpoint_s manual; memset(&manual, 0, sizeof(manual)); struct vehicle_local_position_s local_pos; memset(&local_pos, 0, sizeof(local_pos)); struct vehicle_local_position_setpoint_s local_pos_sp; memset(&local_pos_sp, 0, sizeof(local_pos_sp)); struct vehicle_global_position_setpoint_s global_pos_sp; memset(&global_pos_sp, 0, sizeof(global_pos_sp)); struct vehicle_global_velocity_setpoint_s global_vel_sp; memset(&global_vel_sp, 0, sizeof(global_vel_sp)); /* subscribe to attitude, motor setpoints and system state */ int param_sub = orb_subscribe(ORB_ID(parameter_update)); int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); /* publish setpoint */ orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp); orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); bool reset_mission_sp = false; bool global_pos_sp_valid = false; bool reset_man_sp_z = true; bool reset_man_sp_xy = true; bool reset_int_z = true; bool reset_int_z_manual = false; bool reset_int_xy = true; bool was_armed = false; bool reset_auto_sp_xy = true; bool reset_auto_sp_z = true; bool reset_takeoff_sp = true; hrt_abstime t_prev = 0; const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; float ref_alt = 0.0f; hrt_abstime ref_alt_t = 0; uint64_t local_ref_timestamp = 0; PID_t xy_pos_pids[2]; PID_t xy_vel_pids[2]; PID_t z_pos_pid; thrust_pid_t z_vel_pid; thread_running = true; struct multirotor_position_control_params params; struct multirotor_position_control_param_handles params_h; parameters_init(¶ms_h); parameters_update(¶ms_h, ¶ms); for (int i = 0; i < 2; i++) { pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); } pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f); thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); while (!thread_should_exit) { bool param_updated; orb_check(param_sub, ¶m_updated); if (param_updated) { /* clear updated flag */ struct parameter_update_s ps; orb_copy(ORB_ID(parameter_update), param_sub, &ps); /* update params */ parameters_update(¶ms_h, ¶ms); for (int i = 0; i < 2; i++) { pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); /* use integral_limit_out = tilt_max / 2 */ float i_limit; if (params.xy_vel_i > 0.0f) { i_limit = params.tilt_max / params.xy_vel_i / 2.0f; } else { i_limit = 0.0f; // not used } pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max); } pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min); } bool updated; orb_check(control_mode_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); } orb_check(global_pos_sp_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp); global_pos_sp_valid = true; reset_mission_sp = true; } hrt_abstime t = hrt_absolute_time(); float dt; if (t_prev != 0) { dt = (t - t_prev) * 0.000001f; } else { dt = 0.0f; } if (control_mode.flag_armed && !was_armed) { /* reset setpoints and integrals on arming */ reset_man_sp_z = true; reset_man_sp_xy = true; reset_auto_sp_z = true; reset_auto_sp_xy = true; reset_takeoff_sp = true; reset_int_z = true; reset_int_xy = true; } was_armed = control_mode.flag_armed; t_prev = t; if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) { orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f; float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f; float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f }; if (control_mode.flag_control_manual_enabled) { /* manual control */ /* check for reference point updates and correct setpoint */ if (local_pos.ref_timestamp != ref_alt_t) { if (ref_alt_t != 0) { /* home alt changed, don't follow large ground level changes in manual flight */ local_pos_sp.z += local_pos.ref_alt - ref_alt; } ref_alt_t = local_pos.ref_timestamp; ref_alt = local_pos.ref_alt; // TODO also correct XY setpoint } /* reset setpoints to current position if needed */ if (control_mode.flag_control_altitude_enabled) { if (reset_man_sp_z) { reset_man_sp_z = false; local_pos_sp.z = local_pos.z; mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z); } /* move altitude setpoint with throttle stick */ float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); if (z_sp_ctl != 0.0f) { sp_move_rate[2] = -z_sp_ctl * params.z_vel_max; local_pos_sp.z += sp_move_rate[2] * dt; if (local_pos_sp.z > local_pos.z + z_sp_offs_max) { local_pos_sp.z = local_pos.z + z_sp_offs_max; } else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) { local_pos_sp.z = local_pos.z - z_sp_offs_max; } } } if (control_mode.flag_control_position_enabled) { if (reset_man_sp_xy) { reset_man_sp_xy = false; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; pid_reset_integral(&xy_vel_pids[0]); pid_reset_integral(&xy_vel_pids[1]); mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); } /* move position setpoint with roll/pitch stick */ float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz); float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) { /* calculate direction and increment of control in NED frame */ float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl); float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max; sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed; sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed; local_pos_sp.x += sp_move_rate[0] * dt; local_pos_sp.y += sp_move_rate[1] * dt; /* limit maximum setpoint from position offset and preserve direction * fail safe, should not happen in normal operation */ float pos_vec_x = local_pos_sp.x - local_pos.x; float pos_vec_y = local_pos_sp.y - local_pos.y; float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max; if (pos_vec_norm > 1.0f) { local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm; local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm; } } } /* copy yaw setpoint to vehicle_local_position_setpoint topic */ local_pos_sp.yaw = att_sp.yaw_body; /* local position setpoint is valid and can be used for auto loiter after position controlled mode */ reset_auto_sp_xy = !control_mode.flag_control_position_enabled; reset_auto_sp_z = !control_mode.flag_control_altitude_enabled; reset_takeoff_sp = true; /* force reprojection of global setpoint after manual mode */ reset_mission_sp = true; } else if (control_mode.flag_control_auto_enabled) { /* AUTO mode, use global setpoint */ if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) { reset_auto_sp_xy = true; reset_auto_sp_z = true; } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { if (reset_takeoff_sp) { reset_takeoff_sp = false; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap; att_sp.yaw_body = att.yaw; mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z); } reset_auto_sp_xy = false; reset_auto_sp_z = true; } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { // TODO reset_auto_sp_xy = true; reset_auto_sp_z = true; } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) { /* init local projection using local position ref */ if (local_pos.ref_timestamp != local_ref_timestamp) { reset_mission_sp = true; local_ref_timestamp = local_pos.ref_timestamp; double lat_home = local_pos.ref_lat * 1e-7; double lon_home = local_pos.ref_lon * 1e-7; map_projection_init(lat_home, lon_home); mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home); } if (reset_mission_sp) { reset_mission_sp = false; /* update global setpoint projection */ if (global_pos_sp_valid) { /* global position setpoint valid, use it */ double sp_lat = global_pos_sp.lat * 1e-7; double sp_lon = global_pos_sp.lon * 1e-7; /* project global setpoint to local setpoint */ map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); if (global_pos_sp.altitude_is_relative) { local_pos_sp.z = -global_pos_sp.altitude; } else { local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; } att_sp.yaw_body = global_pos_sp.yaw; mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y); } else { if (reset_auto_sp_xy) { reset_auto_sp_xy = false; /* local position setpoint is invalid, * use current position as setpoint for loiter */ local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; local_pos_sp.yaw = att.yaw; } if (reset_auto_sp_z) { reset_auto_sp_z = false; local_pos_sp.z = local_pos.z; } mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); } } reset_auto_sp_xy = true; reset_auto_sp_z = true; } if (control_mode.auto_state != NAVIGATION_STATE_AUTO_TAKEOFF) { reset_takeoff_sp = true; } if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) { reset_mission_sp = true; } /* copy yaw setpoint to vehicle_local_position_setpoint topic */ local_pos_sp.yaw = att_sp.yaw_body; /* reset setpoints after AUTO mode */ reset_man_sp_xy = true; reset_man_sp_z = true; } else { /* no control (failsafe), loiter or stay on ground */ if (local_pos.landed) { /* landed: move setpoint down */ /* in air: hold altitude */ if (local_pos_sp.z < 5.0f) { /* set altitude setpoint to 5m under ground, * don't set it too deep to avoid unexpected landing in case of false "landed" signal */ local_pos_sp.z = 5.0f; mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z); } reset_man_sp_z = true; } else { /* in air: hold altitude */ if (reset_man_sp_z) { reset_man_sp_z = false; local_pos_sp.z = local_pos.z; mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z); } reset_auto_sp_z = false; } if (control_mode.flag_control_position_enabled) { if (reset_man_sp_xy) { reset_man_sp_xy = false; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; local_pos_sp.yaw = att.yaw; att_sp.yaw_body = att.yaw; mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); } reset_auto_sp_xy = false; } } /* publish local position setpoint */ orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); /* run position & altitude controllers, calculate velocity setpoint */ if (control_mode.flag_control_altitude_enabled) { global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2]; } else { reset_man_sp_z = true; global_vel_sp.vz = 0.0f; } if (control_mode.flag_control_position_enabled) { /* calculate velocity set point in NED frame */ global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0]; global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1]; /* limit horizontal speed */ float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max; if (xy_vel_sp_norm > 1.0f) { global_vel_sp.vx /= xy_vel_sp_norm; global_vel_sp.vy /= xy_vel_sp_norm; } } else { reset_man_sp_xy = true; global_vel_sp.vx = 0.0f; global_vel_sp.vy = 0.0f; } /* publish new velocity setpoint */ orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); // TODO subscribe to velocity setpoint if altitude/position control disabled if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) { /* run velocity controllers, calculate thrust vector with attitude-thrust compensation */ float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; if (control_mode.flag_control_climb_rate_enabled) { if (reset_int_z) { reset_int_z = false; float i = params.thr_min; if (reset_int_z_manual) { i = manual.throttle; if (i < params.thr_min) { i = params.thr_min; } else if (i > params.thr_max) { i = params.thr_max; } } thrust_pid_set_integral(&z_vel_pid, -i); mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); } thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); att_sp.thrust = -thrust_sp[2]; } else { /* reset thrust integral when altitude control enabled */ reset_int_z = true; } if (control_mode.flag_control_velocity_enabled) { /* calculate thrust set point in NED frame */ if (reset_int_xy) { reset_int_xy = false; pid_reset_integral(&xy_vel_pids[0]); pid_reset_integral(&xy_vel_pids[1]); mavlink_log_info(mavlink_fd, "[mpc] reset pos integral"); } thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt); thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt); /* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */ /* limit horizontal part of thrust */ float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]); /* assuming that vertical component of thrust is g, * horizontal component = g * tan(alpha) */ float tilt = atanf(norm(thrust_sp[0], thrust_sp[1])); if (tilt > params.tilt_max) { tilt = params.tilt_max; } /* convert direction to body frame */ thrust_xy_dir -= att.yaw; /* calculate roll and pitch */ att_sp.roll_body = sinf(thrust_xy_dir) * tilt; att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body); } else { reset_int_xy = true; } att_sp.timestamp = hrt_absolute_time(); /* publish new attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } } else { /* position controller disabled, reset setpoints */ reset_man_sp_z = true; reset_man_sp_xy = true; reset_int_z = true; reset_int_xy = true; reset_mission_sp = true; reset_auto_sp_xy = true; reset_auto_sp_z = true; } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled; /* run at approximately 50 Hz */ usleep(20000); } warnx("stopped"); mavlink_log_info(mavlink_fd, "[mpc] stopped"); thread_running = false; fflush(stdout); return 0; }
void MissionFeasibilityChecker::updateNavigationCapabilities() { (void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); }
void Navigator::gps_position_update() { orb_copy(ORB_ID(vehicle_gps_position), _gps_pos_sub, &_gps_pos); }
int uORBTest::UnitTest::test_single() { test_note("try single-topic support"); struct orb_test t, u; int sfd; orb_advert_t ptopic; bool updated; t.val = 0; ptopic = orb_advertise(ORB_ID(orb_test), &t); if (ptopic == nullptr) { return test_fail("advertise failed: %d", errno); } test_note("publish handle 0x%08x", ptopic); sfd = orb_subscribe(ORB_ID(orb_test)); if (sfd < 0) { return test_fail("subscribe failed: %d", errno); } test_note("subscribe fd %d", sfd); u.val = 1; if (PX4_OK != orb_copy(ORB_ID(orb_test), sfd, &u)) { return test_fail("copy(1) failed: %d", errno); } if (u.val != t.val) { return test_fail("copy(1) mismatch: %d expected %d", u.val, t.val); } if (PX4_OK != orb_check(sfd, &updated)) { return test_fail("check(1) failed"); } if (updated) { return test_fail("spurious updated flag"); } t.val = 2; test_note("try publish"); if (PX4_OK != orb_publish(ORB_ID(orb_test), ptopic, &t)) { return test_fail("publish failed"); } if (PX4_OK != orb_check(sfd, &updated)) { return test_fail("check(2) failed"); } if (!updated) { return test_fail("missing updated flag"); } if (PX4_OK != orb_copy(ORB_ID(orb_test), sfd, &u)) { return test_fail("copy(2) failed: %d", errno); } if (u.val != t.val) { return test_fail("copy(2) mismatch: %d expected %d", u.val, t.val); } orb_unsubscribe(sfd); int ret = orb_unadvertise(ptopic); if (ret != PX4_OK) { return test_fail("orb_unadvertise failed: %i", ret); } return test_note("PASS single-topic test"); }
void Navigator::sensor_combined_update() { orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); }
int uORBTest::UnitTest::pubsublatency_main(void) { /* poll on test topic and output latency */ float latency_integral = 0.0f; /* wakeup source(s) */ px4_pollfd_struct_t fds[3]; int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_test), 0); int test_multi_sub_medium = orb_subscribe_multi(ORB_ID(orb_test_medium), 0); int test_multi_sub_large = orb_subscribe_multi(ORB_ID(orb_test_large), 0); struct orb_test_large t; /* clear all ready flags */ orb_copy(ORB_ID(orb_test), test_multi_sub, &t); orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t); orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t); fds[0].fd = test_multi_sub; fds[0].events = POLLIN; fds[1].fd = test_multi_sub_medium; fds[1].events = POLLIN; fds[2].fd = test_multi_sub_large; fds[2].events = POLLIN; const unsigned maxruns = 1000; unsigned timingsgroup = 0; unsigned *timings = new unsigned[maxruns]; for (unsigned i = 0; i < maxruns; i++) { /* wait for up to 500ms for data */ int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); if (fds[0].revents & POLLIN) { orb_copy(ORB_ID(orb_test), test_multi_sub, &t); timingsgroup = 0; } else if (fds[1].revents & POLLIN) { orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t); timingsgroup = 1; } else if (fds[2].revents & POLLIN) { orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t); timingsgroup = 2; } if (pret < 0) { warn("poll error %d, %d", pret, errno); continue; } hrt_abstime elt = hrt_elapsed_time(&t.time); latency_integral += elt; timings[i] = elt; } orb_unsubscribe(test_multi_sub); orb_unsubscribe(test_multi_sub_medium); orb_unsubscribe(test_multi_sub_large); if (pubsubtest_print) { char fname[32]; sprintf(fname, PX4_ROOTFSDIR"/fs/microsd/timings%u.txt", timingsgroup); FILE *f = fopen(fname, "w"); if (f == NULL) { warnx("Error opening file!\n"); return uORB::ERROR; } for (unsigned i = 0; i < maxruns; i++) { fprintf(f, "%u\n", timings[i]); } fclose(f); } delete[] timings; warnx("mean: %8.4f us", static_cast<double>(latency_integral / maxruns)); pubsubtest_passed = true; if (static_cast<float>(latency_integral / maxruns) > 30.0f) { pubsubtest_res = uORB::ERROR; } else { pubsubtest_res = PX4_OK; } return pubsubtest_res; }
void Navigator::fw_pos_ctrl_status_update() { orb_copy(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_sub, &_fw_pos_ctrl_status); }
void AttitudePositionEstimatorEKF::pollData() { //Update arming status bool armedUpdate; orb_check(_armedSub, &armedUpdate); if (armedUpdate) { orb_copy(ORB_ID(actuator_armed), _armedSub, &_armed); } //Update Gyro and Accelerometer static Vector3f lastAngRate; static Vector3f lastAccel; bool accel_updated = false; orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); static hrt_abstime last_accel = 0; static hrt_abstime last_mag = 0; if (last_accel != _sensor_combined.accelerometer_timestamp) { accel_updated = true; } else { accel_updated = false; } last_accel = _sensor_combined.accelerometer_timestamp; // Copy gyro and accel _last_sensor_timestamp = _sensor_combined.timestamp; IMUmsec = _sensor_combined.timestamp / 1e3; IMUusec = _sensor_combined.timestamp; float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f; /* guard against too large deltaT's */ if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; } _last_run = _sensor_combined.timestamp; // Always store data, independent of init status /* fill in last data set */ _ekf->dtIMU = deltaT; int last_gyro_main = _gyro_main; if (isfinite(_sensor_combined.gyro_rad_s[0]) && isfinite(_sensor_combined.gyro_rad_s[1]) && isfinite(_sensor_combined.gyro_rad_s[2]) && (_sensor_combined.gyro_errcount <= (_sensor_combined.gyro1_errcount + GYRO_SWITCH_HYSTERESIS))) { _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; _gyro_main = 0; _gyro_valid = true; } else if (isfinite(_sensor_combined.gyro1_rad_s[0]) && isfinite(_sensor_combined.gyro1_rad_s[1]) && isfinite(_sensor_combined.gyro1_rad_s[2])) { _ekf->angRate.x = _sensor_combined.gyro1_rad_s[0]; _ekf->angRate.y = _sensor_combined.gyro1_rad_s[1]; _ekf->angRate.z = _sensor_combined.gyro1_rad_s[2]; _gyro_main = 1; _gyro_valid = true; } else { _gyro_valid = false; } if (last_gyro_main != _gyro_main) { mavlink_and_console_log_emergency(_mavlink_fd, "GYRO FAILED! Switched from #%d to %d", last_gyro_main, _gyro_main); } if (!_gyro_valid) { /* keep last value if he hit an out of band value */ lastAngRate = _ekf->angRate; } else { perf_count(_perf_gyro); } if (accel_updated) { int last_accel_main = _accel_main; /* fail over to the 2nd accel if we know the first is down */ if (_sensor_combined.accelerometer_errcount <= (_sensor_combined.accelerometer1_errcount + ACCEL_SWITCH_HYSTERESIS)) { _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; _accel_main = 0; } else { _ekf->accel.x = _sensor_combined.accelerometer1_m_s2[0]; _ekf->accel.y = _sensor_combined.accelerometer1_m_s2[1]; _ekf->accel.z = _sensor_combined.accelerometer1_m_s2[2]; _accel_main = 1; } if (!_accel_valid) { lastAccel = _ekf->accel; } if (last_accel_main != _accel_main) { mavlink_and_console_log_emergency(_mavlink_fd, "ACCEL FAILED! Switched from #%d to %d", last_accel_main, _accel_main); } _accel_valid = true; } _ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU; lastAngRate = _ekf->angRate; _ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU; lastAccel = _ekf->accel; if (last_mag != _sensor_combined.magnetometer_timestamp) { _newDataMag = true; } else { _newDataMag = false; } last_mag = _sensor_combined.magnetometer_timestamp; //warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z); //Update Land Detector bool newLandData; orb_check(_landDetectorSub, &newLandData); if (newLandData) { orb_copy(ORB_ID(vehicle_land_detected), _landDetectorSub, &_landDetector); } //Update AirSpeed orb_check(_airspeed_sub, &_newAdsData); if (_newAdsData) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); perf_count(_perf_airspeed); _ekf->VtasMeas = _airspeed.true_airspeed_unfiltered_m_s; } bool gps_update; orb_check(_gps_sub, &gps_update); if (gps_update) { orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); perf_count(_perf_gps); //We are more strict for our first fix float requiredAccuracy = _parameters.pos_stddev_threshold; if (_gpsIsGood) { requiredAccuracy = _parameters.pos_stddev_threshold * 2.0f; } //Check if the GPS fix is good enough for us to use if (_gps.fix_type >= 3 && _gps.eph < requiredAccuracy && _gps.epv < requiredAccuracy) { _gpsIsGood = true; } else { _gpsIsGood = false; } if (_gpsIsGood) { //Calculate time since last good GPS fix const float dtLastGoodGPS = static_cast<float>(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f; //Stop dead-reckoning mode if (_global_pos.dead_reckoning) { mavlink_log_info(_mavlink_fd, "[ekf] stop dead-reckoning"); } _global_pos.dead_reckoning = false; //Fetch new GPS data _ekf->GPSstatus = _gps.fix_type; _ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; _ekf->gpsHgt = _gps.alt / 1e3f; if (_previousGPSTimestamp != 0) { //Calculate average time between GPS updates _ekf->updateDtGpsFilt(math::constrain(dtLastGoodGPS, 0.01f, POS_RESET_THRESHOLD)); // update LPF float filter_step = (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt); if (isfinite(filter_step)) { _gps_alt_filt += filter_step; } } //check if we had a GPS outage for a long time if (_gps_initialized) { //Convert from global frame to local frame map_projection_project(&_pos_ref, (_gps.lat / 1.0e7), (_gps.lon / 1.0e7), &_ekf->posNE[0], &_ekf->posNE[1]); if (dtLastGoodGPS > POS_RESET_THRESHOLD) { _ekf->ResetPosition(); _ekf->ResetVelocity(); } } //warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)dtGoodGPS); // if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) { // _ekf->vneSigma = sqrtf(_gps.s_variance_m_s); // } else { // _ekf->vneSigma = _parameters.velne_noise; // } // if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) { // _ekf->posNeSigma = sqrtf(_gps.p_variance_m); // } else { // _ekf->posNeSigma = _parameters.posne_noise; // } // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); _previousGPSTimestamp = _gps.timestamp_position; } } // If it has gone more than POS_RESET_THRESHOLD amount of seconds since we received a GPS update, // then something is very wrong with the GPS (possibly a hardware failure or comlink error) const float dtLastGoodGPS = static_cast<float>(hrt_absolute_time() - _previousGPSTimestamp) / 1e6f; if (dtLastGoodGPS >= POS_RESET_THRESHOLD) { if (_global_pos.dead_reckoning) { mavlink_log_info(_mavlink_fd, "[ekf] gave up dead-reckoning after long timeout"); } _gpsIsGood = false; _global_pos.dead_reckoning = false; } //If we have no good GPS fix for half a second, then enable dead-reckoning mode while armed (for up to POS_RESET_THRESHOLD seconds) else if (dtLastGoodGPS >= 0.5f) { if (_armed.armed) { if (!_global_pos.dead_reckoning) { mavlink_log_info(_mavlink_fd, "[ekf] dead-reckoning enabled"); } _global_pos.dead_reckoning = true; } else { _global_pos.dead_reckoning = false; } } //Update barometer orb_check(_baro_sub, &_newHgtData); if (_newHgtData) { static hrt_abstime baro_last = 0; orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); // init lowpass filters for baro and gps altitude float baro_elapsed; if (baro_last == 0) { baro_elapsed = 0.0f; } else { baro_elapsed = (_baro.timestamp - baro_last) / 1e6f; } baro_last = _baro.timestamp; if (!_baro_init) { _baro_init = true; _baro_alt_filt = _baro.altitude; } _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f)); _ekf->baroHgt = _baro.altitude; float filter_step = (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt); if (isfinite(filter_step)) { _baro_alt_filt += filter_step; } perf_count(_perf_baro); } //Update Magnetometer if (_newDataMag) { _mag_valid = true; perf_count(_perf_mag); int last_mag_main = _mag_main; Vector3f mag0(_sensor_combined.magnetometer_ga[0], _sensor_combined.magnetometer_ga[1], _sensor_combined.magnetometer_ga[2]); Vector3f mag1(_sensor_combined.magnetometer1_ga[0], _sensor_combined.magnetometer1_ga[1], _sensor_combined.magnetometer1_ga[2]); const unsigned mag_timeout_us = 200000; /* fail over to the 2nd mag if we know the first is down */ if (hrt_elapsed_time(&_sensor_combined.magnetometer_timestamp) < mag_timeout_us && _sensor_combined.magnetometer_errcount <= (_sensor_combined.magnetometer1_errcount + MAG_SWITCH_HYSTERESIS) && mag0.length() > 0.1f) { _ekf->magData.x = mag0.x; _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset _ekf->magData.y = mag0.y; _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset _ekf->magData.z = mag0.z; _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset _mag_main = 0; } else if (hrt_elapsed_time(&_sensor_combined.magnetometer1_timestamp) < mag_timeout_us && mag1.length() > 0.1f) { _ekf->magData.x = mag1.x; _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset _ekf->magData.y = mag1.y; _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset _ekf->magData.z = mag1.z; _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset _mag_main = 1; } else { _mag_valid = false; } if (last_mag_main != _mag_main) { mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Failover from unit %d to unit %d", last_mag_main, _mag_main); } } //Update range data orb_check(_distance_sub, &_newRangeData); if (_newRangeData) { orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance); if (_distance.valid) { _ekf->rngMea = _distance.distance; _distance_last_valid = _distance.timestamp; } else { _newRangeData = false; } } }
void Navigator::vehicle_land_detected_update() { orb_copy(ORB_ID(vehicle_land_detected), _land_detected_sub, &_land_detected); }
void AttitudePositionEstimatorEKF::task_main() { _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); _ekf = new AttPosEKF(); _filter_start_time = hrt_absolute_time(); if (!_ekf) { errx(1, "OUT OF MEM!"); } /* * do subscriptions */ _distance_sub = orb_subscribe(ORB_ID(sensor_range_finder)); _baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_sub = orb_subscribe(ORB_ID(home_position)); _landDetectorSub = orb_subscribe(ORB_ID(vehicle_land_detected)); _armedSub = orb_subscribe(ORB_ID(actuator_armed)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); /* XXX remove this!, BUT increase the data buffer size! */ orb_set_interval(_sensor_combined_sub, 9); /* sets also parameters in the EKF object */ parameters_update(); /* wakeup source(s) */ struct pollfd fds[2]; /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; fds[1].fd = _sensor_combined_sub; fds[1].events = POLLIN; _gps.vel_n_m_s = 0.0f; _gps.vel_e_m_s = 0.0f; _gps.vel_d_m_s = 0.0f; _task_running = true; while (!_task_should_exit) { /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ if (pret == 0) { continue; } /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { warn("POLL ERR %d, %d", pret, errno); continue; } perf_begin(_loop_perf); perf_count(_loop_intvl); /* only update parameters if they changed */ if (fds[0].revents & POLLIN) { /* read from param to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), _params_sub, &update); /* update parameters from storage */ parameters_update(); } /* only run estimator if gyro updated */ if (fds[1].revents & POLLIN) { /* check vehicle status for changes to publication state */ bool prev_hil = (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON); vehicle_status_poll(); perf_count(_perf_gyro); /* Reset baro reference if switching to HIL, reset sensor states */ if (!prev_hil && (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON)) { /* system is in HIL now, wait for measurements to come in one last round */ usleep(60000); /* now read all sensor publications to ensure all real sensor data is purged */ orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); /* set sensors to de-initialized state */ _gyro_valid = false; _accel_valid = false; _mag_valid = false; _baro_init = false; _gps_initialized = false; _last_sensor_timestamp = hrt_absolute_time(); _last_run = _last_sensor_timestamp; _ekf->ZeroVariables(); _ekf->dtIMU = 0.01f; _filter_start_time = _last_sensor_timestamp; /* now skip this loop and get data on the next one, which will also re-init the filter */ continue; } /** * PART ONE: COLLECT ALL DATA **/ pollData(); /* * CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY */ if (hrt_elapsed_time(&_filter_start_time) < FILTER_INIT_DELAY) { continue; } /** * PART TWO: EXECUTE THE FILTER * * We run the filter only once all data has been fetched **/ if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) { // maintain filtered baro and gps altitudes to calculate weather offset // baro sample rate is ~70Hz and measurement bandwidth is high // gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds // maintain heavily filtered values for both baro and gps altitude // Assume the filtered output should be identical for both sensors if (_gpsIsGood) { _baro_gps_offset = _baro_alt_filt - _gps_alt_filt; } // if (hrt_elapsed_time(&_last_debug_print) >= 5e6) { // _last_debug_print = hrt_absolute_time(); // perf_print_counter(_perf_baro); // perf_reset(_perf_baro); // warnx("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f", // (double)_baro_gps_offset, // (double)_baro_alt_filt, // (double)_gps_alt_filt, // (double)_global_pos.alt, // (double)_local_pos.z); // } /* Initialize the filter first */ if (!_ekf->statesInitialised) { // North, East Down position (m) float initVelNED[3] = {0.0f, 0.0f, 0.0f}; _ekf->posNE[0] = 0.0f; _ekf->posNE[1] = 0.0f; _local_pos.ref_alt = 0.0f; _baro_gps_offset = 0.0f; _baro_alt_filt = _baro.altitude; _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); _filter_ref_offset = -_baro.altitude; warnx("filter ref off: baro_alt: %8.4f", (double)_filter_ref_offset); } else { if (!_gps_initialized && _gpsIsGood) { initializeGPS(); continue; } // Check if on ground - status is used by covariance prediction _ekf->setOnGround(_landDetector.landed); // We're apparently initialized in this case now // check (and reset the filter as needed) int check = check_filter_state(); if (check) { // Let the system re-initialize itself continue; } // Run EKF data fusion steps updateSensorFusion(_gpsIsGood, _newDataMag, _newRangeData, _newHgtData, _newAdsData); // Publish attitude estimations publishAttitude(); // Publish Local Position estimations publishLocalPosition(); // Publish Global Position, it will have a large uncertainty // set if only altitude is known publishGlobalPosition(); // Publish wind estimates if (hrt_elapsed_time(&_wind.timestamp) > 99000) { publishWindEstimate(); } } } } perf_end(_loop_perf); } _task_running = false; warnx("exiting.\n"); _estimator_task = -1; _exit(0); }
AP_GPS_PX4::AP_GPS_PX4(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) : AP_GPS_Backend(_gps, _state, _port) { _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); }