bool
MavlinkOrbSubscription::is_published()
{
// If we marked it as published no need to check again
if (_published) {
return true;
}
// Telemetry can sustain an initial published check at 10 Hz
hrt_abstime now = hrt_absolute_time();
if (now - _last_pub_check < 100000) {
return false;
}
// We are checking now
_last_pub_check = now;
// If it does not exist its not published
if (orb_exists(_topic, _instance)) {
return false;
} else if (_fd < 0) {
_fd = orb_subscribe_multi(_topic, _instance);
}
bool updated;
orb_check(_fd, &updated);
if (updated) {
_published = true;
}
return _published;
}
bool
MavlinkOrbSubscription::is_published()
{
// If we marked it as published no need to check again
if (_published) {
return true;
}
// Telemetry can sustain an initial published check at 10 Hz
hrt_abstime now = hrt_absolute_time();
if (now - _last_pub_check < 100000) {
return false;
}
// We are checking now
_last_pub_check = now;
#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE)
// Snapdragon has currently no support for orb_exists, therefore
// we're not using it.
if (_fd < 0) {
_fd = orb_subscribe_multi(_topic, _instance);
}
#else
// We don't want to subscribe to anything that does not exist
// in order to save memory and file descriptors.
// However, for some topics like vehicle_command_ack, we want to subscribe
// from the beginning in order not to miss the first publish respective advertise.
if (!_subscribe_from_beginning && orb_exists(_topic, _instance)) {
return false;
}
if (_fd < 0) {
_fd = orb_subscribe_multi(_topic, _instance);
}
#endif
bool updated;
orb_check(_fd, &updated);
if (updated) {
_published = true;
}
return _published;
}
MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, int instance) :
next(nullptr),
_topic(topic),
_instance(instance),
_fd(orb_subscribe_multi(_topic, instance)),
_published(false)
{
}
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)");
}
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);
}
TemperatureCalibrationBaro::TemperatureCalibrationBaro(float min_temperature_rise)
: TemperatureCalibrationCommon(min_temperature_rise)
{
//init subscriptions
_num_sensor_instances = orb_group_count(ORB_ID(sensor_baro));
if (_num_sensor_instances > SENSOR_COUNT_MAX) {
_num_sensor_instances = SENSOR_COUNT_MAX;
}
for (unsigned i = 0; i < _num_sensor_instances; i++) {
_sensor_subs[i] = orb_subscribe_multi(ORB_ID(sensor_baro), i);
}
}
bool
MavlinkOrbSubscription::is_published()
{
// If we marked it as published no need to check again
if (_published) {
return true;
}
// Telemetry can sustain an initial published check at 10 Hz
hrt_abstime now = hrt_absolute_time();
if (now - _last_pub_check < 100000) {
return false;
}
// We are checking now
_last_pub_check = now;
// We don't want to subscribe to anything that does not exist
// in order to save memory and file descriptors.
// However, for some topics like vehicle_command_ack, we want to subscribe
// from the beginning in order not to miss the first publish respective advertise.
if (!_subscribe_from_beginning && orb_exists(_topic, _instance)) {
return false;
}
if (_fd < 0) {
_fd = orb_subscribe_multi(_topic, _instance);
}
bool updated;
orb_check(_fd, &updated);
if (updated) {
_published = true;
}
// topic may have been last published before we subscribed
uint64_t time_topic = 0;
if (!_published && orb_stat(_fd, &time_topic) == PX4_OK) {
if (time_topic != 0) {
_published = true;
}
}
return _published;
}
SubscriptionBase::SubscriptionBase(const struct orb_metadata *meta,
unsigned interval, unsigned instance) :
_meta(meta),
_instance(instance),
_handle()
{
if (_instance > 0) {
_handle = orb_subscribe_multi(
getMeta(), instance);
} else {
_handle = orb_subscribe(getMeta());
}
if (_handle < 0) { warnx("sub failed"); }
orb_set_interval(getHandle(), interval);
}
bool
MavlinkOrbSubscription::is_published()
{
// If we marked it as published no need to check again
if (_published) {
return true;
}
hrt_abstime now = hrt_absolute_time();
if (now - _last_pub_check < 300000) {
return false;
}
// We are checking now
_last_pub_check = now;
// We don't want to subscribe to anything that does not exist
// in order to save memory and file descriptors.
// However, for some topics like vehicle_command_ack, we want to subscribe
// from the beginning in order not to miss or delay the first publish respective advertise.
if (!_subscribe_from_beginning && orb_exists(_topic, _instance)) {
return false;
}
if (_fd < 0) {
_fd = orb_subscribe_multi(_topic, _instance);
}
bool updated;
orb_check(_fd, &updated);
if (updated) {
_published = true;
}
return _published;
}
static void prvGyroTask(void *pvParameters)
{
int gyro_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 0);//订阅第一个陀螺仪的数据
int ret = 0;
struct sensor_gyro_s gyro;
while(1)
{
struct pollfd fds[1];
memset(fds, 0, sizeof(fds));
fds[0].fd = gyro_sub;
fds[0].events = POLLIN;
// pilot_err("poll start:gyro_sub=%x\n", (int)gyro_sub);
ret = poll(fds, sizeof(fds)/sizeof(fds[0]), 500);
// pilot_err("poll over:gyro_sub=%x\n", (int)gyro_sub);
if(ret < 0)
pilot_err("poll error\n");
else if(ret == 0)
pilot_warn("poll timeout");
else
{
if(fds[0].revents & POLLIN)
{
orb_copy(ORB_ID(sensor_gyro), gyro_sub, &gyro);
pilot_info("get data:rawxyz[%04x,%04x,%04x] x=%f y=%f z=%f size=%x\n", gyro.x_raw, gyro.y_raw, gyro.z_raw, gyro.x, gyro.y, gyro.z, sizeof(struct sensor_gyro_s));
}
else
{
pilot_warn("revents=%x\n", fds[0].revents);
}
}
vTaskDelay( 300 / portTICK_RATE_MS );
}
}
static void prvMagTask(void *pvParameters)
{
int mag_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 0);
int ret = 0;
struct sensor_mag_s mag;
while(1)
{
struct pollfd fds[1];
memset(fds, 0, sizeof(fds));
fds[0].fd = mag_sub;
fds[0].events = POLLIN;
// pilot_info("poll start:mag_sub=%x\n", (int)mag_sub);
ret = poll(fds, sizeof(fds)/sizeof(fds[0]), 500);
if(ret < 0)
pilot_err("poll error\n");
else if(ret == 0)
pilot_warn("poll timeout");
else
{
if(fds[0].revents & POLLIN)
{
orb_copy(ORB_ID(sensor_mag), mag_sub, &mag);
pilot_err("mag data:rawxyz[%04x,%04x,%04x] x=%f y=%f z=%f size=%x\n", mag.x_raw, mag.y_raw, mag.z_raw, mag.x, mag.y, mag.z, sizeof(struct sensor_mag_s));
}
else
{
pilot_warn("revents=%x\n", fds[0].revents);
}
}
vTaskDelay( 300 / portTICK_RATE_MS );
}
}
static void prvAccelTask(void *pvParameters)
{
int accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
int ret = 0;
struct sensor_accel_s accel;
while(1)
{
struct pollfd fds[1];
memset(fds, 0, sizeof(fds));
fds[0].fd = accel_sub;
fds[0].events = POLLIN;
// pilot_err("poll start:accel_sub=%x\n", (int)accel_sub);
ret = poll(fds, sizeof(fds)/sizeof(fds[0]), 500);
// pilot_err("poll over:accel_sub=%x\n", (int)accel_sub);
if(ret < 0)
pilot_err("poll error\n");
else if(ret == 0)
pilot_warn("poll timeout");
else
{
if(fds[0].revents & POLLIN)
{
orb_copy(ORB_ID(sensor_accel), accel_sub, &accel);
pilot_warn("accel data:rawxyz[%04x,%04x,%04x] x=%f y=%f z=%f size=%x\n", accel.x_raw, accel.y_raw, accel.z_raw, accel.x, accel.y, accel.z, sizeof(struct sensor_accel_s));
}
else
{
pilot_warn("revents=%x\n", fds[0].revents);
}
}
vTaskDelay( 300 / portTICK_RATE_MS );
}
}
static void prvBaroTask(void *pvParameters)
{
int baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0);
int ret = 0;
struct sensor_baro_s baro;
while(1)
{
struct pollfd fds[1];
memset(fds, 0, sizeof(fds));
fds[0].fd = baro_sub;
fds[0].events = POLLIN;
// pilot_info("poll start:baro_sub=%x\n", (int)baro_sub);
ret = poll(fds, sizeof(fds)/sizeof(fds[0]), 500);
if(ret < 0)
pilot_err("poll error\n");
else if(ret == 0)
pilot_warn("poll timeout");
else
{
if(fds[0].revents & POLLIN)
{
orb_copy(ORB_ID(sensor_baro), baro_sub, &baro);
pilot_err("baro data:pressure=%f altitude=%f\n", baro.pressure, baro.altitude);
}
else
{
pilot_warn("revents=%x\n", fds[0].revents);
}
}
vTaskDelay( 300 / portTICK_RATE_MS );
}
}
int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_sens][3], float (&accel_T)[max_sens][3][3], unsigned *active_sensors)
{
const unsigned samples_num = 3000;
*active_sensors = 0;
float accel_ref[max_sens][6][3];
bool data_collected[6] = { false, false, false, false, false, false };
const char *orientation_strs[6] = { "back", "front", "left", "right", "up", "down" };
int subs[max_sens];
uint64_t timestamps[max_sens];
for (unsigned i = 0; i < max_sens; i++) {
subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
/* store initial timestamp - used to infer which sensors are active */
struct accel_report arp = {};
(void)orb_copy(ORB_ID(sensor_accel), subs[i], &arp);
timestamps[i] = arp.timestamp;
}
unsigned done_count = 0;
int res = OK;
while (true) {
bool done = true;
unsigned old_done_count = done_count;
done_count = 0;
for (int i = 0; i < 6; i++) {
if (data_collected[i]) {
done_count++;
} else {
done = false;
}
}
if (old_done_count != done_count) {
mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count);
}
if (done) {
break;
}
/* inform user which axes are still needed */
mavlink_and_console_log_info(mavlink_fd, "pending: %s%s%s%s%s%s",
(!data_collected[5]) ? "down " : "",
(!data_collected[0]) ? "back " : "",
(!data_collected[1]) ? "front " : "",
(!data_collected[2]) ? "left " : "",
(!data_collected[3]) ? "right " : "",
(!data_collected[4]) ? "up " : "");
/* allow user enough time to read the message */
sleep(3);
int orient = detect_orientation(mavlink_fd, subs);
if (orient < 0) {
mavlink_and_console_log_info(mavlink_fd, "invalid motion, hold still...");
sleep(3);
continue;
}
/* inform user about already handled side */
if (data_collected[orient]) {
mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]);
sleep(3);
continue;
}
mavlink_and_console_log_info(mavlink_fd, "Hold still, starting to measure %s side", orientation_strs[orient]);
sleep(1);
read_accelerometer_avg(subs, accel_ref, orient, samples_num);
mavlink_and_console_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", orientation_strs[orient],
(double)accel_ref[0][orient][0],
(double)accel_ref[0][orient][1],
(double)accel_ref[0][orient][2]);
data_collected[orient] = true;
tune_neutral(true);
}
/* close all subscriptions */
for (unsigned i = 0; i < max_sens; i++) {
/* figure out which sensors were active */
struct accel_report arp = {};
(void)orb_copy(ORB_ID(sensor_accel), subs[i], &arp);
if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) {
(*active_sensors)++;
}
close(subs[i]);
}
if (res == OK) {
/* calculate offsets and transform matrix */
for (unsigned i = 0; i < (*active_sensors); i++) {
res = calculate_calibration_values(i, accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
/* verbose output on the console */
printf("accel_T transformation matrix:\n");
for (unsigned j = 0; j < 3; j++) {
printf(" %8.4f %8.4f %8.4f\n", (double)accel_T[i][j][0], (double)accel_T[i][j][1], (double)accel_T[i][j][2]);
}
printf("\n");
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, "ERROR: calibration values calculation error");
break;
}
}
}
return res;
}
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 = 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.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.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
*/
/* reset the timestamp for data, because we have no data yet */
_report_gps_pos.timestamp = 0;
_report_gps_pos.timestamp_time_relative = 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);
}
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;
}
void
FixedwingAttitudeControl::task_main()
{
/*
* do subscriptions
*/
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
parameters_update();
/* get an initial update for all sensor and status data */
vehicle_setpoint_poll();
vehicle_accel_poll();
vehicle_control_mode_poll();
vehicle_manual_poll();
vehicle_status_poll();
vehicle_land_detected_poll();
/* wakeup source */
px4_pollfd_struct_t fds[2];
/* Setup of loop */
fds[0].fd = _params_sub;
fds[0].events = POLLIN;
fds[1].fd = _ctrl_state_sub;
fds[1].events = POLLIN;
_task_running = true;
while (!_task_should_exit) {
static int loop_counter = 0;
/* wait for up to 500ms for data */
int pret = px4_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 error %d, %d", pret, errno);
continue;
}
perf_begin(_loop_perf);
/* 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 controller if attitude changed */
if (fds[1].revents & POLLIN) {
static uint64_t last_run = 0;
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
/* guard against too large deltaT's */
if (deltaT > 1.0f) {
deltaT = 0.01f;
}
/* load local copies */
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
/* get current rotation matrix and euler angles from control state quaternions */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
_R = q_att.to_dcm();
math::Vector<3> euler_angles;
euler_angles = _R.to_euler();
_roll = euler_angles(0);
_pitch = euler_angles(1);
_yaw = euler_angles(2);
if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) {
/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
*
* Since the VTOL airframe is initialized as a multicopter we need to
* modify the estimated attitude for the fixed wing operation.
* Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around
* the pitch axis compared to the neutral position of the vehicle in multicopter mode
* we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix.
* Additionally, in order to get the correct sign of the pitch, we need to multiply
* the new x axis of the rotation matrix with -1
*
* original: modified:
*
* Rxx Ryx Rzx -Rzx Ryx Rxx
* Rxy Ryy Rzy -Rzy Ryy Rxy
* Rxz Ryz Rzz -Rzz Ryz Rxz
* */
math::Matrix<3, 3> R_adapted = _R; //modified rotation matrix
/* move z to x */
R_adapted(0, 0) = _R(0, 2);
R_adapted(1, 0) = _R(1, 2);
R_adapted(2, 0) = _R(2, 2);
/* move x to z */
R_adapted(0, 2) = _R(0, 0);
R_adapted(1, 2) = _R(1, 0);
R_adapted(2, 2) = _R(2, 0);
/* change direction of pitch (convert to right handed system) */
R_adapted(0, 0) = -R_adapted(0, 0);
R_adapted(1, 0) = -R_adapted(1, 0);
R_adapted(2, 0) = -R_adapted(2, 0);
euler_angles = R_adapted.to_euler(); //adapted euler angles for fixed wing operation
/* fill in new attitude data */
_R = R_adapted;
_roll = euler_angles(0);
_pitch = euler_angles(1);
_yaw = euler_angles(2);
/* lastly, roll- and yawspeed have to be swaped */
float helper = _ctrl_state.roll_rate;
_ctrl_state.roll_rate = -_ctrl_state.yaw_rate;
_ctrl_state.yaw_rate = helper;
}
vehicle_setpoint_poll();
vehicle_accel_poll();
vehicle_control_mode_poll();
vehicle_manual_poll();
global_pos_poll();
vehicle_status_poll();
vehicle_land_detected_poll();
// the position controller will not emit attitude setpoints in some modes
// we need to make sure that this flag is reset
_att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled;
/* lock integrator until control is started */
bool lock_integrator;
if (_vcontrol_mode.flag_control_attitude_enabled && !_vehicle_status.is_rotary_wing) {
lock_integrator = false;
} else {
lock_integrator = true;
}
/* Simple handling of failsafe: deploy parachute if failsafe is on */
if (_vcontrol_mode.flag_control_termination_enabled) {
_actuators_airframe.control[7] = 1.0f;
//warnx("_actuators_airframe.control[1] = 1.0f;");
} else {
_actuators_airframe.control[7] = 0.0f;
//warnx("_actuators_airframe.control[1] = -1.0f;");
}
/* if we are in rotary wing mode, do nothing */
if (_vehicle_status.is_rotary_wing && !_vehicle_status.is_vtol) {
continue;
}
/* default flaps to center */
float flap_control = 0.0f;
/* map flaps by default to manual if valid */
if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled
&& fabsf(_parameters.flaps_scale) > 0.01f) {
flap_control = 0.5f * (_manual.flaps + 1.0f) * _parameters.flaps_scale;
} else if (_vcontrol_mode.flag_control_auto_enabled
&& fabsf(_parameters.flaps_scale) > 0.01f) {
flap_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaps_scale : 0.0f;
}
// move the actual control value continuous with time, full flap travel in 1sec
if (fabsf(_flaps_applied - flap_control) > 0.01f) {
_flaps_applied += (_flaps_applied - flap_control) < 0 ? deltaT : -deltaT;
} else {
_flaps_applied = flap_control;
}
/* default flaperon to center */
float flaperon_control = 0.0f;
/* map flaperons by default to manual if valid */
if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled
&& fabsf(_parameters.flaperon_scale) > 0.01f) {
flaperon_control = 0.5f * (_manual.aux2 + 1.0f) * _parameters.flaperon_scale;
} else if (_vcontrol_mode.flag_control_auto_enabled
&& fabsf(_parameters.flaperon_scale) > 0.01f) {
flaperon_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaperon_scale : 0.0f;
}
// move the actual control value continuous with time, full flap travel in 1sec
if (fabsf(_flaperons_applied - flaperon_control) > 0.01f) {
_flaperons_applied += (_flaperons_applied - flaperon_control) < 0 ? deltaT : -deltaT;
} else {
_flaperons_applied = flaperon_control;
}
/* decide if in stabilized or full manual control */
if (_vcontrol_mode.flag_control_attitude_enabled) {
/* scale around tuning airspeed */
float airspeed;
/* if airspeed is not updating, we assume the normal average speed */
if (bool nonfinite = !PX4_ISFINITE(_ctrl_state.airspeed) || !_ctrl_state.airspeed_valid) {
airspeed = _parameters.airspeed_trim;
if (nonfinite) {
perf_count(_nonfinite_input_perf);
}
} else {
/* prevent numerical drama by requiring 0.5 m/s minimal speed */
airspeed = math::max(0.5f, _ctrl_state.airspeed);
}
/*
* For scaling our actuators using anything less than the min (close to stall)
* speed doesn't make any sense - its the strongest reasonable deflection we
* want to do in flight and its the baseline a human pilot would choose.
*
* Forcing the scaling to this value allows reasonable handheld tests.
*/
float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min :
airspeed);
/* Use min airspeed to calculate ground speed scaling region.
* Don't scale below gspd_scaling_trim
*/
float groundspeed = sqrtf(_global_pos.vel_n * _global_pos.vel_n +
_global_pos.vel_e * _global_pos.vel_e);
float gspd_scaling_trim = (_parameters.airspeed_min * 0.6f);
float groundspeed_scaler = gspd_scaling_trim / ((groundspeed < gspd_scaling_trim) ? gspd_scaling_trim : groundspeed);
float roll_sp = _parameters.rollsp_offset_rad;
float pitch_sp = _parameters.pitchsp_offset_rad;
float yaw_sp = 0.0f;
float yaw_manual = 0.0f;
float throttle_sp = 0.0f;
// in STABILIZED mode we need to generate the attitude setpoint
// from manual user inputs
if (!_vcontrol_mode.flag_control_climb_rate_enabled) {
_att_sp.roll_body = _manual.y * _parameters.man_roll_max + _parameters.rollsp_offset_rad;
_att_sp.roll_body = math::constrain(_att_sp.roll_body, -_parameters.man_roll_max, _parameters.man_roll_max);
_att_sp.pitch_body = -_manual.x * _parameters.man_pitch_max + _parameters.pitchsp_offset_rad;
_att_sp.pitch_body = math::constrain(_att_sp.pitch_body, -_parameters.man_pitch_max, _parameters.man_pitch_max);
_att_sp.yaw_body = 0.0f;
_att_sp.thrust = _manual.z;
int instance;
orb_publish_auto(_attitude_setpoint_id, &_attitude_sp_pub, &_att_sp, &instance, ORB_PRIO_DEFAULT);
}
roll_sp = _att_sp.roll_body;
pitch_sp = _att_sp.pitch_body;
yaw_sp = _att_sp.yaw_body;
throttle_sp = _att_sp.thrust;
/* allow manual yaw in manual modes */
if (_vcontrol_mode.flag_control_manual_enabled) {
yaw_manual = _manual.r;
}
/* reset integrals where needed */
if (_att_sp.roll_reset_integral) {
_roll_ctrl.reset_integrator();
}
if (_att_sp.pitch_reset_integral) {
_pitch_ctrl.reset_integrator();
}
if (_att_sp.yaw_reset_integral) {
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
}
/* If the aircraft is on ground reset the integrators */
if (_vehicle_land_detected.landed || _vehicle_status.is_rotary_wing) {
_roll_ctrl.reset_integrator();
_pitch_ctrl.reset_integrator();
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
}
/* Prepare speed_body_u and speed_body_w */
float speed_body_u = _R(0, 0) * _global_pos.vel_n + _R(1, 0) * _global_pos.vel_e + _R(2, 0) * _global_pos.vel_d;
float speed_body_v = _R(0, 1) * _global_pos.vel_n + _R(1, 1) * _global_pos.vel_e + _R(2, 1) * _global_pos.vel_d;
float speed_body_w = _R(0, 2) * _global_pos.vel_n + _R(1, 2) * _global_pos.vel_e + _R(2, 2) * _global_pos.vel_d;
/* Prepare data for attitude controllers */
struct ECL_ControlData control_input = {};
control_input.roll = _roll;
control_input.pitch = _pitch;
control_input.yaw = _yaw;
control_input.roll_rate = _ctrl_state.roll_rate;
control_input.pitch_rate = _ctrl_state.pitch_rate;
control_input.yaw_rate = _ctrl_state.yaw_rate;
control_input.speed_body_u = speed_body_u;
control_input.speed_body_v = speed_body_v;
control_input.speed_body_w = speed_body_w;
control_input.acc_body_x = _accel.x;
control_input.acc_body_y = _accel.y;
control_input.acc_body_z = _accel.z;
control_input.roll_setpoint = roll_sp;
control_input.pitch_setpoint = pitch_sp;
control_input.yaw_setpoint = yaw_sp;
control_input.airspeed_min = _parameters.airspeed_min;
control_input.airspeed_max = _parameters.airspeed_max;
control_input.airspeed = airspeed;
control_input.scaler = airspeed_scaling;
control_input.lock_integrator = lock_integrator;
control_input.groundspeed = groundspeed;
control_input.groundspeed_scaler = groundspeed_scaler;
_yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method);
/* Run attitude controllers */
if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) {
_roll_ctrl.control_attitude(control_input);
_pitch_ctrl.control_attitude(control_input);
_yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude
_wheel_ctrl.control_attitude(control_input);
/* Update input data for rate controllers */
control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate();
control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate();
control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate();
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
float roll_u = _roll_ctrl.control_bodyrate(control_input);
_actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + _parameters.trim_roll :
_parameters.trim_roll;
if (!PX4_ISFINITE(roll_u)) {
_roll_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (_debug && loop_counter % 10 == 0) {
warnx("roll_u %.4f", (double)roll_u);
}
}
float pitch_u = _pitch_ctrl.control_bodyrate(control_input);
_actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + _parameters.trim_pitch :
_parameters.trim_pitch;
if (!PX4_ISFINITE(pitch_u)) {
_pitch_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (_debug && loop_counter % 10 == 0) {
warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
" airspeed %.4f, airspeed_scaling %.4f,"
" roll_sp %.4f, pitch_sp %.4f,"
" _roll_ctrl.get_desired_rate() %.4f,"
" _pitch_ctrl.get_desired_rate() %.4f"
" att_sp.roll_body %.4f",
(double)pitch_u, (double)_yaw_ctrl.get_desired_rate(),
(double)airspeed, (double)airspeed_scaling,
(double)roll_sp, (double)pitch_sp,
(double)_roll_ctrl.get_desired_rate(),
(double)_pitch_ctrl.get_desired_rate(),
(double)_att_sp.roll_body);
}
}
float yaw_u = 0.0f;
if (_att_sp.fw_control_yaw == true) {
yaw_u = _wheel_ctrl.control_bodyrate(control_input);
}
else {
yaw_u = _yaw_ctrl.control_bodyrate(control_input);
}
_actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw :
_parameters.trim_yaw;
/* add in manual rudder control */
_actuators.control[actuator_controls_s::INDEX_YAW] += yaw_manual;
if (!PX4_ISFINITE(yaw_u)) {
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (_debug && loop_counter % 10 == 0) {
warnx("yaw_u %.4f", (double)yaw_u);
}
}
/* throttle passed through if it is finite and if no engine failure was
* detected */
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(throttle_sp) &&
!(_vehicle_status.engine_failure ||
_vehicle_status.engine_failure_cmd)) ?
throttle_sp : 0.0f;
if (!PX4_ISFINITE(throttle_sp)) {
if (_debug && loop_counter % 10 == 0) {
warnx("throttle_sp %.4f", (double)throttle_sp);
}
}
} else {
perf_count(_nonfinite_input_perf);
if (_debug && loop_counter % 10 == 0) {
warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
}
}
/*
* Lazily publish the rate setpoint (for analysis, the actuators are published below)
* only once available
*/
_rates_sp.roll = _roll_ctrl.get_desired_rate();
_rates_sp.pitch = _pitch_ctrl.get_desired_rate();
_rates_sp.yaw = _yaw_ctrl.get_desired_rate();
_rates_sp.timestamp = hrt_absolute_time();
if (_rate_sp_pub != nullptr) {
/* publish the attitude rates setpoint */
orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp);
} else if (_rates_sp_id) {
/* advertise the attitude rates setpoint */
_rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp);
}
} else {
/* manual/direct control */
_actuators.control[actuator_controls_s::INDEX_ROLL] = _manual.y * _parameters.man_roll_scale + _parameters.trim_roll;
_actuators.control[actuator_controls_s::INDEX_PITCH] = -_manual.x * _parameters.man_pitch_scale +
_parameters.trim_pitch;
_actuators.control[actuator_controls_s::INDEX_YAW] = _manual.r * _parameters.man_yaw_scale + _parameters.trim_yaw;
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z;
}
_actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_applied;
_actuators.control[actuator_controls_s::INDEX_SPOILERS] = _manual.aux1;
_actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = _flaperons_applied;
// FIXME: this should use _vcontrol_mode.landing_gear_pos in the future
_actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = _actuators.control[actuator_controls_s::INDEX_YAW] - _parameters.trim_yaw + _parameters.trim_steer + _manual.aux3;
/* lazily publish the setpoint only once available */
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _ctrl_state.timestamp;
_actuators_airframe.timestamp = hrt_absolute_time();
_actuators_airframe.timestamp_sample = _ctrl_state.timestamp;
/* Only publish if any of the proper modes are enabled */
if (_vcontrol_mode.flag_control_rates_enabled ||
_vcontrol_mode.flag_control_attitude_enabled ||
_vcontrol_mode.flag_control_manual_enabled) {
/* publish the actuator controls */
if (_actuators_0_pub != nullptr) {
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
} else if (_actuators_id) {
_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
}
if (_actuators_2_pub != nullptr) {
/* publish the actuator controls*/
orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe);
} else {
/* advertise and publish */
_actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe);
}
}
}
loop_counter++;
perf_end(_loop_perf);
}
warnx("exiting.\n");
_control_task = -1;
_task_running = false;
}
int matlab_csv_serial_thread_main(int argc, char *argv[])
{
if (argc < 2) {
errx(1, "need a serial port name as argument");
}
const char* uart_name = argv[1];
warnx("opening port %s", uart_name);
int serial_fd = open(uart_name, O_RDWR | O_NOCTTY);
unsigned speed = 921600;
if (serial_fd < 0) {
err(1, "failed to open port: %s", uart_name);
}
/* Try to set baud rate */
struct termios uart_config;
int termios_state;
/* Back up the original uart configuration to restore it after exit */
if ((termios_state = tcgetattr(serial_fd, &uart_config)) < 0) {
warnx("ERR GET CONF %s: %d\n", uart_name, termios_state);
close(serial_fd);
return -1;
}
/* Clear ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag &= ~ONLCR;
/* USB serial is indicated by /dev/ttyACM0*/
if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) {
/* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
warnx("ERR SET BAUD %s: %d\n", uart_name, termios_state);
close(serial_fd);
return -1;
}
}
if ((termios_state = tcsetattr(serial_fd, TCSANOW, &uart_config)) < 0) {
warnx("ERR SET CONF %s\n", uart_name);
close(serial_fd);
return -1;
}
/* subscribe to vehicle status, attitude, sensors and flow*/
struct accel_report accel0;
struct accel_report accel1;
struct gyro_report gyro0;
struct gyro_report gyro1;
/* subscribe to parameter changes */
int accel0_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
int accel1_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 1);
int gyro0_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 0);
int gyro1_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 1);
thread_running = true;
while (!thread_should_exit)
{
/*This runs at the rate of the sensors */
struct pollfd fds[] = {
{ .fd = accel0_sub, .events = POLLIN }
};
/* wait for a sensor update, check for exit condition every 500 ms */
int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 500);
if (ret < 0)
{
/* poll error, ignore */
}
else if (ret == 0)
{
/* no return value, ignore */
warnx("no sensor data");
}
else
{
/* accel0 update available? */
if (fds[0].revents & POLLIN)
{
orb_copy(ORB_ID(sensor_accel), accel0_sub, &accel0);
orb_copy(ORB_ID(sensor_accel), accel1_sub, &accel1);
orb_copy(ORB_ID(sensor_gyro), gyro0_sub, &gyro0);
orb_copy(ORB_ID(sensor_gyro), gyro1_sub, &gyro1);
// write out on accel 0, but collect for all other sensors as they have updates
dprintf(serial_fd, "%llu,%d,%d,%d,%d,%d,%d\n", accel0.timestamp, (int)accel0.x_raw, (int)accel0.y_raw, (int)accel0.z_raw,
(int)accel1.x_raw, (int)accel1.y_raw, (int)accel1.z_raw);
}
}
}
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");
}
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));
_battery_status_sub = orb_subscribe(ORB_ID(battery_status));
_gyro_count = math::min(orb_group_count(ORB_ID(sensor_gyro)), MAX_GYRO_COUNT);
for (unsigned s = 0; s < _gyro_count; s++) {
_sensor_gyro_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
}
_sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
/* initialize parameters cache */
parameters_update();
/* wakeup source: gyro data from sensor selected by the sensor app */
px4_pollfd_struct_t poll_fds = {};
poll_fds.fd = _sensor_gyro_sub[_selected_gyro];
poll_fds.events = POLLIN;
while (!_task_should_exit) {
/* wait for up to 100ms for data */
int pret = px4_poll(&poll_fds, 1, 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 gyro changes */
if (poll_fds.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 gyro data */
orb_copy(ORB_ID(sensor_gyro), _sensor_gyro_sub[_selected_gyro], &_sensor_gyro);
/* 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();
battery_status_poll();
control_state_poll();
sensor_correction_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 (_v_control_mode.flag_control_rattitude_enabled) {
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.control[7] = _v_att_sp.landing_gear;
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _ctrl_state.timestamp;
/* scale effort by battery status */
if (_params.bat_scale_en && _battery_status.scale > 0.0f) {
for (int i = 0; i < 4; i++) {
_actuators.control[i] *= _battery_status.scale;
}
}
_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);
}
}
if (_v_control_mode.flag_control_termination_enabled) {
if (!_vehicle_status.is_vtol) {
_rates_sp.zero();
_rates_int.zero();
_thrust_sp = 0.0f;
_att_control.zero();
/* publish actuator controls */
_actuators.control[0] = 0.0f;
_actuators.control[1] = 0.0f;
_actuators.control[2] = 0.0f;
_actuators.control[3] = 0.0f;
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _ctrl_state.timestamp;
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);
}
}
_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();
/* 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);
}
/* 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);
}
}
}
}
perf_end(_loop_perf);
}
_control_task = -1;
return;
}
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;
int current_value = t.val;
int num_missed = 0;
// timings has to be on the heap to keep frame size below 2048 bytes
unsigned *timings = new unsigned[maxruns];
unsigned timing_min = 9999999, timing_max = 0;
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) {
PX4_ERR("poll error %d, %d", pret, errno);
continue;
}
num_missed += t.val - current_value - 1;
current_value = t.val;
unsigned elt = (unsigned)hrt_elapsed_time(&t.time);
latency_integral += elt;
timings[i] = elt;
if (elt > timing_max) {
timing_max = elt;
}
if (elt < timing_min) {
timing_min = 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_STORAGEDIR"/uorb_timings%u.txt", timingsgroup);
FILE *f = fopen(fname, "w");
if (f == nullptr) {
PX4_ERR("Error opening file!");
delete[] timings;
return PX4_ERROR;
}
for (unsigned i = 0; i < maxruns; i++) {
fprintf(f, "%u\n", timings[i]);
}
fclose(f);
}
float std_dev = 0.f;
float mean = latency_integral / maxruns;
for (unsigned i = 0; i < maxruns; i++) {
float diff = (float)timings[i] - mean;
std_dev += diff * diff;
}
delete[] timings;
PX4_INFO("mean: %8.4f us", static_cast<double>(mean));
PX4_INFO("std dev: %8.4f us", static_cast<double>(sqrtf(std_dev / (maxruns - 1))));
PX4_INFO("min: %3i us", timing_min);
PX4_INFO("max: %3i us", timing_max);
PX4_INFO("missed topic updates: %i", num_missed);
pubsubtest_passed = true;
if (static_cast<float>(latency_integral / maxruns) > 100.0f) {
pubsubtest_res = PX4_ERROR;
} else {
pubsubtest_res = PX4_OK;
}
return pubsubtest_res;
}
void
FixedwingAttitudeControl::task_main()
{
/*
* do subscriptions
*/
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vcontrol_mode_sub, 200);
/* do not limit the attitude updates in order to minimize latency.
* actuator outputs are still limited by the individual drivers
* properly to not saturate IO or physical limitations */
parameters_update();
/* get an initial update for all sensor and status data */
vehicle_airspeed_poll();
vehicle_setpoint_poll();
vehicle_accel_poll();
vehicle_control_mode_poll();
vehicle_manual_poll();
vehicle_status_poll();
/* wakeup source(s) */
struct pollfd fds[2];
/* Setup of loop */
fds[0].fd = _params_sub;
fds[0].events = POLLIN;
fds[1].fd = _att_sub;
fds[1].events = POLLIN;
_task_running = true;
while (!_task_should_exit) {
static int loop_counter = 0;
/* wait for up to 500ms 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 error %d, %d", pret, errno);
continue;
}
perf_begin(_loop_perf);
/* 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 controller if attitude changed */
if (fds[1].revents & POLLIN) {
static uint64_t last_run = 0;
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
/* guard against too large deltaT's */
if (deltaT > 1.0f)
deltaT = 0.01f;
/* load local copies */
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) {
/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
*
* Since the VTOL airframe is initialized as a multicopter we need to
* modify the estimated attitude for the fixed wing operation.
* Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around
* the pitch axis compared to the neutral position of the vehicle in multicopter mode
* we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix.
* Additionally, in order to get the correct sign of the pitch, we need to multiply
* the new x axis of the rotation matrix with -1
*
* original: modified:
*
* Rxx Ryx Rzx -Rzx Ryx Rxx
* Rxy Ryy Rzy -Rzy Ryy Rxy
* Rxz Ryz Rzz -Rzz Ryz Rxz
* */
math::Matrix<3, 3> R; //original rotation matrix
math::Matrix<3, 3> R_adapted; //modified rotation matrix
R.set(_att.R);
R_adapted.set(_att.R);
/* move z to x */
R_adapted(0, 0) = R(0, 2);
R_adapted(1, 0) = R(1, 2);
R_adapted(2, 0) = R(2, 2);
/* move x to z */
R_adapted(0, 2) = R(0, 0);
R_adapted(1, 2) = R(1, 0);
R_adapted(2, 2) = R(2, 0);
/* change direction of pitch (convert to right handed system) */
R_adapted(0, 0) = -R_adapted(0, 0);
R_adapted(1, 0) = -R_adapted(1, 0);
R_adapted(2, 0) = -R_adapted(2, 0);
math::Vector<3> euler_angles; //adapted euler angles for fixed wing operation
euler_angles = R_adapted.to_euler();
/* fill in new attitude data */
_att.roll = euler_angles(0);
_att.pitch = euler_angles(1);
_att.yaw = euler_angles(2);
PX4_R(_att.R, 0, 0) = R_adapted(0, 0);
PX4_R(_att.R, 0, 1) = R_adapted(0, 1);
PX4_R(_att.R, 0, 2) = R_adapted(0, 2);
PX4_R(_att.R, 1, 0) = R_adapted(1, 0);
PX4_R(_att.R, 1, 1) = R_adapted(1, 1);
PX4_R(_att.R, 1, 2) = R_adapted(1, 2);
PX4_R(_att.R, 2, 0) = R_adapted(2, 0);
PX4_R(_att.R, 2, 1) = R_adapted(2, 1);
PX4_R(_att.R, 2, 2) = R_adapted(2, 2);
/* lastly, roll- and yawspeed have to be swaped */
float helper = _att.rollspeed;
_att.rollspeed = -_att.yawspeed;
_att.yawspeed = helper;
}
vehicle_airspeed_poll();
vehicle_setpoint_poll();
vehicle_accel_poll();
vehicle_control_mode_poll();
vehicle_manual_poll();
global_pos_poll();
vehicle_status_poll();
/* lock integrator until control is started */
bool lock_integrator;
if (_vcontrol_mode.flag_control_attitude_enabled && !_vehicle_status.is_rotary_wing) {
lock_integrator = false;
} else {
lock_integrator = true;
}
/* Simple handling of failsafe: deploy parachute if failsafe is on */
if (_vcontrol_mode.flag_control_termination_enabled) {
_actuators_airframe.control[7] = 1.0f;
//warnx("_actuators_airframe.control[1] = 1.0f;");
} else {
_actuators_airframe.control[7] = 0.0f;
//warnx("_actuators_airframe.control[1] = -1.0f;");
}
/* default flaps to center */
float flaps_control = 0.0f;
/* map flaps by default to manual if valid */
if (isfinite(_manual.flaps)) {
flaps_control = _manual.flaps;
}
/* decide if in stabilized or full manual control */
if (_vcontrol_mode.flag_control_attitude_enabled) {
/* scale around tuning airspeed */
float airspeed;
/* if airspeed is not updating, we assume the normal average speed */
if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
airspeed = _parameters.airspeed_trim;
if (nonfinite) {
perf_count(_nonfinite_input_perf);
}
} else {
/* prevent numerical drama by requiring 0.5 m/s minimal speed */
airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s);
}
/*
* For scaling our actuators using anything less than the min (close to stall)
* speed doesn't make any sense - its the strongest reasonable deflection we
* want to do in flight and its the baseline a human pilot would choose.
*
* Forcing the scaling to this value allows reasonable handheld tests.
*/
float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed);
float roll_sp = _parameters.rollsp_offset_rad;
float pitch_sp = _parameters.pitchsp_offset_rad;
float yaw_manual = 0.0f;
float throttle_sp = 0.0f;
/* Read attitude setpoint from uorb if
* - velocity control or position control is enabled (pos controller is running)
* - manual control is disabled (another app may send the setpoint, but it should
* for sure not be set from the remote control values)
*/
if (_vcontrol_mode.flag_control_auto_enabled ||
!_vcontrol_mode.flag_control_manual_enabled) {
/* read in attitude setpoint from attitude setpoint uorb topic */
roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
throttle_sp = _att_sp.thrust;
/* reset integrals where needed */
if (_att_sp.roll_reset_integral) {
_roll_ctrl.reset_integrator();
}
if (_att_sp.pitch_reset_integral) {
_pitch_ctrl.reset_integrator();
}
if (_att_sp.yaw_reset_integral) {
_yaw_ctrl.reset_integrator();
}
} else if (_vcontrol_mode.flag_control_velocity_enabled) {
/* the pilot does not want to change direction,
* take straight attitude setpoint from position controller
*/
if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) {
roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
} else {
roll_sp = (_manual.y * _parameters.man_roll_max)
+ _parameters.rollsp_offset_rad;
}
pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
throttle_sp = _att_sp.thrust;
/* reset integrals where needed */
if (_att_sp.roll_reset_integral) {
_roll_ctrl.reset_integrator();
}
if (_att_sp.pitch_reset_integral) {
_pitch_ctrl.reset_integrator();
}
if (_att_sp.yaw_reset_integral) {
_yaw_ctrl.reset_integrator();
}
} else if (_vcontrol_mode.flag_control_altitude_enabled) {
/*
* Velocity should be controlled and manual is enabled.
*/
roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad;
pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
throttle_sp = _att_sp.thrust;
/* reset integrals where needed */
if (_att_sp.roll_reset_integral) {
_roll_ctrl.reset_integrator();
}
if (_att_sp.pitch_reset_integral) {
_pitch_ctrl.reset_integrator();
}
if (_att_sp.yaw_reset_integral) {
_yaw_ctrl.reset_integrator();
}
} else {
/*
* Scale down roll and pitch as the setpoints are radians
* and a typical remote can only do around 45 degrees, the mapping is
* -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch)
*
* With this mapping the stick angle is a 1:1 representation of
* the commanded attitude.
*
* The trim gets subtracted here from the manual setpoint to get
* the intended attitude setpoint. Later, after the rate control step the
* trim is added again to the control signal.
*/
roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad;
pitch_sp = -(_manual.x * _parameters.man_pitch_max) + _parameters.pitchsp_offset_rad;
/* allow manual control of rudder deflection */
yaw_manual = _manual.r;
throttle_sp = _manual.z;
/*
* in manual mode no external source should / does emit attitude setpoints.
* emit the manual setpoint here to allow attitude controller tuning
* in attitude control mode.
*/
struct vehicle_attitude_setpoint_s att_sp;
att_sp.timestamp = hrt_absolute_time();
att_sp.roll_body = roll_sp;
att_sp.pitch_body = pitch_sp;
att_sp.yaw_body = 0.0f - _parameters.trim_yaw;
att_sp.thrust = throttle_sp;
/* lazily publish the setpoint only once available */
if (_attitude_sp_pub > 0 && !_vehicle_status.is_rotary_wing) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp);
} else if (_attitude_sp_pub < 0 && !_vehicle_status.is_rotary_wing) {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
}
}
/* If the aircraft is on ground reset the integrators */
if (_vehicle_status.condition_landed || _vehicle_status.is_rotary_wing) {
_roll_ctrl.reset_integrator();
_pitch_ctrl.reset_integrator();
_yaw_ctrl.reset_integrator();
}
/* Prepare speed_body_u and speed_body_w */
float speed_body_u = 0.0f;
float speed_body_v = 0.0f;
float speed_body_w = 0.0f;
if(_att.R_valid) {
speed_body_u = PX4_R(_att.R, 0, 0) * _global_pos.vel_n + PX4_R(_att.R, 1, 0) * _global_pos.vel_e + PX4_R(_att.R, 2, 0) * _global_pos.vel_d;
speed_body_v = PX4_R(_att.R, 0, 1) * _global_pos.vel_n + PX4_R(_att.R, 1, 1) * _global_pos.vel_e + PX4_R(_att.R, 2, 1) * _global_pos.vel_d;
speed_body_w = PX4_R(_att.R, 0, 2) * _global_pos.vel_n + PX4_R(_att.R, 1, 2) * _global_pos.vel_e + PX4_R(_att.R, 2, 2) * _global_pos.vel_d;
} else {
if (_debug && loop_counter % 10 == 0) {
warnx("Did not get a valid R\n");
}
}
/* Prepare data for attitude controllers */
struct ECL_ControlData control_input = {};
control_input.roll = _att.roll;
control_input.pitch = _att.pitch;
control_input.yaw = _att.yaw;
control_input.roll_rate = _att.rollspeed;
control_input.pitch_rate = _att.pitchspeed;
control_input.yaw_rate = _att.yawspeed;
control_input.speed_body_u = speed_body_u;
control_input.speed_body_v = speed_body_v;
control_input.speed_body_w = speed_body_w;
control_input.acc_body_x = _accel.x;
control_input.acc_body_y = _accel.y;
control_input.acc_body_z = _accel.z;
control_input.roll_setpoint = roll_sp;
control_input.pitch_setpoint = pitch_sp;
control_input.airspeed_min = _parameters.airspeed_min;
control_input.airspeed_max = _parameters.airspeed_max;
control_input.airspeed = airspeed;
control_input.scaler = airspeed_scaling;
control_input.lock_integrator = lock_integrator;
/* Run attitude controllers */
if (isfinite(roll_sp) && isfinite(pitch_sp)) {
_roll_ctrl.control_attitude(control_input);
_pitch_ctrl.control_attitude(control_input);
_yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude
/* Update input data for rate controllers */
control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate();
control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate();
control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate();
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
float roll_u = _roll_ctrl.control_bodyrate(control_input);
_actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
if (!isfinite(roll_u)) {
_roll_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (_debug && loop_counter % 10 == 0) {
warnx("roll_u %.4f", (double)roll_u);
}
}
float pitch_u = _pitch_ctrl.control_bodyrate(control_input);
_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
if (!isfinite(pitch_u)) {
_pitch_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (_debug && loop_counter % 10 == 0) {
warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
" airspeed %.4f, airspeed_scaling %.4f,"
" roll_sp %.4f, pitch_sp %.4f,"
" _roll_ctrl.get_desired_rate() %.4f,"
" _pitch_ctrl.get_desired_rate() %.4f"
" att_sp.roll_body %.4f",
(double)pitch_u, (double)_yaw_ctrl.get_desired_rate(),
(double)airspeed, (double)airspeed_scaling,
(double)roll_sp, (double)pitch_sp,
(double)_roll_ctrl.get_desired_rate(),
(double)_pitch_ctrl.get_desired_rate(),
(double)_att_sp.roll_body);
}
}
float yaw_u = _yaw_ctrl.control_bodyrate(control_input);
_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
/* add in manual rudder control */
_actuators.control[2] += yaw_manual;
if (!isfinite(yaw_u)) {
_yaw_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (_debug && loop_counter % 10 == 0) {
warnx("yaw_u %.4f", (double)yaw_u);
}
}
/* throttle passed through if it is finite and if no engine failure was
* detected */
_actuators.control[3] = (isfinite(throttle_sp) &&
!(_vehicle_status.engine_failure ||
_vehicle_status.engine_failure_cmd)) ?
throttle_sp : 0.0f;
if (!isfinite(throttle_sp)) {
if (_debug && loop_counter % 10 == 0) {
warnx("throttle_sp %.4f", (double)throttle_sp);
}
}
} else {
perf_count(_nonfinite_input_perf);
if (_debug && loop_counter % 10 == 0) {
warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
}
}
/*
* Lazily publish the rate setpoint (for analysis, the actuators are published below)
* only once available
*/
_rates_sp.roll = _roll_ctrl.get_desired_rate();
_rates_sp.pitch = _pitch_ctrl.get_desired_rate();
_rates_sp.yaw = _yaw_ctrl.get_desired_rate();
_rates_sp.timestamp = hrt_absolute_time();
if (_rate_sp_pub > 0) {
/* publish the attitude rates setpoint */
orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp);
} else if (_rates_sp_id) {
/* advertise the attitude rates setpoint */
_rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp);
}
} else {
/* manual/direct control */
_actuators.control[actuator_controls_s::INDEX_ROLL] = _manual.y + _parameters.trim_roll;
_actuators.control[actuator_controls_s::INDEX_PITCH] = -_manual.x + _parameters.trim_pitch;
_actuators.control[actuator_controls_s::INDEX_YAW] = _manual.r + _parameters.trim_yaw;
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z;
}
_actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_control;
_actuators.control[5] = _manual.aux1;
_actuators.control[6] = _manual.aux2;
_actuators.control[7] = _manual.aux3;
/* lazily publish the setpoint only once available */
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _att.timestamp;
_actuators_airframe.timestamp = hrt_absolute_time();
_actuators_airframe.timestamp_sample = _att.timestamp;
/* Only publish if any of the proper modes are enabled */
if(_vcontrol_mode.flag_control_rates_enabled ||
_vcontrol_mode.flag_control_attitude_enabled ||
_vcontrol_mode.flag_control_manual_enabled)
{
/* publish the actuator controls */
if (_actuators_0_pub > 0) {
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
} else if (_actuators_id) {
_actuators_0_pub= orb_advertise(_actuators_id, &_actuators);
}
if (_actuators_2_pub > 0) {
/* publish the actuator controls*/
orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe);
} else {
/* advertise and publish */
_actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe);
}
}
}
loop_counter++;
perf_end(_loop_perf);
}
warnx("exiting.\n");
_control_task = -1;
_task_running = false;
_exit(0);
}
int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id)
{
/* 45 seconds */
uint64_t calibration_interval = 25 * 1000 * 1000;
/* maximum 500 values */
const unsigned int calibration_maxcount = 240;
unsigned int calibration_counter;
float *x = NULL;
float *y = NULL;
float *z = NULL;
char str[30];
int res = OK;
/* allocate memory */
mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20);
x = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
y = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
z = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
if (x == NULL || y == NULL || z == NULL) {
mavlink_and_console_log_critical(mavlink_fd, "ERROR: out of memory");
/* clean up */
if (x != NULL) {
free(x);
}
if (y != NULL) {
free(y);
}
if (z != NULL) {
free(z);
}
res = ERROR;
return res;
}
if (res == OK) {
int sub_mag = orb_subscribe_multi(ORB_ID(sensor_mag), s);
if (sub_mag < 0) {
mavlink_and_console_log_critical(mavlink_fd, "No mag found, abort");
res = ERROR;
} else {
struct mag_report mag;
/* limit update rate to get equally spaced measurements over time (in ms) */
orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
/* calibrate offsets */
uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
unsigned poll_errcount = 0;
mavlink_and_console_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down");
calibration_counter = 0U;
while (hrt_absolute_time() < calibration_deadline &&
calibration_counter < calibration_maxcount) {
/* wait blocking for new data */
struct pollfd fds[1];
fds[0].fd = sub_mag;
fds[0].events = POLLIN;
int poll_ret = poll(fds, 1, 1000);
if (poll_ret > 0) {
orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
x[calibration_counter] = mag.x;
y[calibration_counter] = mag.y;
z[calibration_counter] = mag.z;
calibration_counter++;
if (calibration_counter % (calibration_maxcount / 20) == 0) {
mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount);
}
} else {
poll_errcount++;
}
if (poll_errcount > 1000) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
res = ERROR;
break;
}
}
close(sub_mag);
}
}
float sphere_x;
float sphere_y;
float sphere_z;
float sphere_radius;
if (res == OK && calibration_counter > (calibration_maxcount / 2)) {
/* sphere fit */
mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70);
sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 80);
if (!isfinite(sphere_x) || !isfinite(sphere_y) || !isfinite(sphere_z)) {
mavlink_and_console_log_critical(mavlink_fd, "ERROR: NaN in sphere fit");
res = ERROR;
}
}
if (x != NULL) {
free(x);
}
if (y != NULL) {
free(y);
}
if (z != NULL) {
free(z);
}
if (res == OK) {
/* apply calibration and set parameters */
struct mag_scale mscale;
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
int fd = open(str, 0);
res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale);
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, "ERROR: failed to get current calibration");
}
if (res == OK) {
mscale.x_offset = sphere_x;
mscale.y_offset = sphere_y;
mscale.z_offset = sphere_z;
res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale);
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
}
}
close(fd);
if (res == OK) {
bool failed = false;
/* set parameters */
(void)sprintf(str, "CAL_MAG%u_ID", s);
failed |= (OK != param_set(param_find(str), &(device_id)));
(void)sprintf(str, "CAL_MAG%u_XOFF", s);
failed |= (OK != param_set(param_find(str), &(mscale.x_offset)));
(void)sprintf(str, "CAL_MAG%u_YOFF", s);
failed |= (OK != param_set(param_find(str), &(mscale.y_offset)));
(void)sprintf(str, "CAL_MAG%u_ZOFF", s);
failed |= (OK != param_set(param_find(str), &(mscale.z_offset)));
(void)sprintf(str, "CAL_MAG%u_XSCALE", s);
failed |= (OK != param_set(param_find(str), &(mscale.x_scale)));
(void)sprintf(str, "CAL_MAG%u_YSCALE", s);
failed |= (OK != param_set(param_find(str), &(mscale.y_scale)));
(void)sprintf(str, "CAL_MAG%u_ZSCALE", s);
failed |= (OK != param_set(param_find(str), &(mscale.z_scale)));
if (failed) {
res = ERROR;
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
}
mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 90);
}
mavlink_and_console_log_info(mavlink_fd, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
(double)mscale.y_offset, (double)mscale.z_offset);
mavlink_and_console_log_info(mavlink_fd, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
(double)mscale.y_scale, (double)mscale.z_scale);
}
return res;
}
calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
{
calibrate_return result = calibrate_return_ok;
mag_worker_data_t worker_data;
worker_data.mavlink_log_pub = mavlink_log_pub;
worker_data.done_count = 0;
worker_data.calibration_points_perside = calibration_total_points / calibration_sides;
worker_data.calibration_interval_perside_seconds = calibraton_duration_seconds / calibration_sides;
worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;
// Collect: Right-side up, Left Side, Nose down
worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = false;
worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] = false;
worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = false;
worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = true;
worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = true;
worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = true;
calibration_log_info(mavlink_log_pub,
"[cal] %s side done, rotate to a different side",
detect_orientation_str(DETECT_ORIENTATION_TAIL_DOWN));
usleep(100000);
calibration_log_info(mavlink_log_pub,
"[cal] %s side done, rotate to a different side",
detect_orientation_str(DETECT_ORIENTATION_TAIL_DOWN));
usleep(100000);
calibration_log_info(mavlink_log_pub,
"[cal] %s side done, rotate to a different side",
detect_orientation_str(DETECT_ORIENTATION_UPSIDE_DOWN));
usleep(100000);
calibration_log_info(mavlink_log_pub,
"[cal] %s side done, rotate to a different side",
detect_orientation_str(DETECT_ORIENTATION_UPSIDE_DOWN));
usleep(100000);
calibration_log_info(mavlink_log_pub,
"[cal] %s side done, rotate to a different side",
detect_orientation_str(DETECT_ORIENTATION_RIGHT));
usleep(100000);
calibration_log_info(mavlink_log_pub,
"[cal] %s side done, rotate to a different side",
detect_orientation_str(DETECT_ORIENTATION_RIGHT));
usleep(100000);
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
// Initialize to no subscription
worker_data.sub_mag[cur_mag] = -1;
// Initialize to no memory allocated
worker_data.x[cur_mag] = NULL;
worker_data.y[cur_mag] = NULL;
worker_data.z[cur_mag] = NULL;
worker_data.calibration_counter_total[cur_mag] = 0;
}
const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside;
char str[30];
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
worker_data.x[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
worker_data.y[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
worker_data.z[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) {
calibration_log_critical(mavlink_log_pub, "[cal] ERROR: out of memory");
result = calibrate_return_error;
}
}
// Setup subscriptions to mag sensors
if (result == calibrate_return_ok) {
// We should not try to subscribe if the topic doesn't actually exist and can be counted.
const unsigned mag_count = orb_group_count(ORB_ID(sensor_mag));
for (unsigned cur_mag = 0; cur_mag < mag_count; cur_mag++) {
// Mag in this slot is available
worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag);
#ifdef __PX4_QURT
// For QURT respectively the driver framework, we need to get the device ID by copying one report.
struct mag_report mag_report;
orb_copy(ORB_ID(sensor_mag), worker_data.sub_mag[cur_mag], &mag_report);
device_ids[cur_mag] = mag_report.device_id;
#endif
if (worker_data.sub_mag[cur_mag] < 0) {
calibration_log_critical(mavlink_log_pub, "[cal] Mag #%u not found, abort", cur_mag);
result = calibrate_return_error;
break;
}
if (device_ids[cur_mag] != 0) {
// Get priority
int32_t prio;
orb_priority(worker_data.sub_mag[cur_mag], &prio);
if (prio > device_prio_max) {
device_prio_max = prio;
device_id_primary = device_ids[cur_mag];
}
} else {
calibration_log_critical(mavlink_log_pub, "[cal] Mag #%u no device id, abort", cur_mag);
result = calibrate_return_error;
break;
}
}
}
// Limit update rate to get equally spaced measurements over time (in ms)
if (result == calibrate_return_ok) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
// Mag in this slot is available
unsigned int orb_interval_msecs = (worker_data.calibration_interval_perside_useconds / 1000) / worker_data.calibration_points_perside;
//calibration_log_info(mavlink_log_pub, "Orb interval %u msecs", orb_interval_msecs);
orb_set_interval(worker_data.sub_mag[cur_mag], orb_interval_msecs);
}
}
}
if (result == calibrate_return_ok) {
int cancel_sub = calibrate_cancel_subscribe();
result = calibrate_from_orientation(mavlink_log_pub, // uORB handle to write output
cancel_sub, // Subscription to vehicle_command for cancel support
worker_data.side_data_collected, // Sides to calibrate
mag_calibration_worker, // Calibration worker
&worker_data, // Opaque data for calibration worked
true); // true: lenient still detection
calibrate_cancel_unsubscribe(cancel_sub);
}
// Close subscriptions
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (worker_data.sub_mag[cur_mag] >= 0) {
px4_close(worker_data.sub_mag[cur_mag]);
}
}
// Calculate calibration values for each mag
float sphere_x[max_mags];
float sphere_y[max_mags];
float sphere_z[max_mags];
float sphere_radius[max_mags];
// Sphere fit the data to get calibration values
if (result == calibrate_return_ok) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
// Mag in this slot is available and we should have values for it to calibrate
sphere_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag],
worker_data.calibration_counter_total[cur_mag],
100, 0.0f,
&sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag],
&sphere_radius[cur_mag]);
if (!PX4_ISFINITE(sphere_x[cur_mag]) || !PX4_ISFINITE(sphere_y[cur_mag]) || !PX4_ISFINITE(sphere_z[cur_mag])) {
calibration_log_emergency(mavlink_log_pub, "ERROR: Retry calibration (sphere NaN, #%u)", cur_mag);
result = calibrate_return_error;
}
if (fabsf(sphere_x[cur_mag]) > MAG_MAX_OFFSET_LEN ||
fabsf(sphere_y[cur_mag]) > MAG_MAX_OFFSET_LEN ||
fabsf(sphere_z[cur_mag]) > MAG_MAX_OFFSET_LEN) {
calibration_log_emergency(mavlink_log_pub, "ERROR: Replace %s mag fault", (internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
calibration_log_info(mavlink_log_pub, "Excessive offsets: %8.4f, %8.4f, %8.4f, #%u", (double)sphere_x[cur_mag],
(double)sphere_y[cur_mag], (double)sphere_z[cur_mag], cur_mag);
result = calibrate_return_ok;
}
}
}
}
// Print uncalibrated data points
if (result == calibrate_return_ok) {
// DO NOT REMOVE! Critical validation data!
// printf("RAW DATA:\n--------------------\n");
// for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
// if (worker_data.calibration_counter_total[cur_mag] == 0) {
// continue;
// }
// printf("RAW: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]);
// for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
// float x = worker_data.x[cur_mag][i];
// float y = worker_data.y[cur_mag][i];
// float z = worker_data.z[cur_mag][i];
// printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z);
// }
// printf(">>>>>>>\n");
// }
// printf("CALIBRATED DATA:\n--------------------\n");
// for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
// if (worker_data.calibration_counter_total[cur_mag] == 0) {
// continue;
// }
// printf("Calibrated: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]);
// for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
// float x = worker_data.x[cur_mag][i] - sphere_x[cur_mag];
// float y = worker_data.y[cur_mag][i] - sphere_y[cur_mag];
// float z = worker_data.z[cur_mag][i] - sphere_z[cur_mag];
// printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z);
// }
// printf("SPHERE RADIUS: %8.4f\n", (double)sphere_radius[cur_mag]);
// printf(">>>>>>>\n");
// }
}
// Data points are no longer needed
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
free(worker_data.x[cur_mag]);
free(worker_data.y[cur_mag]);
free(worker_data.z[cur_mag]);
}
if (result == calibrate_return_ok) {
(void)param_set_no_notification(param_find("CAL_MAG_PRIME"), &(device_id_primary));
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
struct mag_calibration_s mscale;
#ifndef __PX4_QURT
int fd_mag = -1;
// Set new scale
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
fd_mag = px4_open(str, 0);
if (fd_mag < 0) {
calibration_log_critical(mavlink_log_pub, "[cal] ERROR: unable to open mag device #%u", cur_mag);
result = calibrate_return_error;
}
if (result == calibrate_return_ok) {
if (px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != OK) {
calibration_log_critical(mavlink_log_pub, "[cal] ERROR: failed to get current calibration #%u", cur_mag);
result = calibrate_return_error;
}
}
#endif
if (result == calibrate_return_ok) {
mscale.x_offset = sphere_x[cur_mag];
mscale.y_offset = sphere_y[cur_mag];
mscale.z_offset = sphere_z[cur_mag];
#ifndef __PX4_QURT
if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, cur_mag);
result = calibrate_return_error;
}
#endif
}
#ifndef __PX4_QURT
// Mag device no longer needed
if (fd_mag >= 0) {
px4_close(fd_mag);
}
#endif
if (result == calibrate_return_ok) {
bool failed = false;
/* set parameters */
(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag])));
(void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_offset)));
(void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_offset)));
(void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_offset)));
(void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag);
// FIXME: scaling is not used right now on QURT
#ifndef __PX4_QURT
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale)));
(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale)));
(void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale)));
#endif
if (failed) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, cur_mag);
result = calibrate_return_error;
} else {
calibration_log_info(mavlink_log_pub, "[cal] mag #%u off: x:%.2f y:%.2f z:%.2f Ga",
cur_mag,
(double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset);
#ifndef __PX4_QURT
calibration_log_info(mavlink_log_pub, "[cal] mag #%u scale: x:%.2f y:%.2f z:%.2f",
cur_mag,
(double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale);
#endif
usleep(200000);
}
}
}
}
}
return result;
}
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 AttitudePositionEstimatorEKF::task_main()
{
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
_ekf = new AttPosEKF();
_filter_start_time = hrt_absolute_time();
if (!_ekf) {
warnx("OUT OF MEM!");
return;
}
/*
* do subscriptions
*/
_distance_sub = orb_subscribe(ORB_ID(distance_sensor));
_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) */
px4_pollfd_struct_t 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 = px4_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
_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_ref_offset = 0.0f;
_baro_gps_offset = 0.0f;
_baro_alt_filt = _baro.altitude;
_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
} 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, but only if it's any good
if (_gps_initialized && (_gpsIsGood || _global_pos.dead_reckoning)) {
publishGlobalPosition();
}
//Publish wind estimates
if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
publishWindEstimate();
}
}
}
}
perf_end(_loop_perf);
}
_task_running = false;
_estimator_task = -1;
return;
}
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;
}
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 };
// 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 accel_count = orb_group_count(ORB_ID(sensor_accel));
for (unsigned i = 0; i < accel_count; i++) {
worker_data.subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
if (worker_data.subs[i] < 0) {
result = calibrate_return_error;
break;
}
#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_RPI)
// For QURT respectively the driver framework, we need to get the device ID by copying one report.
struct accel_report accel_report;
orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &accel_report);
device_id[i] = accel_report.device_id;
#endif
/* store initial timestamp - used to infer which sensors are active */
struct accel_report arp = {};
(void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
timestamps[i] = arp.timestamp;
if (device_id[i] != 0) {
// Get priority
int32_t prio;
orb_priority(worker_data.subs[i], &prio);
if (prio > device_prio_max) {
device_prio_max = prio;
device_id_primary = device_id[i];
}
} else {
calibration_log_critical(mavlink_log_pub, "[cal] Accel #%u no device id, abort", i);
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]);
}
}
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, "[cal] ERROR: calibration calculation error");
break;
}
}
}
return result;
}
calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
{
calibrate_return result = calibrate_return_ok;
mag_worker_data_t worker_data;
worker_data.mavlink_fd = mavlink_fd;
worker_data.done_count = 0;
worker_data.calibration_points_perside = 40;
worker_data.calibration_interval_perside_seconds = 20;
worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;
// Collect: Right-side up, Left Side, Nose down
worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = false;
worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] = false;
worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = false;
worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = false;
worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = false;
worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = false;
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
// Initialize to no subscription
worker_data.sub_mag[cur_mag] = -1;
// Initialize to no memory allocated
worker_data.x[cur_mag] = NULL;
worker_data.y[cur_mag] = NULL;
worker_data.z[cur_mag] = NULL;
worker_data.calibration_counter_total[cur_mag] = 0;
}
const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside;
char str[30];
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
worker_data.x[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
worker_data.y[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
worker_data.z[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: out of memory");
result = calibrate_return_error;
}
}
// Setup subscriptions to mag sensors
if (result == calibrate_return_ok) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
// Mag in this slot is available
worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag);
if (worker_data.sub_mag[cur_mag] < 0) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] Mag #%u not found, abort", cur_mag);
result = calibrate_return_error;
break;
}
}
}
}
// Limit update rate to get equally spaced measurements over time (in ms)
if (result == calibrate_return_ok) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
// Mag in this slot is available
unsigned int orb_interval_msecs = (worker_data.calibration_interval_perside_useconds / 1000) / worker_data.calibration_points_perside;
//mavlink_and_console_log_info(mavlink_fd, "Orb interval %u msecs", orb_interval_msecs);
orb_set_interval(worker_data.sub_mag[cur_mag], orb_interval_msecs);
}
}
}
if (result == calibrate_return_ok) {
int cancel_sub = calibrate_cancel_subscribe();
result = calibrate_from_orientation(mavlink_fd, // Mavlink fd to write output
cancel_sub, // Subscription to vehicle_command for cancel support
worker_data.side_data_collected, // Sides to calibrate
mag_calibration_worker, // Calibration worker
&worker_data, // Opaque data for calibration worked
true); // true: lenient still detection
calibrate_cancel_unsubscribe(cancel_sub);
}
// Close subscriptions
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (worker_data.sub_mag[cur_mag] >= 0) {
px4_close(worker_data.sub_mag[cur_mag]);
}
}
// Calculate calibration values for each mag
float sphere_x[max_mags];
float sphere_y[max_mags];
float sphere_z[max_mags];
float sphere_radius[max_mags];
// Sphere fit the data to get calibration values
if (result == calibrate_return_ok) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
// Mag in this slot is available and we should have values for it to calibrate
sphere_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag],
worker_data.calibration_counter_total[cur_mag],
100, 0.0f,
&sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag],
&sphere_radius[cur_mag]);
if (!PX4_ISFINITE(sphere_x[cur_mag]) || !PX4_ISFINITE(sphere_y[cur_mag]) || !PX4_ISFINITE(sphere_z[cur_mag])) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: NaN in sphere fit for mag #%u", cur_mag);
result = calibrate_return_error;
}
}
}
}
// Print uncalibrated data points
if (result == calibrate_return_ok) {
printf("RAW DATA:\n--------------------\n");
for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
printf("RAW: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]);
for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
float x = worker_data.x[cur_mag][i];
float y = worker_data.y[cur_mag][i];
float z = worker_data.z[cur_mag][i];
printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z);
}
printf(">>>>>>>\n");
}
printf("CALIBRATED DATA:\n--------------------\n");
for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
printf("Calibrated: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]);
for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
float x = worker_data.x[cur_mag][i] - sphere_x[cur_mag];
float y = worker_data.y[cur_mag][i] - sphere_y[cur_mag];
float z = worker_data.z[cur_mag][i] - sphere_z[cur_mag];
printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z);
}
printf("SPHERE RADIUS: %8.4f\n", (double)sphere_radius[cur_mag]);
printf(">>>>>>>\n");
}
}
// Data points are no longer needed
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
free(worker_data.x[cur_mag]);
free(worker_data.y[cur_mag]);
free(worker_data.z[cur_mag]);
}
if (result == calibrate_return_ok) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
int fd_mag = -1;
struct mag_scale mscale;
// Set new scale
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
fd_mag = px4_open(str, 0);
if (fd_mag < 0) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: unable to open mag device #%u", cur_mag);
result = calibrate_return_error;
}
if (result == calibrate_return_ok) {
if (px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != OK) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: failed to get current calibration #%u", cur_mag);
result = calibrate_return_error;
}
}
if (result == calibrate_return_ok) {
mscale.x_offset = sphere_x[cur_mag];
mscale.y_offset = sphere_y[cur_mag];
mscale.z_offset = sphere_z[cur_mag];
if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, cur_mag);
result = calibrate_return_error;
}
}
// Mag device no longer needed
if (fd_mag >= 0) {
px4_close(fd_mag);
}
if (result == calibrate_return_ok) {
bool failed = false;
/* set parameters */
(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag])));
(void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_offset)));
(void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_offset)));
(void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_offset)));
(void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale)));
(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale)));
(void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale)));
if (failed) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, cur_mag);
result = calibrate_return_error;
} else {
mavlink_and_console_log_info(mavlink_fd, "[cal] mag #%u off: x:%.2f y:%.2f z:%.2f Ga",
cur_mag,
(double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset);
mavlink_and_console_log_info(mavlink_fd, "[cal] mag #%u scale: x:%.2f y:%.2f z:%.2f",
cur_mag,
(double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale);
}
}
}
}
}
return result;
}
calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
{
calibrate_return result = calibrate_return_ok;
mag_worker_data_t worker_data;
worker_data.mavlink_log_pub = mavlink_log_pub;
worker_data.done_count = 0;
worker_data.calibration_points_perside = calibration_total_points / calibration_sides;
worker_data.calibration_interval_perside_seconds = calibraton_duration_seconds / calibration_sides;
worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;
// Collect: As defined by configuration
// start with a full mask, all six bits set
uint32_t cal_mask = (1 << 6) - 1;
param_get(param_find("CAL_MAG_SIDES"), &cal_mask);
calibration_sides = 0;
for (unsigned i = 0; i < (sizeof(worker_data.side_data_collected) /
sizeof(worker_data.side_data_collected[0])); i++) {
if ((cal_mask & (1 << i)) > 0) {
// mark as missing
worker_data.side_data_collected[i] = false;
calibration_sides++;
} else {
// mark as completed from the beginning
worker_data.side_data_collected[i] = true;
calibration_log_info(mavlink_log_pub,
"[cal] %s side done, rotate to a different side",
detect_orientation_str(static_cast<enum detect_orientation_return>(i)));
usleep(100000);
}
}
for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
// Initialize to no subscription
worker_data.sub_mag[cur_mag] = -1;
// Initialize to no memory allocated
worker_data.x[cur_mag] = nullptr;
worker_data.y[cur_mag] = nullptr;
worker_data.z[cur_mag] = nullptr;
worker_data.calibration_counter_total[cur_mag] = 0;
}
const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside;
char str[30];
for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
worker_data.x[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
worker_data.y[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
worker_data.z[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
if (worker_data.x[cur_mag] == nullptr || worker_data.y[cur_mag] == nullptr || worker_data.z[cur_mag] == nullptr) {
calibration_log_critical(mavlink_log_pub, "[cal] ERROR: out of memory");
result = calibrate_return_error;
}
}
// Setup subscriptions to mag sensors
if (result == calibrate_return_ok) {
// We should not try to subscribe if the topic doesn't actually exist and can be counted.
const unsigned mag_count = orb_group_count(ORB_ID(sensor_mag));
// Warn that we will not calibrate more than max_mags magnetometers
if (mag_count > max_mags) {
calibration_log_critical(mavlink_log_pub, "[cal] Detected %u mags, but will calibrate only %u", mag_count, max_mags);
}
for (unsigned cur_mag = 0; cur_mag < mag_count && cur_mag < max_mags; cur_mag++) {
// Mag in this slot is available
worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag);
#if defined(__PX4_QURT) || defined(__PX4_POSIX_RPI) || defined(__PX4_POSIX_BEBOP)
// For QURT respectively the driver framework, we need to get the device ID by copying one report.
struct mag_report mag_report;
orb_copy(ORB_ID(sensor_mag), worker_data.sub_mag[cur_mag], &mag_report);
device_ids[cur_mag] = mag_report.device_id;
#endif
if (worker_data.sub_mag[cur_mag] < 0) {
calibration_log_critical(mavlink_log_pub, "[cal] Mag #%u not found, abort", cur_mag);
result = calibrate_return_error;
break;
}
if (device_ids[cur_mag] != 0) {
// Get priority
int32_t prio;
orb_priority(worker_data.sub_mag[cur_mag], &prio);
if (prio > device_prio_max) {
device_prio_max = prio;
device_id_primary = device_ids[cur_mag];
}
} else {
calibration_log_critical(mavlink_log_pub, "[cal] Mag #%u no device id, abort", cur_mag);
result = calibrate_return_error;
break;
}
}
}
// Limit update rate to get equally spaced measurements over time (in ms)
if (result == calibrate_return_ok) {
for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
// Mag in this slot is available
unsigned int orb_interval_msecs = (worker_data.calibration_interval_perside_useconds / 1000) /
worker_data.calibration_points_perside;
//calibration_log_info(mavlink_log_pub, "Orb interval %u msecs", orb_interval_msecs);
orb_set_interval(worker_data.sub_mag[cur_mag], orb_interval_msecs);
}
}
}
if (result == calibrate_return_ok) {
int cancel_sub = calibrate_cancel_subscribe();
result = calibrate_from_orientation(mavlink_log_pub, // uORB handle to write output
cancel_sub, // Subscription to vehicle_command for cancel support
worker_data.side_data_collected, // Sides to calibrate
mag_calibration_worker, // Calibration worker
&worker_data, // Opaque data for calibration worked
true); // true: lenient still detection
calibrate_cancel_unsubscribe(cancel_sub);
}
// Close subscriptions
for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {
if (worker_data.sub_mag[cur_mag] >= 0) {
px4_close(worker_data.sub_mag[cur_mag]);
}
}
// Calculate calibration values for each mag
float sphere_x[max_mags];
float sphere_y[max_mags];
float sphere_z[max_mags];
float sphere_radius[max_mags];
float diag_x[max_mags];
float diag_y[max_mags];
float diag_z[max_mags];
float offdiag_x[max_mags];
float offdiag_y[max_mags];
float offdiag_z[max_mags];
for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {
sphere_x[cur_mag] = 0.0f;
sphere_y[cur_mag] = 0.0f;
sphere_z[cur_mag] = 0.0f;
sphere_radius[cur_mag] = 0.2f;
diag_x[cur_mag] = 1.0f;
diag_y[cur_mag] = 1.0f;
diag_z[cur_mag] = 1.0f;
offdiag_x[cur_mag] = 0.0f;
offdiag_y[cur_mag] = 0.0f;
offdiag_z[cur_mag] = 0.0f;
}
// Sphere fit the data to get calibration values
if (result == calibrate_return_ok) {
for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
// Mag in this slot is available and we should have values for it to calibrate
ellipsoid_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag],
worker_data.calibration_counter_total[cur_mag],
100, 0.0f,
&sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag],
&sphere_radius[cur_mag],
&diag_x[cur_mag], &diag_y[cur_mag], &diag_z[cur_mag],
&offdiag_x[cur_mag], &offdiag_y[cur_mag], &offdiag_z[cur_mag]);
result = check_calibration_result(sphere_x[cur_mag], sphere_y[cur_mag], sphere_z[cur_mag],
sphere_radius[cur_mag],
diag_x[cur_mag], diag_y[cur_mag], diag_z[cur_mag],
offdiag_x[cur_mag], offdiag_y[cur_mag], offdiag_z[cur_mag],
mavlink_log_pub, cur_mag);
if (result == calibrate_return_error) {
break;
}
}
}
}
// Print uncalibrated data points
if (result == calibrate_return_ok) {
// DO NOT REMOVE! Critical validation data!
// printf("RAW DATA:\n--------------------\n");
// for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
// if (worker_data.calibration_counter_total[cur_mag] == 0) {
// continue;
// }
// printf("RAW: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]);
// for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
// float x = worker_data.x[cur_mag][i];
// float y = worker_data.y[cur_mag][i];
// float z = worker_data.z[cur_mag][i];
// printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z);
// }
// printf(">>>>>>>\n");
// }
// printf("CALIBRATED DATA:\n--------------------\n");
// for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
// if (worker_data.calibration_counter_total[cur_mag] == 0) {
// continue;
// }
// printf("Calibrated: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]);
// for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
// float x = worker_data.x[cur_mag][i] - sphere_x[cur_mag];
// float y = worker_data.y[cur_mag][i] - sphere_y[cur_mag];
// float z = worker_data.z[cur_mag][i] - sphere_z[cur_mag];
// printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z);
// }
// printf("SPHERE RADIUS: %8.4f\n", (double)sphere_radius[cur_mag]);
// printf(">>>>>>>\n");
// }
}
// Data points are no longer needed
for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
free(worker_data.x[cur_mag]);
free(worker_data.y[cur_mag]);
free(worker_data.z[cur_mag]);
}
if (result == calibrate_return_ok) {
for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
struct mag_calibration_s mscale;
mscale.x_scale = 1.0;
mscale.y_scale = 1.0;
mscale.z_scale = 1.0;
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
int fd_mag = -1;
// Set new scale
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
fd_mag = px4_open(str, 0);
if (fd_mag < 0) {
calibration_log_critical(mavlink_log_pub, "[cal] ERROR: unable to open mag device #%u", cur_mag);
result = calibrate_return_error;
}
if (result == calibrate_return_ok) {
if (px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != PX4_OK) {
calibration_log_critical(mavlink_log_pub, "[cal] ERROR: failed to get current calibration #%u", cur_mag);
result = calibrate_return_error;
}
}
#endif
if (result == calibrate_return_ok) {
mscale.x_offset = sphere_x[cur_mag];
mscale.y_offset = sphere_y[cur_mag];
mscale.z_offset = sphere_z[cur_mag];
mscale.x_scale = diag_x[cur_mag];
mscale.y_scale = diag_y[cur_mag];
mscale.z_scale = diag_z[cur_mag];
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, cur_mag);
result = calibrate_return_error;
}
#endif
}
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
// Mag device no longer needed
if (fd_mag >= 0) {
px4_close(fd_mag);
}
#endif
if (result == calibrate_return_ok) {
bool failed = false;
/* set parameters */
(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag])));
(void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.x_offset)));
(void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.y_offset)));
(void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.z_offset)));
// FIXME: scaling is not used right now on QURT
#ifndef __PX4_QURT
(void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.x_scale)));
(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.y_scale)));
(void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.z_scale)));
#endif
if (failed) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, cur_mag);
result = calibrate_return_error;
} else {
calibration_log_info(mavlink_log_pub, "[cal] mag #%u off: x:%.2f y:%.2f z:%.2f Ga",
cur_mag,
(double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset);
#ifndef __PX4_QURT
calibration_log_info(mavlink_log_pub, "[cal] mag #%u scale: x:%.2f y:%.2f z:%.2f",
cur_mag,
(double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale);
#endif
usleep(200000);
}
}
}
}
// Trigger a param set on the last step so the whole
// system updates
(void)param_set(param_find("CAL_MAG_PRIME"), &(device_id_primary));
}
return result;
}
