int do_airspeed_calibration(int mavlink_fd)
{
/* give directions */
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
const unsigned calibration_count = 2000;
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s diff_pres;
float diff_pres_offset = 0.0f;
/* Reset sensor parameters */
struct airspeed_scale airscale = {
diff_pres_offset,
1.0f,
};
bool paramreset_successful = false;
int fd = open(AIRSPEED_DEVICE_PATH, 0);
if (fd > 0) {
if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
paramreset_successful = true;
} else {
mavlink_log_critical(mavlink_fd, "airspeed offset zero failed");
}
close(fd);
}
if (!paramreset_successful) {
/* only warn if analog scaling is zero */
float analog_scaling = 0.0f;
param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
if (fabsf(analog_scaling) < 0.1f) {
mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]");
close(diff_pres_sub);
return ERROR;
}
/* set scaling offset parameter */
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
close(diff_pres_sub);
return ERROR;
}
}
unsigned calibration_counter = 0;
mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind");
usleep(500 * 1000);
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
struct pollfd fds[1];
fds[0].fd = diff_pres_sub;
fds[0].events = POLLIN;
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
diff_pres_offset += diff_pres.differential_pressure_raw_pa;
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0) {
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 80) / calibration_count);
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
close(diff_pres_sub);
return ERROR;
}
}
diff_pres_offset = diff_pres_offset / calibration_count;
if (isfinite(diff_pres_offset)) {
int fd_scale = open(AIRSPEED_DEVICE_PATH, 0);
airscale.offset_pa = diff_pres_offset;
if (fd_scale > 0) {
if (OK != ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
mavlink_log_critical(mavlink_fd, "airspeed offset update failed");
}
close(fd_scale);
}
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
close(diff_pres_sub);
return ERROR;
}
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
close(diff_pres_sub);
return ERROR;
}
} else {
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
close(diff_pres_sub);
return ERROR;
}
mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset);
/* wait 500 ms to ensure parameter propagated through the system */
usleep(500 * 1000);
calibration_counter = 0;
const int maxcount = 3000;
/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
while (calibration_counter < maxcount) {
/* wait blocking for new data */
struct pollfd fds[1];
fds[0].fd = diff_pres_sub;
fds[0].events = POLLIN;
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
calibration_counter++;
if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
if (calibration_counter % 100 == 0) {
mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)",
(int)diff_pres.differential_pressure_raw_pa);
}
continue;
}
/* do not allow negative values */
if (diff_pres.differential_pressure_raw_pa < 0.0f) {
mavlink_log_info(mavlink_fd, "negative pressure: ERROR (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa);
close(diff_pres_sub);
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
diff_pres_offset = 0.0f;
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
close(diff_pres_sub);
return ERROR;
}
/* save */
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 0);
(void)param_save_default();
close(diff_pres_sub);
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
return ERROR;
} else {
mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
break;
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
close(diff_pres_sub);
return ERROR;
}
}
if (calibration_counter == maxcount) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
close(diff_pres_sub);
return ERROR;
}
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100);
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
tune_neutral(true);
close(diff_pres_sub);
return OK;
}
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 do_airspeed_calibration(int mavlink_fd)
{
int result = OK;
unsigned calibration_counter = 0;
const unsigned maxcount = 3000;
/* give directions */
mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
const unsigned calibration_count = 2000;
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s diff_pres;
float diff_pres_offset = 0.0f;
/* Reset sensor parameters */
struct airspeed_scale airscale = {
diff_pres_offset,
1.0f,
};
bool paramreset_successful = false;
int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0);
if (fd > 0) {
if (OK == px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
paramreset_successful = true;
} else {
mavlink_log_critical(mavlink_fd, "[cal] airspeed offset zero failed");
}
px4_close(fd);
}
int cancel_sub = calibrate_cancel_subscribe();
if (!paramreset_successful) {
/* only warn if analog scaling is zero */
float analog_scaling = 0.0f;
param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
if (fabsf(analog_scaling) < 0.1f) {
mavlink_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd");
goto error_return;
}
/* set scaling offset parameter */
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
goto error_return;
}
}
mavlink_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind");
usleep(500 * 1000);
while (calibration_counter < calibration_count) {
if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
goto error_return;
}
/* wait blocking for new data */
px4_pollfd_struct_t fds[1];
fds[0].fd = diff_pres_sub;
fds[0].events = POLLIN;
int poll_ret = px4_poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
diff_pres_offset += diff_pres.differential_pressure_raw_pa;
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0) {
mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
feedback_calibration_failed(mavlink_fd);
goto error_return;
}
}
diff_pres_offset = diff_pres_offset / calibration_count;
if (PX4_ISFINITE(diff_pres_offset)) {
int fd_scale = px4_open(AIRSPEED0_DEVICE_PATH, 0);
airscale.offset_pa = diff_pres_offset;
if (fd_scale > 0) {
if (OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
mavlink_log_critical(mavlink_fd, "[cal] airspeed offset update failed");
}
px4_close(fd_scale);
}
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
goto error_return;
}
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
goto error_return;
}
} else {
feedback_calibration_failed(mavlink_fd);
goto error_return;
}
mavlink_log_critical(mavlink_fd, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
/* wait 500 ms to ensure parameter propagated through the system */
usleep(500 * 1000);
mavlink_log_critical(mavlink_fd, "[cal] Create airflow now");
calibration_counter = 0;
/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
while (calibration_counter < maxcount) {
if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
goto error_return;
}
/* wait blocking for new data */
px4_pollfd_struct_t fds[1];
fds[0].fd = diff_pres_sub;
fds[0].events = POLLIN;
int poll_ret = px4_poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
calibration_counter++;
if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
if (calibration_counter % 500 == 0) {
mavlink_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
(int)diff_pres.differential_pressure_raw_pa);
}
continue;
}
/* do not allow negative values */
if (diff_pres.differential_pressure_raw_pa < 0.0f) {
mavlink_log_info(mavlink_fd, "[cal] Negative pressure difference detected (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
mavlink_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!");
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
diff_pres_offset = 0.0f;
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
goto error_return;
}
/* save */
mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0);
(void)param_save_default();
feedback_calibration_failed(mavlink_fd);
goto error_return;
} else {
mavlink_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
break;
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
feedback_calibration_failed(mavlink_fd);
goto error_return;
}
}
if (calibration_counter == maxcount) {
feedback_calibration_failed(mavlink_fd);
goto error_return;
}
mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
tune_neutral(true);
normal_return:
calibrate_cancel_unsubscribe(cancel_sub);
px4_close(diff_pres_sub);
// This give a chance for the log messages to go out of the queue before someone else stomps on then
sleep(1);
return result;
error_return:
result = ERROR;
goto normal_return;
}
int do_level_calibration(orb_advert_t *mavlink_log_pub) {
const unsigned cal_time = 5;
const unsigned cal_hz = 100;
unsigned settle_time = 30;
bool success = false;
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "level");
param_t roll_offset_handle = param_find("SENS_BOARD_X_OFF");
param_t pitch_offset_handle = param_find("SENS_BOARD_Y_OFF");
param_t board_rot_handle = param_find("SENS_BOARD_ROT");
// save old values if calibration fails
float roll_offset_current;
float pitch_offset_current;
int32_t board_rot_current = 0;
param_get(roll_offset_handle, &roll_offset_current);
param_get(pitch_offset_handle, &pitch_offset_current);
param_get(board_rot_handle, &board_rot_current);
// give attitude some time to settle if there have been changes to the board rotation parameters
if (board_rot_current == 0 && fabsf(roll_offset_current) < FLT_EPSILON && fabsf(pitch_offset_current) < FLT_EPSILON ) {
settle_time = 0;
}
float zero = 0.0f;
param_set_no_notification(roll_offset_handle, &zero);
param_set_no_notification(pitch_offset_handle, &zero);
param_notify_changes();
px4_pollfd_struct_t fds[1];
fds[0].fd = att_sub;
fds[0].events = POLLIN;
float roll_mean = 0.0f;
float pitch_mean = 0.0f;
unsigned counter = 0;
// sleep for some time
hrt_abstime start = hrt_absolute_time();
while(hrt_elapsed_time(&start) < settle_time * 1000000) {
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (int)(90*hrt_elapsed_time(&start)/1e6f/(float)settle_time));
sleep(settle_time / 10);
}
start = hrt_absolute_time();
// average attitude for 5 seconds
while(hrt_elapsed_time(&start) < cal_time * 1000000) {
int pollret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
if (pollret <= 0) {
// attitude estimator is not running
calibration_log_critical(mavlink_log_pub, "attitude estimator not running - check system boot");
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level");
goto out;
}
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
matrix::Eulerf euler = matrix::Quatf(att.q);
roll_mean += euler.phi();
pitch_mean += euler.theta();
counter++;
}
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);
if (counter > (cal_time * cal_hz / 2 )) {
roll_mean /= counter;
pitch_mean /= counter;
} else {
calibration_log_info(mavlink_log_pub, "not enough measurements taken");
success = false;
goto out;
}
if (fabsf(roll_mean) > 0.8f ) {
calibration_log_critical(mavlink_log_pub, "excess roll angle");
} else if (fabsf(pitch_mean) > 0.8f ) {
calibration_log_critical(mavlink_log_pub, "excess pitch angle");
} else {
roll_mean *= (float)M_RAD_TO_DEG;
pitch_mean *= (float)M_RAD_TO_DEG;
param_set_no_notification(roll_offset_handle, &roll_mean);
param_set_no_notification(pitch_offset_handle, &pitch_mean);
param_notify_changes();
success = true;
}
out:
if (success) {
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "level");
return 0;
} else {
// set old parameters
param_set_no_notification(roll_offset_handle, &roll_offset_current);
param_set_no_notification(pitch_offset_handle, &pitch_offset_current);
param_notify_changes();
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level");
return 1;
}
}
void
PX4FMU::cycle()
{
if (!_initialized) {
/* force a reset of the update rate */
_current_update_rate = 0;
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
//_param_sub = orb_subscribe(ORB_ID(parameter_update));
/* initialize PWM limit lib */
pwm_limit_init(&_pwm_limit);
update_pwm_rev_mask();
#ifdef RC_SERIAL_PORT
_sbus_fd = sbus_init(RC_SERIAL_PORT, true);
#endif
_initialized = true;
}
if (_groups_subscribed != _groups_required) {
subscribe();
_groups_subscribed = _groups_required;
/* force setting update rate */
_current_update_rate = 0;
}
/*
* Adjust actuator topic update rate to keep up with
* the highest servo update rate configured.
*
* We always mix at max rate; some channels may update slower.
*/
unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate;
if (_current_update_rate != max_rate) {
_current_update_rate = max_rate;
int update_rate_in_ms = int(1000 / _current_update_rate);
/* reject faster than 500 Hz updates */
if (update_rate_in_ms < 2) {
update_rate_in_ms = 2;
}
/* reject slower than 10 Hz updates */
if (update_rate_in_ms > 100) {
update_rate_in_ms = 100;
}
//DEVICE_DEBUG("adjusted actuator update interval to %ums\n", update_rate_in_ms);
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
orb_set_interval(_control_subs[i], update_rate_in_ms);
}
}
// set to current max rate, even if we are actually checking slower/faster
_current_update_rate = max_rate;
}
/* check if anything updated */
//从mkblctrl-blctrl电子模块驱动拿数据，貌似没用到，而且poll里面也在等待定时器导致定时器卡死
int ret = 0;//::poll(_poll_fds, _poll_fds_num, 0);
/* this would be bad... */
if (ret < 0) {
DEVICE_LOG("poll error %d\n", ret);
} else if (ret == 0) {
/* timeout: no control data, switch to failsafe values */
// warnx("no PWM: failsafe\n");
} else {
/* get controls for required topics */
unsigned poll_id = 0;
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
if (_poll_fds[poll_id].revents & POLLIN) {
orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
}
poll_id++;
}
}
/* can we mix? */
if (_mixers != NULL) {
size_t num_outputs;
switch (_mode) {
case MODE_2PWM:
num_outputs = 2;
break;
case MODE_4PWM:
num_outputs = 4;
break;
case MODE_6PWM:
num_outputs = 6;
break;
case MODE_8PWM:
num_outputs = 8;
break;
default:
num_outputs = 0;
break;
}
/* do mixing */
float outputs[_max_actuators];
num_outputs = _mixers->mix(outputs, num_outputs, NULL);
/* disable unused ports by setting their output to NaN */
for (size_t i = 0; i < sizeof(outputs) / sizeof(outputs[0]); i++) {
if (i >= num_outputs) {
outputs[i] = NAN_VALUE;
}
}
uint16_t pwm_limited[_max_actuators];
/* the PWM limit call takes care of out of band errors, NaN and constrains */
pwm_limit_calc(_servo_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm,
outputs, pwm_limited, &_pwm_limit);
/* output to the servos */
for (size_t i = 0; i < num_outputs; i++) {
up_pwm_servo_set(i, pwm_limited[i]);
}
publish_pwm_outputs(pwm_limited, num_outputs);
}
}
/* check arming state */
bool updated = false;
orb_check(_armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
/* update the armed status and check that we're not locked down */
bool set_armed = (_armed.armed || _armed.prearmed) && !_armed.lockdown;
if (_servo_armed != set_armed) {
_servo_armed = set_armed;
}
/* update PWM status if armed or if disarmed PWM values are set */
bool pwm_on = (set_armed || _num_disarmed_set > 0);
if (_pwm_on != pwm_on) {
_pwm_on = pwm_on;
up_pwm_servo_arm(pwm_on);
}
}
/* TODO:F
orb_check(_param_sub, &updated);
if (updated) {
parameter_update_s pupdate;
orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate);
update_pwm_rev_mask();
}
*/
bool rc_updated = false;
#ifdef RC_SERIAL_PORT
bool sbus_failsafe, sbus_frame_drop;
uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
uint16_t raw_rc_count;
bool sbus_updated = sbus_input(_sbus_fd, &raw_rc_values[0], &raw_rc_count, &sbus_failsafe, &sbus_frame_drop,
input_rc_s::RC_INPUT_MAX_CHANNELS);
if (sbus_updated) {
// we have a new PPM frame. Publish it.
_rc_in.channel_count = raw_rc_count;
if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
_rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
}
for (uint8_t i = 0; i < _rc_in.channel_count; i++) {
_rc_in.values[i] = raw_rc_values[i];
// pilot_info("value[%d]=%d\n", i, _rc_in.values[i]);
}
_rc_in.timestamp_publication = hrt_absolute_time();
_rc_in.timestamp_last_signal = _rc_in.timestamp_publication;
_rc_in.rc_ppm_frame_length = 0;
_rc_in.rssi = (!sbus_frame_drop) ? RC_INPUT_RSSI_MAX : 0;
_rc_in.rc_failsafe = false;
_rc_in.rc_lost = false;
_rc_in.rc_lost_frame_count = 0;
_rc_in.rc_total_frame_count = 0;
rc_updated = true;
}
#endif
#ifdef HRT_PPM_CHANNEL
// see if we have new PPM input data
if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) &&
ppm_decoded_channels > 3) {
// we have a new PPM frame. Publish it.
_rc_in.channel_count = ppm_decoded_channels;
if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
_rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
}
for (uint8_t i = 0; i < _rc_in.channel_count; i++) {
_rc_in.values[i] = ppm_buffer[i];
}
_rc_in.timestamp_publication = ppm_last_valid_decode;
_rc_in.timestamp_last_signal = ppm_last_valid_decode;
_rc_in.rc_ppm_frame_length = ppm_frame_length;
_rc_in.rssi = RC_INPUT_RSSI_MAX;
_rc_in.rc_failsafe = false;
_rc_in.rc_lost = false;
_rc_in.rc_lost_frame_count = 0;
_rc_in.rc_total_frame_count = 0;
rc_updated = true;
}
#endif
if (rc_updated) {
/* lazily advertise on first publication */
if (_to_input_rc == NULL) {
_to_input_rc = orb_advertise(ORB_ID(input_rc), &_rc_in);
} else {
orb_publish(ORB_ID(input_rc), _to_input_rc, &_rc_in);
}
}
// xTimerStart(_work, (2/portTICK_PERIOD_MS));
}
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));
_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits));
/* initialize parameters cache */
parameters_update();
/* wakeup source: vehicle attitude */
struct pollfd fds[1];
fds[0].fd = _v_att_sub;
fds[0].events = POLLIN;
while (!_task_should_exit) {
/* wait for up to 100ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit */
if (pret == 0)
continue;
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("poll error %d, %d", pret, errno);
/* sleep a bit before next try */
usleep(100000);
continue;
}
perf_begin(_loop_perf);
/* run controller on attitude changes */
if (fds[0].revents & POLLIN) {
static uint64_t last_run = 0;
float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
/* guard against too small (< 2ms) and too large (> 20ms) dt's */
if (dt < 0.002f) {
dt = 0.002f;
} else if (dt > 0.02f) {
dt = 0.02f;
}
/* copy attitude topic */
orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
/* 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();
if (_v_control_mode.flag_control_attitude_enabled) {
control_attitude(dt);
/* 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 > 0) {
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 > 0) {
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] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
_actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _v_att.timestamp;
_controller_status.roll_rate_integ = _rates_int(0);
_controller_status.pitch_rate_integ = _rates_int(1);
_controller_status.yaw_rate_integ = _rates_int(2);
_controller_status.timestamp = hrt_absolute_time();
if (!_actuators_0_circuit_breaker_enabled) {
if (_actuators_0_pub > 0) {
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 > 0) {
orb_publish(ORB_ID(mc_att_ctrl_status),_controller_status_pub, &_controller_status);
} else {
_controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status);
}
}
}
perf_end(_loop_perf);
}
warnx("exit");
_control_task = -1;
_exit(0);
}
int do_accel_calibration(orb_advert_t *mavlink_log_pub)
{
#ifdef __PX4_NUTTX
int fd;
#endif
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
struct accel_calibration_s accel_scale;
accel_scale.x_offset = 0.0f;
accel_scale.x_scale = 1.0f;
accel_scale.y_offset = 0.0f;
accel_scale.y_scale = 1.0f;
accel_scale.z_offset = 0.0f;
accel_scale.z_scale = 1.0f;
int res = PX4_OK;
char str[30];
/* reset all sensors */
for (unsigned s = 0; s < max_accel_sens; s++) {
#ifdef __PX4_NUTTX
sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
/* reset all offsets to zero and all scales to one */
fd = px4_open(str, 0);
if (fd < 0) {
continue;
}
device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
px4_close(fd);
if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, s);
}
#else
(void)sprintf(str, "CAL_ACC%u_XOFF", s);
res = param_set_no_notification(param_find(str), &accel_scale.x_offset);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_YOFF", s);
res = param_set_no_notification(param_find(str), &accel_scale.y_offset);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_ZOFF", s);
res = param_set_no_notification(param_find(str), &accel_scale.z_offset);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_XSCALE", s);
res = param_set_no_notification(param_find(str), &accel_scale.x_scale);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_YSCALE", s);
res = param_set_no_notification(param_find(str), &accel_scale.y_scale);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_ZSCALE", s);
res = param_set_no_notification(param_find(str), &accel_scale.z_scale);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
param_notify_changes();
#endif
}
float accel_offs[max_accel_sens][3];
float accel_T[max_accel_sens][3][3];
unsigned active_sensors = 0;
/* measure and calculate offsets & scales */
if (res == PX4_OK) {
calibrate_return cal_return = do_accel_calibration_measurements(mavlink_log_pub, accel_offs, accel_T, &active_sensors);
if (cal_return == calibrate_return_cancelled) {
// Cancel message already displayed, nothing left to do
return PX4_ERROR;
} else if (cal_return == calibrate_return_ok) {
res = PX4_OK;
} else {
res = PX4_ERROR;
}
}
if (res != PX4_OK) {
if (active_sensors == 0) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
}
return PX4_ERROR;
}
/* measurements completed successfully, rotate calibration values */
param_t board_rotation_h = param_find("SENS_BOARD_ROT");
int32_t board_rotation_int;
param_get(board_rotation_h, &(board_rotation_int));
enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
matrix::Dcmf board_rotation = get_rot_matrix(board_rotation_id);
matrix::Dcmf board_rotation_t = board_rotation.transpose();
bool tc_locked[3] = {false}; // true when the thermal parameter instance has already been adjusted by the calibrator
for (unsigned uorb_index = 0; uorb_index < active_sensors; uorb_index++) {
/* handle individual sensors, one by one */
matrix::Vector3f accel_offs_vec(accel_offs[uorb_index]);
matrix::Vector3f accel_offs_rotated = board_rotation_t * accel_offs_vec;
matrix::Matrix3f accel_T_mat(accel_T[uorb_index]);
matrix::Matrix3f accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
accel_scale.x_offset = accel_offs_rotated(0);
accel_scale.x_scale = accel_T_rotated(0, 0);
accel_scale.y_offset = accel_offs_rotated(1);
accel_scale.y_scale = accel_T_rotated(1, 1);
accel_scale.z_offset = accel_offs_rotated(2);
accel_scale.z_scale = accel_T_rotated(2, 2);
bool failed = false;
failed = failed || (PX4_OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary)));
PX4_INFO("found offset %d: x: %.6f, y: %.6f, z: %.6f", uorb_index,
(double)accel_scale.x_offset,
(double)accel_scale.y_offset,
(double)accel_scale.z_offset);
PX4_INFO("found scale %d: x: %.6f, y: %.6f, z: %.6f", uorb_index,
(double)accel_scale.x_scale,
(double)accel_scale.y_scale,
(double)accel_scale.z_scale);
/* check if thermal compensation is enabled */
int32_t tc_enabled_int;
param_get(param_find("TC_A_ENABLE"), &(tc_enabled_int));
if (tc_enabled_int == 1) {
/* Get struct containing sensor thermal compensation data */
struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */
memset(&sensor_correction, 0, sizeof(sensor_correction));
int sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
orb_copy(ORB_ID(sensor_correction), sensor_correction_sub, &sensor_correction);
orb_unsubscribe(sensor_correction_sub);
/* don't allow a parameter instance to be calibrated more than once by another uORB instance */
if (!tc_locked[sensor_correction.accel_mapping[uorb_index]]) {
tc_locked[sensor_correction.accel_mapping[uorb_index]] = true;
/* update the _X0_ terms to include the additional offset */
int32_t handle;
float val;
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
val = 0.0f;
(void)sprintf(str, "TC_A%u_X0_%u", sensor_correction.accel_mapping[uorb_index], axis_index);
handle = param_find(str);
param_get(handle, &val);
if (axis_index == 0) {
val += accel_scale.x_offset;
} else if (axis_index == 1) {
val += accel_scale.y_offset;
} else if (axis_index == 2) {
val += accel_scale.z_offset;
}
failed |= (PX4_OK != param_set_no_notification(handle, &val));
}
/* update the _SCL_ terms to include the scale factor */
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
val = 1.0f;
(void)sprintf(str, "TC_A%u_SCL_%u", sensor_correction.accel_mapping[uorb_index], axis_index);
handle = param_find(str);
if (axis_index == 0) {
val = accel_scale.x_scale;
} else if (axis_index == 1) {
val = accel_scale.y_scale;
} else if (axis_index == 2) {
val = accel_scale.z_scale;
}
failed |= (PX4_OK != param_set_no_notification(handle, &val));
}
param_notify_changes();
}
// Ensure the calibration values used by the driver are at default settings when we are using thermal calibration data
accel_scale.x_offset = 0.f;
accel_scale.y_offset = 0.f;
accel_scale.z_offset = 0.f;
accel_scale.x_scale = 1.f;
accel_scale.y_scale = 1.f;
accel_scale.z_scale = 1.f;
}
// save the driver level calibration data
(void)sprintf(str, "CAL_ACC%u_XOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset)));
(void)sprintf(str, "CAL_ACC%u_YOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.y_offset)));
(void)sprintf(str, "CAL_ACC%u_ZOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.z_offset)));
(void)sprintf(str, "CAL_ACC%u_XSCALE", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.x_scale)));
(void)sprintf(str, "CAL_ACC%u_YSCALE", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.y_scale)));
(void)sprintf(str, "CAL_ACC%u_ZSCALE", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.z_scale)));
(void)sprintf(str, "CAL_ACC%u_ID", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(device_id[uorb_index])));
if (failed) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, uorb_index);
return PX4_ERROR;
}
#ifdef __PX4_NUTTX
sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, uorb_index);
fd = px4_open(str, 0);
if (fd < 0) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "sensor does not exist");
res = PX4_ERROR;
} else {
res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
px4_close(fd);
}
if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, uorb_index);
}
#endif
}
if (res == PX4_OK) {
/* if there is a any preflight-check system response, let the barrage of messages through */
usleep(200000);
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
} else {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
}
/* give this message enough time to propagate */
usleep(600000);
return res;
}
void
FixedwingEstimator::task_main()
{
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
_ekf = new AttPosEKF();
float dt = 0.0f; // time lapsed since last covariance prediction
_filter_start_time = hrt_absolute_time();
if (!_ekf) {
errx(1, "OUT OF MEM!");
}
/*
* do subscriptions
*/
_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
_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));
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vstatus_sub, 200);
#ifndef SENSOR_COMBINED_SUB
_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
/* rate limit gyro updates to 50 Hz */
/* XXX remove this!, BUT increase the data buffer size! */
orb_set_interval(_gyro_sub, 4);
#else
_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);
#endif
/* sets also parameters in the EKF object */
parameters_update();
Vector3f lastAngRate;
Vector3f lastAccel;
/* wakeup source(s) */
struct pollfd fds[2];
/* Setup of loop */
fds[0].fd = _params_sub;
fds[0].events = POLLIN;
#ifndef SENSOR_COMBINED_SUB
fds[1].fd = _gyro_sub;
fds[1].events = POLLIN;
#else
fds[1].fd = _sensor_combined_sub;
fds[1].events = POLLIN;
#endif
bool newDataGps = false;
bool newHgtData = false;
bool newAdsData = false;
bool newDataMag = false;
float posNED[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m)
uint64_t last_gps = 0;
_gps.vel_n_m_s = 0.0f;
_gps.vel_e_m_s = 0.0f;
_gps.vel_d_m_s = 0.0f;
while (!_task_should_exit) {
/* 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 ERR %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 estimator if gyro updated */
if (fds[1].revents & POLLIN) {
/* check vehicle status for changes to publication state */
bool prev_hil = (_vstatus.hil_state == HIL_STATE_ON);
vehicle_status_poll();
bool accel_updated;
bool mag_updated;
perf_count(_perf_gyro);
/* Reset baro reference if switching to HIL, reset sensor states */
if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) {
/* system is in HIL now, wait for measurements to come in one last round */
usleep(60000);
#ifndef SENSOR_COMBINED_SUB
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro);
orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag);
#else
/* now read all sensor publications to ensure all real sensor data is purged */
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
#endif
/* 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
**/
/* load local copies */
#ifndef SENSOR_COMBINED_SUB
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro);
orb_check(_accel_sub, &accel_updated);
if (accel_updated) {
orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
}
_last_sensor_timestamp = _gyro.timestamp;
IMUmsec = _gyro.timestamp / 1e3f;
float deltaT = (_gyro.timestamp - _last_run) / 1e6f;
_last_run = _gyro.timestamp;
/* guard against too large deltaT's */
if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
deltaT = 0.01f;
}
// Always store data, independent of init status
/* fill in last data set */
_ekf->dtIMU = deltaT;
if (isfinite(_gyro.x) &&
isfinite(_gyro.y) &&
isfinite(_gyro.z)) {
_ekf->angRate.x = _gyro.x;
_ekf->angRate.y = _gyro.y;
_ekf->angRate.z = _gyro.z;
if (!_gyro_valid) {
lastAngRate = _ekf->angRate;
}
_gyro_valid = true;
}
if (accel_updated) {
_ekf->accel.x = _accel.x;
_ekf->accel.y = _accel.y;
_ekf->accel.z = _accel.z;
if (!_accel_valid) {
lastAccel = _ekf->accel;
}
_accel_valid = true;
}
_ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
_ekf->lastAngRate = angRate;
_ekf->dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
_ekf->lastAccel = accel;
#else
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
static hrt_abstime last_accel = 0;
static hrt_abstime last_mag = 0;
if (last_accel != _sensor_combined.accelerometer_timestamp) {
accel_updated = true;
} else {
accel_updated = false;
}
last_accel = _sensor_combined.accelerometer_timestamp;
// Copy gyro and accel
_last_sensor_timestamp = _sensor_combined.timestamp;
IMUmsec = _sensor_combined.timestamp / 1e3f;
float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;
/* guard against too large deltaT's */
if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
deltaT = 0.01f;
}
_last_run = _sensor_combined.timestamp;
// Always store data, independent of init status
/* fill in last data set */
_ekf->dtIMU = deltaT;
if (isfinite(_sensor_combined.gyro_rad_s[0]) &&
isfinite(_sensor_combined.gyro_rad_s[1]) &&
isfinite(_sensor_combined.gyro_rad_s[2])) {
_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
_ekf->angRate.z = _sensor_combined.gyro_rad_s[2];
if (!_gyro_valid) {
lastAngRate = _ekf->angRate;
}
_gyro_valid = true;
perf_count(_perf_gyro);
}
if (accel_updated) {
_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
if (!_accel_valid) {
lastAccel = _ekf->accel;
}
_accel_valid = true;
}
_ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU;
lastAngRate = _ekf->angRate;
_ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU;
lastAccel = _ekf->accel;
if (last_mag != _sensor_combined.magnetometer_timestamp) {
mag_updated = true;
newDataMag = true;
} else {
newDataMag = false;
}
last_mag = _sensor_combined.magnetometer_timestamp;
#endif
//warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z);
bool airspeed_updated;
orb_check(_airspeed_sub, &airspeed_updated);
if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
perf_count(_perf_airspeed);
_ekf->VtasMeas = _airspeed.true_airspeed_m_s;
newAdsData = true;
} else {
newAdsData = false;
}
bool gps_updated;
orb_check(_gps_sub, &gps_updated);
if (gps_updated) {
last_gps = _gps.timestamp_position;
orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
perf_count(_perf_gps);
if (_gps.fix_type < 3) {
newDataGps = false;
} else {
/* store time of valid GPS measurement */
_gps_start_time = hrt_absolute_time();
/* check if we had a GPS outage for a long time */
if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) {
_ekf->ResetPosition();
_ekf->ResetVelocity();
_ekf->ResetStoredStates();
}
/* fuse GPS updates */
//_gps.timestamp / 1e3;
_ekf->GPSstatus = _gps.fix_type;
_ekf->velNED[0] = _gps.vel_n_m_s;
_ekf->velNED[1] = _gps.vel_e_m_s;
_ekf->velNED[2] = _gps.vel_d_m_s;
// warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]);
_ekf->gpsLat = math::radians(_gps.lat / (double)1e7);
_ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
_ekf->gpsHgt = _gps.alt / 1e3f;
// if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) {
// _ekf->vneSigma = sqrtf(_gps.s_variance_m_s);
// } else {
// _ekf->vneSigma = _parameters.velne_noise;
// }
// if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) {
// _ekf->posNeSigma = sqrtf(_gps.p_variance_m);
// } else {
// _ekf->posNeSigma = _parameters.posne_noise;
// }
// warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m);
newDataGps = true;
}
}
bool baro_updated;
orb_check(_baro_sub, &baro_updated);
if (baro_updated) {
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
_ekf->baroHgt = _baro.altitude;
if (!_baro_init) {
_baro_ref = _baro.altitude;
_baro_init = true;
warnx("ALT REF INIT");
}
perf_count(_perf_baro);
newHgtData = true;
} else {
newHgtData = false;
}
#ifndef SENSOR_COMBINED_SUB
orb_check(_mag_sub, &mag_updated);
#endif
if (mag_updated) {
_mag_valid = true;
perf_count(_perf_mag);
#ifndef SENSOR_COMBINED_SUB
orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag);
// XXX we compensate the offsets upfront - should be close to zero.
// 0.001f
_ekf->magData.x = _mag.x;
_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
_ekf->magData.y = _mag.y;
_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
_ekf->magData.z = _mag.z;
_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
#else
// XXX we compensate the offsets upfront - should be close to zero.
// 0.001f
_ekf->magData.x = _sensor_combined.magnetometer_ga[0];
_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
_ekf->magData.y = _sensor_combined.magnetometer_ga[1];
_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
_ekf->magData.z = _sensor_combined.magnetometer_ga[2];
_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
#endif
newDataMag = true;
} else {
newDataMag = false;
}
/*
* 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) {
float initVelNED[3];
/* Initialize the filter first */
if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) {
// GPS is in scaled integers, convert
double lat = _gps.lat / 1.0e7;
double lon = _gps.lon / 1.0e7;
float gps_alt = _gps.alt / 1e3f;
initVelNED[0] = _gps.vel_n_m_s;
initVelNED[1] = _gps.vel_e_m_s;
initVelNED[2] = _gps.vel_d_m_s;
// Set up height correctly
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
_baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame
_baro_gps_offset = _baro.altitude - gps_alt;
_ekf->baroHgt = _baro.altitude;
_ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref));
// Set up position variables correctly
_ekf->GPSstatus = _gps.fix_type;
_ekf->gpsLat = math::radians(lat);
_ekf->gpsLon = math::radians(lon) - M_PI;
_ekf->gpsHgt = gps_alt;
// Look up mag declination based on current position
float declination = math::radians(get_mag_declination(lat, lon));
_ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination);
// Initialize projection
_local_pos.ref_lat = lat;
_local_pos.ref_lon = lon;
_local_pos.ref_alt = gps_alt;
_local_pos.ref_timestamp = _gps.timestamp_position;
map_projection_init(&_pos_ref, lat, lon);
mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
#if 0
warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt,
(double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_ref_offset);
warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination));
#endif
_gps_initialized = true;
} else if (!_ekf->statesInitialised) {
initVelNED[0] = 0.0f;
initVelNED[1] = 0.0f;
initVelNED[2] = 0.0f;
_ekf->posNE[0] = posNED[0];
_ekf->posNE[1] = posNED[1];
_local_pos.ref_alt = _baro_ref;
_baro_ref_offset = 0.0f;
_baro_gps_offset = 0.0f;
_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
} else if (_ekf->statesInitialised) {
// We're apparently initialized in this case now
int check = check_filter_state();
if (check) {
// Let the system re-initialize itself
continue;
}
// Run the strapdown INS equations every IMU update
_ekf->UpdateStrapdownEquationsNED();
#if 0
// debug code - could be tunred into a filter mnitoring/watchdog function
float tempQuat[4];
for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j];
quat2eul(eulerEst, tempQuat);
for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j];
if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi;
if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi;
#endif
// store the predicted states for subsequent use by measurement fusion
_ekf->StoreStates(IMUmsec);
// Check if on ground - status is used by covariance prediction
_ekf->OnGroundCheck();
// sum delta angles and time used by covariance prediction
_ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng;
_ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU;
dt += _ekf->dtIMU;
// perform a covariance prediction if the total delta angle has exceeded the limit
// or the time limit will be exceeded at the next IMU update
if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) {
_ekf->CovariancePrediction(dt);
_ekf->summedDelAng.zero();
_ekf->summedDelVel.zero();
dt = 0.0f;
}
// Fuse GPS Measurements
if (newDataGps && _gps_initialized) {
// Convert GPS measurements to Pos NE, hgt and Vel NED
float gps_dt = (_gps.timestamp_position - last_gps) / 1e6f;
// Calculate acceleration predicted by GPS velocity change
if (((fabsf(_ekf->velNED[0] - _gps.vel_n_m_s) > FLT_EPSILON) ||
(fabsf(_ekf->velNED[1] - _gps.vel_e_m_s) > FLT_EPSILON) ||
(fabsf(_ekf->velNED[2] - _gps.vel_d_m_s) > FLT_EPSILON)) && (gps_dt > 0.00001f)) {
_ekf->accelGPSNED[0] = (_ekf->velNED[0] - _gps.vel_n_m_s) / gps_dt;
_ekf->accelGPSNED[1] = (_ekf->velNED[1] - _gps.vel_e_m_s) / gps_dt;
_ekf->accelGPSNED[2] = (_ekf->velNED[2] - _gps.vel_d_m_s) / gps_dt;
}
_ekf->velNED[0] = _gps.vel_n_m_s;
_ekf->velNED[1] = _gps.vel_e_m_s;
_ekf->velNED[2] = _gps.vel_d_m_s;
_ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef);
_ekf->posNE[0] = posNED[0];
_ekf->posNE[1] = posNED[1];
// set fusion flags
_ekf->fuseVelData = true;
_ekf->fusePosData = true;
// recall states stored at time of measurement after adjusting for delays
_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
// run the fusion step
_ekf->FuseVelposNED();
} else if (_ekf->statesInitialised) {
// Convert GPS measurements to Pos NE, hgt and Vel NED
_ekf->velNED[0] = 0.0f;
_ekf->velNED[1] = 0.0f;
_ekf->velNED[2] = 0.0f;
_ekf->posNE[0] = 0.0f;
_ekf->posNE[1] = 0.0f;
// set fusion flags
_ekf->fuseVelData = true;
_ekf->fusePosData = true;
// recall states stored at time of measurement after adjusting for delays
_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
// run the fusion step
_ekf->FuseVelposNED();
} else {
_ekf->fuseVelData = false;
_ekf->fusePosData = false;
}
if (newHgtData && _ekf->statesInitialised) {
// Could use a blend of GPS and baro alt data if desired
_ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
_ekf->fuseHgtData = true;
// recall states stored at time of measurement after adjusting for delays
_ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
// run the fusion step
_ekf->FuseVelposNED();
} else {
_ekf->fuseHgtData = false;
}
// Fuse Magnetometer Measurements
if (newDataMag && _ekf->statesInitialised) {
_ekf->fuseMagData = true;
_ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
_ekf->magstate.obsIndex = 0;
_ekf->FuseMagnetometer();
_ekf->FuseMagnetometer();
_ekf->FuseMagnetometer();
} else {
_ekf->fuseMagData = false;
}
// Fuse Airspeed Measurements
if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) {
_ekf->fuseVtasData = true;
_ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
_ekf->FuseAirspeed();
} else {
_ekf->fuseVtasData = false;
}
// Output results
math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
math::Matrix<3, 3> R = q.to_dcm();
math::Vector<3> euler = R.to_euler();
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
_att.R[i][j] = R(i, j);
_att.timestamp = _last_sensor_timestamp;
_att.q[0] = _ekf->states[0];
_att.q[1] = _ekf->states[1];
_att.q[2] = _ekf->states[2];
_att.q[3] = _ekf->states[3];
_att.q_valid = true;
_att.R_valid = true;
_att.timestamp = _last_sensor_timestamp;
_att.roll = euler(0);
_att.pitch = euler(1);
_att.yaw = euler(2);
_att.rollspeed = _ekf->angRate.x - _ekf->states[10];
_att.pitchspeed = _ekf->angRate.y - _ekf->states[11];
_att.yawspeed = _ekf->angRate.z - _ekf->states[12];
// gyro offsets
_att.rate_offsets[0] = _ekf->states[10];
_att.rate_offsets[1] = _ekf->states[11];
_att.rate_offsets[2] = _ekf->states[12];
/* lazily publish the attitude only once available */
if (_att_pub > 0) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att);
} else {
/* advertise and publish */
_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att);
}
if (_gps_initialized) {
_local_pos.timestamp = _last_sensor_timestamp;
_local_pos.x = _ekf->states[7];
_local_pos.y = _ekf->states[8];
// XXX need to announce change of Z reference somehow elegantly
_local_pos.z = _ekf->states[9] - _baro_ref_offset;
_local_pos.vx = _ekf->states[4];
_local_pos.vy = _ekf->states[5];
_local_pos.vz = _ekf->states[6];
_local_pos.xy_valid = _gps_initialized;
_local_pos.z_valid = true;
_local_pos.v_xy_valid = _gps_initialized;
_local_pos.v_z_valid = true;
_local_pos.xy_global = true;
_velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy);
_velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz);
_airspeed_filtered = 0.95f*_airspeed_filtered + + 0.05f*_airspeed.true_airspeed_m_s;
/* crude land detector for fixedwing only,
* TODO: adapt so that it works for both, maybe move to another location
*/
if (_velocity_xy_filtered < 5
&& _velocity_z_filtered < 10
&& _airspeed_filtered < 10) {
_local_pos.landed = true;
} else {
_local_pos.landed = false;
}
_local_pos.z_global = false;
_local_pos.yaw = _att.yaw;
/* lazily publish the local position only once available */
if (_local_pos_pub > 0) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos);
} else {
/* advertise and publish */
_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos);
}
_global_pos.timestamp = _local_pos.timestamp;
if (_local_pos.xy_global) {
double est_lat, est_lon;
map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon);
_global_pos.lat = est_lat;
_global_pos.lon = est_lon;
_global_pos.time_gps_usec = _gps.time_gps_usec;
_global_pos.eph = _gps.eph;
_global_pos.epv = _gps.epv;
}
if (_local_pos.v_xy_valid) {
_global_pos.vel_n = _local_pos.vx;
_global_pos.vel_e = _local_pos.vy;
} else {
_global_pos.vel_n = 0.0f;
_global_pos.vel_e = 0.0f;
}
/* local pos alt is negative, change sign and add alt offsets */
_global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset;
if (_local_pos.v_z_valid) {
_global_pos.vel_d = _local_pos.vz;
}
_global_pos.yaw = _local_pos.yaw;
_global_pos.eph = _gps.eph;
_global_pos.epv = _gps.epv;
_global_pos.timestamp = _local_pos.timestamp;
/* lazily publish the global position only once available */
if (_global_pos_pub > 0) {
/* publish the global position */
orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos);
} else {
/* advertise and publish */
_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos);
}
if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
_wind.timestamp = _global_pos.timestamp;
_wind.windspeed_north = _ekf->states[14];
_wind.windspeed_east = _ekf->states[15];
_wind.covariance_north = 0.0f; // XXX get form filter
_wind.covariance_east = 0.0f;
/* lazily publish the wind estimate only once available */
if (_wind_pub > 0) {
/* publish the wind estimate */
orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);
} else {
/* advertise and publish */
_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
}
}
}
}
if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
_wind.timestamp = _global_pos.timestamp;
_wind.windspeed_north = _ekf->states[14];
_wind.windspeed_east = _ekf->states[15];
_wind.covariance_north = _ekf->P[14][14];
_wind.covariance_east = _ekf->P[15][15];
/* lazily publish the wind estimate only once available */
if (_wind_pub > 0) {
/* publish the wind estimate */
orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);
} else {
/* advertise and publish */
_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
}
}
}
}
perf_end(_loop_perf);
}
warnx("exiting.\n");
_estimator_task = -1;
_exit(0);
}
/****************************************************************************
* main
****************************************************************************/
int position_estimator_inav_thread_main(int argc, char *argv[])
{
warnx("started");
int mavlink_fd;
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(mavlink_fd, "[inav] started");
float x_est[3] = { 0.0f, 0.0f, 0.0f };
float y_est[3] = { 0.0f, 0.0f, 0.0f };
float z_est[3] = { 0.0f, 0.0f, 0.0f };
int baro_init_cnt = 0;
int baro_init_num = 200;
float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted
float surface_offset = 0.0f; // ground level offset from reference altitude
float surface_offset_rate = 0.0f; // surface offset change rate
float alt_avg = 0.0f;
bool landed = true;
hrt_abstime landed_time = 0;
hrt_abstime accel_timestamp = 0;
hrt_abstime baro_timestamp = 0;
bool ref_inited = false;
hrt_abstime ref_init_start = 0;
const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix
uint16_t accel_updates = 0;
uint16_t baro_updates = 0;
uint16_t gps_updates = 0;
uint16_t attitude_updates = 0;
uint16_t flow_updates = 0;
hrt_abstime updates_counter_start = hrt_absolute_time();
hrt_abstime pub_last = hrt_absolute_time();
hrt_abstime t_prev = 0;
/* acceleration in NED frame */
float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G };
/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
float corr_acc[] = { 0.0f, 0.0f, 0.0f }; // N E D
float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame
float corr_baro = 0.0f; // D
float corr_gps[3][2] = {
{ 0.0f, 0.0f }, // N (pos, vel)
{ 0.0f, 0.0f }, // E (pos, vel)
{ 0.0f, 0.0f }, // D (pos, vel)
};
float w_gps_xy = 1.0f;
float w_gps_z = 1.0f;
float corr_sonar = 0.0f;
float corr_sonar_filtered = 0.0f;
float corr_flow[] = { 0.0f, 0.0f }; // N E
float w_flow = 0.0f;
float sonar_prev = 0.0f;
hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered)
hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered)
hrt_abstime xy_src_time = 0; // time of last available position data
bool gps_valid = false; // GPS is valid
bool sonar_valid = false; // sonar is valid
bool flow_valid = false; // flow is valid
bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false)
/* declare and safely initialize all structs */
struct actuator_controls_s actuator;
memset(&actuator, 0, sizeof(actuator));
struct actuator_armed_s armed;
memset(&armed, 0, sizeof(armed));
struct sensor_combined_s sensor;
memset(&sensor, 0, sizeof(sensor));
struct vehicle_gps_position_s gps;
memset(&gps, 0, sizeof(gps));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_local_position_s local_pos;
memset(&local_pos, 0, sizeof(local_pos));
struct optical_flow_s flow;
memset(&flow, 0, sizeof(flow));
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
/* subscribe */
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
/* advertise */
orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
orb_advert_t vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
struct position_estimator_inav_params params;
struct position_estimator_inav_param_handles pos_inav_param_handles;
/* initialize parameter handles */
parameters_init(&pos_inav_param_handles);
/* first parameters read at start up */
struct parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param topic to clear updated flag */
/* first parameters update */
parameters_update(&pos_inav_param_handles, ¶ms);
struct pollfd fds_init[1] = {
{ .fd = sensor_combined_sub, .events = POLLIN },
};
pthread_addr_t UavcanServers::run(pthread_addr_t)
{
prctl(PR_SET_NAME, "uavcan fw srv", 0);
Lock lock(_subnode_mutex);
/*
Copy any firmware bundled in the ROMFS to the appropriate location on the
SD card, unless the user has copied other firmware for that device.
*/
unpackFwFromROMFS(UAVCAN_FIRMWARE_PATH, UAVCAN_ROMFS_FW_PATH);
/* the subscribe call needs to happen in the same thread,
* so not in the constructor */
int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
int param_request_sub = orb_subscribe(ORB_ID(uavcan_parameter_request));
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
/* Set up shared service clients */
_param_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_getset));
_param_opcode_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_opcode));
_param_restartnode_client.setCallback(RestartNodeCallback(this, &UavcanServers::cb_restart));
_enumeration_client.setCallback(EnumerationBeginCallback(this, &UavcanServers::cb_enumeration_begin));
_enumeration_indication_sub.start(EnumerationIndicationCallback(this, &UavcanServers::cb_enumeration_indication));
_enumeration_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_enumeration_getset));
_enumeration_save_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_enumeration_save));
_count_in_progress = _param_in_progress = _param_list_in_progress = _cmd_in_progress = _param_list_all_nodes = false;
memset(_param_counts, 0, sizeof(_param_counts));
_esc_enumeration_active = false;
memset(_esc_enumeration_ids, 0, sizeof(_esc_enumeration_ids));
_esc_enumeration_index = 0;
while (!_subnode_thread_should_exit) {
if (_check_fw == true) {
_check_fw = false;
_node_info_retriever.invalidateAll();
}
const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(10));
if (spin_res < 0) {
warnx("node spin error %i", spin_res);
}
// Check for parameter requests (get/set/list)
bool param_request_ready;
orb_check(param_request_sub, ¶m_request_ready);
if (param_request_ready && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
struct uavcan_parameter_request_s request;
orb_copy(ORB_ID(uavcan_parameter_request), param_request_sub, &request);
if (_param_counts[request.node_id]) {
/*
* We know how many parameters are exposed by this node, so
* process the request.
*/
if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_READ) {
uavcan::protocol::param::GetSet::Request req;
if (request.param_index >= 0) {
req.index = request.param_index;
} else {
req.name = (char*)request.param_id;
}
int call_res = _param_getset_client.call(request.node_id, req);
if (call_res < 0) {
warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res);
} else {
_param_in_progress = true;
_param_index = request.param_index;
}
} else if (request.message_type == MAVLINK_MSG_ID_PARAM_SET) {
uavcan::protocol::param::GetSet::Request req;
if (request.param_index >= 0) {
req.index = request.param_index;
} else {
req.name = (char*)request.param_id;
}
if (request.param_type == MAV_PARAM_TYPE_REAL32) {
req.value.to<uavcan::protocol::param::Value::Tag::real_value>() = request.real_value;
} else if (request.param_type == MAV_PARAM_TYPE_UINT8) {
req.value.to<uavcan::protocol::param::Value::Tag::boolean_value>() = request.int_value;
} else {
req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = request.int_value;
}
// Set the dirty bit for this node
set_node_params_dirty(request.node_id);
int call_res = _param_getset_client.call(request.node_id, req);
if (call_res < 0) {
warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res);
} else {
_param_in_progress = true;
_param_index = request.param_index;
}
} else if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
// This triggers the _param_list_in_progress case below.
_param_index = 0;
_param_list_in_progress = true;
_param_list_node_id = request.node_id;
_param_list_all_nodes = false;
warnx("UAVCAN command bridge: starting component-specific param list");
}
} else if (request.node_id == MAV_COMP_ID_ALL) {
if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
/*
* This triggers the _param_list_in_progress case below,
* but additionally iterates over all active nodes.
*/
_param_index = 0;
_param_list_in_progress = true;
_param_list_node_id = get_next_active_node_id(1);
_param_list_all_nodes = true;
warnx("UAVCAN command bridge: starting global param list with node %hhu", _param_list_node_id);
if (_param_counts[_param_list_node_id] == 0) {
param_count(_param_list_node_id);
}
}
} else {
/*
* Need to know how many parameters this node has before we can
* continue; count them now and then process the request.
*/
param_count(request.node_id);
}
}
// Handle parameter listing index/node ID advancement
if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
if (_param_index >= _param_counts[_param_list_node_id]) {
warnx("UAVCAN command bridge: completed param list for node %hhu", _param_list_node_id);
// Reached the end of the current node's parameter set.
_param_list_in_progress = false;
if (_param_list_all_nodes) {
// We're listing all parameters for all nodes -- get the next node ID
uint8_t next_id = get_next_active_node_id(_param_list_node_id);
if (next_id < 128) {
_param_list_node_id = next_id;
/*
* If there is a next node ID, check if that node's parameters
* have been counted before. If not, do it now.
*/
if (_param_counts[_param_list_node_id] == 0) {
param_count(_param_list_node_id);
}
// Keep on listing.
_param_index = 0;
_param_list_in_progress = true;
warnx("UAVCAN command bridge: started param list for node %hhu", _param_list_node_id);
}
}
}
}
// Check if we're still listing, and need to get the next parameter
if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
// Ready to request the next value -- _param_index is incremented
// after each successful fetch by cb_getset
uavcan::protocol::param::GetSet::Request req;
req.index = _param_index;
int call_res = _param_getset_client.call(_param_list_node_id, req);
if (call_res < 0) {
_param_list_in_progress = false;
warnx("UAVCAN command bridge: couldn't send param list GetSet: %d", call_res);
} else {
_param_in_progress = true;
}
}
// Check for ESC enumeration commands
bool cmd_ready;
orb_check(cmd_sub, &cmd_ready);
if (cmd_ready && !_cmd_in_progress) {
struct vehicle_command_s cmd;
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN) {
int command_id = static_cast<int>(cmd.param1 + 0.5f);
int node_id = static_cast<int>(cmd.param2 + 0.5f);
int call_res;
warnx("UAVCAN command bridge: received UAVCAN command ID %d, node ID %d", command_id, node_id);
switch (command_id) {
case 0:
case 1: {
_esc_enumeration_active = command_id;
_esc_enumeration_index = 0;
_esc_count = 0;
uavcan::protocol::enumeration::Begin::Request req;
req.parameter_name = "esc_index";
req.timeout_sec = _esc_enumeration_active ? 65535 : 0;
call_res = _enumeration_client.call(get_next_active_node_id(1), req);
if (call_res < 0) {
warnx("UAVCAN ESC enumeration: couldn't send initial Begin request: %d", call_res);
}
break;
}
default: {
warnx("UAVCAN command bridge: unknown command ID %d", command_id);
break;
}
}
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE) {
int command_id = static_cast<int>(cmd.param1 + 0.5f);
warnx("UAVCAN command bridge: received storage command ID %d", command_id);
switch (command_id) {
case 1: {
// Param save request
int node_id;
node_id = get_next_dirty_node_id(1);
if (node_id < 128) {
_param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE;
param_opcode(node_id);
}
break;
}
case 2: {
// Command is a param erase request -- apply it to all active nodes by setting the dirty bit
_param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_ERASE;
for (int i = 1; i < 128; i = get_next_active_node_id(i)) {
set_node_params_dirty(i);
}
param_opcode(get_next_dirty_node_id(1));
break;
}
}
}
}
// Shut down once armed
// TODO (elsewhere): start up again once disarmed?
bool updated;
orb_check(armed_sub, &updated);
if (updated) {
struct actuator_armed_s armed;
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
if (armed.armed && !armed.lockdown) {
warnx("UAVCAN command bridge: system armed, exiting now.");
break;
}
}
}
_subnode_thread_should_exit = false;
warnx("exiting");
return (pthread_addr_t) 0;
}
void
PX4FMU::task_main()
{
/* force a reset of the update rate */
_current_update_rate = 0;
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
/* advertise the mixed control outputs */
actuator_outputs_s outputs;
memset(&outputs, 0, sizeof(outputs));
#ifdef HRT_PPM_CHANNEL
// rc input, published to ORB
struct rc_input_values rc_in;
orb_advert_t to_input_rc = 0;
memset(&rc_in, 0, sizeof(rc_in));
rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM;
#endif
/* initialize PWM limit lib */
pwm_limit_init(&_pwm_limit);
log("starting");
/* loop until killed */
while (!_task_should_exit) {
if (_groups_subscribed != _groups_required) {
subscribe();
_groups_subscribed = _groups_required;
/* force setting update rate */
_current_update_rate = 0;
}
/*
* Adjust actuator topic update rate to keep up with
* the highest servo update rate configured.
*
* We always mix at max rate; some channels may update slower.
*/
unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate;
if (_current_update_rate != max_rate) {
_current_update_rate = max_rate;
int update_rate_in_ms = int(1000 / _current_update_rate);
/* reject faster than 500 Hz updates */
if (update_rate_in_ms < 2) {
update_rate_in_ms = 2;
}
/* reject slower than 10 Hz updates */
if (update_rate_in_ms > 100) {
update_rate_in_ms = 100;
}
debug("adjusted actuator update interval to %ums", update_rate_in_ms);
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
orb_set_interval(_control_subs[i], update_rate_in_ms);
}
}
// set to current max rate, even if we are actually checking slower/faster
_current_update_rate = max_rate;
}
/* sleep waiting for data, stopping to check for PPM
* input at 50Hz */
int ret = ::poll(_poll_fds, _poll_fds_num, CONTROL_INPUT_DROP_LIMIT_MS);
/* this would be bad... */
if (ret < 0) {
log("poll error %d", errno);
continue;
} else if (ret == 0) {
/* timeout: no control data, switch to failsafe values */
// warnx("no PWM: failsafe");
} else {
/* get controls for required topics */
unsigned poll_id = 0;
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
if (_poll_fds[poll_id].revents & POLLIN) {
orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
}
poll_id++;
}
}
/* can we mix? */
if (_mixers != nullptr) {
unsigned num_outputs;
switch (_mode) {
case MODE_2PWM:
num_outputs = 2;
break;
case MODE_4PWM:
num_outputs = 4;
break;
case MODE_6PWM:
num_outputs = 6;
break;
case MODE_8PWM:
num_outputs = 8;
break;
default:
num_outputs = 0;
break;
}
/* do mixing */
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
outputs.timestamp = hrt_absolute_time();
/* iterate actuators */
for (unsigned i = 0; i < num_outputs; i++) {
/* last resort: catch NaN and INF */
if ((i >= outputs.noutputs) ||
!isfinite(outputs.output[i])) {
/*
* Value is NaN, INF or out of band - set to the minimum value.
* This will be clearly visible on the servo status and will limit the risk of accidentally
* spinning motors. It would be deadly in flight.
*/
outputs.output[i] = -1.0f;
}
}
uint16_t pwm_limited[num_outputs];
/* the PWM limit call takes care of out of band errors and constrains */
pwm_limit_calc(_servo_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
/* output to the servos */
for (unsigned i = 0; i < num_outputs; i++) {
up_pwm_servo_set(i, pwm_limited[i]);
}
/* publish mixed control outputs */
if (_outputs_pub < 0) {
_outputs_pub = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &outputs);
} else {
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _outputs_pub, &outputs);
}
}
}
/* check arming state */
bool updated = false;
orb_check(_armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
/* update the armed status and check that we're not locked down */
bool set_armed = _armed.armed && !_armed.lockdown;
if (_servo_armed != set_armed)
_servo_armed = set_armed;
/* update PWM status if armed or if disarmed PWM values are set */
bool pwm_on = (_armed.armed || _num_disarmed_set > 0);
if (_pwm_on != pwm_on) {
_pwm_on = pwm_on;
up_pwm_servo_arm(pwm_on);
}
}
#ifdef HRT_PPM_CHANNEL
// see if we have new PPM input data
if (ppm_last_valid_decode != rc_in.timestamp_last_signal) {
// we have a new PPM frame. Publish it.
rc_in.channel_count = ppm_decoded_channels;
if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) {
rc_in.channel_count = RC_INPUT_MAX_CHANNELS;
}
for (uint8_t i = 0; i < rc_in.channel_count; i++) {
rc_in.values[i] = ppm_buffer[i];
}
rc_in.timestamp_publication = ppm_last_valid_decode;
rc_in.timestamp_last_signal = ppm_last_valid_decode;
rc_in.rc_ppm_frame_length = ppm_frame_length;
rc_in.rssi = RC_INPUT_RSSI_MAX;
rc_in.rc_failsafe = false;
rc_in.rc_lost = false;
rc_in.rc_lost_frame_count = 0;
rc_in.rc_total_frame_count = 0;
/* lazily advertise on first publication */
if (to_input_rc == 0) {
to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in);
} else {
orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in);
}
}
#endif
}
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
::close(_control_subs[i]);
_control_subs[i] = -1;
}
}
::close(_armed_sub);
/* make sure servos are off */
up_pwm_servo_deinit();
log("stopping");
/* note - someone else is responsible for restoring the GPIO config */
/* tell the dtor that we are exiting */
_task = -1;
_exit(0);
}
int do_mag_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "don't move system");
/* 45 seconds */
uint64_t calibration_interval = 45 * 1000 * 1000;
/* maximum 500 values */
const unsigned int calibration_maxcount = 500;
unsigned int calibration_counter;
struct mag_scale mscale_null = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
int res = OK;
/* erase old calibration */
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
}
if (res == OK) {
/* calibrate range */
res = ioctl(fd, MAGIOCCALIBRATE, fd);
if (res != OK) {
mavlink_log_critical(mavlink_fd, "Skipped scale calibration");
/* this is non-fatal - mark it accordingly */
res = OK;
}
}
close(fd);
float *x = NULL;
float *y = NULL;
float *z = NULL;
if (res == OK) {
/* allocate memory */
mavlink_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_log_critical(mavlink_fd, "ERROR: out of memory");
res = ERROR;
return res;
}
} else {
/* exit */
return ERROR;
}
if (res == OK) {
int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
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_log_info(mavlink_fd, "rotate in a figure 8 around all axis");
calibration_counter = 0;
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_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount);
} else {
poll_errcount++;
}
if (poll_errcount > 1000) {
mavlink_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) {
/* sphere fit */
mavlink_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_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 80);
if (!isfinite(sphere_x) || !isfinite(sphere_y) || !isfinite(sphere_z)) {
mavlink_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;
fd = open(MAG_DEVICE_PATH, 0);
res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale);
if (res != OK) {
mavlink_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_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
}
}
close(fd);
if (res == OK) {
/* set parameters */
if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset)))
res = ERROR;
if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset)))
res = ERROR;
if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset)))
res = ERROR;
if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale)))
res = ERROR;
if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale)))
res = ERROR;
if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale)))
res = ERROR;
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
}
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 90);
}
if (res == OK) {
/* auto-save to EEPROM */
res = param_save_default();
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
}
}
mavlink_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_log_info(mavlink_fd, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
(double)mscale.y_scale, (double)mscale.z_scale);
if (res == OK) {
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
} else {
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
}
}
return res;
}
void
Gimbal::cycle()
{
if (!_initialized) {
/* get a subscription handle on the vehicle command topic */
_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
/* get a publication handle on actuator output topic */
struct actuator_controls_s zero_report;
memset(&zero_report, 0, sizeof(zero_report));
zero_report.timestamp = hrt_absolute_time();
_actuator_controls_2_topic = orb_advertise(ORB_ID(actuator_controls_2), &zero_report);
if (_actuator_controls_2_topic == nullptr) {
warnx("advert err");
}
_initialized = true;
}
bool updated = false;
perf_begin(_sample_perf);
float roll = 0.0f;
float pitch = 0.0f;
float yaw = 0.0f;
if (_att_sub < 0) {
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
}
vehicle_attitude_s att;
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att);
if (_attitude_compensation_roll) {
roll = 1.0f / M_PI_F * -att.roll;
updated = true;
}
if (_attitude_compensation_pitch) {
pitch = 1.0f / M_PI_F * -att.pitch;
updated = true;
}
if (_attitude_compensation_yaw) {
yaw = 1.0f / M_PI_F * att.yaw;
updated = true;
}
struct vehicle_command_s cmd;
bool cmd_updated;
orb_check(_vehicle_command_sub, &cmd_updated);
if (cmd_updated) {
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL
|| cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) {
_control_cmd = cmd;
_control_cmd_set = true;
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) {
_config_cmd = cmd;
_config_cmd_set = true;
}
}
if (_config_cmd_set) {
_config_cmd_set = false;
_attitude_compensation_roll = (int)_config_cmd.param2 == 1;
_attitude_compensation_pitch = (int)_config_cmd.param3 == 1;
_attitude_compensation_yaw = (int)_config_cmd.param4 == 1;
}
if (_control_cmd_set) {
unsigned mountMode = _control_cmd.param7;
DEVICE_DEBUG("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode, (double)_control_cmd.param1, (double)_control_cmd.param2);
if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL &&
mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
/* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */
roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param1;
pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param2;
yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param3;
updated = true;
}
if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT &&
mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
float gimbalDirectionQuat[] = {_control_cmd.param1, _control_cmd.param2, _control_cmd.param3, _control_cmd.param4};
math::Vector<3> gimablDirectionEuler = math::Quaternion(gimbalDirectionQuat).to_dcm().to_euler();
roll += gimablDirectionEuler(0);
pitch += gimablDirectionEuler(1);
yaw += gimablDirectionEuler(2);
updated = true;
}
}
if (updated) {
struct actuator_controls_s controls;
// DEVICE_DEBUG("publishing | roll: %8.4f pitch: %8.4f yaw: %8.4f", (double)roll, (double)pitch, (double)yaw);
/* fill in the final control values */
controls.timestamp = hrt_absolute_time();
controls.control[0] = roll;
controls.control[1] = pitch;
controls.control[2] = yaw;
/* publish it */
orb_publish(ORB_ID(actuator_controls_2), _actuator_controls_2_topic, &controls);
}
/* notify anyone waiting for data */
poll_notify(POLLIN);
perf_end(_sample_perf);
/* schedule a fresh cycle call when the measurement is done */
work_queue(LPWORK,
&_work,
(worker_t)&Gimbal::cycle_trampoline,
this,
USEC2TICK(GIMBAL_UPDATE_INTERVAL));
}
int ardrone_interface_thread_main(int argc, char *argv[])
{
thread_running = true;
char *device = "/dev/ttyS1";
/* File descriptors */
int gpios;
char *commandline_usage = "\tusage: ardrone_interface start|status|stop [-t for motor test (10%% thrust)]\n";
bool motor_test_mode = false;
int test_motor = -1;
/* read commandline arguments */
for (int i = 0; i < argc && argv[i]; i++) {
if (strcmp(argv[i], "-t") == 0 || strcmp(argv[i], "--test") == 0) {
motor_test_mode = true;
}
if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--motor") == 0) {
if (i + 1 < argc) {
int motor = atoi(argv[i + 1]);
if (motor > 0 && motor < 5) {
test_motor = motor;
} else {
thread_running = false;
errx(1, "supply a motor # between 1 and 4. Example: -m 1\n %s", commandline_usage);
}
} else {
thread_running = false;
errx(1, "missing parameter to -m 1..4\n %s", commandline_usage);
}
}
if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set
if (argc > i + 1) {
device = argv[i + 1];
} else {
thread_running = false;
errx(1, "missing parameter to -m 1..4\n %s", commandline_usage);
}
}
}
struct termios uart_config_original;
if (motor_test_mode) {
warnx("setting 10 %% thrust.\n");
}
/* Led animation */
int counter = 0;
int led_counter = 0;
/* declare and safely initialize all structs */
struct actuator_controls_s actuator_controls;
memset(&actuator_controls, 0, sizeof(actuator_controls));
struct actuator_armed_s armed;
//XXX is this necessairy?
armed.armed = false;
/* subscribe to attitude, motor setpoints and system state */
int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
ardrone_write = ardrone_open_uart(device, &uart_config_original);
/* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */
gpios = ar_multiplexing_init();
if (ardrone_write < 0) {
warnx("No UART, exiting.");
thread_running = false;
exit(ERROR);
}
/* initialize motors */
if (OK != ar_init_motors(ardrone_write, gpios)) {
close(ardrone_write);
warnx("motor init fail");
thread_running = false;
exit(ERROR);
}
ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);
// XXX Re-done initialization to make sure it is accepted by the motors
// XXX should be removed after more testing, but no harm
/* close uarts */
close(ardrone_write);
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
ardrone_write = ardrone_open_uart(device, &uart_config_original);
/* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */
gpios = ar_multiplexing_init();
if (ardrone_write < 0) {
warnx("write fail");
thread_running = false;
exit(ERROR);
}
/* initialize motors */
if (OK != ar_init_motors(ardrone_write, gpios)) {
close(ardrone_write);
warnx("motor init fail");
thread_running = false;
exit(ERROR);
}
while (!thread_should_exit) {
if (motor_test_mode) {
/* set motors to idle speed */
if (test_motor > 0 && test_motor < 5) {
int motors[4] = {0, 0, 0, 0};
motors[test_motor - 1] = 10;
ardrone_write_motor_commands(ardrone_write, motors[0], motors[1], motors[2], motors[3]);
} else {
ardrone_write_motor_commands(ardrone_write, 10, 10, 10, 10);
}
} else {
/* MAIN OPERATION MODE */
/* get a local copy of the actuator controls */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
/* for now only spin if armed and immediately shut down
* if in failsafe
*/
if (armed.armed && !(armed.lockdown || armed.manual_lockdown)) {
ardrone_mixing_and_output(ardrone_write, &actuator_controls);
} else {
/* Silently lock down motor speeds to zero */
ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);
}
}
if (counter % 24 == 0) {
if (led_counter == 0) { ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0, 0); }
if (led_counter == 1) { ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0, 0); }
if (led_counter == 2) { ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0, 0); }
if (led_counter == 3) { ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0, 0); }
if (led_counter == 4) { ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0, 0); }
if (led_counter == 5) { ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0, 0); }
if (led_counter == 6) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0, 0); }
if (led_counter == 7) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0, 0); }
if (led_counter == 8) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0, 0); }
if (led_counter == 9) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0, 1); }
if (led_counter == 10) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1, 1); }
if (led_counter == 11) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1, 0); }
led_counter++;
if (led_counter == 12) { led_counter = 0; }
}
/* run at approximately 200 Hz */
usleep(4500);
counter++;
}
/* restore old UART config */
int termios_state;
if ((termios_state = tcsetattr(ardrone_write, TCSANOW, &uart_config_original)) < 0) {
warnx("ERR: tcsetattr");
}
/* close uarts */
close(ardrone_write);
ar_multiplexing_deinit(gpios);
fflush(stdout);
thread_running = false;
return OK;
}
void
BlinkM::led()
{
if(!topic_initialized) {
vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
orb_set_interval(vehicle_status_sub_fd, 250);
vehicle_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode));
orb_set_interval(vehicle_control_mode_sub_fd, 250);
actuator_armed_sub_fd = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(actuator_armed_sub_fd, 250);
vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
orb_set_interval(vehicle_gps_position_sub_fd, 250);
/* Subscribe to safety topic */
safety_sub_fd = orb_subscribe(ORB_ID(safety));
orb_set_interval(safety_sub_fd, 250);
topic_initialized = true;
}
if(led_thread_ready == true) {
if(!detected_cells_blinked) {
if(num_of_cells > 0) {
t_led_color[0] = LED_PURPLE;
}
if(num_of_cells > 1) {
t_led_color[1] = LED_PURPLE;
}
if(num_of_cells > 2) {
t_led_color[2] = LED_PURPLE;
}
if(num_of_cells > 3) {
t_led_color[3] = LED_PURPLE;
}
if(num_of_cells > 4) {
t_led_color[4] = LED_PURPLE;
}
if(num_of_cells > 5) {
t_led_color[5] = LED_PURPLE;
}
t_led_color[6] = LED_OFF;
t_led_color[7] = LED_OFF;
t_led_blink = LED_BLINK;
} else {
t_led_color[0] = led_color_1;
t_led_color[1] = led_color_2;
t_led_color[2] = led_color_3;
t_led_color[3] = led_color_4;
t_led_color[4] = led_color_5;
t_led_color[5] = led_color_6;
t_led_color[6] = led_color_7;
t_led_color[7] = led_color_8;
t_led_blink = led_blink;
}
led_thread_ready = false;
}
if (led_thread_runcount & 1) {
if (t_led_blink)
setLEDColor(LED_OFF);
led_interval = LED_OFFTIME;
} else {
setLEDColor(t_led_color[(led_thread_runcount / 2) % 8]);
//led_interval = (led_thread_runcount & 1) : LED_ONTIME;
led_interval = LED_ONTIME;
}
if (led_thread_runcount == 15) {
/* obtained data for the first file descriptor */
struct vehicle_status_s vehicle_status_raw;
struct vehicle_control_mode_s vehicle_control_mode;
struct actuator_armed_s actuator_armed;
struct vehicle_gps_position_s vehicle_gps_position_raw;
struct safety_s safety;
memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw));
memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw));
memset(&safety, 0, sizeof(safety));
bool new_data_vehicle_status;
bool new_data_vehicle_control_mode;
bool new_data_actuator_armed;
bool new_data_vehicle_gps_position;
bool new_data_safety;
orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
int no_data_vehicle_status = 0;
int no_data_vehicle_control_mode = 0;
int no_data_actuator_armed = 0;
int no_data_vehicle_gps_position = 0;
if (new_data_vehicle_status) {
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw);
no_data_vehicle_status = 0;
} else {
no_data_vehicle_status++;
if(no_data_vehicle_status >= 3)
no_data_vehicle_status = 3;
}
orb_check(vehicle_control_mode_sub_fd, &new_data_vehicle_control_mode);
if (new_data_vehicle_control_mode) {
orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub_fd, &vehicle_control_mode);
no_data_vehicle_control_mode = 0;
} else {
no_data_vehicle_control_mode++;
if(no_data_vehicle_control_mode >= 3)
no_data_vehicle_control_mode = 3;
}
orb_check(actuator_armed_sub_fd, &new_data_actuator_armed);
if (new_data_actuator_armed) {
orb_copy(ORB_ID(actuator_armed), actuator_armed_sub_fd, &actuator_armed);
no_data_actuator_armed = 0;
} else {
no_data_actuator_armed++;
if(no_data_actuator_armed >= 3)
no_data_actuator_armed = 3;
}
orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position);
if (new_data_vehicle_gps_position) {
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw);
no_data_vehicle_gps_position = 0;
} else {
no_data_vehicle_gps_position++;
if(no_data_vehicle_gps_position >= 3)
no_data_vehicle_gps_position = 3;
}
/* update safety topic */
orb_check(safety_sub_fd, &new_data_safety);
if (new_data_safety) {
orb_copy(ORB_ID(safety), safety_sub_fd, &safety);
}
/* get number of used satellites in navigation */
num_of_used_sats = vehicle_gps_position_raw.satellites_used;
if (new_data_vehicle_status || no_data_vehicle_status < 3) {
if (num_of_cells == 0) {
/* looking for lipo cells that are connected */
printf("<blinkm> checking cells\n");
for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) {
if(vehicle_status_raw.battery_voltage < num_of_cells * MAX_CELL_VOLTAGE) break;
}
printf("<blinkm> cells found:%d\n", num_of_cells);
} else {
if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
/* LED Pattern for battery critical alerting */
led_color_1 = LED_RED;
led_color_2 = LED_RED;
led_color_3 = LED_RED;
led_color_4 = LED_RED;
led_color_5 = LED_RED;
led_color_6 = LED_RED;
led_color_7 = LED_RED;
led_color_8 = LED_RED;
led_blink = LED_BLINK;
} else if(vehicle_status_raw.rc_signal_lost) {
/* LED Pattern for FAILSAFE */
led_color_1 = LED_BLUE;
led_color_2 = LED_BLUE;
led_color_3 = LED_BLUE;
led_color_4 = LED_BLUE;
led_color_5 = LED_BLUE;
led_color_6 = LED_BLUE;
led_color_7 = LED_BLUE;
led_color_8 = LED_BLUE;
led_blink = LED_BLINK;
} else if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW) {
/* LED Pattern for battery low warning */
led_color_1 = LED_YELLOW;
led_color_2 = LED_YELLOW;
led_color_3 = LED_YELLOW;
led_color_4 = LED_YELLOW;
led_color_5 = LED_YELLOW;
led_color_6 = LED_YELLOW;
led_color_7 = LED_YELLOW;
led_color_8 = LED_YELLOW;
led_blink = LED_BLINK;
} else {
/* no battery warnings here */
if(actuator_armed.armed == false) {
/* system not armed */
if(safety.safety_off){
led_color_1 = LED_ORANGE;
led_color_2 = LED_ORANGE;
led_color_3 = LED_ORANGE;
led_color_4 = LED_ORANGE;
led_color_5 = LED_ORANGE;
led_color_6 = LED_ORANGE;
led_color_7 = LED_ORANGE;
led_color_8 = LED_ORANGE;
led_blink = LED_BLINK;
}else{
led_color_1 = LED_CYAN;
led_color_2 = LED_CYAN;
led_color_3 = LED_CYAN;
led_color_4 = LED_CYAN;
led_color_5 = LED_CYAN;
led_color_6 = LED_CYAN;
led_color_7 = LED_CYAN;
led_color_8 = LED_CYAN;
led_blink = LED_NOBLINK;
}
} else {
/* armed system - initial led pattern */
led_color_1 = LED_RED;
led_color_2 = LED_RED;
led_color_3 = LED_RED;
led_color_4 = LED_OFF;
led_color_5 = LED_OFF;
led_color_6 = LED_OFF;
led_color_7 = LED_OFF;
led_color_8 = LED_OFF;
led_blink = LED_BLINK;
if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
/* indicate main control state */
if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_POSCTL)
led_color_4 = LED_GREEN;
/* TODO: add other Auto modes */
else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION)
led_color_4 = LED_BLUE;
else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_ALTCTL)
led_color_4 = LED_YELLOW;
else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_MANUAL)
led_color_4 = LED_WHITE;
else
led_color_4 = LED_OFF;
led_color_5 = led_color_4;
}
if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
/* handling used satus */
if(num_of_used_sats >= 7) {
led_color_1 = LED_OFF;
led_color_2 = LED_OFF;
led_color_3 = LED_OFF;
} else if(num_of_used_sats == 6) {
led_color_2 = LED_OFF;
led_color_3 = LED_OFF;
} else if(num_of_used_sats == 5) {
led_color_3 = LED_OFF;
}
} else {
/* no vehicle_gps_position data */
led_color_1 = LED_WHITE;
led_color_2 = LED_WHITE;
led_color_3 = LED_WHITE;
}
}
}
}
} else {
/* LED Pattern for general Error - no vehicle_status can retrieved */
led_color_1 = LED_WHITE;
led_color_2 = LED_WHITE;
led_color_3 = LED_WHITE;
led_color_4 = LED_WHITE;
led_color_5 = LED_WHITE;
led_color_6 = LED_WHITE;
led_color_7 = LED_WHITE;
led_color_8 = LED_WHITE;
led_blink = LED_BLINK;
}
/*
printf( "<blinkm> Volt:%8.4f\tArmed:%4u\tMode:%4u\tCells:%4u\tNDVS:%4u\tNDSAT:%4u\tSats:%4u\tFix:%4u\tVisible:%4u\n",
vehicle_status_raw.voltage_battery,
vehicle_status_raw.flag_system_armed,
vehicle_status_raw.flight_mode,
num_of_cells,
no_data_vehicle_status,
no_data_vehicle_gps_position,
num_of_used_sats,
vehicle_gps_position_raw.fix_type,
vehicle_gps_position_raw.satellites_visible);
*/
led_thread_runcount=0;
led_thread_ready = true;
led_interval = LED_OFFTIME;
if(detected_cells_runcount < 4){
detected_cells_runcount++;
} else {
detected_cells_blinked = true;
}
} else {
led_thread_runcount++;
}
if(systemstate_run == true) {
/* re-queue ourselves to run again later */
work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval);
} else {
stop_script();
set_rgb(0,0,0);
}
}
void
Navigator::task_main()
{
/* inform about start */
warnx("Initializing..");
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
/* Try to load the geofence:
* if /fs/microsd/etc/geofence.txt load from this file
* else clear geofence data in datamanager */
struct stat buffer;
if (stat(GEOFENCE_FILENAME, &buffer) == 0) {
warnx("Try to load geofence.txt");
_geofence.loadFromFile(GEOFENCE_FILENAME);
} else {
if (_geofence.clearDm() > 0)
warnx("Geofence cleared");
else
warnx("Could not clear geofence");
}
/* do subscriptions */
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_home_pos_sub = orb_subscribe(ORB_ID(home_position));
_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
/* copy all topics first time */
vehicle_status_update();
vehicle_control_mode_update();
global_position_update();
home_position_update();
navigation_capabilities_update();
params_update();
/* rate limit position updates to 50 Hz */
orb_set_interval(_global_pos_sub, 20);
hrt_abstime mavlink_open_time = 0;
const hrt_abstime mavlink_open_interval = 500000;
/* wakeup source(s) */
struct pollfd fds[6];
/* Setup of loop */
fds[0].fd = _global_pos_sub;
fds[0].events = POLLIN;
fds[1].fd = _home_pos_sub;
fds[1].events = POLLIN;
fds[2].fd = _capabilities_sub;
fds[2].events = POLLIN;
fds[3].fd = _vstatus_sub;
fds[3].events = POLLIN;
fds[4].fd = _control_mode_sub;
fds[4].events = POLLIN;
fds[5].fd = _param_update_sub;
fds[5].events = POLLIN;
while (!_task_should_exit) {
/* wait for up to 100ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
if (pret == 0) {
/* timed out - periodic check for _task_should_exit, etc. */
continue;
} else if (pret < 0) {
/* this is undesirable but not much we can do - might want to flag unhappy status */
warn("poll error %d, %d", pret, errno);
continue;
}
perf_begin(_loop_perf);
if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time) {
/* try to reopen the mavlink log device with specified interval */
mavlink_open_time = hrt_abstime() + mavlink_open_interval;
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
}
/* parameters updated */
if (fds[5].revents & POLLIN) {
params_update();
updateParams();
}
/* vehicle control mode updated */
if (fds[4].revents & POLLIN) {
vehicle_control_mode_update();
}
/* vehicle status updated */
if (fds[3].revents & POLLIN) {
vehicle_status_update();
}
/* navigation capabilities updated */
if (fds[2].revents & POLLIN) {
navigation_capabilities_update();
}
/* home position updated */
if (fds[1].revents & POLLIN) {
home_position_update();
}
/* global position updated */
if (fds[0].revents & POLLIN) {
global_position_update();
/* Check geofence violation */
if (!_geofence.inside(&_global_pos)) {
/* Issue a warning about the geofence violation once */
if (!_geofence_violation_warning_sent) {
mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation");
_geofence_violation_warning_sent = true;
}
} else {
/* Reset the _geofence_violation_warning_sent field */
_geofence_violation_warning_sent = false;
}
}
/* Do stuff according to navigation state set by commander */
switch (_vstatus.nav_state) {
case NAVIGATION_STATE_MANUAL:
case NAVIGATION_STATE_ACRO:
case NAVIGATION_STATE_ALTCTL:
case NAVIGATION_STATE_POSCTL:
_navigation_mode = nullptr;
_can_loiter_at_sp = false;
break;
case NAVIGATION_STATE_AUTO_MISSION:
_navigation_mode = &_mission;
break;
case NAVIGATION_STATE_AUTO_LOITER:
_navigation_mode = &_loiter;
break;
case NAVIGATION_STATE_AUTO_RTL:
_navigation_mode = &_rtl;
break;
case NAVIGATION_STATE_AUTO_RTGS:
_navigation_mode = &_rtl; /* TODO: change this to something else */
break;
case NAVIGATION_STATE_LAND:
case NAVIGATION_STATE_TERMINATION:
default:
_navigation_mode = nullptr;
_can_loiter_at_sp = false;
break;
}
/* iterate through navigation modes and set active/inactive for each */
for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
if (_navigation_mode == _navigation_mode_array[i]) {
_update_triplet = _navigation_mode_array[i]->on_active(&_pos_sp_triplet);
} else {
_navigation_mode_array[i]->on_inactive();
}
}
/* if nothing is running, set position setpoint triplet invalid */
if (_navigation_mode == nullptr) {
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = false;
_pos_sp_triplet.next.valid = false;
_update_triplet = true;
}
if (_update_triplet) {
publish_position_setpoint_triplet();
_update_triplet = false;
}
perf_end(_loop_perf);
}
warnx("exiting.");
_navigator_task = -1;
_exit(0);
}
static int
mpc_thread_main(int argc, char *argv[])
{
/* welcome user */
printf("[multirotor pos control] Control started, taking over position control\n");
/* structures */
struct vehicle_status_s state;
struct vehicle_attitude_s att;
//struct vehicle_global_position_setpoint_s global_pos_sp;
struct vehicle_local_position_setpoint_s local_pos_sp;
struct vehicle_local_position_s local_pos;
struct manual_control_setpoint_s manual;
struct vehicle_attitude_setpoint_s att_sp;
/* subscribe to attitude, motor setpoints and system state */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
//int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
/* publish attitude setpoint */
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
while (1) {
/* get a local copy of the vehicle state */
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
/* get a local copy of manual setpoint */
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
/* get a local copy of local position */
orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
/* get a local copy of local position setpoint */
orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp);
if (state.state_machine == SYSTEM_STATE_AUTO) {
position_control(&state, &manual, &att, &local_pos, &local_pos_sp, &att_sp);
/* publish new attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
} else if (state.state_machine == SYSTEM_STATE_STABILIZE) {
/* set setpoint to current position */
// XXX select pos reset channel on remote
/* reset setpoint to current position (position hold) */
// if (1 == 2) {
// local_pos_sp.x = local_pos.x;
// local_pos_sp.y = local_pos.y;
// local_pos_sp.z = local_pos.z;
// local_pos_sp.yaw = att.yaw;
// }
}
/* run at approximately 50 Hz */
usleep(20000);
counter++;
}
/* close uarts */
close(ardrone_write);
ar_multiplexing_deinit(gpios);
printf("[multirotor pos control] ending now...\r\n");
fflush(stdout);
return 0;
}
calibrate_return calibrate_from_orientation(int mavlink_fd,
int cancel_sub,
bool side_data_collected[detect_orientation_side_count],
calibration_from_orientation_worker_t calibration_worker,
void* worker_data,
bool lenient_still_position)
{
calibrate_return result = calibrate_return_ok;
// Setup subscriptions to onboard accel sensor
int sub_accel = orb_subscribe(ORB_ID(sensor_combined));
if (sub_accel < 0) {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "No onboard accel");
return calibrate_return_error;
}
unsigned orientation_failures = 0;
// Rotate through all requested orientation
while (true) {
if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
result = calibrate_return_cancelled;
break;
}
if (orientation_failures > 4) {
result = calibrate_return_error;
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "timeout: no motion");
break;
}
unsigned int side_complete_count = 0;
// Update the number of completed sides
for (unsigned i = 0; i < detect_orientation_side_count; i++) {
if (side_data_collected[i]) {
side_complete_count++;
}
}
if (side_complete_count == detect_orientation_side_count) {
// We have completed all sides, move on
break;
}
/* inform user which orientations are still needed */
char pendingStr[256];
pendingStr[0] = 0;
for (unsigned int cur_orientation=0; cur_orientation<detect_orientation_side_count; cur_orientation++) {
if (!side_data_collected[cur_orientation]) {
strcat(pendingStr, " ");
strcat(pendingStr, detect_orientation_str((enum detect_orientation_return)cur_orientation));
}
}
mavlink_and_console_log_info(mavlink_fd, "[cal] pending:%s", pendingStr);
mavlink_and_console_log_info(mavlink_fd, "[cal] hold vehicle still on a pending side");
enum detect_orientation_return orient = detect_orientation(mavlink_fd, cancel_sub, sub_accel, lenient_still_position);
if (orient == DETECT_ORIENTATION_ERROR) {
orientation_failures++;
mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still...");
continue;
}
/* inform user about already handled side */
if (side_data_collected[orient]) {
orientation_failures++;
mavlink_and_console_log_critical(mavlink_fd, "%s side already completed", detect_orientation_str(orient));
mavlink_and_console_log_critical(mavlink_fd, "rotate to a pending side");
continue;
}
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient));
orientation_failures = 0;
// Call worker routine
result = calibration_worker(orient, cancel_sub, worker_data);
if (result != calibrate_return_ok ) {
break;
}
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient));
// Note that this side is complete
side_data_collected[orient] = true;
tune_neutral(true);
usleep(200000);
}
if (sub_accel >= 0) {
px4_close(sub_accel);
}
return result;
}
void VtolAttitudeControl::task_main()
{
fflush(stdout);
/* do subscriptions */
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_mc_virtual_att_sp_sub = orb_subscribe(ORB_ID(mc_virtual_attitude_setpoint));
_fw_virtual_att_sp_sub = orb_subscribe(ORB_ID(fw_virtual_attitude_setpoint));
_mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint));
_fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint));
_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_battery_status_sub = orb_subscribe(ORB_ID(battery_status));
_vehicle_cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
_tecs_status_sub = orb_subscribe(ORB_ID(tecs_status));
_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
_actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc));
_actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw));
parameters_update(); // initialize parameter cache
/* update vtol vehicle status*/
_vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false;
// make sure we start with idle in mc mode
_vtol_type->set_idle_mc();
/* wakeup source*/
px4_pollfd_struct_t fds[3] = {}; /*input_mc, input_fw, parameters*/
fds[0].fd = _actuator_inputs_mc;
fds[0].events = POLLIN;
fds[1].fd = _actuator_inputs_fw;
fds[1].events = POLLIN;
fds[2].fd = _params_sub;
fds[2].events = POLLIN;
while (!_task_should_exit) {
/*Advertise/Publish vtol vehicle status*/
_vtol_vehicle_status.timestamp = hrt_absolute_time();
if (_vtol_vehicle_status_pub != nullptr) {
orb_publish(ORB_ID(vtol_vehicle_status), _vtol_vehicle_status_pub, &_vtol_vehicle_status);
} else {
_vtol_vehicle_status_pub = orb_advertise(ORB_ID(vtol_vehicle_status), &_vtol_vehicle_status);
}
/* wait for up to 100ms for data */
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit */
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("poll error %d, %d", pret, errno);
/* sleep a bit before next try */
usleep(100000);
continue;
}
if (fds[2].revents & POLLIN) { //parameters were updated, read them now
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
/* update parameters from storage */
parameters_update();
}
_vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false;
mc_virtual_att_sp_poll();
fw_virtual_att_sp_poll();
vehicle_control_mode_poll(); //Check for changes in vehicle control mode.
vehicle_manual_poll(); //Check for changes in manual inputs.
arming_status_poll(); //Check for arming status updates.
vehicle_attitude_setpoint_poll();//Check for changes in attitude set points
vehicle_attitude_poll(); //Check for changes in attitude
actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output
actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output
vehicle_rates_sp_mc_poll();
vehicle_rates_sp_fw_poll();
parameters_update_poll();
vehicle_local_pos_poll(); // Check for new sensor values
vehicle_airspeed_poll();
vehicle_battery_poll();
vehicle_cmd_poll();
tecs_status_poll();
land_detected_poll();
// update the vtol state machine which decides which mode we are in
_vtol_type->update_vtol_state();
// reset transition command if not auto control
if (_v_control_mode.flag_control_manual_enabled) {
if (_vtol_type->get_mode() == ROTARY_WING) {
_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
} else if (_vtol_type->get_mode() == FIXED_WING) {
_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;
} else if (_vtol_type->get_mode() == TRANSITION_TO_MC) {
/* We want to make sure that a mode change (manual>auto) during the back transition
* doesn't result in an unsafe state. This prevents the instant fall back to
* fixed-wing on the switch from manual to auto */
_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
}
}
// check in which mode we are in and call mode specific functions
if (_vtol_type->get_mode() == ROTARY_WING) {
// vehicle is in rotary wing mode
_vtol_vehicle_status.vtol_in_rw_mode = true;
_vtol_vehicle_status.vtol_in_trans_mode = false;
// got data from mc attitude controller
if (fds[0].revents & POLLIN) {
orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in);
_vtol_type->update_mc_state();
fill_mc_att_rates_sp();
}
} else if (_vtol_type->get_mode() == FIXED_WING) {
// vehicle is in fw mode
_vtol_vehicle_status.vtol_in_rw_mode = false;
_vtol_vehicle_status.vtol_in_trans_mode = false;
// got data from fw attitude controller
if (fds[1].revents & POLLIN) {
orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in);
vehicle_manual_poll();
_vtol_type->update_fw_state();
fill_fw_att_rates_sp();
}
} else if (_vtol_type->get_mode() == TRANSITION_TO_MC || _vtol_type->get_mode() == TRANSITION_TO_FW) {
// vehicle is doing a transition
_vtol_vehicle_status.vtol_in_trans_mode = true;
_vtol_vehicle_status.vtol_in_rw_mode = true; //making mc attitude controller work during transition
_vtol_vehicle_status.in_transition_to_fw = (_vtol_type->get_mode() == TRANSITION_TO_FW);
bool got_new_data = false;
if (fds[0].revents & POLLIN) {
orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in);
got_new_data = true;
}
if (fds[1].revents & POLLIN) {
orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in);
got_new_data = true;
}
// update transition state if got any new data
if (got_new_data) {
_vtol_type->update_transition_state();
fill_mc_att_rates_sp();
publish_att_sp();
}
} else if (_vtol_type->get_mode() == EXTERNAL) {
// we are using external module to generate attitude/thrust setpoint
_vtol_type->update_external_state();
}
publish_att_sp();
_vtol_type->fill_actuator_outputs();
/* Only publish if the proper mode(s) are enabled */
if (_v_control_mode.flag_control_attitude_enabled ||
_v_control_mode.flag_control_rates_enabled ||
_v_control_mode.flag_control_manual_enabled) {
if (_actuators_0_pub != nullptr) {
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
} else {
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
}
if (_actuators_1_pub != nullptr) {
orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
} else {
_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
}
}
// publish the attitude rates setpoint
if (_v_rates_sp_pub != nullptr) {
orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
} else {
_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
}
}
warnx("exit");
_control_task = -1;
return;
}
calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
int cancel_sub,
bool side_data_collected[detect_orientation_side_count],
calibration_from_orientation_worker_t calibration_worker,
void *worker_data,
bool lenient_still_position)
{
calibrate_return result = calibrate_return_ok;
// Setup subscriptions to onboard accel sensor
int sub_accel = orb_subscribe(ORB_ID(sensor_combined));
if (sub_accel < 0) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "No onboard accel");
return calibrate_return_error;
}
unsigned orientation_failures = 0;
// Rotate through all requested orientation
while (true) {
if (calibrate_cancel_check(mavlink_log_pub, cancel_sub)) {
result = calibrate_return_cancelled;
break;
}
if (orientation_failures > 4) {
result = calibrate_return_error;
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "timeout: no motion");
break;
}
unsigned int side_complete_count = 0;
// Update the number of completed sides
for (unsigned i = 0; i < detect_orientation_side_count; i++) {
if (side_data_collected[i]) {
side_complete_count++;
}
}
if (side_complete_count == detect_orientation_side_count) {
// We have completed all sides, move on
break;
}
/* inform user which orientations are still needed */
char pendingStr[80];
pendingStr[0] = 0;
for (unsigned int cur_orientation = 0; cur_orientation < detect_orientation_side_count; cur_orientation++) {
if (!side_data_collected[cur_orientation]) {
strncat(pendingStr, " ", sizeof(pendingStr) - 1);
strncat(pendingStr, detect_orientation_str((enum detect_orientation_return)cur_orientation), sizeof(pendingStr) - 1);
}
}
calibration_log_info(mavlink_log_pub, "[cal] pending:%s", pendingStr);
usleep(20000);
calibration_log_info(mavlink_log_pub, "[cal] hold vehicle still on a pending side");
usleep(20000);
enum detect_orientation_return orient = detect_orientation(mavlink_log_pub, cancel_sub, sub_accel,
lenient_still_position);
if (orient == DETECT_ORIENTATION_ERROR) {
orientation_failures++;
calibration_log_info(mavlink_log_pub, "[cal] detected motion, hold still...");
usleep(20000);
continue;
}
/* inform user about already handled side */
if (side_data_collected[orient]) {
orientation_failures++;
set_tune(TONE_NOTIFY_NEGATIVE_TUNE);
calibration_log_info(mavlink_log_pub, "[cal] %s side already completed", detect_orientation_str(orient));
usleep(20000);
continue;
}
calibration_log_info(mavlink_log_pub, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient));
usleep(20000);
calibration_log_info(mavlink_log_pub, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient));
usleep(20000);
orientation_failures = 0;
// Call worker routine
result = calibration_worker(orient, cancel_sub, worker_data);
if (result != calibrate_return_ok) {
break;
}
calibration_log_info(mavlink_log_pub, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient));
usleep(20000);
calibration_log_info(mavlink_log_pub, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient));
usleep(20000);
// Note that this side is complete
side_data_collected[orient] = true;
// output neutral tune
set_tune(TONE_NOTIFY_NEUTRAL_TUNE);
// temporary priority boost for the white blinking led to come trough
rgbled_set_color_and_mode(led_control_s::COLOR_WHITE, led_control_s::MODE_BLINK_FAST, 3, 1);
usleep(200000);
}
if (sub_accel >= 0) {
px4_close(sub_accel);
}
return result;
}
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 == (uint32_t)device_id[cur_accel]) {
// Device IDs match, correct ORB instance for this accel
found_cur_accel = true;
// store initial timestamp - used to infer which sensors are active
timestamps[cur_accel] = report.timestamp;
} else {
orb_unsubscribe(worker_data.subs[cur_accel]);
}
#else
// For the DriverFramework drivers, we fill device ID (this is the first time) by copying one report.
device_id[cur_accel] = report.device_id;
found_cur_accel = true;
#endif
}
if(!found_cur_accel) {
calibration_log_critical(mavlink_log_pub, "Accel #%u (ID %u) no matching uORB devid", cur_accel, device_id[cur_accel]);
result = calibrate_return_error;
break;
}
if (device_id[cur_accel] != 0) {
// Get priority
int32_t prio;
orb_priority(worker_data.subs[cur_accel], &prio);
if (prio > device_prio_max) {
device_prio_max = prio;
device_id_primary = device_id[cur_accel];
}
} else {
calibration_log_critical(mavlink_log_pub, "Accel #%u no device id, abort", cur_accel);
result = calibrate_return_error;
break;
}
}
if (result == calibrate_return_ok) {
int cancel_sub = calibrate_cancel_subscribe();
result = calibrate_from_orientation(mavlink_log_pub, cancel_sub, data_collected, accel_calibration_worker, &worker_data, false /* normal still */);
calibrate_cancel_unsubscribe(cancel_sub);
}
/* close all subscriptions */
for (unsigned i = 0; i < max_accel_sens; i++) {
if (worker_data.subs[i] >= 0) {
/* figure out which sensors were active */
struct accel_report arp = {};
(void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) {
(*active_sensors)++;
}
px4_close(worker_data.subs[i]);
}
}
orb_unsubscribe(worker_data.sensor_correction_sub);
if (result == calibrate_return_ok) {
/* calculate offsets and transform matrix */
for (unsigned i = 0; i < (*active_sensors); i++) {
result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
if (result != calibrate_return_ok) {
calibration_log_critical(mavlink_log_pub, "ERROR: calibration calculation error");
break;
}
}
}
return result;
}
int calibrate_cancel_subscribe()
{
return orb_subscribe(ORB_ID(vehicle_command));
}
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;");
}
/* if we are in rotary wing mode, do nothing */
if (_vehicle_status.is_rotary_wing) {
continue;
}
/* 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 (!_vehicle_status.is_rotary_wing) {
if (_attitude_sp_pub != nullptr) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp);
} else {
/* 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 != 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.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 != 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;
_exit(0);
}
void task_main(int argc, char *argv[])
{
_is_running = true;
if (uart_initialize(_device) < 0) {
PX4_ERR("Failed to initialize UART.");
return;
}
// Subscribe for orb topics
_controls_sub = orb_subscribe(ORB_ID(actuator_controls_0));
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
// Start disarmed
_armed.armed = false;
_armed.prearmed = false;
// Set up poll topic
px4_pollfd_struct_t fds[1];
fds[0].fd = _controls_sub;
fds[0].events = POLLIN;
/* Don't limit poll intervall for now, 250 Hz should be fine. */
//orb_set_interval(_controls_sub, 10);
// Set up mixer
if (initialize_mixer(MIXER_FILENAME) < 0) {
PX4_ERR("Mixer initialization failed.");
return;
}
pwm_limit_init(&_pwm_limit);
// TODO XXX: this is needed otherwise we crash in the callback context.
_rc_pub = orb_advertise(ORB_ID(input_rc), &_rc);
// Main loop
while (!_task_should_exit) {
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 10);
/* Timed out, do a periodic check for _task_should_exit. */
if (pret == 0) {
continue;
}
/* This is undesirable but not much we can do. */
if (pret < 0) {
PX4_WARN("poll error %d, %d", pret, errno);
/* sleep a bit before next try */
usleep(100000);
continue;
}
if (fds[0].revents & POLLIN) {
orb_copy(ORB_ID(actuator_controls_0), _controls_sub, &_controls);
_outputs.timestamp = _controls.timestamp;
/* do mixing */
_outputs.noutputs = _mixer->mix(_outputs.output, 0 /* not used */, NULL);
/* disable unused ports by setting their output to NaN */
for (size_t i = _outputs.noutputs; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
_outputs.output[i] = NAN;
}
const uint16_t reverse_mask = 0;
uint16_t disarmed_pwm[4];
uint16_t min_pwm[4];
uint16_t max_pwm[4];
for (unsigned int i = 0; i < 4; i++) {
disarmed_pwm[i] = _pwm_disarmed;
min_pwm[i] = _pwm_min;
max_pwm[i] = _pwm_max;
}
uint16_t pwm[4];
// TODO FIXME: pre-armed seems broken
pwm_limit_calc(_armed.armed, false/*_armed.prearmed*/, _outputs.noutputs, reverse_mask,
disarmed_pwm, min_pwm, max_pwm, _outputs.output, pwm, &_pwm_limit);
send_outputs_mavlink(pwm, 4);
if (_outputs_pub != nullptr) {
orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);
} else {
_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
}
}
bool updated;
orb_check(_armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
}
}
uart_deinitialize();
orb_unsubscribe(_controls_sub);
orb_unsubscribe(_armed_sub);
_is_running = false;
}
/*******************************************************************************
* *nav_loop()
*
* Navigation control loop of the fixedwing controller
* This loop calculates the roll,pitch and throttle setpoints, which are needed
* to attain the desired heading, altitude and velocity.
*
* Input: none
*
* Output: none
*
******************************************************************************/
static void *nav_loop(void *arg)
{
// Set thread name
prctl(1, "fixedwing_control nav", getpid());
/* Set up to publish fixed wing control messages */
struct fixedwing_control_s control;
int fixedwing_control_pub = orb_advertise(ORB_ID(fixedwing_control), &control);
/* Subscribe to global position, attitude and rc */
struct vehicle_global_position_s global_pos;
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
struct vehicle_attitude_s att;
int attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
struct rc_channels_s rc;
int rc_sub = orb_subscribe(ORB_ID(rc_channels));
while (1) {
/* get position, attitude and rc inputs */
// XXX add error checking
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &attitude);
orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
/* scaling factors are defined by the data from the APM Planner
* TODO: ifdef for other parameters (HIL/Real world switch)
*/
plane_data.lat = global_pos.lat / 10000000;
plane_data.lon = global_pos.lon / 10000000;
plane_data.alt = global_pos.alt / 1000;
plane_data.vx = global_pos.vx / 100;
plane_data.vy = global_pos.vy / 100;
plane_data.vz = global_pos.vz / 100;
plane_data.roll = att.roll;
plane_data.pitch = att.pitch;
plane_data.yaw = att.yaw;
plane_data.rollspeed = att.rollspeed;
plane_data.pitchspeed = att.pitchspeed;
plane_data.yawspeed = att.yawspeed;
// Get GPS Waypoint
// if(global_data_wait(&global_data_position_setpoint->access_conf) == 0)
// {
// plane_data.wp_x = global_data_position_setpoint->x;
// plane_data.wp_y = global_data_position_setpoint->y;
// plane_data.wp_z = global_data_position_setpoint->z;
// }
// global_data_unlock(&global_data_position_setpoint->access_conf);
if (global_data_rc_channels->chan[global_data_rc_channels->function[OVERRIDE]].scale < MANUAL) { // AUTO mode
// AUTO/HYBRID switch
if (global_data_rc_channels->chan[global_data_rc_channels->function[OVERRIDE]].scale < AUTO) {
plane_data.roll_setpoint = calc_roll_setpoint();
plane_data.pitch_setpoint = calc_pitch_setpoint();
plane_data.throttle_setpoint = calc_throttle_setpoint();
} else {
plane_data.roll_setpoint = global_data_rc_channels->chan[global_data_rc_channels->function[ROLL]].scale / 200;
plane_data.pitch_setpoint = global_data_rc_channels->chan[global_data_rc_channels->function[PITCH]].scale / 200;
plane_data.throttle_setpoint = global_data_rc_channels->chan[global_data_rc_channels->function[THROTTLE]].scale / 200;
}
//control_outputs.yaw_rudder = calc_yaw_rudder(plane_data.hdg);
// 10 Hz loop
usleep(100000);
} else {
control->attitude_control_output[ROLL] = global_data_rc_channels->chan[global_data_rc_channels->function[ROLL]].scale;
control->attitude_control_output[PITCH] = global_data_rc_channels->chan[global_data_rc_channels->function[PITCH]].scale;
control->attitude_control_output[THROTTLE] = global_data_rc_channels->chan[global_data_rc_channels->function[THROTTLE]].scale;
// since we don't have a yaw rudder
//global_data_fixedwing_control->attitude_control_output[3] = global_data_rc_channels->chan[YAW].scale;
control->counter++;
control->timestamp = hrt_absolute_time();
#error Either publish here or above, not at two locations
orb_publish(ORB_ID(fixedwing_control), fixedwing_control_pub, &control);
usleep(100000);
}
}
return NULL;
}
void Ekf2::task_main()
{
// subscribe to relevant topics
_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
_range_finder_sub = orb_subscribe(ORB_ID(distance_sensor));
px4_pollfd_struct_t fds[2] = {};
fds[0].fd = _sensors_sub;
fds[0].events = POLLIN;
fds[1].fd = _params_sub;
fds[1].events = POLLIN;
// initialise parameter cache
updateParams();
// initialize data structures outside of loop
// because they will else not always be
// properly populated
sensor_combined_s sensors = {};
vehicle_gps_position_s gps = {};
airspeed_s airspeed = {};
vehicle_control_mode_s vehicle_control_mode = {};
optical_flow_s optical_flow = {};
distance_sensor_s range_finder = {};
while (!_task_should_exit) {
int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);
if (ret < 0) {
// Poll error, sleep and try again
usleep(10000);
continue;
} else if (ret == 0) {
// Poll timeout or no new data, do nothing
continue;
}
if (fds[1].revents & POLLIN) {
// read from param to clear updated flag
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
updateParams();
// fetch sensor data in next loop
continue;
} else if (!(fds[0].revents & POLLIN)) {
// no new data
continue;
}
bool gps_updated = false;
bool airspeed_updated = false;
bool vehicle_status_updated = false;
bool optical_flow_updated = false;
bool range_finder_updated = false;
orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors);
// update all other topics if they have new data
orb_check(_gps_sub, &gps_updated);
if (gps_updated) {
orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps);
}
orb_check(_airspeed_sub, &airspeed_updated);
if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed);
}
orb_check(_optical_flow_sub, &optical_flow_updated);
if (optical_flow_updated) {
orb_copy(ORB_ID(optical_flow), _optical_flow_sub, &optical_flow);
}
orb_check(_range_finder_sub, &range_finder_updated);
if (range_finder_updated) {
orb_copy(ORB_ID(distance_sensor), _range_finder_sub, &range_finder);
}
// in replay mode we are getting the actual timestamp from the sensor topic
hrt_abstime now = 0;
if (_replay_mode) {
now = sensors.timestamp;
} else {
now = hrt_absolute_time();
}
// push imu data into estimator
_ekf->setIMUData(now, sensors.gyro_integral_dt[0], sensors.accelerometer_integral_dt[0],
&sensors.gyro_integral_rad[0], &sensors.accelerometer_integral_m_s[0]);
// read mag data
_ekf->setMagData(sensors.magnetometer_timestamp[0], &sensors.magnetometer_ga[0]);
// read baro data
_ekf->setBaroData(sensors.baro_timestamp[0], &sensors.baro_alt_meter[0]);
// read gps data if available
if (gps_updated) {
struct gps_message gps_msg = {};
gps_msg.time_usec = gps.timestamp_position;
gps_msg.lat = gps.lat;
gps_msg.lon = gps.lon;
gps_msg.alt = gps.alt;
gps_msg.fix_type = gps.fix_type;
gps_msg.eph = gps.eph;
gps_msg.epv = gps.epv;
gps_msg.sacc = gps.s_variance_m_s;
gps_msg.time_usec_vel = gps.timestamp_velocity;
gps_msg.vel_m_s = gps.vel_m_s;
gps_msg.vel_ned[0] = gps.vel_n_m_s;
gps_msg.vel_ned[1] = gps.vel_e_m_s;
gps_msg.vel_ned[2] = gps.vel_d_m_s;
gps_msg.vel_ned_valid = gps.vel_ned_valid;
gps_msg.nsats = gps.satellites_used;
//TODO add gdop to gps topic
gps_msg.gdop = 0.0f;
_ekf->setGpsData(gps.timestamp_position, &gps_msg);
}
// read airspeed data if available
if (airspeed_updated) {
_ekf->setAirspeedData(airspeed.timestamp, &airspeed.true_airspeed_m_s); // Only TAS is now fed into the estimator
}
if (optical_flow_updated) {
flow_message flow;
flow.flowdata(0) = optical_flow.pixel_flow_x_integral;
flow.flowdata(1) = optical_flow.pixel_flow_y_integral;
flow.quality = optical_flow.quality;
flow.gyrodata(0) = optical_flow.gyro_x_rate_integral;
flow.gyrodata(1) = optical_flow.gyro_y_rate_integral;
flow.dt = optical_flow.integration_timespan;
if (!isnan(optical_flow.pixel_flow_y_integral) && !isnan(optical_flow.pixel_flow_x_integral)) {
_ekf->setOpticalFlowData(optical_flow.timestamp, &flow);
}
}
if (range_finder_updated) {
_ekf->setRangeData(range_finder.timestamp, &range_finder.current_distance);
}
// read vehicle status if available for 'landed' information
orb_check(_vehicle_status_sub, &vehicle_status_updated);
if (vehicle_status_updated) {
struct vehicle_status_s status = {};
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &status);
_ekf->set_in_air_status(!status.condition_landed);
_ekf->set_arm_status(status.arming_state & vehicle_status_s::ARMING_STATE_ARMED);
}
// run the EKF update and output
if (_ekf->update()) {
// generate vehicle attitude quaternion data
struct vehicle_attitude_s att = {};
_ekf->copy_quaternion(att.q);
matrix::Quaternion<float> q(att.q[0], att.q[1], att.q[2], att.q[3]);
// generate control state data
control_state_s ctrl_state = {};
ctrl_state.timestamp = hrt_absolute_time();
ctrl_state.roll_rate = _lp_roll_rate.apply(sensors.gyro_rad_s[0]);
ctrl_state.pitch_rate = _lp_pitch_rate.apply(sensors.gyro_rad_s[1]);
ctrl_state.yaw_rate = _lp_yaw_rate.apply(sensors.gyro_rad_s[2]);
ctrl_state.q[0] = q(0);
ctrl_state.q[1] = q(1);
ctrl_state.q[2] = q(2);
ctrl_state.q[3] = q(3);
// publish control state data
if (_control_state_pub == nullptr) {
_control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);
} else {
orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state);
}
// generate remaining vehicle attitude data
att.timestamp = hrt_absolute_time();
matrix::Euler<float> euler(q);
att.roll = euler(0);
att.pitch = euler(1);
att.yaw = euler(2);
att.q[0] = q(0);
att.q[1] = q(1);
att.q[2] = q(2);
att.q[3] = q(3);
att.q_valid = true;
att.rollspeed = sensors.gyro_rad_s[0];
att.pitchspeed = sensors.gyro_rad_s[1];
att.yawspeed = sensors.gyro_rad_s[2];
// publish vehicle attitude data
if (_att_pub == nullptr) {
_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
} else {
orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
}
// generate vehicle local position data
struct vehicle_local_position_s lpos = {};
float pos[3] = {};
float vel[3] = {};
lpos.timestamp = hrt_absolute_time();
// Position in local NED frame
_ekf->copy_position(pos);
lpos.x = pos[0];
lpos.y = pos[1];
lpos.z = pos[2];
// Velocity in NED frame (m/s)
_ekf->copy_velocity(vel);
lpos.vx = vel[0];
lpos.vy = vel[1];
lpos.vz = vel[2];
// TODO: better status reporting
lpos.xy_valid = _ekf->local_position_is_valid();
lpos.z_valid = true;
lpos.v_xy_valid = _ekf->local_position_is_valid();
lpos.v_z_valid = true;
// Position of local NED origin in GPS / WGS84 frame
struct map_projection_reference_s ekf_origin = {};
// true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon)
_ekf->get_ekf_origin(&lpos.ref_timestamp, &ekf_origin, &lpos.ref_alt);
lpos.xy_global = _ekf->global_position_is_valid();
lpos.z_global = true; // true if z is valid and has valid global reference (ref_alt)
lpos.ref_lat = ekf_origin.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees
lpos.ref_lon = ekf_origin.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees
// The rotation of the tangent plane vs. geographical north
lpos.yaw = att.yaw;
float terrain_vpos;
lpos.dist_bottom_valid = _ekf->get_terrain_vert_pos(&terrain_vpos);
lpos.dist_bottom = terrain_vpos - pos[2]; // Distance to bottom surface (ground) in meters
lpos.dist_bottom_rate = -vel[2]; // Distance to bottom surface (ground) change rate
lpos.surface_bottom_timestamp = hrt_absolute_time(); // Time when new bottom surface found
// TODO: uORB definition does not define what these variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres
Vector3f pos_var, vel_var;
_ekf->get_pos_var(pos_var);
_ekf->get_vel_var(vel_var);
lpos.eph = sqrt(pos_var(0) + pos_var(1));
lpos.epv = sqrt(pos_var(2));
// publish vehicle local position data
if (_lpos_pub == nullptr) {
_lpos_pub = orb_advertise(ORB_ID(vehicle_local_position), &lpos);
} else {
orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos);
}
// generate and publish global position data
struct vehicle_global_position_s global_pos = {};
if (_ekf->global_position_is_valid()) {
global_pos.timestamp = hrt_absolute_time(); // Time of this estimate, in microseconds since system start
global_pos.time_utc_usec = gps.time_utc_usec; // GPS UTC timestamp in microseconds
double est_lat, est_lon;
map_projection_reproject(&ekf_origin, lpos.x, lpos.y, &est_lat, &est_lon);
global_pos.lat = est_lat; // Latitude in degrees
global_pos.lon = est_lon; // Longitude in degrees
global_pos.alt = -pos[2] + lpos.ref_alt; // Altitude AMSL in meters
global_pos.vel_n = vel[0]; // Ground north velocity, m/s
global_pos.vel_e = vel[1]; // Ground east velocity, m/s
global_pos.vel_d = vel[2]; // Ground downside velocity, m/s
global_pos.yaw = euler(2); // Yaw in radians -PI..+PI.
global_pos.eph = sqrt(pos_var(0) + pos_var(1));; // Standard deviation of position estimate horizontally
global_pos.epv = sqrt(pos_var(2)); // Standard deviation of position vertically
// TODO: implement terrain estimator
global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84
global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid
// TODO use innovatun consistency check timouts to set this
global_pos.dead_reckoning = false; // True if this position is estimated through dead-reckoning
global_pos.pressure_alt = sensors.baro_alt_meter[0]; // Pressure altitude AMSL (m)
if (_vehicle_global_position_pub == nullptr) {
_vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
} else {
orb_publish(ORB_ID(vehicle_global_position), _vehicle_global_position_pub, &global_pos);
}
}
}
// publish estimator status
struct estimator_status_s status = {};
status.timestamp = hrt_absolute_time();
_ekf->get_state_delayed(status.states);
_ekf->get_covariances(status.covariances);
//status.gps_check_fail_flags = _ekf->_gps_check_fail_status.value;
if (_estimator_status_pub == nullptr) {
_estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &status);
} else {
orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &status);
}
// publish estimator innovation data
struct ekf2_innovations_s innovations = {};
innovations.timestamp = hrt_absolute_time();
_ekf->get_vel_pos_innov(&innovations.vel_pos_innov[0]);
_ekf->get_mag_innov(&innovations.mag_innov[0]);
_ekf->get_heading_innov(&innovations.heading_innov);
_ekf->get_airspeed_innov(&innovations.airspeed_innov);
_ekf->get_flow_innov(&innovations.flow_innov[0]);
_ekf->get_hagl_innov(&innovations.hagl_innov);
_ekf->get_vel_pos_innov_var(&innovations.vel_pos_innov_var[0]);
_ekf->get_mag_innov_var(&innovations.mag_innov_var[0]);
_ekf->get_heading_innov_var(&innovations.heading_innov_var);
_ekf->get_airspeed_innov_var(&innovations.airspeed_innov_var);
_ekf->get_flow_innov_var(&innovations.flow_innov_var[0]);
_ekf->get_hagl_innov_var(&innovations.hagl_innov_var);
if (_estimator_innovations_pub == nullptr) {
_estimator_innovations_pub = orb_advertise(ORB_ID(ekf2_innovations), &innovations);
} else {
orb_publish(ORB_ID(ekf2_innovations), _estimator_innovations_pub, &innovations);
}
// save the declination to the EKF2_MAG_DECL parameter when a dis-arm event is detected
if ((_params->mag_declination_source & (1 << 1)) && _prev_motors_armed && !vehicle_control_mode.flag_armed) {
float decl_deg;
_ekf->copy_mag_decl_deg(&decl_deg);
_mag_declination_deg->set(decl_deg);
}
// publish replay message if in replay mode
bool publish_replay_message = (bool)_param_record_replay_msg->get();
if (publish_replay_message) {
struct ekf2_replay_s replay = {};
replay.time_ref = now;
replay.gyro_integral_dt = sensors.gyro_integral_dt[0];
replay.accelerometer_integral_dt = sensors.accelerometer_integral_dt[0];
replay.magnetometer_timestamp = sensors.magnetometer_timestamp[0];
replay.baro_timestamp = sensors.baro_timestamp[0];
memcpy(&replay.gyro_integral_rad[0], &sensors.gyro_integral_rad[0], sizeof(replay.gyro_integral_rad));
memcpy(&replay.accelerometer_integral_m_s[0], &sensors.accelerometer_integral_m_s[0],
sizeof(replay.accelerometer_integral_m_s));
memcpy(&replay.magnetometer_ga[0], &sensors.magnetometer_ga[0], sizeof(replay.magnetometer_ga));
replay.baro_alt_meter = sensors.baro_alt_meter[0];
// only write gps data if we had a gps update.
if (gps_updated) {
replay.time_usec = gps.timestamp_position;
replay.time_usec_vel = gps.timestamp_velocity;
replay.lat = gps.lat;
replay.lon = gps.lon;
replay.alt = gps.alt;
replay.fix_type = gps.fix_type;
replay.nsats = gps.satellites_used;
replay.eph = gps.eph;
replay.epv = gps.epv;
replay.sacc = gps.s_variance_m_s;
replay.vel_m_s = gps.vel_m_s;
replay.vel_n_m_s = gps.vel_n_m_s;
replay.vel_e_m_s = gps.vel_e_m_s;
replay.vel_d_m_s = gps.vel_d_m_s;
replay.vel_ned_valid = gps.vel_ned_valid;
} else {
// this will tell the logging app not to bother logging any gps replay data
replay.time_usec = 0;
}
if (optical_flow_updated) {
replay.flow_timestamp = optical_flow.timestamp;
replay.flow_pixel_integral[0] = optical_flow.pixel_flow_x_integral;
replay.flow_pixel_integral[1] = optical_flow.pixel_flow_y_integral;
replay.flow_gyro_integral[0] = optical_flow.gyro_x_rate_integral;
replay.flow_gyro_integral[1] = optical_flow.gyro_y_rate_integral;
replay.flow_time_integral = optical_flow.integration_timespan;
replay.flow_quality = optical_flow.quality;
} else {
replay.flow_timestamp = 0;
}
if (range_finder_updated) {
replay.rng_timestamp = range_finder.timestamp;
replay.range_to_ground = range_finder.current_distance;
} else {
replay.rng_timestamp = 0;
}
if (_replay_pub == nullptr) {
_replay_pub = orb_advertise(ORB_ID(ekf2_replay), &replay);
} else {
orb_publish(ORB_ID(ekf2_replay), _replay_pub, &replay);
}
}
}
delete ekf2::instance;
ekf2::instance = nullptr;
}
int sdlog2_thread_main(int argc, char *argv[])
{
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
if (mavlink_fd < 0) {
warnx("failed to open MAVLink log stream, start mavlink app first");
}
/* delay = 1 / rate (rate defined by -r option), default log rate: 50 Hz */
useconds_t sleep_delay = 20000;
int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT;
logging_enabled = false;
/* enable logging on start (-e option) */
bool log_on_start = false;
/* enable logging when armed (-a option) */
bool log_when_armed = false;
log_name_timestamp = false;
flag_system_armed = false;
/* work around some stupidity in task_create's argv handling */
argc -= 2;
argv += 2;
int ch;
/* don't exit from getopt loop to leave getopt global variables in consistent state,
* set error flag instead */
bool err_flag = false;
while ((ch = getopt(argc, argv, "r:b:eat")) != EOF) {
switch (ch) {
case 'r': {
unsigned long r = strtoul(optarg, NULL, 10);
if (r <= 0) {
r = 1;
}
sleep_delay = 1000000 / r;
}
break;
case 'b': {
unsigned long s = strtoul(optarg, NULL, 10);
if (s < 1) {
s = 1;
}
log_buffer_size = 1024 * s;
}
break;
case 'e':
log_on_start = true;
break;
case 'a':
log_when_armed = true;
break;
case 't':
log_name_timestamp = true;
break;
case '?':
if (optopt == 'c') {
warnx("option -%c requires an argument", optopt);
} else if (isprint(optopt)) {
warnx("unknown option `-%c'", optopt);
} else {
warnx("unknown option character `\\x%x'", optopt);
}
err_flag = true;
break;
default:
warnx("unrecognized flag");
err_flag = true;
break;
}
}
if (err_flag) {
sdlog2_usage(NULL);
}
gps_time = 0;
/* create log root dir */
int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO);
if (mkdir_ret != 0 && errno != EEXIST) {
err(1, "failed creating log root dir: %s", log_root);
}
/* copy conversion scripts */
const char *converter_in = "/etc/logging/conv.zip";
char *converter_out = malloc(64);
snprintf(converter_out, 64, "%s/conv.zip", log_root);
if (file_copy(converter_in, converter_out) != OK) {
warn("unable to copy conversion scripts");
}
free(converter_out);
/* initialize log buffer with specified size */
warnx("log buffer size: %i bytes", log_buffer_size);
if (OK != logbuffer_init(&lb, log_buffer_size)) {
errx(1, "can't allocate log buffer, exiting");
}
struct vehicle_status_s buf_status;
struct vehicle_gps_position_s buf_gps_pos;
memset(&buf_status, 0, sizeof(buf_status));
memset(&buf_gps_pos, 0, sizeof(buf_gps_pos));
/* warning! using union here to save memory, elements should be used separately! */
union {
struct vehicle_command_s cmd;
struct sensor_combined_s sensor;
struct vehicle_attitude_s att;
struct vehicle_attitude_setpoint_s att_sp;
struct vehicle_rates_setpoint_s rates_sp;
struct actuator_outputs_s act_outputs;
struct actuator_controls_s act_controls;
struct vehicle_local_position_s local_pos;
struct vehicle_local_position_setpoint_s local_pos_sp;
struct vehicle_global_position_s global_pos;
struct position_setpoint_triplet_s triplet;
struct vehicle_vicon_position_s vicon_pos;
struct optical_flow_s flow;
struct rc_channels_s rc;
struct differential_pressure_s diff_pres;
struct airspeed_s airspeed;
struct esc_status_s esc;
struct vehicle_global_velocity_setpoint_s global_vel_sp;
struct battery_status_s battery;
struct telemetry_status_s telemetry;
struct range_finder_report range_finder;
struct estimator_status_report estimator_status;
struct system_power_s system_power;
struct servorail_status_s servorail_status;
} buf;
memset(&buf, 0, sizeof(buf));
/* log message buffer: header + body */
#pragma pack(push, 1)
struct {
LOG_PACKET_HEADER;
union {
struct log_TIME_s log_TIME;
struct log_ATT_s log_ATT;
struct log_ATSP_s log_ATSP;
struct log_IMU_s log_IMU;
struct log_SENS_s log_SENS;
struct log_LPOS_s log_LPOS;
struct log_LPSP_s log_LPSP;
struct log_GPS_s log_GPS;
struct log_ATTC_s log_ATTC;
struct log_STAT_s log_STAT;
struct log_RC_s log_RC;
struct log_OUT0_s log_OUT0;
struct log_AIRS_s log_AIRS;
struct log_ARSP_s log_ARSP;
struct log_FLOW_s log_FLOW;
struct log_GPOS_s log_GPOS;
struct log_GPSP_s log_GPSP;
struct log_ESC_s log_ESC;
struct log_GVSP_s log_GVSP;
struct log_BATT_s log_BATT;
struct log_DIST_s log_DIST;
struct log_TELE_s log_TELE;
struct log_ESTM_s log_ESTM;
struct log_PWR_s log_PWR;
struct log_VICN_s log_VICN;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
};
#pragma pack(pop)
memset(&log_msg.body, 0, sizeof(log_msg.body));
struct {
int cmd_sub;
int status_sub;
int sensor_sub;
int att_sub;
int att_sp_sub;
int rates_sp_sub;
int act_outputs_sub;
int act_controls_sub;
int local_pos_sub;
int local_pos_sp_sub;
int global_pos_sub;
int triplet_sub;
int gps_pos_sub;
int vicon_pos_sub;
int flow_sub;
int rc_sub;
int airspeed_sub;
int esc_sub;
int global_vel_sp_sub;
int battery_sub;
int telemetry_sub;
int range_finder_sub;
int estimator_status_sub;
int system_power_sub;
int servorail_status_sub;
} subs;
subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
subs.status_sub = orb_subscribe(ORB_ID(vehicle_status));
subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
subs.act_outputs_sub = orb_subscribe(ORB_ID_VEHICLE_CONTROLS);
subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
subs.local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
subs.flow_sub = orb_subscribe(ORB_ID(optical_flow));
subs.rc_sub = orb_subscribe(ORB_ID(rc_channels));
subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
subs.esc_sub = orb_subscribe(ORB_ID(esc_status));
subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
subs.battery_sub = orb_subscribe(ORB_ID(battery_status));
subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
subs.system_power_sub = orb_subscribe(ORB_ID(system_power));
subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status));
thread_running = true;
/* initialize thread synchronization */
pthread_mutex_init(&logbuffer_mutex, NULL);
pthread_cond_init(&logbuffer_cond, NULL);
/* track changes in sensor_combined topic */
hrt_abstime gyro_timestamp = 0;
hrt_abstime accelerometer_timestamp = 0;
hrt_abstime magnetometer_timestamp = 0;
hrt_abstime barometer_timestamp = 0;
hrt_abstime differential_pressure_timestamp = 0;
/* track changes in distance status */
bool dist_bottom_present = false;
/* enable logging on start if needed */
if (log_on_start) {
/* check GPS topic to get GPS time */
if (log_name_timestamp) {
if (copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) {
gps_time = buf_gps_pos.time_gps_usec;
}
}
sdlog2_start_log();
}
while (!main_thread_should_exit) {
usleep(sleep_delay);
/* --- VEHICLE COMMAND - LOG MANAGEMENT --- */
if (copy_if_updated(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd)) {
handle_command(&buf.cmd);
}
/* --- VEHICLE STATUS - LOG MANAGEMENT --- */
bool status_updated = copy_if_updated(ORB_ID(vehicle_status), subs.status_sub, &buf_status);
if (status_updated) {
if (log_when_armed) {
handle_status(&buf_status);
}
}
/* --- GPS POSITION - LOG MANAGEMENT --- */
bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos);
if (gps_pos_updated && log_name_timestamp) {
gps_time = buf_gps_pos.time_gps_usec;
}
if (!logging_enabled) {
continue;
}
pthread_mutex_lock(&logbuffer_mutex);
/* write time stamp message */
log_msg.msg_type = LOG_TIME_MSG;
log_msg.body.log_TIME.t = hrt_absolute_time();
LOGBUFFER_WRITE_AND_COUNT(TIME);
/* --- VEHICLE STATUS --- */
if (status_updated) {
log_msg.msg_type = LOG_STAT_MSG;
log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state;
log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning;
log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed;
LOGBUFFER_WRITE_AND_COUNT(STAT);
}
/* --- GPS POSITION --- */
if (gps_pos_updated) {
log_msg.msg_type = LOG_GPS_MSG;
log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec;
log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type;
log_msg.body.log_GPS.eph = buf_gps_pos.eph_m;
log_msg.body.log_GPS.epv = buf_gps_pos.epv_m;
log_msg.body.log_GPS.lat = buf_gps_pos.lat;
log_msg.body.log_GPS.lon = buf_gps_pos.lon;
log_msg.body.log_GPS.alt = buf_gps_pos.alt * 0.001f;
log_msg.body.log_GPS.vel_n = buf_gps_pos.vel_n_m_s;
log_msg.body.log_GPS.vel_e = buf_gps_pos.vel_e_m_s;
log_msg.body.log_GPS.vel_d = buf_gps_pos.vel_d_m_s;
log_msg.body.log_GPS.cog = buf_gps_pos.cog_rad;
LOGBUFFER_WRITE_AND_COUNT(GPS);
}
/* --- SENSOR COMBINED --- */
if (copy_if_updated(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor)) {
bool write_IMU = false;
bool write_SENS = false;
if (buf.sensor.timestamp != gyro_timestamp) {
gyro_timestamp = buf.sensor.timestamp;
write_IMU = true;
}
if (buf.sensor.accelerometer_timestamp != accelerometer_timestamp) {
accelerometer_timestamp = buf.sensor.accelerometer_timestamp;
write_IMU = true;
}
if (buf.sensor.magnetometer_timestamp != magnetometer_timestamp) {
magnetometer_timestamp = buf.sensor.magnetometer_timestamp;
write_IMU = true;
}
if (buf.sensor.baro_timestamp != barometer_timestamp) {
barometer_timestamp = buf.sensor.baro_timestamp;
write_SENS = true;
}
if (buf.sensor.differential_pressure_timestamp != differential_pressure_timestamp) {
differential_pressure_timestamp = buf.sensor.differential_pressure_timestamp;
write_SENS = true;
}
if (write_IMU) {
log_msg.msg_type = LOG_IMU_MSG;
log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[0];
log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[1];
log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[2];
log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[0];
log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[1];
log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[2];
log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0];
log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1];
log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2];
LOGBUFFER_WRITE_AND_COUNT(IMU);
}
if (write_SENS) {
log_msg.msg_type = LOG_SENS_MSG;
log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar;
log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter;
log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius;
log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa;
log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure_filtered_pa;
LOGBUFFER_WRITE_AND_COUNT(SENS);
}
}
/* --- ATTITUDE --- */
if (copy_if_updated(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att)) {
log_msg.msg_type = LOG_ATT_MSG;
log_msg.body.log_ATT.roll = buf.att.roll;
log_msg.body.log_ATT.pitch = buf.att.pitch;
log_msg.body.log_ATT.yaw = buf.att.yaw;
log_msg.body.log_ATT.roll_rate = buf.att.rollspeed;
log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed;
log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed;
log_msg.body.log_ATT.gx = buf.att.g_comp[0];
log_msg.body.log_ATT.gy = buf.att.g_comp[1];
log_msg.body.log_ATT.gz = buf.att.g_comp[2];
LOGBUFFER_WRITE_AND_COUNT(ATT);
}
/* --- ATTITUDE SETPOINT --- */
if (copy_if_updated(ORB_ID(vehicle_attitude_setpoint), subs.att_sp_sub, &buf.att_sp)) {
log_msg.msg_type = LOG_ATSP_MSG;
log_msg.body.log_ATSP.roll_sp = buf.att_sp.roll_body;
log_msg.body.log_ATSP.pitch_sp = buf.att_sp.pitch_body;
log_msg.body.log_ATSP.yaw_sp = buf.att_sp.yaw_body;
log_msg.body.log_ATSP.thrust_sp = buf.att_sp.thrust;
LOGBUFFER_WRITE_AND_COUNT(ATSP);
}
/* --- RATES SETPOINT --- */
if (copy_if_updated(ORB_ID(vehicle_rates_setpoint), subs.rates_sp_sub, &buf.rates_sp)) {
log_msg.msg_type = LOG_ARSP_MSG;
log_msg.body.log_ARSP.roll_rate_sp = buf.rates_sp.roll;
log_msg.body.log_ARSP.pitch_rate_sp = buf.rates_sp.pitch;
log_msg.body.log_ARSP.yaw_rate_sp = buf.rates_sp.yaw;
LOGBUFFER_WRITE_AND_COUNT(ARSP);
}
/* --- ACTUATOR OUTPUTS --- */
if (copy_if_updated(ORB_ID(actuator_outputs_0), subs.act_outputs_sub, &buf.act_outputs)) {
log_msg.msg_type = LOG_OUT0_MSG;
memcpy(log_msg.body.log_OUT0.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT0.output));
LOGBUFFER_WRITE_AND_COUNT(OUT0);
}
/* --- ACTUATOR CONTROL --- */
if (copy_if_updated(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.act_controls_sub, &buf.act_controls)) {
log_msg.msg_type = LOG_ATTC_MSG;
log_msg.body.log_ATTC.roll = buf.act_controls.control[0];
log_msg.body.log_ATTC.pitch = buf.act_controls.control[1];
log_msg.body.log_ATTC.yaw = buf.act_controls.control[2];
log_msg.body.log_ATTC.thrust = buf.act_controls.control[3];
LOGBUFFER_WRITE_AND_COUNT(ATTC);
}
/* --- LOCAL POSITION --- */
if (copy_if_updated(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos)) {
log_msg.msg_type = LOG_LPOS_MSG;
log_msg.body.log_LPOS.x = buf.local_pos.x;
log_msg.body.log_LPOS.y = buf.local_pos.y;
log_msg.body.log_LPOS.z = buf.local_pos.z;
log_msg.body.log_LPOS.vx = buf.local_pos.vx;
log_msg.body.log_LPOS.vy = buf.local_pos.vy;
log_msg.body.log_LPOS.vz = buf.local_pos.vz;
log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat;
log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon;
log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt;
log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0);
log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0);
log_msg.body.log_LPOS.landed = buf.local_pos.landed;
LOGBUFFER_WRITE_AND_COUNT(LPOS);
if (buf.local_pos.dist_bottom_valid) {
dist_bottom_present = true;
}
if (dist_bottom_present) {
log_msg.msg_type = LOG_DIST_MSG;
log_msg.body.log_DIST.bottom = buf.local_pos.dist_bottom;
log_msg.body.log_DIST.bottom_rate = buf.local_pos.dist_bottom_rate;
log_msg.body.log_DIST.flags = (buf.local_pos.dist_bottom_valid ? 1 : 0);
LOGBUFFER_WRITE_AND_COUNT(DIST);
}
}
/* --- LOCAL POSITION SETPOINT --- */
if (copy_if_updated(ORB_ID(vehicle_local_position_setpoint), subs.local_pos_sp_sub, &buf.local_pos_sp)) {
log_msg.msg_type = LOG_LPSP_MSG;
log_msg.body.log_LPSP.x = buf.local_pos_sp.x;
log_msg.body.log_LPSP.y = buf.local_pos_sp.y;
log_msg.body.log_LPSP.z = buf.local_pos_sp.z;
log_msg.body.log_LPSP.yaw = buf.local_pos_sp.yaw;
LOGBUFFER_WRITE_AND_COUNT(LPSP);
}
/* --- GLOBAL POSITION --- */
if (copy_if_updated(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos)) {
log_msg.msg_type = LOG_GPOS_MSG;
log_msg.body.log_GPOS.lat = buf.global_pos.lat * 1e7;
log_msg.body.log_GPOS.lon = buf.global_pos.lon * 1e7;
log_msg.body.log_GPOS.alt = buf.global_pos.alt;
log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n;
log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e;
log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d;
log_msg.body.log_GPOS.baro_alt = buf.global_pos.baro_alt;
log_msg.body.log_GPOS.flags = (buf.global_pos.baro_valid ? 1 : 0) | (buf.global_pos.global_valid ? 2 : 0);
LOGBUFFER_WRITE_AND_COUNT(GPOS);
}
/* --- GLOBAL POSITION SETPOINT --- */
if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) {
log_msg.msg_type = LOG_GPSP_MSG;
log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state;
log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d);
log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d);
log_msg.body.log_GPSP.alt = buf.triplet.current.alt;
log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw;
log_msg.body.log_GPSP.type = buf.triplet.current.type;
log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius;
log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction;
log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min;
LOGBUFFER_WRITE_AND_COUNT(GPSP);
}
/* --- VICON POSITION --- */
if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) {
log_msg.msg_type = LOG_VICN_MSG;
log_msg.body.log_VICN.x = buf.vicon_pos.x;
log_msg.body.log_VICN.y = buf.vicon_pos.y;
log_msg.body.log_VICN.z = buf.vicon_pos.z;
log_msg.body.log_VICN.pitch = buf.vicon_pos.pitch;
log_msg.body.log_VICN.roll = buf.vicon_pos.roll;
log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw;
LOGBUFFER_WRITE_AND_COUNT(VICN);
}
/* --- FLOW --- */
if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) {
log_msg.msg_type = LOG_FLOW_MSG;
log_msg.body.log_FLOW.flow_raw_x = buf.flow.flow_raw_x;
log_msg.body.log_FLOW.flow_raw_y = buf.flow.flow_raw_y;
log_msg.body.log_FLOW.flow_comp_x = buf.flow.flow_comp_x_m;
log_msg.body.log_FLOW.flow_comp_y = buf.flow.flow_comp_y_m;
log_msg.body.log_FLOW.distance = buf.flow.ground_distance_m;
log_msg.body.log_FLOW.quality = buf.flow.quality;
log_msg.body.log_FLOW.sensor_id = buf.flow.sensor_id;
LOGBUFFER_WRITE_AND_COUNT(FLOW);
}
/* --- RC CHANNELS --- */
if (copy_if_updated(ORB_ID(rc_channels), subs.rc_sub, &buf.rc)) {
log_msg.msg_type = LOG_RC_MSG;
/* Copy only the first 8 channels of 14 */
memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel));
log_msg.body.log_RC.channel_count = buf.rc.chan_count;
LOGBUFFER_WRITE_AND_COUNT(RC);
}
/* --- AIRSPEED --- */
if (copy_if_updated(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed)) {
log_msg.msg_type = LOG_AIRS_MSG;
log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s;
log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s;
log_msg.body.log_AIRS.air_temperature_celsius = buf.airspeed.air_temperature_celsius;
LOGBUFFER_WRITE_AND_COUNT(AIRS);
}
/* --- ESCs --- */
if (copy_if_updated(ORB_ID(esc_status), subs.esc_sub, &buf.esc)) {
for (uint8_t i = 0; i < buf.esc.esc_count; i++) {
log_msg.msg_type = LOG_ESC_MSG;
log_msg.body.log_ESC.counter = buf.esc.counter;
log_msg.body.log_ESC.esc_count = buf.esc.esc_count;
log_msg.body.log_ESC.esc_connectiontype = buf.esc.esc_connectiontype;
log_msg.body.log_ESC.esc_num = i;
log_msg.body.log_ESC.esc_address = buf.esc.esc[i].esc_address;
log_msg.body.log_ESC.esc_version = buf.esc.esc[i].esc_version;
log_msg.body.log_ESC.esc_voltage = buf.esc.esc[i].esc_voltage;
log_msg.body.log_ESC.esc_current = buf.esc.esc[i].esc_current;
log_msg.body.log_ESC.esc_rpm = buf.esc.esc[i].esc_rpm;
log_msg.body.log_ESC.esc_temperature = buf.esc.esc[i].esc_temperature;
log_msg.body.log_ESC.esc_setpoint = buf.esc.esc[i].esc_setpoint;
log_msg.body.log_ESC.esc_setpoint_raw = buf.esc.esc[i].esc_setpoint_raw;
LOGBUFFER_WRITE_AND_COUNT(ESC);
}
}
/* --- GLOBAL VELOCITY SETPOINT --- */
if (copy_if_updated(ORB_ID(vehicle_global_velocity_setpoint), subs.global_vel_sp_sub, &buf.global_vel_sp)) {
log_msg.msg_type = LOG_GVSP_MSG;
log_msg.body.log_GVSP.vx = buf.global_vel_sp.vx;
log_msg.body.log_GVSP.vy = buf.global_vel_sp.vy;
log_msg.body.log_GVSP.vz = buf.global_vel_sp.vz;
LOGBUFFER_WRITE_AND_COUNT(GVSP);
}
/* --- BATTERY --- */
if (copy_if_updated(ORB_ID(battery_status), subs.battery_sub, &buf.battery)) {
log_msg.msg_type = LOG_BATT_MSG;
log_msg.body.log_BATT.voltage = buf.battery.voltage_v;
log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v;
log_msg.body.log_BATT.current = buf.battery.current_a;
log_msg.body.log_BATT.discharged = buf.battery.discharged_mah;
LOGBUFFER_WRITE_AND_COUNT(BATT);
}
/* --- SYSTEM POWER RAILS --- */
if (copy_if_updated(ORB_ID(system_power), subs.system_power_sub, &buf.system_power)) {
log_msg.msg_type = LOG_PWR_MSG;
log_msg.body.log_PWR.peripherals_5v = buf.system_power.voltage5V_v;
log_msg.body.log_PWR.usb_ok = buf.system_power.usb_connected;
log_msg.body.log_PWR.brick_ok = buf.system_power.brick_valid;
log_msg.body.log_PWR.servo_ok = buf.system_power.servo_valid;
log_msg.body.log_PWR.low_power_rail_overcurrent = buf.system_power.periph_5V_OC;
log_msg.body.log_PWR.high_power_rail_overcurrent = buf.system_power.hipower_5V_OC;
/* copy servo rail status topic here too */
orb_copy(ORB_ID(servorail_status), subs.servorail_status_sub, &buf.servorail_status);
log_msg.body.log_PWR.servo_rail_5v = buf.servorail_status.voltage_v;
log_msg.body.log_PWR.servo_rssi = buf.servorail_status.rssi_v;
LOGBUFFER_WRITE_AND_COUNT(PWR);
}
/* --- TELEMETRY --- */
if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) {
log_msg.msg_type = LOG_TELE_MSG;
log_msg.body.log_TELE.rssi = buf.telemetry.rssi;
log_msg.body.log_TELE.remote_rssi = buf.telemetry.remote_rssi;
log_msg.body.log_TELE.noise = buf.telemetry.noise;
log_msg.body.log_TELE.remote_noise = buf.telemetry.remote_noise;
log_msg.body.log_TELE.rxerrors = buf.telemetry.rxerrors;
log_msg.body.log_TELE.fixed = buf.telemetry.fixed;
log_msg.body.log_TELE.txbuf = buf.telemetry.txbuf;
LOGBUFFER_WRITE_AND_COUNT(TELE);
}
/* --- BOTTOM DISTANCE --- */
if (copy_if_updated(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder)) {
log_msg.msg_type = LOG_DIST_MSG;
log_msg.body.log_DIST.bottom = buf.range_finder.distance;
log_msg.body.log_DIST.bottom_rate = 0.0f;
log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0);
LOGBUFFER_WRITE_AND_COUNT(DIST);
}
/* --- ESTIMATOR STATUS --- */
if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) {
log_msg.msg_type = LOG_ESTM_MSG;
unsigned maxcopy = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_ESTM.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_ESTM.s);
memset(&(log_msg.body.log_ESTM.s), 0, sizeof(log_msg.body.log_ESTM.s));
memcpy(&(log_msg.body.log_ESTM.s), buf.estimator_status.states, maxcopy);
log_msg.body.log_ESTM.n_states = buf.estimator_status.n_states;
log_msg.body.log_ESTM.states_nan = buf.estimator_status.states_nan;
log_msg.body.log_ESTM.covariance_nan = buf.estimator_status.covariance_nan;
log_msg.body.log_ESTM.kalman_gain_nan = buf.estimator_status.kalman_gain_nan;
LOGBUFFER_WRITE_AND_COUNT(ESTM);
}
/* signal the other thread new data, but not yet unlock */
if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
/* only request write if several packets can be written at once */
pthread_cond_signal(&logbuffer_cond);
}
/* unlock, now the writer thread may run */
pthread_mutex_unlock(&logbuffer_mutex);
}
if (logging_enabled) {
sdlog2_stop_log();
}
pthread_mutex_destroy(&logbuffer_mutex);
pthread_cond_destroy(&logbuffer_cond);
free(lb.data);
warnx("exiting");
thread_running = false;
return 0;
}
int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3])
{
const int samples_num = 2500;
float accel_ref[6][3];
bool data_collected[6] = { false, false, false, false, false, false };
const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" };
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
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_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count);
}
if (done) {
break;
}
mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s",
(!data_collected[0]) ? "x+ " : "",
(!data_collected[1]) ? "x- " : "",
(!data_collected[2]) ? "y+ " : "",
(!data_collected[3]) ? "y- " : "",
(!data_collected[4]) ? "z+ " : "",
(!data_collected[5]) ? "z- " : "");
int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
if (orient < 0) {
res = ERROR;
break;
}
if (data_collected[orient]) {
mavlink_log_info(mavlink_fd, "%s done, rotate to a different axis", orientation_strs[orient]);
continue;
}
mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]);
read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num);
mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient],
(double)accel_ref[orient][0],
(double)accel_ref[orient][1],
(double)accel_ref[orient][2]);
data_collected[orient] = true;
tune_neutral(true);
}
close(sensor_combined_sub);
if (res == OK) {
/* calculate offsets and transform matrix */
res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
if (res != OK) {
mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error");
}
}
return res;
}
int sdlog_thread_main(int argc, char *argv[])
{
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
if (mavlink_fd < 0) {
warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n");
}
/* log every n'th value (skip three per default) */
int skip_value = 3;
/* work around some stupidity in task_create's argv handling */
argc -= 2;
argv += 2;
int ch;
while ((ch = getopt(argc, argv, "s:r")) != EOF) {
switch (ch) {
case 's':
{
/* log only every n'th (gyro clocked) value */
unsigned s = strtoul(optarg, NULL, 10);
if (s < 1 || s > 250) {
errx(1, "Wrong skip value of %d, out of range (1..250)\n", s);
} else {
skip_value = s;
}
}
break;
case 'r':
/* log only on request, disable logging per default */
logging_enabled = false;
break;
case '?':
if (optopt == 'c') {
warnx("Option -%c requires an argument.\n", optopt);
} else if (isprint(optopt)) {
warnx("Unknown option `-%c'.\n", optopt);
} else {
warnx("Unknown option character `\\x%x'.\n", optopt);
}
default:
usage("unrecognized flag");
errx(1, "exiting.");
}
}
if (file_exist(mountpoint) != OK) {
errx(1, "logging mount point %s not present, exiting.", mountpoint);
}
char folder_path[64];
if (create_logfolder(folder_path))
errx(1, "unable to create logging folder, exiting.");
FILE *gpsfile;
FILE *blackbox_file;
/* string to hold the path to the sensorfile */
char path_buf[64] = "";
/* only print logging path, important to find log file later */
warnx("logging to directory %s\n", folder_path);
/* set up file path: e.g. /mnt/sdcard/session0001/actuator_controls0.bin */
sprintf(path_buf, "%s/%s.bin", folder_path, "sysvector");
if (0 == (sysvector_file = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) {
errx(1, "opening %s failed.\n", path_buf);
}
/* set up file path: e.g. /mnt/sdcard/session0001/gps.txt */
sprintf(path_buf, "%s/%s.txt", folder_path, "gps");
if (NULL == (gpsfile = fopen(path_buf, "w"))) {
errx(1, "opening %s failed.\n", path_buf);
}
int gpsfile_no = fileno(gpsfile);
/* set up file path: e.g. /mnt/sdcard/session0001/blackbox.txt */
sprintf(path_buf, "%s/%s.txt", folder_path, "blackbox");
if (NULL == (blackbox_file = fopen(path_buf, "w"))) {
errx(1, "opening %s failed.\n", path_buf);
}
// XXX for fsync() calls
int blackbox_file_no = fileno(blackbox_file);
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* number of messages */
const ssize_t fdsc = 25;
/* Sanity check variable and index */
ssize_t fdsc_count = 0;
/* file descriptors to wait for */
struct pollfd fds[fdsc];
struct {
struct sensor_combined_s raw;
struct vehicle_attitude_s att;
struct vehicle_attitude_setpoint_s att_sp;
struct actuator_outputs_s act_outputs;
struct actuator_controls_s act_controls;
struct actuator_controls_effective_s act_controls_effective;
struct vehicle_command_s cmd;
struct vehicle_local_position_s local_pos;
struct vehicle_global_position_s global_pos;
struct vehicle_gps_position_s gps_pos;
struct vehicle_vicon_position_s vicon_pos;
struct optical_flow_s flow;
struct battery_status_s batt;
struct differential_pressure_s diff_pres;
struct airspeed_s airspeed;
} buf;
memset(&buf, 0, sizeof(buf));
struct {
int cmd_sub;
int sensor_sub;
int att_sub;
int spa_sub;
int act_0_sub;
int controls_0_sub;
int controls_effective_0_sub;
int local_pos_sub;
int global_pos_sub;
int gps_pos_sub;
int vicon_pos_sub;
int flow_sub;
int batt_sub;
int diff_pres_sub;
int airspeed_sub;
} subs;
/* --- MANAGEMENT - LOGGING COMMAND --- */
/* subscribe to ORB for vehicle command */
subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
fds[fdsc_count].fd = subs.cmd_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- GPS POSITION --- */
/* subscribe to ORB for global position */
subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
fds[fdsc_count].fd = subs.gps_pos_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- SENSORS RAW VALUE --- */
/* subscribe to ORB for sensors raw */
subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
fds[fdsc_count].fd = subs.sensor_sub;
/* do not rate limit, instead use skip counter (aliasing on rate limit) */
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- ATTITUDE VALUE --- */
/* subscribe to ORB for attitude */
subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
fds[fdsc_count].fd = subs.att_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- ATTITUDE SETPOINT VALUE --- */
/* subscribe to ORB for attitude setpoint */
/* struct already allocated */
subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
fds[fdsc_count].fd = subs.spa_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/** --- ACTUATOR OUTPUTS --- */
subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0));
fds[fdsc_count].fd = subs.act_0_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- ACTUATOR CONTROL VALUE --- */
/* subscribe to ORB for actuator control */
subs.controls_0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
fds[fdsc_count].fd = subs.controls_0_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- ACTUATOR CONTROL EFFECTIVE VALUE --- */
/* subscribe to ORB for actuator control */
subs.controls_effective_0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
fds[fdsc_count].fd = subs.controls_effective_0_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- LOCAL POSITION --- */
/* subscribe to ORB for local position */
subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
fds[fdsc_count].fd = subs.local_pos_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- GLOBAL POSITION --- */
/* subscribe to ORB for global position */
subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
fds[fdsc_count].fd = subs.global_pos_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- VICON POSITION --- */
/* subscribe to ORB for vicon position */
subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
fds[fdsc_count].fd = subs.vicon_pos_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- FLOW measurements --- */
/* subscribe to ORB for flow measurements */
subs.flow_sub = orb_subscribe(ORB_ID(optical_flow));
fds[fdsc_count].fd = subs.flow_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- BATTERY STATUS --- */
/* subscribe to ORB for flow measurements */
subs.batt_sub = orb_subscribe(ORB_ID(battery_status));
fds[fdsc_count].fd = subs.batt_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- DIFFERENTIAL PRESSURE --- */
/* subscribe to ORB for flow measurements */
subs.diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
fds[fdsc_count].fd = subs.diff_pres_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- AIRSPEED --- */
/* subscribe to ORB for airspeed */
subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
fds[fdsc_count].fd = subs.airspeed_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* WARNING: If you get the error message below,
* then the number of registered messages (fdsc)
* differs from the number of messages in the above list.
*/
if (fdsc_count > fdsc) {
warn("WARNING: Not enough space for poll fds allocated. Check %s:%d.\n", __FILE__, __LINE__);
fdsc_count = fdsc;
}
/*
* set up poll to block for new data,
* wait for a maximum of 1000 ms (1 second)
*/
// const int timeout = 1000;
thread_running = true;
/* initialize log buffer with a size of 10 */
sdlog_logbuffer_init(&lb, 10);
/* initialize thread synchronization */
pthread_mutex_init(&sysvector_mutex, NULL);
pthread_cond_init(&sysvector_cond, NULL);
/* start logbuffer emptying thread */
pthread_t sysvector_pthread = sysvector_write_start(&lb);
starttime = hrt_absolute_time();
/* track skipping */
int skip_count = 0;
while (!thread_should_exit) {
/* only poll for commands, gps and sensor_combined */
int poll_ret = poll(fds, 3, 1000);
/* handle the poll result */
if (poll_ret == 0) {
/* XXX this means none of our providers is giving us data - might be an error? */
} else if (poll_ret < 0) {
/* XXX this is seriously bad - should be an emergency */
} else {
int ifds = 0;
/* --- VEHICLE COMMAND VALUE --- */
if (fds[ifds++].revents & POLLIN) {
/* copy command into local buffer */
orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd);
/* always log to blackbox, even when logging disabled */
blackbox_file_bytes += fprintf(blackbox_file, "[%10.4f\tVCMD] CMD #%d [%f\t%f\t%f\t%f\t%f\t%f\t%f]\n", hrt_absolute_time()/1000000.0d,
buf.cmd.command, (double)buf.cmd.param1, (double)buf.cmd.param2, (double)buf.cmd.param3, (double)buf.cmd.param4,
(double)buf.cmd.param5, (double)buf.cmd.param6, (double)buf.cmd.param7);
handle_command(&buf.cmd);
}
/* --- VEHICLE GPS VALUE --- */
if (fds[ifds++].revents & POLLIN) {
/* copy gps position into local buffer */
orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
/* if logging disabled, continue */
if (logging_enabled) {
/* write KML line */
}
}
/* --- SENSORS RAW VALUE --- */
if (fds[ifds++].revents & POLLIN) {
// /* copy sensors raw data into local buffer */
// orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw);
// /* write out */
// sensor_combined_bytes += write(sensorfile, (const char*)&(buf.raw), sizeof(buf.raw));
/* always copy sensors raw data into local buffer, since poll flags won't clear else */
orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw);
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls_0_sub, &buf.act_controls);
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.controls_effective_0_sub, &buf.act_controls_effective);
orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs);
orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp);
orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos);
orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);
orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos);
orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow);
orb_copy(ORB_ID(differential_pressure), subs.diff_pres_sub, &buf.diff_pres);
orb_copy(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed);
orb_copy(ORB_ID(battery_status), subs.batt_sub, &buf.batt);
/* if skipping is on or logging is disabled, ignore */
if (skip_count < skip_value || !logging_enabled) {
skip_count++;
/* do not log data */
continue;
} else {
/* log data, reset */
skip_count = 0;
}
struct sdlog_sysvector sysvect = {
.timestamp = buf.raw.timestamp,
.gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]},
.accel = {buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], buf.raw.accelerometer_m_s2[2]},
.mag = {buf.raw.magnetometer_ga[0], buf.raw.magnetometer_ga[1], buf.raw.magnetometer_ga[2]},
.baro = buf.raw.baro_pres_mbar,
.baro_alt = buf.raw.baro_alt_meter,
.baro_temp = buf.raw.baro_temp_celcius,
.control = {buf.act_controls.control[0], buf.act_controls.control[1], buf.act_controls.control[2], buf.act_controls.control[3]},
.actuators = {
buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3],
buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]
},
.vbat = buf.batt.voltage_v,
.bat_current = buf.batt.current_a,
.bat_discharged = buf.batt.discharged_mah,
.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2], buf.raw.adc_voltage_v[3]},
.local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z},
.gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt},
.attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw},
.rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]},
.vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw},
.control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]},
.flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality},
.diff_pressure = buf.diff_pres.differential_pressure_pa,
.ind_airspeed = buf.airspeed.indicated_airspeed_m_s,
.true_airspeed = buf.airspeed.true_airspeed_m_s
};
/* put into buffer for later IO */
pthread_mutex_lock(&sysvector_mutex);
sdlog_logbuffer_write(&lb, &sysvect);
/* signal the other thread new data, but not yet unlock */
if ((unsigned)lb.count > (lb.size / 2)) {
/* only request write if several packets can be written at once */
pthread_cond_signal(&sysvector_cond);
}
/* unlock, now the writer thread may run */
pthread_mutex_unlock(&sysvector_mutex);
}
}
}
print_sdlog_status();
/* wake up write thread one last time */
pthread_mutex_lock(&sysvector_mutex);
pthread_cond_signal(&sysvector_cond);
/* unlock, now the writer thread may return */
pthread_mutex_unlock(&sysvector_mutex);
/* wait for write thread to return */
(void)pthread_join(sysvector_pthread, NULL);
pthread_mutex_destroy(&sysvector_mutex);
pthread_cond_destroy(&sysvector_cond);
warnx("exiting.\n\n");
/* finish KML file */
// XXX
fclose(gpsfile);
fclose(blackbox_file);
thread_running = false;
return 0;
}
/*
* EKF Attitude Estimator main function.
*
* Estimates the attitude recursively once started.
*
* @param argc number of commandline arguments (plus command name)
* @param argv strings containing the arguments
*/
void app_att_est_ekf_main(void* parameter)
{
const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
float dt = 0.005f;
/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */
float x_aposteriori_k[12]; /**< states */
float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f,
}; /**< init: diagonal matrix with big values */
float x_aposteriori[12];
float P_aposteriori[144];
/* output euler angles */
float euler[3] = {0.0f, 0.0f, 0.0f};
float Rot_matrix[9] = {1.f, 0, 0,
0, 1.f, 0,
0, 0, 1.f
}; /**< init: identity matrix */
// print text
printf("Extended Kalman Filter Attitude Estimator initialized..\n\n");
fflush(stdout);
int overloadcounter = 19;
/* Initialize filter */
attitudeKalmanfilter_initialize();
/* store start time to guard against too slow update rates */
uint64_t last_run = hrt_absolute_time();
sensor_combined_s raw;
rt_memset(&raw, 0, sizeof(raw));
//struct vehicle_gps_position_s gps;
//rt_memset(&gps, 0, sizeof(gps));
//struct vehicle_global_position_s global_pos;
//rt_memset(&global_pos, 0, sizeof(global_pos));
vehicle_attitude_s att;
rt_memset(&att, 0, sizeof(att));
//struct vehicle_control_mode_s control_mode;
//rt_memset(&control_mode, 0, sizeof(control_mode));
uint64_t last_data = 0;
uint64_t last_measurement = 0;
rt_uint32_t sensors_sub = 0;
uint64_t last_timestamp_acc = 0;
uint64_t last_timestamp_gyro = 0;
uint64_t last_timestamp_mag = 0;
/* rotation matrix */
float R[3][3] = {1,0,0,
0,1,0,
0,0,1};
/* subscribe to raw data */
orb_subscribe(ORB_ID(sensor_combined),&sensors_sub);
orb_advertise(ORB_ID(vehicle_attitude), &att);
att_est_ekf_running = true;
bool initialized = false;
/* magnetic declination, in radians */
float mag_decl = 0.0f;
/* rotation matrix for magnetic declination */
float R_mag[3][3] = {1,0,0,
0,1,0,
0,0,1};
uint8_t update_vect[3] = {0, 0, 0};
rt_memset(&ekf_params, 0, sizeof(ekf_params));
ekf_params.q[0] = 1e-4f;
ekf_params.q[1] = 0.08f;
ekf_params.q[2] = 0.009f;
ekf_params.q[3] = 0.005f;
ekf_params.q[4] = 0.0f;
ekf_params.r[0] = 0.0008f;
ekf_params.r[1] = 10000.0f;
ekf_params.r[2] = 100.0f;
ekf_params.r[3] = 0.0f;
/* Main loop*/
while (!att_est_ekf_should_exit) {
/* only run filter if sensor values changed */
if (orb_check(&sensors_sub,5000) == RT_EOK) {
/* get latest measurements */
orb_copy(ORB_ID(sensor_combined), &raw);
}
else{
rt_kprintf("sensors data lost!\n");
}
if (!initialized) {
initialized = true;
}
else {
/* Calculate data time difference in seconds */
dt = (raw.acc_timestamp - last_measurement) / 1000000.0f;
last_measurement = raw.acc_timestamp;
if(raw.gyro_timestamp != last_timestamp_gyro)
{
update_vect[0] = 1;
last_timestamp_gyro = raw.gyro_timestamp;
}
if(raw.acc_timestamp != last_timestamp_acc)
{
update_vect[1] = 1;
last_timestamp_acc = raw.acc_timestamp;
}
if(raw.mag_timestamp != last_timestamp_mag)
{
update_vect[2] = 1;
last_timestamp_mag = raw.mag_timestamp;
}
z_k[0] = raw.gyro_rad_s[0];
z_k[1] = raw.gyro_rad_s[1];
z_k[2] = raw.gyro_rad_s[2];
z_k[3] = raw.acc_m_s2[0];
z_k[4] = raw.acc_m_s2[1];
z_k[5] = raw.acc_m_s2[2];
z_k[6] = raw.mag_ga[0];
z_k[7] = raw.mag_ga[1];
z_k[8] = raw.mag_ga[2];
uint64_t now = hrt_absolute_time();
unsigned int time_elapsed = now - last_run;
last_run = now;
if (time_elapsed > loop_interval_alarm) {
/* cpu overload */
overloadcounter++;
}
static bool const_initialized = false;
/* initialize with good values once we have a reasonable dt estimate */
if (!const_initialized && dt < 0.05f && dt > 0.001f) {
dt = 0.005f;
/* update mag declination rotation matrix */
euler_to_rot_mat(R_mag,0.0f, 0.0f, mag_decl);
x_aposteriori_k[0] = z_k[0];
x_aposteriori_k[1] = z_k[1];
x_aposteriori_k[2] = z_k[2];
x_aposteriori_k[3] = 0.0f;
x_aposteriori_k[4] = 0.0f;
x_aposteriori_k[5] = 0.0f;
x_aposteriori_k[6] = z_k[3];
x_aposteriori_k[7] = z_k[4];
x_aposteriori_k[8] = z_k[5];
x_aposteriori_k[9] = z_k[6];
x_aposteriori_k[10] = z_k[7];
x_aposteriori_k[11] = z_k[8];
const_initialized = true;
}
/* do not execute the filter if not initialized */
if (!const_initialized) {
continue;
}
attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
euler, Rot_matrix, x_aposteriori, P_aposteriori);
for(int i = 0;i < 3;i++)
{
update_vect[i] = 0;
}
/* swap values for next iteration, check for fatal inputs */
if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
rt_memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
rt_memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));
} else {
/* due to inputs or numerical failure the output is invalid, skip it */
continue;
}
if (last_data > 0 && raw.acc_timestamp - last_data > 30000)
printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.acc_timestamp - last_data);
last_data = raw.acc_timestamp;
/* send out */
att.timestamp = raw.acc_timestamp;
att.roll = euler[0];
att.pitch = euler[1];
att.yaw = euler[2] + mag_decl;
att.rollspeed = x_aposteriori[0];
att.pitchspeed = x_aposteriori[1];
att.yawspeed = x_aposteriori[2];
att.rollacc = x_aposteriori[3];
att.pitchacc = x_aposteriori[4];
att.yawacc = x_aposteriori[5];
att.g_comp[0] = raw.acc_m_s2[0];
att.g_comp[1] = raw.acc_m_s2[1];
att.g_comp[2] = raw.acc_m_s2[2];
/* copy offsets */
rt_memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
/* magnetic declination */
float R_body[3][3] = {0};
rt_memcpy(&R_body, &Rot_matrix, sizeof(R_body));
//R = R_mag * R_body;
for(int i = 0;i < 3;i++){
for(int j = 0;j < 3;i++)
{
R[i][j] = 0;
for(int k = 0;k < 3;k++)
R[i][j] += R_mag[i][k] * R_body[k][j];
}
}
/* copy rotation matrix */
rt_memcpy(&att.R, &R, sizeof(att.R));
att.R_valid = true;
if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
// Broadcast
orb_publish(ORB_ID(vehicle_attitude), &att);
} else {
rt_kprintf("NaN in roll/pitch/yaw estimate!");
}
}
}
att_est_ekf_running = false;
}
