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