/*
* Possible Bug: We assume Home Position is already set by commander.
*/
Search::Search() {
// Clear out stuct data
memset(&search_mission, 0, sizeof (struct mission_s));
memset(&next_point, 0, sizeof (struct mission_item_s));
memset(&last, 0, sizeof (struct home_position_s));
memset(&home, 0, sizeof (struct home_position_s));
// Get Current Mission!
int mission_sub = orb_subscribe(ORB_ID(offboard_mission));
orb_copy(ORB_ID(offboard_mission), mission_sub, &search_mission);
orb_unsubscribe(mission_sub);
// Set Start Position to Home Position
int home_sub = orb_subscribe(ORB_ID(home_position));
orb_copy(ORB_ID(home_position), home_sub, &home);
orb_unsubscribe(home_sub);
// Read in the last waypoint on the offboard mission, store it to a local variable
dm_read(DM_KEY_WAYPOINTS_OFFBOARD(offboard), 0, &last, sizeof (struct mission_item_s));
offboard = 1;
// Initialize Next Waypoint
next_point.altitude_is_relative = true;
next_point.altitude = home.alt + 7;
next_point.autocontinue = true;
next_point.nav_cmd = NAV_CMD_TAKEOFF;
next_point.acceptance_radius = 5;
next_point.lat = home.lat;
next_point.lon = home.lon;
next_point.time_inside = 0;
next_point.frame = last.frame;
mission_pub = orb_advertise(ORB_ID(offboard_mission), &search_mission);
}
CRSFTelemetry::~CRSFTelemetry()
{
orb_unsubscribe(_vehicle_gps_position_sub);
orb_unsubscribe(_battery_status_sub);
orb_unsubscribe(_vehicle_attitude_sub);
orb_unsubscribe(_vehicle_status_sub);
}
bool MicroBenchORB::time_px4_uorb()
{
int fd_status = orb_subscribe(ORB_ID(vehicle_status));
int fd_lpos = orb_subscribe(ORB_ID(vehicle_local_position));
int fd_gyro = orb_subscribe(ORB_ID(sensor_gyro));
int ret = 0;
bool updated = false;
uint64_t time = 0;
PERF("orb_check vehicle_status", ret = orb_check(fd_status, &updated), 1000);
PERF("orb_stat vehicle_status", ret = orb_stat(fd_status, &time), 1000);
PERF("orb_copy vehicle_status", ret = orb_copy(ORB_ID(vehicle_status), fd_status, &status), 1000);
PERF("orb_check vehicle_local_position", ret = orb_check(fd_lpos, &updated), 1000);
PERF("orb_stat vehicle_local_position", ret = orb_stat(fd_lpos, &time), 1000);
PERF("orb_copy vehicle_local_position", ret = orb_copy(ORB_ID(vehicle_local_position), fd_lpos, &lpos), 1000);
PERF("orb_check sensor_gyro", ret = orb_check(fd_gyro, &updated), 1000);
PERF("orb_stat sensor_gyro", ret = orb_stat(fd_gyro, &time), 1000);
PERF("orb_copy sensor_gyro", ret = orb_copy(ORB_ID(sensor_gyro), fd_gyro, &lpos), 1000);
PERF("orb_exists sensor_accel 0", ret = orb_exists(ORB_ID(sensor_accel), 0), 100);
PERF("orb_exists sensor_accel 1", ret = orb_exists(ORB_ID(sensor_accel), 1), 100);
PERF("orb_exists sensor_accel 2", ret = orb_exists(ORB_ID(sensor_accel), 2), 100);
PERF("orb_exists sensor_accel 3", ret = orb_exists(ORB_ID(sensor_accel), 3), 100);
PERF("orb_exists sensor_accel 4", ret = orb_exists(ORB_ID(sensor_accel), 4), 100);
orb_unsubscribe(fd_status);
orb_unsubscribe(fd_lpos);
orb_unsubscribe(fd_gyro);
return true;
}
InputMavlinkROI::~InputMavlinkROI()
{
if (_vehicle_roi_sub >= 0) {
orb_unsubscribe(_vehicle_roi_sub);
}
if (_position_setpoint_triplet_sub >= 0) {
orb_unsubscribe(_position_setpoint_triplet_sub);
}
}
OutputBase::~OutputBase()
{
if (_vehicle_attitude_sub >= 0) {
orb_unsubscribe(_vehicle_attitude_sub);
}
if (_vehicle_global_position_sub >= 0) {
orb_unsubscribe(_vehicle_global_position_sub);
}
}
UavcanNode::~UavcanNode()
{
fw_server(Stop);
if (_task != -1) {
/* tell the task we want it to go away */
_task_should_exit = true;
unsigned i = 10;
do {
/* wait 5ms - it should wake every 10ms or so worst-case */
usleep(5000);
/* if we have given up, kill it */
if (--i == 0) {
task_delete(_task);
break;
}
} while (_task != -1);
}
(void)orb_unsubscribe(_armed_sub);
(void)orb_unsubscribe(_test_motor_sub);
(void)orb_unsubscribe(_actuator_direct_sub);
// Removing the sensor bridges
auto br = _sensor_bridges.getHead();
while (br != nullptr) {
auto next = br->getSibling();
delete br;
br = next;
}
_instance = nullptr;
perf_free(_perfcnt_node_spin_elapsed);
perf_free(_perfcnt_esc_mixer_output_elapsed);
perf_free(_perfcnt_esc_mixer_total_elapsed);
pthread_mutex_destroy(&_node_mutex);
px4_sem_destroy(&_server_command_sem);
// Is it allowed to delete it like that?
if (_mixers != nullptr) {
delete _mixers;
}
}
int
UavcanNode::teardown()
{
px4_sem_post(&_server_command_sem);
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
orb_unsubscribe(_control_subs[i]);
_control_subs[i] = -1;
}
}
return (_armed_sub >= 0) ? orb_unsubscribe(_armed_sub) : 0;
}
OutputBase::~OutputBase()
{
if (_vehicle_attitude_sub >= 0) {
orb_unsubscribe(_vehicle_attitude_sub);
}
if (_vehicle_global_position_sub >= 0) {
orb_unsubscribe(_vehicle_global_position_sub);
}
if (_mount_orientation_pub) {
orb_unadvertise(_mount_orientation_pub);
}
}
// return false if the magnetomer measurements are inconsistent
static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_status)
{
// get the sensor preflight data
int sensors_sub = orb_subscribe(ORB_ID(sensor_preflight));
struct sensor_preflight_s sensors = {};
if (orb_copy(ORB_ID(sensor_preflight), sensors_sub, &sensors) != 0) {
// can happen if not advertised (yet)
return true;
}
orb_unsubscribe(sensors_sub);
// Use the difference between sensors to detect a bad calibration, orientation or magnetic interference.
// If a single sensor is fitted, the value being checked will be zero so this check will always pass.
float test_limit;
param_get(param_find("COM_ARM_MAG"), &test_limit);
if (sensors.mag_inconsistency_ga > test_limit) {
if (report_status) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG SENSORS INCONSISTENT");
}
return false;
}
return true;
}
void
UavcanNode::subscribe()
{
// Subscribe/unsubscribe to required actuator control groups
uint32_t sub_groups = _groups_required & ~_groups_subscribed;
uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
// the first fd used by CAN
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (sub_groups & (1 << i)) {
warnx("subscribe to actuator_controls_%d", i);
_control_subs[i] = orb_subscribe(_control_topics[i]);
}
if (unsub_groups & (1 << i)) {
warnx("unsubscribe from actuator_controls_%d", i);
orb_unsubscribe(_control_subs[i]);
_control_subs[i] = -1;
}
if (_control_subs[i] > 0) {
_poll_ids[i] = add_poll_fd(_control_subs[i]);
}
}
}
void TAP_ESC::work_stop()
{
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
orb_unsubscribe(_control_subs[i]);
_control_subs[i] = -1;
}
}
orb_unsubscribe(_armed_sub);
_armed_sub = -1;
orb_unsubscribe(_test_motor_sub);
_test_motor_sub = -1;
DEVICE_LOG("stopping");
_initialized = false;
}
void
UavcanNode::print_info()
{
if (!_instance) {
warnx("not running, start first");
}
(void)pthread_mutex_lock(&_node_mutex);
// ESC mixer status
printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n",
(unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK");
if (_outputs.noutputs != 0) {
printf("ESC output: ");
for (uint8_t i = 0; i < _outputs.noutputs; i++) {
printf("%d ", (int)(_outputs.output[i] * 1000));
}
printf("\n");
// ESC status
int esc_sub = orb_subscribe(ORB_ID(esc_status));
struct esc_status_s esc;
memset(&esc, 0, sizeof(esc));
orb_copy(ORB_ID(esc_status), esc_sub, &esc);
printf("ESC Status:\n");
printf("Addr\tV\tA\tTemp\tSetpt\tRPM\tErr\n");
for (uint8_t i = 0; i < _outputs.noutputs; i++) {
printf("%d\t", esc.esc[i].esc_address);
printf("%3.2f\t", (double)esc.esc[i].esc_voltage);
printf("%3.2f\t", (double)esc.esc[i].esc_current);
printf("%3.2f\t", (double)esc.esc[i].esc_temperature);
printf("%3.2f\t", (double)esc.esc[i].esc_setpoint);
printf("%d\t", esc.esc[i].esc_rpm);
printf("%d", esc.esc[i].esc_errorcount);
printf("\n");
}
orb_unsubscribe(esc_sub);
}
// Sensor bridges
auto br = _sensor_bridges.getHead();
while (br != nullptr) {
printf("Sensor '%s':\n", br->get_name());
br->print_status();
printf("\n");
br = br->getSibling();
}
(void)pthread_mutex_unlock(&_node_mutex);
}
int uORBTest::UnitTest::test_multi2()
{
test_note("Testing multi-topic 2 test (queue simulation)");
//test: first subscribe, then advertise
_thread_should_exit = false;
const int num_instances = 3;
int orb_data_fd[num_instances];
int orb_data_next = 0;
for (int i = 0; i < num_instances; ++i) {
// PX4_WARN("subscribe %i, t=%" PRIu64, i, hrt_absolute_time());
orb_data_fd[i] = orb_subscribe_multi(ORB_ID(orb_test_medium_multi), i);
}
char *const args[1] = { NULL };
int pubsub_task = px4_task_spawn_cmd("uorb_test_multi",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
1500,
(px4_main_t)&uORBTest::UnitTest::pub_test_multi2_entry,
args);
if (pubsub_task < 0) {
return test_fail("failed launching task");
}
hrt_abstime last_time = 0;
while (!_thread_should_exit) {
bool updated = false;
int orb_data_cur_fd = orb_data_fd[orb_data_next];
orb_check(orb_data_cur_fd, &updated);
if (updated) {
struct orb_test_medium msg;
orb_copy(ORB_ID(orb_test_medium_multi), orb_data_cur_fd, &msg);
usleep(1000);
if (last_time >= msg.time && last_time != 0) {
return test_fail("Timestamp not increasing! (%" PRIu64 " >= %" PRIu64 ")", last_time, msg.time);
}
last_time = msg.time;
// PX4_WARN(" got message (val=%i, idx=%i, t=%" PRIu64 ")", msg.val, orb_data_next, msg.time);
orb_data_next = (orb_data_next + 1) % num_instances;
}
}
for (int i = 0; i < num_instances; ++i) {
orb_unsubscribe(orb_data_fd[i]);
}
return test_note("PASS multi-topic 2 test (queue simulation)");
}
MavlinkULog::~MavlinkULog()
{
if (_ulog_stream_ack_pub) {
orb_unadvertise(_ulog_stream_ack_pub);
}
if (_ulog_stream_sub >= 0) {
orb_unsubscribe(_ulog_stream_sub);
}
}
MavlinkParametersManager::~MavlinkParametersManager()
{
if (_uavcan_parameter_value_sub >= 0) {
orb_unsubscribe(_uavcan_parameter_value_sub);
}
if (_uavcan_parameter_request_pub) {
orb_unadvertise(_uavcan_parameter_request_pub);
}
}
static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_status)
{
bool success = true; // start with a pass and change to a fail if any test fails
float test_limit = 1.0f; // pass limit re-used for each test
// Get sensor_preflight data if available and exit with a fail recorded if not
int sensors_sub = orb_subscribe(ORB_ID(sensor_preflight));
sensor_preflight_s sensors = {};
if (orb_copy(ORB_ID(sensor_preflight), sensors_sub, &sensors) != PX4_OK) {
goto out;
}
// Use the difference between IMU's to detect a bad calibration.
// If a single IMU is fitted, the value being checked will be zero so this check will always pass.
param_get(param_find("COM_ARM_IMU_ACC"), &test_limit);
if (sensors.accel_inconsistency_m_s_s > test_limit) {
if (report_status) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCELS INCONSISTENT - CHECK CAL");
}
success = false;
goto out;
} else if (sensors.accel_inconsistency_m_s_s > test_limit * 0.8f) {
if (report_status) {
mavlink_log_info(mavlink_log_pub, "PREFLIGHT ADVICE: ACCELS INCONSISTENT - CHECK CAL");
}
}
// Fail if gyro difference greater than 5 deg/sec and notify if greater than 2.5 deg/sec
param_get(param_find("COM_ARM_IMU_GYR"), &test_limit);
if (sensors.gyro_inconsistency_rad_s > test_limit) {
if (report_status) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYROS INCONSISTENT - CHECK CAL");
}
success = false;
goto out;
} else if (sensors.gyro_inconsistency_rad_s > test_limit * 0.5f) {
if (report_status) {
mavlink_log_info(mavlink_log_pub, "PREFLIGHT ADVICE: GYROS INCONSISTENT - CHECK CAL");
}
}
out:
orb_unsubscribe(sensors_sub);
return success;
}
void listener(const orb_id_t &id, unsigned num_msgs, unsigned topic_instance, unsigned topic_interval)
{
if (orb_exists(id, topic_instance) != 0) {
printf("never published\n");
return;
}
int sub = orb_subscribe_multi(id, topic_instance);
orb_set_interval(sub, topic_interval);
bool updated = false;
unsigned i = 0;
hrt_abstime start_time = hrt_absolute_time();
while (i < num_msgs) {
orb_check(sub, &updated);
if (i == 0) {
updated = true;
} else {
usleep(500);
}
if (updated) {
start_time = hrt_absolute_time();
i++;
printf("\nTOPIC: %s instance %d #%d\n", id->o_name, topic_instance, i);
T container;
if (orb_copy(id, sub, &container) == PX4_OK) {
print_message(container);
} else {
PX4_ERR("orb_copy failed");
}
} else {
if (hrt_elapsed_time(&start_time) > 2 * 1000 * 1000) {
printf("Waited for 2 seconds without a message. Giving up.\n");
break;
}
}
}
orb_unsubscribe(sub);
}
static bool powerCheck(orb_advert_t *mavlink_log_pub, bool report_fail, bool prearm)
{
bool success = true;
if (!prearm) {
// Ignore power check after arming.
return true;
} else {
int system_power_sub = orb_subscribe(ORB_ID(system_power));
system_power_s system_power;
if (orb_copy(ORB_ID(system_power), system_power_sub, &system_power) == PX4_OK) {
if (hrt_elapsed_time(&system_power.timestamp) < 200000) {
/* copy avionics voltage */
float avionics_power_rail_voltage = system_power.voltage5V_v;
// avionics rail
// Check avionics rail voltages
if (avionics_power_rail_voltage < 4.5f) {
success = false;
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: Avionics power low: %6.2f Volt", (double)avionics_power_rail_voltage);
}
} else if (avionics_power_rail_voltage < 4.9f) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics power low: %6.2f Volt", (double)avionics_power_rail_voltage);
}
} else if (avionics_power_rail_voltage > 5.4f) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics power high: %6.2f Volt", (double)avionics_power_rail_voltage);
}
}
}
}
orb_unsubscribe(system_power_sub);
}
return success;
}
void Search::printDebug() {
int mission_sub = orb_subscribe(ORB_ID(offboard_mission));
orb_copy(ORB_ID(offboard_mission), mission_sub, &true_mission);
orb_unsubscribe(mission_sub);
// Timing for Navigator message to display properly
usleep(20000);
warnx("Mission count: %d", true_mission.count);
warnx("Mission current sequence: %d", search_mission.current_seq);
warnx("Start Point Latitude: %.7f", next_point.lat);
warnx("Start Point Longitude: %.7f", next_point.lon);
warnx("Last Point Frame is: %d", last.frame);
warnx("Home Latitude: %.7f", home.lat);
warnx("Home Longitude: %.7f", home.lon);
if ((home.lat - next_point.lat) < 0.0001 && (home.lon - next_point.lon) < 0.0001)
warnx("Start Point correctly copied Home's Lat/Lon!");
else
warnx("Start Point did not copy Home's Lat/Lon");
warnx("========================================================");
warnx("Last Waypoint Latitude: %.7f", last.lat);
warnx("Last Waypoint Longitude: %.7f", last.lon);
}
MavlinkOrbSubscription::~MavlinkOrbSubscription()
{
if (_fd >= 0) {
orb_unsubscribe(_fd);
}
}
void
UavcanNode::print_info()
{
if (!_instance) {
warnx("not running, start first");
}
(void)pthread_mutex_lock(&_node_mutex);
// Memory status
printf("Pool allocator status:\n");
printf("\tCapacity hard/soft: %u/%u blocks\n",
_pool_allocator.getBlockCapacityHardLimit(), _pool_allocator.getBlockCapacity());
printf("\tReserved: %u blocks\n", _pool_allocator.getNumReservedBlocks());
printf("\tAllocated: %u blocks\n", _pool_allocator.getNumAllocatedBlocks());
// UAVCAN node perfcounters
printf("UAVCAN node status:\n");
printf("\tInternal failures: %llu\n", _node.getInternalFailureCount());
printf("\tTransfer errors: %llu\n", _node.getDispatcher().getTransferPerfCounter().getErrorCount());
printf("\tRX transfers: %llu\n", _node.getDispatcher().getTransferPerfCounter().getRxTransferCount());
printf("\tTX transfers: %llu\n", _node.getDispatcher().getTransferPerfCounter().getTxTransferCount());
// CAN driver status
for (unsigned i = 0; i < _node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) {
printf("CAN%u status:\n", unsigned(i + 1));
auto iface = _node.getDispatcher().getCanIOManager().getCanDriver().getIface(i);
printf("\tHW errors: %llu\n", iface->getErrorCount());
auto iface_perf_cnt = _node.getDispatcher().getCanIOManager().getIfacePerfCounters(i);
printf("\tIO errors: %llu\n", iface_perf_cnt.errors);
printf("\tRX frames: %llu\n", iface_perf_cnt.frames_rx);
printf("\tTX frames: %llu\n", iface_perf_cnt.frames_tx);
}
// ESC mixer status
printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n",
(unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK");
if (_outputs.noutputs != 0) {
printf("ESC output: ");
for (uint8_t i = 0; i < _outputs.noutputs; i++) {
printf("%d ", (int)(_outputs.output[i] * 1000));
}
printf("\n");
// ESC status
int esc_sub = orb_subscribe(ORB_ID(esc_status));
struct esc_status_s esc;
memset(&esc, 0, sizeof(esc));
orb_copy(ORB_ID(esc_status), esc_sub, &esc);
printf("ESC Status:\n");
printf("Addr\tV\tA\tTemp\tSetpt\tRPM\tErr\n");
for (uint8_t i = 0; i < _outputs.noutputs; i++) {
printf("%d\t", esc.esc[i].esc_address);
printf("%3.2f\t", (double)esc.esc[i].esc_voltage);
printf("%3.2f\t", (double)esc.esc[i].esc_current);
printf("%3.2f\t", (double)esc.esc[i].esc_temperature);
printf("%3.2f\t", (double)esc.esc[i].esc_setpoint);
printf("%d\t", esc.esc[i].esc_rpm);
printf("%d", esc.esc[i].esc_errorcount);
printf("\n");
}
orb_unsubscribe(esc_sub);
}
// Sensor bridges
auto br = _sensor_bridges.getHead();
while (br != nullptr) {
printf("Sensor '%s':\n", br->get_name());
br->print_status();
printf("\n");
br = br->getSibling();
}
(void)pthread_mutex_unlock(&_node_mutex);
}
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");
}
int uORBTest::UnitTest::pubsublatency_main()
{
/* 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;
// timings has to be on the heap to keep frame size below 2048 bytes
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 == nullptr) {
warnx("Error opening file!\n");
delete[] timings;
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) > 100.0f) {
pubsubtest_res = uORB::ERROR;
} else {
pubsubtest_res = PX4_OK;
}
return pubsubtest_res;
}
/**
* Mission waypoint manager main function
*/
void *mission_waypoint_manager_thread_main(void* args)
{
absolute_time now;
int updated;
float turn_distance;
/* initialize waypoint manager */
// ************************************* subscribe ******************************************
global_position_setpoint_pub = orb_advertise (ORB_ID(vehicle_global_position_setpoint));
if (global_position_setpoint_pub == -1)
{
fprintf (stderr, "Waypoint manager thread failed to advertise the vehicle_global_position_setpoint topic\n");
exit (-1);
}
global_position_set_triplet_pub = orb_advertise (ORB_ID(vehicle_global_position_set_triplet));
if (global_position_set_triplet_pub == -1)
{
fprintf (stderr, "Waypoint manager thread failed to advertise the vehicle_global_position_set_triplet topic\n");
exit (-1);
}
// ************************************* subscribe ******************************************
struct vehicle_global_position_s global_pos;
orb_subscr_t global_pos_sub = orb_subscribe (ORB_ID(vehicle_global_position));
if (global_pos_sub == -1)
{
fprintf (stderr, "Waypoint manager thread failed to subscribe the vehicle_global_position topic\n");
exit (-1);
}
struct vehicle_control_flags_s control_flags;
orb_subscr_t control_flags_sub = orb_subscribe(ORB_ID(vehicle_control_flags));
if (control_flags_sub < 0)
{
fprintf (stderr, "Waypoint manager thread failed to subscribe to vehicle_control_flags topic\n");
exit(-1);
}
struct navigation_capabilities_s nav_cap;
orb_subscr_t nav_cap_sub = orb_subscribe (ORB_ID(navigation_capabilities));
if (nav_cap_sub == -1)
{
fprintf (stderr, "Waypoint manager thread failed to subscribe the navigation_capabilities topic\n");
exit (-1);
}
struct parameter_update_s params;
orb_subscr_t param_sub = orb_subscribe (ORB_ID(parameter_update));
if (param_sub == -1)
{
fprintf (stderr, "Waypoint manager thread failed to subscribe the parameter_update topic\n");
exit (-1);
}
orb_subscr_t mission_sub = orb_subscribe (ORB_ID(mission));
if (mission_sub == -1)
{
fprintf (stderr, "Waypoint manager thread failed to subscribe the mission topic\n");
exit (-1);
}
orb_subscr_t home_sub = orb_subscribe (ORB_ID(home_position));
if (home_sub == -1)
{
fprintf (stderr, "Waypoint manager thread failed to subscribe the home_position topic\n");
exit (-1);
}
/* abort on a nonzero return value from the parameter init */
if (mission_waypoint_manager_params_init() != 0 || wp_manager_init (mission_sub, home_sub) != 0)
{
fprintf (stderr, "Waypoint manager exiting on startup due to an error\n");
exit (-1);
}
turn_distance = (is_rotary_wing)? mission_waypoint_manager_parameters.mr_turn_distance :
mission_waypoint_manager_parameters.fw_turn_distance;
/* welcome user */
fprintf (stdout, "Waypoint manager started\n");
fflush(stdout);
/* waypoint main eventloop */
while (!_shutdown_all_systems) {
// wait for the position estimation for a max of 500ms
updated = orb_poll (ORB_ID(vehicle_global_position), global_pos_sub, 500000);
if (updated < 0)
{
// that's undesiderable but there is not much we can do
fprintf (stderr, "Waypoint manager thread experienced an error waiting for actuator_controls topic\n");
continue;
}
else if (updated == 0)
{
continue;
}
else
orb_copy (ORB_ID(vehicle_global_position), global_pos_sub, (void *) &global_pos);
updated = orb_check (ORB_ID(vehicle_control_flags), control_flags_sub);
if (updated) {
orb_copy(ORB_ID(vehicle_control_flags), control_flags_sub, &control_flags);
}
updated = orb_check (ORB_ID(navigation_capabilities), nav_cap_sub);
if (updated)
{
orb_copy (ORB_ID(navigation_capabilities), nav_cap_sub, (void *) &nav_cap);
turn_distance = (nav_cap.turn_distance > 0)? nav_cap.turn_distance : turn_distance;
}
updated = orb_check (ORB_ID(parameter_update), param_sub);
if (updated) {
/* clear updated flag */
orb_copy(ORB_ID(parameter_update), param_sub, ¶ms);
/* update multirotor_position_control_parameters */
mission_waypoint_manager_params_update();
}
if (!control_flags.flag_control_manual_enabled && control_flags.flag_control_auto_enabled)
{
orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &g_sp);
orb_publish(ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, &triplet);
}
now = get_absolute_time();
/* check if waypoint has been reached against the last positions */
check_waypoints_reached(now, &global_pos, turn_distance);
/* sleep 25 ms */
usleep(25000);
}
/*
* do unsubscriptions
*/
if (orb_unsubscribe (ORB_ID(vehicle_global_position), global_pos_sub, pthread_self()) < 0)
fprintf (stderr, "Waypoint manager thread failed to unsubscribe to vehicle_global_position topic\n");
if (orb_unsubscribe(ORB_ID(vehicle_control_flags), control_flags_sub, pthread_self()) < 0)
fprintf (stderr, "Waypoint manager thread failed to unsubscribe to vehicle_control_flags topic\n");
if (orb_unsubscribe (ORB_ID(navigation_capabilities), nav_cap_sub, pthread_self()) < 0)
fprintf (stderr, "Waypoint manager thread failed to unsubscribe to navigation_capabilities topic\n");
if (orb_unsubscribe (ORB_ID(parameter_update), param_sub, pthread_self()) < 0)
fprintf (stderr, "Waypoint manager thread failed to unsubscribe to parameter_update topic\n");
if (orb_unsubscribe (ORB_ID(mission), mission_sub, pthread_self()) < 0)
fprintf (stderr, "Waypoint manager thread failed to unsubscribe to mission topic\n");
if (orb_unsubscribe (ORB_ID(home_position), home_sub, pthread_self()) < 0)
fprintf (stderr, "Waypoint manager thread failed to unsubscribe to home_position topic\n");
/*
* do unadvertises
*/
if (orb_unadvertise (ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, pthread_self()) < 0)
fprintf (stderr, "Waypoint manager thread failed to unadvertise the global_position_setpoint_pub topic\n");
if (orb_unadvertise (ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, pthread_self()) < 0)
fprintf (stderr, "Waypoint manager thread failed to unadvertise the vehicle_global_position_set_triplet topic\n");
return 0;
}
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 Ekf2::task_main()
{
// subscribe to relevant topics
int sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
int airspeed_sub = orb_subscribe(ORB_ID(airspeed));
int params_sub = orb_subscribe(ORB_ID(parameter_update));
int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
int range_finder_sub = orb_subscribe(ORB_ID(distance_sensor));
int ev_pos_sub = orb_subscribe(ORB_ID(vehicle_vision_position));
int ev_att_sub = orb_subscribe(ORB_ID(vehicle_vision_attitude));
int vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
int status_sub = orb_subscribe(ORB_ID(vehicle_status));
px4_pollfd_struct_t fds[2] = {};
fds[0].fd = sensors_sub;
fds[0].events = POLLIN;
fds[1].fd = params_sub;
fds[1].events = POLLIN;
// initialise parameter cache
updateParams();
// initialize data structures outside of loop
// because they will else not always be
// properly populated
sensor_combined_s sensors = {};
vehicle_gps_position_s gps = {};
airspeed_s airspeed = {};
optical_flow_s optical_flow = {};
distance_sensor_s range_finder = {};
vehicle_land_detected_s vehicle_land_detected = {};
vehicle_local_position_s ev_pos = {};
vehicle_attitude_s ev_att = {};
vehicle_status_s vehicle_status = {};
while (!_task_should_exit) {
int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);
if (ret < 0) {
// Poll error, sleep and try again
usleep(10000);
continue;
} else if (ret == 0) {
// Poll timeout or no new data, do nothing
continue;
}
if (fds[1].revents & POLLIN) {
// read from param to clear updated flag
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), params_sub, &update);
updateParams();
// fetch sensor data in next loop
continue;
} else if (!(fds[0].revents & POLLIN)) {
// no new data
continue;
}
bool gps_updated = false;
bool airspeed_updated = false;
bool optical_flow_updated = false;
bool range_finder_updated = false;
bool vehicle_land_detected_updated = false;
bool vision_position_updated = false;
bool vision_attitude_updated = false;
bool vehicle_status_updated = false;
orb_copy(ORB_ID(sensor_combined), sensors_sub, &sensors);
// update all other topics if they have new data
orb_check(status_sub, &vehicle_status_updated);
if (vehicle_status_updated) {
orb_copy(ORB_ID(vehicle_status), status_sub, &vehicle_status);
}
orb_check(gps_sub, &gps_updated);
if (gps_updated) {
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps);
}
orb_check(airspeed_sub, &airspeed_updated);
if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
}
orb_check(optical_flow_sub, &optical_flow_updated);
if (optical_flow_updated) {
orb_copy(ORB_ID(optical_flow), optical_flow_sub, &optical_flow);
}
orb_check(range_finder_sub, &range_finder_updated);
if (range_finder_updated) {
orb_copy(ORB_ID(distance_sensor), range_finder_sub, &range_finder);
if (range_finder.min_distance > range_finder.current_distance
|| range_finder.max_distance < range_finder.current_distance) {
range_finder_updated = false;
}
}
orb_check(ev_pos_sub, &vision_position_updated);
if (vision_position_updated) {
orb_copy(ORB_ID(vehicle_vision_position), ev_pos_sub, &ev_pos);
}
orb_check(ev_att_sub, &vision_attitude_updated);
if (vision_attitude_updated) {
orb_copy(ORB_ID(vehicle_vision_attitude), ev_att_sub, &ev_att);
}
// in replay mode we are getting the actual timestamp from the sensor topic
hrt_abstime now = 0;
if (_replay_mode) {
now = sensors.timestamp;
} else {
now = hrt_absolute_time();
}
// push imu data into estimator
float gyro_integral[3];
gyro_integral[0] = sensors.gyro_rad[0] * sensors.gyro_integral_dt;
gyro_integral[1] = sensors.gyro_rad[1] * sensors.gyro_integral_dt;
gyro_integral[2] = sensors.gyro_rad[2] * sensors.gyro_integral_dt;
float accel_integral[3];
accel_integral[0] = sensors.accelerometer_m_s2[0] * sensors.accelerometer_integral_dt;
accel_integral[1] = sensors.accelerometer_m_s2[1] * sensors.accelerometer_integral_dt;
accel_integral[2] = sensors.accelerometer_m_s2[2] * sensors.accelerometer_integral_dt;
_ekf.setIMUData(now, sensors.gyro_integral_dt * 1.e6f, sensors.accelerometer_integral_dt * 1.e6f,
gyro_integral, accel_integral);
// read mag data
if (sensors.magnetometer_timestamp_relative == sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
// set a zero timestamp to let the ekf replay program know that this data is not valid
_timestamp_mag_us = 0;
} else {
if ((sensors.timestamp + sensors.magnetometer_timestamp_relative) != _timestamp_mag_us) {
_timestamp_mag_us = sensors.timestamp + sensors.magnetometer_timestamp_relative;
// If the time last used by the EKF is less than specified, then accumulate the
// data and push the average when the 50msec is reached.
_mag_time_sum_ms += _timestamp_mag_us / 1000;
_mag_sample_count++;
_mag_data_sum[0] += sensors.magnetometer_ga[0];
_mag_data_sum[1] += sensors.magnetometer_ga[1];
_mag_data_sum[2] += sensors.magnetometer_ga[2];
uint32_t mag_time_ms = _mag_time_sum_ms / _mag_sample_count;
if (mag_time_ms - _mag_time_ms_last_used > _params->sensor_interval_min_ms) {
float mag_sample_count_inv = 1.0f / (float)_mag_sample_count;
float mag_data_avg_ga[3] = {_mag_data_sum[0] *mag_sample_count_inv, _mag_data_sum[1] *mag_sample_count_inv, _mag_data_sum[2] *mag_sample_count_inv};
_ekf.setMagData(1000 * (uint64_t)mag_time_ms, mag_data_avg_ga);
_mag_time_ms_last_used = mag_time_ms;
_mag_time_sum_ms = 0;
_mag_sample_count = 0;
_mag_data_sum[0] = 0.0f;
_mag_data_sum[1] = 0.0f;
_mag_data_sum[2] = 0.0f;
}
}
}
// read baro data
if (sensors.baro_timestamp_relative == sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
// set a zero timestamp to let the ekf replay program know that this data is not valid
_timestamp_balt_us = 0;
} else {
if ((sensors.timestamp + sensors.baro_timestamp_relative) != _timestamp_balt_us) {
_timestamp_balt_us = sensors.timestamp + sensors.baro_timestamp_relative;
// If the time last used by the EKF is less than specified, then accumulate the
// data and push the average when the 50msec is reached.
_balt_time_sum_ms += _timestamp_balt_us / 1000;
_balt_sample_count++;
_balt_data_sum += sensors.baro_alt_meter;
uint32_t balt_time_ms = _balt_time_sum_ms / _balt_sample_count;
if (balt_time_ms - _balt_time_ms_last_used > (uint32_t)_params->sensor_interval_min_ms) {
float balt_data_avg = _balt_data_sum / (float)_balt_sample_count;
_ekf.setBaroData(1000 * (uint64_t)balt_time_ms, balt_data_avg);
_balt_time_ms_last_used = balt_time_ms;
_balt_time_sum_ms = 0;
_balt_sample_count = 0;
_balt_data_sum = 0.0f;
}
}
}
// read gps data if available
if (gps_updated) {
struct gps_message gps_msg = {};
gps_msg.time_usec = gps.timestamp;
gps_msg.lat = gps.lat;
gps_msg.lon = gps.lon;
gps_msg.alt = gps.alt;
gps_msg.fix_type = gps.fix_type;
gps_msg.eph = gps.eph;
gps_msg.epv = gps.epv;
gps_msg.sacc = gps.s_variance_m_s;
gps_msg.vel_m_s = gps.vel_m_s;
gps_msg.vel_ned[0] = gps.vel_n_m_s;
gps_msg.vel_ned[1] = gps.vel_e_m_s;
gps_msg.vel_ned[2] = gps.vel_d_m_s;
gps_msg.vel_ned_valid = gps.vel_ned_valid;
gps_msg.nsats = gps.satellites_used;
//TODO add gdop to gps topic
gps_msg.gdop = 0.0f;
_ekf.setGpsData(gps.timestamp, &gps_msg);
}
// only set airspeed data if condition for airspeed fusion are met
bool fuse_airspeed = airspeed_updated && !vehicle_status.is_rotary_wing
&& _arspFusionThreshold.get() <= airspeed.true_airspeed_m_s && _arspFusionThreshold.get() >= 0.1f;
if (fuse_airspeed) {
float eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s;
_ekf.setAirspeedData(airspeed.timestamp, airspeed.true_airspeed_m_s, eas2tas);
}
// only fuse synthetic sideslip measurements if conditions are met
bool fuse_beta = !vehicle_status.is_rotary_wing && _fuseBeta.get();
_ekf.set_fuse_beta_flag(fuse_beta);
if (optical_flow_updated) {
flow_message flow;
flow.flowdata(0) = optical_flow.pixel_flow_x_integral;
flow.flowdata(1) = optical_flow.pixel_flow_y_integral;
flow.quality = optical_flow.quality;
flow.gyrodata(0) = optical_flow.gyro_x_rate_integral;
flow.gyrodata(1) = optical_flow.gyro_y_rate_integral;
flow.gyrodata(2) = optical_flow.gyro_z_rate_integral;
flow.dt = optical_flow.integration_timespan;
if (PX4_ISFINITE(optical_flow.pixel_flow_y_integral) &&
PX4_ISFINITE(optical_flow.pixel_flow_x_integral)) {
_ekf.setOpticalFlowData(optical_flow.timestamp, &flow);
}
}
if (range_finder_updated) {
_ekf.setRangeData(range_finder.timestamp, range_finder.current_distance);
}
// get external vision data
// if error estimates are unavailable, use parameter defined defaults
if (vision_position_updated || vision_attitude_updated) {
ext_vision_message ev_data;
ev_data.posNED(0) = ev_pos.x;
ev_data.posNED(1) = ev_pos.y;
ev_data.posNED(2) = ev_pos.z;
Quaternion q(ev_att.q);
ev_data.quat = q;
// position measurement error from parameters. TODO : use covariances from topic
ev_data.posErr = _default_ev_pos_noise;
ev_data.angErr = _default_ev_ang_noise;
// use timestamp from external computer, clocks are synchronized when using MAVROS
_ekf.setExtVisionData(vision_position_updated ? ev_pos.timestamp : ev_att.timestamp, &ev_data);
}
orb_check(vehicle_land_detected_sub, &vehicle_land_detected_updated);
if (vehicle_land_detected_updated) {
orb_copy(ORB_ID(vehicle_land_detected), vehicle_land_detected_sub, &vehicle_land_detected);
_ekf.set_in_air_status(!vehicle_land_detected.landed);
}
// run the EKF update and output
if (_ekf.update()) {
matrix::Quaternion<float> q;
_ekf.copy_quaternion(q.data());
float velocity[3];
_ekf.get_velocity(velocity);
float gyro_rad[3];
{
// generate control state data
control_state_s ctrl_state = {};
float gyro_bias[3] = {};
_ekf.get_gyro_bias(gyro_bias);
ctrl_state.timestamp = _replay_mode ? now : hrt_absolute_time();
gyro_rad[0] = sensors.gyro_rad[0] - gyro_bias[0];
gyro_rad[1] = sensors.gyro_rad[1] - gyro_bias[1];
gyro_rad[2] = sensors.gyro_rad[2] - gyro_bias[2];
ctrl_state.roll_rate = _lp_roll_rate.apply(gyro_rad[0]);
ctrl_state.pitch_rate = _lp_pitch_rate.apply(gyro_rad[1]);
ctrl_state.yaw_rate = _lp_yaw_rate.apply(gyro_rad[2]);
ctrl_state.roll_rate_bias = gyro_bias[0];
ctrl_state.pitch_rate_bias = gyro_bias[1];
ctrl_state.yaw_rate_bias = gyro_bias[2];
// Velocity in body frame
Vector3f v_n(velocity);
matrix::Dcm<float> R_to_body(q.inversed());
Vector3f v_b = R_to_body * v_n;
ctrl_state.x_vel = v_b(0);
ctrl_state.y_vel = v_b(1);
ctrl_state.z_vel = v_b(2);
// Local Position NED
float position[3];
_ekf.get_position(position);
ctrl_state.x_pos = position[0];
ctrl_state.y_pos = position[1];
ctrl_state.z_pos = position[2];
// Attitude quaternion
q.copyTo(ctrl_state.q);
_ekf.get_quat_reset(&ctrl_state.delta_q_reset[0], &ctrl_state.quat_reset_counter);
// Acceleration data
matrix::Vector<float, 3> acceleration(sensors.accelerometer_m_s2);
float accel_bias[3];
_ekf.get_accel_bias(accel_bias);
ctrl_state.x_acc = acceleration(0) - accel_bias[0];
ctrl_state.y_acc = acceleration(1) - accel_bias[1];
ctrl_state.z_acc = acceleration(2) - accel_bias[2];
// compute lowpass filtered horizontal acceleration
acceleration = R_to_body.transpose() * acceleration;
_acc_hor_filt = 0.95f * _acc_hor_filt + 0.05f * sqrtf(acceleration(0) * acceleration(0) +
acceleration(1) * acceleration(1));
ctrl_state.horz_acc_mag = _acc_hor_filt;
ctrl_state.airspeed_valid = false;
// use estimated velocity for airspeed estimate
if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_MEAS) {
// use measured airspeed
if (PX4_ISFINITE(airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - airspeed.timestamp < 1e6
&& airspeed.timestamp > 0) {
ctrl_state.airspeed = airspeed.indicated_airspeed_m_s;
ctrl_state.airspeed_valid = true;
}
} else if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_EST) {
if (_ekf.local_position_is_valid()) {
ctrl_state.airspeed = sqrtf(velocity[0] * velocity[0] + velocity[1] * velocity[1] + velocity[2] * velocity[2]);
ctrl_state.airspeed_valid = true;
}
} else if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_DISABLED) {
// do nothing, airspeed has been declared as non-valid above, controllers
// will handle this assuming always trim airspeed
}
// publish control state data
if (_control_state_pub == nullptr) {
_control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);
} else {
orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state);
}
}
{
// generate vehicle attitude quaternion data
struct vehicle_attitude_s att = {};
att.timestamp = _replay_mode ? now : hrt_absolute_time();
q.copyTo(att.q);
att.rollspeed = gyro_rad[0];
att.pitchspeed = gyro_rad[1];
att.yawspeed = gyro_rad[2];
// publish vehicle attitude data
if (_att_pub == nullptr) {
_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
} else {
orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
}
}
// generate vehicle local position data
struct vehicle_local_position_s lpos = {};
float pos[3] = {};
lpos.timestamp = _replay_mode ? now : hrt_absolute_time();
// Position of body origin in local NED frame
_ekf.get_position(pos);
lpos.x = (_ekf.local_position_is_valid()) ? pos[0] : 0.0f;
lpos.y = (_ekf.local_position_is_valid()) ? pos[1] : 0.0f;
lpos.z = pos[2];
// Velocity of body origin in local NED frame (m/s)
lpos.vx = velocity[0];
lpos.vy = velocity[1];
lpos.vz = velocity[2];
// TODO: better status reporting
lpos.xy_valid = _ekf.local_position_is_valid();
lpos.z_valid = true;
lpos.v_xy_valid = _ekf.local_position_is_valid();
lpos.v_z_valid = true;
// Position of local NED origin in GPS / WGS84 frame
struct map_projection_reference_s ekf_origin = {};
// true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon)
_ekf.get_ekf_origin(&lpos.ref_timestamp, &ekf_origin, &lpos.ref_alt);
lpos.xy_global = _ekf.global_position_is_valid();
lpos.z_global = true; // true if z is valid and has valid global reference (ref_alt)
lpos.ref_lat = ekf_origin.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees
lpos.ref_lon = ekf_origin.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees
// The rotation of the tangent plane vs. geographical north
matrix::Eulerf euler(q);
lpos.yaw = euler.psi();
float terrain_vpos;
lpos.dist_bottom_valid = _ekf.get_terrain_vert_pos(&terrain_vpos);
lpos.dist_bottom = terrain_vpos - pos[2]; // Distance to bottom surface (ground) in meters
lpos.dist_bottom_rate = -velocity[2]; // Distance to bottom surface (ground) change rate
lpos.surface_bottom_timestamp = hrt_absolute_time(); // Time when new bottom surface found
// TODO: uORB definition does not define what these variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres
Vector3f pos_var, vel_var;
_ekf.get_pos_var(pos_var);
_ekf.get_vel_var(vel_var);
lpos.eph = sqrtf(pos_var(0) + pos_var(1));
lpos.epv = sqrtf(pos_var(2));
// get state reset information of position and velocity
_ekf.get_posD_reset(&lpos.delta_z, &lpos.z_reset_counter);
_ekf.get_velD_reset(&lpos.delta_vz, &lpos.vz_reset_counter);
_ekf.get_posNE_reset(&lpos.delta_xy[0], &lpos.xy_reset_counter);
_ekf.get_velNE_reset(&lpos.delta_vxy[0], &lpos.vxy_reset_counter);
// publish vehicle local position data
if (_lpos_pub == nullptr) {
_lpos_pub = orb_advertise(ORB_ID(vehicle_local_position), &lpos);
} else {
orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos);
}
if (_ekf.global_position_is_valid()) {
// generate and publish global position data
struct vehicle_global_position_s global_pos = {};
global_pos.timestamp = _replay_mode ? now : hrt_absolute_time();
global_pos.time_utc_usec = gps.time_utc_usec; // GPS UTC timestamp in microseconds
double est_lat, est_lon, lat_pre_reset, lon_pre_reset;
map_projection_reproject(&ekf_origin, lpos.x, lpos.y, &est_lat, &est_lon);
global_pos.lat = est_lat; // Latitude in degrees
global_pos.lon = est_lon; // Longitude in degrees
map_projection_reproject(&ekf_origin, lpos.x - lpos.delta_xy[0], lpos.y - lpos.delta_xy[1], &lat_pre_reset,
&lon_pre_reset);
global_pos.delta_lat_lon[0] = est_lat - lat_pre_reset;
global_pos.delta_lat_lon[1] = est_lon - lon_pre_reset;
global_pos.lat_lon_reset_counter = lpos.xy_reset_counter;
global_pos.alt = -pos[2] + lpos.ref_alt; // Altitude AMSL in meters
_ekf.get_posD_reset(&global_pos.delta_alt, &global_pos.alt_reset_counter);
// global altitude has opposite sign of local down position
global_pos.delta_alt *= -1.0f;
global_pos.vel_n = velocity[0]; // Ground north velocity, m/s
global_pos.vel_e = velocity[1]; // Ground east velocity, m/s
global_pos.vel_d = velocity[2]; // Ground downside velocity, m/s
global_pos.yaw = euler.psi(); // Yaw in radians -PI..+PI.
global_pos.eph = sqrtf(pos_var(0) + pos_var(1));; // Standard deviation of position estimate horizontally
global_pos.epv = sqrtf(pos_var(2)); // Standard deviation of position vertically
if (lpos.dist_bottom_valid) {
global_pos.terrain_alt = lpos.ref_alt - terrain_vpos; // Terrain altitude in m, WGS84
global_pos.terrain_alt_valid = true; // Terrain altitude estimate is valid
} else {
global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84
global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid
}
// TODO use innovatun consistency check timouts to set this
global_pos.dead_reckoning = false; // True if this position is estimated through dead-reckoning
global_pos.pressure_alt = sensors.baro_alt_meter; // Pressure altitude AMSL (m)
if (_vehicle_global_position_pub == nullptr) {
_vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
} else {
orb_publish(ORB_ID(vehicle_global_position), _vehicle_global_position_pub, &global_pos);
}
}
} else if (_replay_mode) {
// in replay mode we have to tell the replay module not to wait for an update
// we do this by publishing an attitude with zero timestamp
struct vehicle_attitude_s att = {};
att.timestamp = now;
if (_att_pub == nullptr) {
_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
} else {
orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
}
}
// publish estimator status
struct estimator_status_s status = {};
status.timestamp = _replay_mode ? now : hrt_absolute_time();
_ekf.get_state_delayed(status.states);
_ekf.get_covariances(status.covariances);
_ekf.get_gps_check_status(&status.gps_check_fail_flags);
_ekf.get_control_mode(&status.control_mode_flags);
_ekf.get_filter_fault_status(&status.filter_fault_flags);
_ekf.get_innovation_test_status(&status.innovation_check_flags, &status.mag_test_ratio,
&status.vel_test_ratio, &status.pos_test_ratio,
&status.hgt_test_ratio, &status.tas_test_ratio,
&status.hagl_test_ratio);
bool dead_reckoning;
_ekf.get_ekf_lpos_accuracy(&status.pos_horiz_accuracy, &status.pos_vert_accuracy, &dead_reckoning);
_ekf.get_ekf_soln_status(&status.solution_status_flags);
_ekf.get_imu_vibe_metrics(status.vibe);
if (_estimator_status_pub == nullptr) {
_estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &status);
} else {
orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &status);
}
// Publish wind estimate
struct wind_estimate_s wind_estimate = {};
wind_estimate.timestamp = _replay_mode ? now : hrt_absolute_time();
wind_estimate.windspeed_north = status.states[22];
wind_estimate.windspeed_east = status.states[23];
wind_estimate.covariance_north = status.covariances[22];
wind_estimate.covariance_east = status.covariances[23];
if (_wind_pub == nullptr) {
_wind_pub = orb_advertise(ORB_ID(wind_estimate), &wind_estimate);
} else {
orb_publish(ORB_ID(wind_estimate), _wind_pub, &wind_estimate);
}
// publish estimator innovation data
{
struct ekf2_innovations_s innovations = {};
innovations.timestamp = _replay_mode ? now : hrt_absolute_time();
_ekf.get_vel_pos_innov(&innovations.vel_pos_innov[0]);
_ekf.get_mag_innov(&innovations.mag_innov[0]);
_ekf.get_heading_innov(&innovations.heading_innov);
_ekf.get_airspeed_innov(&innovations.airspeed_innov);
_ekf.get_beta_innov(&innovations.beta_innov);
_ekf.get_flow_innov(&innovations.flow_innov[0]);
_ekf.get_hagl_innov(&innovations.hagl_innov);
_ekf.get_vel_pos_innov_var(&innovations.vel_pos_innov_var[0]);
_ekf.get_mag_innov_var(&innovations.mag_innov_var[0]);
_ekf.get_heading_innov_var(&innovations.heading_innov_var);
_ekf.get_airspeed_innov_var(&innovations.airspeed_innov_var);
_ekf.get_beta_innov_var(&innovations.beta_innov_var);
_ekf.get_flow_innov_var(&innovations.flow_innov_var[0]);
_ekf.get_hagl_innov_var(&innovations.hagl_innov_var);
_ekf.get_output_tracking_error(&innovations.output_tracking_error[0]);
if (_estimator_innovations_pub == nullptr) {
_estimator_innovations_pub = orb_advertise(ORB_ID(ekf2_innovations), &innovations);
} else {
orb_publish(ORB_ID(ekf2_innovations), _estimator_innovations_pub, &innovations);
}
}
// save the declination to the EKF2_MAG_DECL parameter when a land event is detected
if ((_params->mag_declination_source & (1 << 1)) && !_prev_landed && vehicle_land_detected.landed) {
float decl_deg;
_ekf.copy_mag_decl_deg(&decl_deg);
_mag_declination_deg.set(decl_deg);
}
_prev_landed = vehicle_land_detected.landed;
// publish ekf2_timestamps (using 0.1 ms relative timestamps)
{
ekf2_timestamps_s ekf2_timestamps;
ekf2_timestamps.timestamp = sensors.timestamp;
if (gps_updated) {
// divide individually to get consistent rounding behavior
ekf2_timestamps.gps_timestamp_rel = (int16_t)((int64_t)gps.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100);
} else {
ekf2_timestamps.gps_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
}
if (optical_flow_updated) {
ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
} else {
ekf2_timestamps.optical_flow_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
}
if (range_finder_updated) {
ekf2_timestamps.distance_sensor_timestamp_rel = (int16_t)((int64_t)range_finder.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
} else {
ekf2_timestamps.distance_sensor_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
}
if (airspeed_updated) {
ekf2_timestamps.airspeed_timestamp_rel = (int16_t)((int64_t)airspeed.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
} else {
ekf2_timestamps.airspeed_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
}
if (vision_position_updated) {
ekf2_timestamps.vision_position_timestamp_rel = (int16_t)((int64_t)ev_pos.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
} else {
ekf2_timestamps.vision_position_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
}
if (vision_attitude_updated) {
ekf2_timestamps.vision_attitude_timestamp_rel = (int16_t)((int64_t)ev_att.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
} else {
ekf2_timestamps.vision_attitude_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
}
if (_ekf2_timestamps_pub == nullptr) {
_ekf2_timestamps_pub = orb_advertise(ORB_ID(ekf2_timestamps), &ekf2_timestamps);
} else {
orb_publish(ORB_ID(ekf2_timestamps), _ekf2_timestamps_pub, &ekf2_timestamps);
}
}
// publish replay message if in replay mode
bool publish_replay_message = (bool)_param_record_replay_msg.get();
if (publish_replay_message) {
struct ekf2_replay_s replay = {};
replay.timestamp = now;
replay.gyro_integral_dt = sensors.gyro_integral_dt;
replay.accelerometer_integral_dt = sensors.accelerometer_integral_dt;
replay.magnetometer_timestamp = _timestamp_mag_us;
replay.baro_timestamp = _timestamp_balt_us;
memcpy(replay.gyro_rad, sensors.gyro_rad, sizeof(replay.gyro_rad));
memcpy(replay.accelerometer_m_s2, sensors.accelerometer_m_s2, sizeof(replay.accelerometer_m_s2));
memcpy(replay.magnetometer_ga, sensors.magnetometer_ga, sizeof(replay.magnetometer_ga));
replay.baro_alt_meter = sensors.baro_alt_meter;
// only write gps data if we had a gps update.
if (gps_updated) {
replay.time_usec = gps.timestamp;
replay.lat = gps.lat;
replay.lon = gps.lon;
replay.alt = gps.alt;
replay.fix_type = gps.fix_type;
replay.nsats = gps.satellites_used;
replay.eph = gps.eph;
replay.epv = gps.epv;
replay.sacc = gps.s_variance_m_s;
replay.vel_m_s = gps.vel_m_s;
replay.vel_n_m_s = gps.vel_n_m_s;
replay.vel_e_m_s = gps.vel_e_m_s;
replay.vel_d_m_s = gps.vel_d_m_s;
replay.vel_ned_valid = gps.vel_ned_valid;
} else {
// this will tell the logging app not to bother logging any gps replay data
replay.time_usec = 0;
}
if (optical_flow_updated) {
replay.flow_timestamp = optical_flow.timestamp;
replay.flow_pixel_integral[0] = optical_flow.pixel_flow_x_integral;
replay.flow_pixel_integral[1] = optical_flow.pixel_flow_y_integral;
replay.flow_gyro_integral[0] = optical_flow.gyro_x_rate_integral;
replay.flow_gyro_integral[1] = optical_flow.gyro_y_rate_integral;
replay.flow_time_integral = optical_flow.integration_timespan;
replay.flow_quality = optical_flow.quality;
} else {
replay.flow_timestamp = 0;
}
if (range_finder_updated) {
replay.rng_timestamp = range_finder.timestamp;
replay.range_to_ground = range_finder.current_distance;
} else {
replay.rng_timestamp = 0;
}
if (airspeed_updated) {
replay.asp_timestamp = airspeed.timestamp;
replay.indicated_airspeed_m_s = airspeed.indicated_airspeed_m_s;
replay.true_airspeed_m_s = airspeed.true_airspeed_m_s;
} else {
replay.asp_timestamp = 0;
}
if (vision_position_updated || vision_attitude_updated) {
replay.ev_timestamp = vision_position_updated ? ev_pos.timestamp : ev_att.timestamp;
replay.pos_ev[0] = ev_pos.x;
replay.pos_ev[1] = ev_pos.y;
replay.pos_ev[2] = ev_pos.z;
replay.quat_ev[0] = ev_att.q[0];
replay.quat_ev[1] = ev_att.q[1];
replay.quat_ev[2] = ev_att.q[2];
replay.quat_ev[3] = ev_att.q[3];
// TODO : switch to covariances from topic later
replay.pos_err = _default_ev_pos_noise;
replay.ang_err = _default_ev_ang_noise;
} else {
replay.ev_timestamp = 0;
}
if (_replay_pub == nullptr) {
_replay_pub = orb_advertise(ORB_ID(ekf2_replay), &replay);
} else {
orb_publish(ORB_ID(ekf2_replay), _replay_pub, &replay);
}
}
}
orb_unsubscribe(sensors_sub);
orb_unsubscribe(gps_sub);
orb_unsubscribe(airspeed_sub);
orb_unsubscribe(params_sub);
orb_unsubscribe(optical_flow_sub);
orb_unsubscribe(range_finder_sub);
orb_unsubscribe(ev_pos_sub);
orb_unsubscribe(vehicle_land_detected_sub);
orb_unsubscribe(status_sub);
delete ekf2::instance;
ekf2::instance = nullptr;
}
void
CameraFeedback::task_main()
{
_trigger_sub = orb_subscribe(ORB_ID(camera_trigger));
// Polling sources
struct camera_trigger_s trig = {};
px4_pollfd_struct_t fds[1] = {};
fds[0].fd = _trigger_sub;
fds[0].events = POLLIN;
// Geotagging subscriptions
_gpos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
struct vehicle_global_position_s gpos = {};
struct vehicle_attitude_s att = {};
bool updated = false;
while (!_task_should_exit) {
/* wait for up to 20ms for data */
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 20);
if (pret < 0) {
PX4_WARN("poll error %d, %d", pret, errno);
continue;
}
/* trigger subscription updated */
if (fds[0].revents & POLLIN) {
orb_copy(ORB_ID(camera_trigger), _trigger_sub, &trig);
/* update geotagging subscriptions */
orb_check(_gpos_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_global_position), _gpos_sub, &gpos);
}
orb_check(_att_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att);
}
if (trig.timestamp == 0 ||
gpos.timestamp == 0 ||
att.timestamp == 0) {
// reject until we have valid data
continue;
}
struct camera_capture_s capture = {};
// Fill timestamps
capture.timestamp = trig.timestamp;
capture.timestamp_utc = trig.timestamp_utc;
// Fill image sequence
capture.seq = trig.seq;
// Fill position data
capture.lat = gpos.lat;
capture.lon = gpos.lon;
capture.alt = gpos.alt;
capture.ground_distance = gpos.terrain_alt_valid ? (gpos.alt - gpos.terrain_alt) : -1.0f;
// Fill attitude data
// TODO : this needs to be rotated by camera orientation or set to gimbal orientation when available
capture.q[0] = att.q[0];
capture.q[1] = att.q[1];
capture.q[2] = att.q[2];
capture.q[3] = att.q[3];
// Indicate whether capture feedback from camera is available
// What is case 0 for capture.result?
if (!_camera_capture_feedback) {
capture.result = -1;
} else {
capture.result = 1;
}
int instance_id;
orb_publish_auto(ORB_ID(camera_capture), &_capture_pub, &capture, &instance_id, ORB_PRIO_DEFAULT);
}
}
orb_unsubscribe(_trigger_sub);
orb_unsubscribe(_gpos_sub);
orb_unsubscribe(_att_sub);
_main_task = -1;
}
SubscriptionBase::~SubscriptionBase()
{
int ret = orb_unsubscribe(_handle);
if (ret != PX4_OK) { PX4_ERR("orb unsubscribe failed"); }
}
calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors)
{
calibrate_return result = calibrate_return_ok;
*active_sensors = 0;
accel_worker_data_t worker_data;
worker_data.mavlink_log_pub = mavlink_log_pub;
worker_data.done_count = 0;
bool data_collected[detect_orientation_side_count] = { false, false, false, false, false, false };
// Initialise sub to sensor thermal compensation data
worker_data.sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
// Initialize subs to error condition so we know which ones are open and which are not
for (size_t i=0; i<max_accel_sens; i++) {
worker_data.subs[i] = -1;
}
uint64_t timestamps[max_accel_sens] = {};
// We should not try to subscribe if the topic doesn't actually exist and can be counted.
const unsigned orb_accel_count = orb_group_count(ORB_ID(sensor_accel));
// Warn that we will not calibrate more than max_accels accelerometers
if (orb_accel_count > max_accel_sens) {
calibration_log_critical(mavlink_log_pub, "Detected %u accels, but will calibrate only %u", orb_accel_count, max_accel_sens);
}
for (unsigned cur_accel = 0; cur_accel < orb_accel_count && cur_accel < max_accel_sens; cur_accel++) {
// Lock in to correct ORB instance
bool found_cur_accel = false;
for(unsigned i = 0; i < orb_accel_count && !found_cur_accel; i++) {
worker_data.subs[cur_accel] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
struct accel_report report = {};
orb_copy(ORB_ID(sensor_accel), worker_data.subs[cur_accel], &report);
#ifdef __PX4_NUTTX
// For NuttX, we get the UNIQUE device ID from the sensor driver via an IOCTL
// and match it up with the one from the uORB subscription, because the
// instance ordering of uORB and the order of the FDs may not be the same.
if(report.device_id == device_id[cur_accel]) {
// Device IDs match, correct ORB instance for this accel
found_cur_accel = true;
// store initial timestamp - used to infer which sensors are active
timestamps[cur_accel] = report.timestamp;
} else {
orb_unsubscribe(worker_data.subs[cur_accel]);
}
#else
// For the DriverFramework drivers, we fill device ID (this is the first time) by copying one report.
device_id[cur_accel] = report.device_id;
found_cur_accel = true;
#endif
}
if(!found_cur_accel) {
calibration_log_critical(mavlink_log_pub, "Accel #%u (ID %u) no matching uORB devid", cur_accel, device_id[cur_accel]);
result = calibrate_return_error;
break;
}
if (device_id[cur_accel] != 0) {
// Get priority
int32_t prio;
orb_priority(worker_data.subs[cur_accel], &prio);
if (prio > device_prio_max) {
device_prio_max = prio;
device_id_primary = device_id[cur_accel];
}
} else {
calibration_log_critical(mavlink_log_pub, "Accel #%u no device id, abort", cur_accel);
result = calibrate_return_error;
break;
}
}
if (result == calibrate_return_ok) {
int cancel_sub = calibrate_cancel_subscribe();
result = calibrate_from_orientation(mavlink_log_pub, cancel_sub, data_collected, accel_calibration_worker, &worker_data, false /* normal still */);
calibrate_cancel_unsubscribe(cancel_sub);
}
/* close all subscriptions */
for (unsigned i = 0; i < max_accel_sens; i++) {
if (worker_data.subs[i] >= 0) {
/* figure out which sensors were active */
struct accel_report arp = {};
(void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) {
(*active_sensors)++;
}
px4_close(worker_data.subs[i]);
}
}
orb_unsubscribe(worker_data.sensor_correction_sub);
if (result == calibrate_return_ok) {
/* calculate offsets and transform matrix */
for (unsigned i = 0; i < (*active_sensors); i++) {
result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
if (result != calibrate_return_ok) {
calibration_log_critical(mavlink_log_pub, "ERROR: calibration calculation error");
break;
}
}
}
return result;
}
int do_accel_calibration(orb_advert_t *mavlink_log_pub)
{
#ifdef __PX4_NUTTX
int fd;
#endif
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
struct accel_calibration_s accel_scale;
accel_scale.x_offset = 0.0f;
accel_scale.x_scale = 1.0f;
accel_scale.y_offset = 0.0f;
accel_scale.y_scale = 1.0f;
accel_scale.z_offset = 0.0f;
accel_scale.z_scale = 1.0f;
int res = PX4_OK;
char str[30];
/* reset all sensors */
for (unsigned s = 0; s < max_accel_sens; s++) {
#ifdef __PX4_NUTTX
sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
/* reset all offsets to zero and all scales to one */
fd = px4_open(str, 0);
if (fd < 0) {
continue;
}
device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
px4_close(fd);
if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, s);
}
#else
(void)sprintf(str, "CAL_ACC%u_XOFF", s);
res = param_set_no_notification(param_find(str), &accel_scale.x_offset);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_YOFF", s);
res = param_set_no_notification(param_find(str), &accel_scale.y_offset);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_ZOFF", s);
res = param_set_no_notification(param_find(str), &accel_scale.z_offset);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_XSCALE", s);
res = param_set_no_notification(param_find(str), &accel_scale.x_scale);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_YSCALE", s);
res = param_set_no_notification(param_find(str), &accel_scale.y_scale);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_ZSCALE", s);
res = param_set_no_notification(param_find(str), &accel_scale.z_scale);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
param_notify_changes();
#endif
}
float accel_offs[max_accel_sens][3];
float accel_T[max_accel_sens][3][3];
unsigned active_sensors = 0;
/* measure and calculate offsets & scales */
if (res == PX4_OK) {
calibrate_return cal_return = do_accel_calibration_measurements(mavlink_log_pub, accel_offs, accel_T, &active_sensors);
if (cal_return == calibrate_return_cancelled) {
// Cancel message already displayed, nothing left to do
return PX4_ERROR;
} else if (cal_return == calibrate_return_ok) {
res = PX4_OK;
} else {
res = PX4_ERROR;
}
}
if (res != PX4_OK) {
if (active_sensors == 0) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
}
return PX4_ERROR;
}
/* measurements completed successfully, rotate calibration values */
param_t board_rotation_h = param_find("SENS_BOARD_ROT");
int32_t board_rotation_int;
param_get(board_rotation_h, &(board_rotation_int));
enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
math::Matrix<3, 3> board_rotation;
get_rot_matrix(board_rotation_id, &board_rotation);
math::Matrix<3, 3> board_rotation_t = board_rotation.transposed();
bool tc_locked[3] = {false}; // true when the thermal parameter instance has already been adjusted by the calibrator
for (unsigned uorb_index = 0; uorb_index < active_sensors; uorb_index++) {
/* handle individual sensors, one by one */
math::Vector<3> accel_offs_vec(accel_offs[uorb_index]);
math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec;
math::Matrix<3, 3> accel_T_mat(accel_T[uorb_index]);
math::Matrix<3, 3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
accel_scale.x_offset = accel_offs_rotated(0);
accel_scale.x_scale = accel_T_rotated(0, 0);
accel_scale.y_offset = accel_offs_rotated(1);
accel_scale.y_scale = accel_T_rotated(1, 1);
accel_scale.z_offset = accel_offs_rotated(2);
accel_scale.z_scale = accel_T_rotated(2, 2);
bool failed = false;
failed = failed || (PX4_OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary)));
PX4_INFO("found offset %d: x: %.6f, y: %.6f, z: %.6f", uorb_index,
(double)accel_scale.x_offset,
(double)accel_scale.y_offset,
(double)accel_scale.z_offset);
PX4_INFO("found scale %d: x: %.6f, y: %.6f, z: %.6f", uorb_index,
(double)accel_scale.x_scale,
(double)accel_scale.y_scale,
(double)accel_scale.z_scale);
/* check if thermal compensation is enabled */
int32_t tc_enabled_int;
param_get(param_find("TC_A_ENABLE"), &(tc_enabled_int));
if (tc_enabled_int == 1) {
/* Get struct containing sensor thermal compensation data */
struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */
memset(&sensor_correction, 0, sizeof(sensor_correction));
int sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
orb_copy(ORB_ID(sensor_correction), sensor_correction_sub, &sensor_correction);
orb_unsubscribe(sensor_correction_sub);
/* don't allow a parameter instance to be calibrated more than once by another uORB instance */
if (!tc_locked[sensor_correction.accel_mapping[uorb_index]]) {
tc_locked[sensor_correction.accel_mapping[uorb_index]] = true;
/* update the _X0_ terms to include the additional offset */
int32_t handle;
float val;
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
val = 0.0f;
(void)sprintf(str, "TC_A%u_X0_%u", sensor_correction.accel_mapping[uorb_index], axis_index);
handle = param_find(str);
param_get(handle, &val);
if (axis_index == 0) {
val += accel_scale.x_offset;
} else if (axis_index == 1) {
val += accel_scale.y_offset;
} else if (axis_index == 2) {
val += accel_scale.z_offset;
}
failed |= (PX4_OK != param_set_no_notification(handle, &val));
}
/* update the _SCL_ terms to include the scale factor */
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
val = 1.0f;
(void)sprintf(str, "TC_A%u_SCL_%u", sensor_correction.accel_mapping[uorb_index], axis_index);
handle = param_find(str);
if (axis_index == 0) {
val = accel_scale.x_scale;
} else if (axis_index == 1) {
val = accel_scale.y_scale;
} else if (axis_index == 2) {
val = accel_scale.z_scale;
}
failed |= (PX4_OK != param_set_no_notification(handle, &val));
}
param_notify_changes();
}
// Ensure the calibration values used by the driver are at default settings when we are using thermal calibration data
accel_scale.x_offset = 0.f;
accel_scale.y_offset = 0.f;
accel_scale.z_offset = 0.f;
accel_scale.x_scale = 1.f;
accel_scale.y_scale = 1.f;
accel_scale.z_scale = 1.f;
}
// save the driver level calibration data
(void)sprintf(str, "CAL_ACC%u_XOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset)));
(void)sprintf(str, "CAL_ACC%u_YOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.y_offset)));
(void)sprintf(str, "CAL_ACC%u_ZOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.z_offset)));
(void)sprintf(str, "CAL_ACC%u_XSCALE", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.x_scale)));
(void)sprintf(str, "CAL_ACC%u_YSCALE", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.y_scale)));
(void)sprintf(str, "CAL_ACC%u_ZSCALE", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.z_scale)));
(void)sprintf(str, "CAL_ACC%u_ID", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(device_id[uorb_index])));
if (failed) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, uorb_index);
return PX4_ERROR;
}
#ifdef __PX4_NUTTX
sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, uorb_index);
fd = px4_open(str, 0);
if (fd < 0) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "sensor does not exist");
res = PX4_ERROR;
} else {
res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
px4_close(fd);
}
if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, uorb_index);
}
#endif
}
if (res == PX4_OK) {
/* if there is a any preflight-check system response, let the barrage of messages through */
usleep(200000);
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
} else {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
}
/* give this message enough time to propagate */
usleep(600000);
return res;
}