int do_accel_calibration(orb_advert_t *mavlink_log_pub)
{
#ifdef __PX4_NUTTX
int fd;
#endif
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
struct accel_calibration_s accel_scale;
accel_scale.x_offset = 0.0f;
accel_scale.x_scale = 1.0f;
accel_scale.y_offset = 0.0f;
accel_scale.y_scale = 1.0f;
accel_scale.z_offset = 0.0f;
accel_scale.z_scale = 1.0f;
int res = PX4_OK;
char str[30];
/* reset all sensors */
for (unsigned s = 0; s < max_accel_sens; s++) {
#ifdef __PX4_NUTTX
sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
/* reset all offsets to zero and all scales to one */
fd = px4_open(str, 0);
if (fd < 0) {
continue;
}
device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
px4_close(fd);
if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, s);
}
#else
(void)sprintf(str, "CAL_ACC%u_XOFF", s);
res = param_set_no_notification(param_find(str), &accel_scale.x_offset);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_YOFF", s);
res = param_set_no_notification(param_find(str), &accel_scale.y_offset);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_ZOFF", s);
res = param_set_no_notification(param_find(str), &accel_scale.z_offset);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_XSCALE", s);
res = param_set_no_notification(param_find(str), &accel_scale.x_scale);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_YSCALE", s);
res = param_set_no_notification(param_find(str), &accel_scale.y_scale);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_ZSCALE", s);
res = param_set_no_notification(param_find(str), &accel_scale.z_scale);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
param_notify_changes();
#endif
}
float accel_offs[max_accel_sens][3];
float accel_T[max_accel_sens][3][3];
unsigned active_sensors = 0;
/* measure and calculate offsets & scales */
if (res == PX4_OK) {
calibrate_return cal_return = do_accel_calibration_measurements(mavlink_log_pub, accel_offs, accel_T, &active_sensors);
if (cal_return == calibrate_return_cancelled) {
// Cancel message already displayed, nothing left to do
return PX4_ERROR;
} else if (cal_return == calibrate_return_ok) {
res = PX4_OK;
} else {
res = PX4_ERROR;
}
}
if (res != PX4_OK) {
if (active_sensors == 0) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
}
return PX4_ERROR;
}
/* measurements completed successfully, rotate calibration values */
param_t board_rotation_h = param_find("SENS_BOARD_ROT");
int32_t board_rotation_int;
param_get(board_rotation_h, &(board_rotation_int));
enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
math::Matrix<3, 3> board_rotation;
get_rot_matrix(board_rotation_id, &board_rotation);
math::Matrix<3, 3> board_rotation_t = board_rotation.transposed();
bool tc_locked[3] = {false}; // true when the thermal parameter instance has already been adjusted by the calibrator
for (unsigned uorb_index = 0; uorb_index < active_sensors; uorb_index++) {
/* handle individual sensors, one by one */
math::Vector<3> accel_offs_vec(accel_offs[uorb_index]);
math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec;
math::Matrix<3, 3> accel_T_mat(accel_T[uorb_index]);
math::Matrix<3, 3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
accel_scale.x_offset = accel_offs_rotated(0);
accel_scale.x_scale = accel_T_rotated(0, 0);
accel_scale.y_offset = accel_offs_rotated(1);
accel_scale.y_scale = accel_T_rotated(1, 1);
accel_scale.z_offset = accel_offs_rotated(2);
accel_scale.z_scale = accel_T_rotated(2, 2);
bool failed = false;
failed = failed || (PX4_OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary)));
PX4_INFO("found offset %d: x: %.6f, y: %.6f, z: %.6f", uorb_index,
(double)accel_scale.x_offset,
(double)accel_scale.y_offset,
(double)accel_scale.z_offset);
PX4_INFO("found scale %d: x: %.6f, y: %.6f, z: %.6f", uorb_index,
(double)accel_scale.x_scale,
(double)accel_scale.y_scale,
(double)accel_scale.z_scale);
/* check if thermal compensation is enabled */
int32_t tc_enabled_int;
param_get(param_find("TC_A_ENABLE"), &(tc_enabled_int));
if (tc_enabled_int == 1) {
/* Get struct containing sensor thermal compensation data */
struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */
memset(&sensor_correction, 0, sizeof(sensor_correction));
int sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
orb_copy(ORB_ID(sensor_correction), sensor_correction_sub, &sensor_correction);
orb_unsubscribe(sensor_correction_sub);
/* don't allow a parameter instance to be calibrated more than once by another uORB instance */
if (!tc_locked[sensor_correction.accel_mapping[uorb_index]]) {
tc_locked[sensor_correction.accel_mapping[uorb_index]] = true;
/* update the _X0_ terms to include the additional offset */
int32_t handle;
float val;
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
val = 0.0f;
(void)sprintf(str, "TC_A%u_X0_%u", sensor_correction.accel_mapping[uorb_index], axis_index);
handle = param_find(str);
param_get(handle, &val);
if (axis_index == 0) {
val += accel_scale.x_offset;
} else if (axis_index == 1) {
val += accel_scale.y_offset;
} else if (axis_index == 2) {
val += accel_scale.z_offset;
}
failed |= (PX4_OK != param_set_no_notification(handle, &val));
}
/* update the _SCL_ terms to include the scale factor */
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
val = 1.0f;
(void)sprintf(str, "TC_A%u_SCL_%u", sensor_correction.accel_mapping[uorb_index], axis_index);
handle = param_find(str);
if (axis_index == 0) {
val = accel_scale.x_scale;
} else if (axis_index == 1) {
val = accel_scale.y_scale;
} else if (axis_index == 2) {
val = accel_scale.z_scale;
}
failed |= (PX4_OK != param_set_no_notification(handle, &val));
}
param_notify_changes();
}
// Ensure the calibration values used by the driver are at default settings when we are using thermal calibration data
accel_scale.x_offset = 0.f;
accel_scale.y_offset = 0.f;
accel_scale.z_offset = 0.f;
accel_scale.x_scale = 1.f;
accel_scale.y_scale = 1.f;
accel_scale.z_scale = 1.f;
}
// save the driver level calibration data
(void)sprintf(str, "CAL_ACC%u_XOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset)));
(void)sprintf(str, "CAL_ACC%u_YOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.y_offset)));
(void)sprintf(str, "CAL_ACC%u_ZOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.z_offset)));
(void)sprintf(str, "CAL_ACC%u_XSCALE", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.x_scale)));
(void)sprintf(str, "CAL_ACC%u_YSCALE", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.y_scale)));
(void)sprintf(str, "CAL_ACC%u_ZSCALE", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.z_scale)));
(void)sprintf(str, "CAL_ACC%u_ID", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(device_id[uorb_index])));
if (failed) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, uorb_index);
return PX4_ERROR;
}
#ifdef __PX4_NUTTX
sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, uorb_index);
fd = px4_open(str, 0);
if (fd < 0) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "sensor does not exist");
res = PX4_ERROR;
} else {
res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
px4_close(fd);
}
if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, uorb_index);
}
#endif
}
if (res == PX4_OK) {
/* if there is a any preflight-check system response, let the barrage of messages through */
usleep(200000);
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
} else {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
}
/* give this message enough time to propagate */
usleep(600000);
return res;
}
static void
hardware_init(void)
{
__pdata uint16_t i;
// Disable the watchdog timer
PCA0MD &= ~0x40;
// Select the internal oscillator, prescale by 1
FLSCL = 0x40;
OSCICN = 0x8F;
CLKSEL = 0x00;
// Configure the VDD brown out detector
VDM0CN = 0x80;
for (i = 0; i < 350; i++); // Wait 100us for initialization
RSTSRC = 0x06; // enable brown out and missing clock reset sources
#ifdef _BOARD_RFD900A // Redefine port skips to override bootloader defs
P0SKIP = 0xCF; // P0 UART avail on XBAR
P1SKIP = 0xF8; // P1 SPI1, CEX0 avail on XBAR
P2SKIP = 0x01; // P2 CEX3 avail on XBAR, rest GPIO
#endif
// Configure crossbar for UART
P0MDOUT = 0x10; // UART Tx push-pull
SFRPAGE = CONFIG_PAGE;
P0DRV = 0x10; // UART TX
SFRPAGE = LEGACY_PAGE;
XBR0 = 0x01; // UART enable
// SPI1
#ifdef _BOARD_RFD900A
XBR1 |= 0x44; // enable SPI in 3-wire mode
P1MDOUT |= 0xF5; // SCK1, MOSI1, MISO1 push-pull
P2MDOUT |= 0xFF; // SCK1, MOSI1, MISO1 push-pull
#else
XBR1 |= 0x40; // enable SPI in 3-wire mode
P1MDOUT |= 0xF5; // SCK1, MOSI1, MISO1 push-pull
#endif
SFRPAGE = CONFIG_PAGE;
P1DRV |= 0xF5; // SPI signals use high-current mode, LEDs and PAEN High current drive
P2DRV |= 0xFF;
SFRPAGE = LEGACY_PAGE;
SPI1CFG = 0x40; // master mode
SPI1CN = 0x00; // 3 wire master mode
SPI1CKR = 0x00; // Initialise SPI prescaler to divide-by-2 (12.25MHz, technically out of spec)
SPI1CN |= 0x01; // enable SPI
NSS1 = 1; // set NSS high
// Clear the radio interrupt state
IE0 = 0;
// initialise timers
timer_init();
// UART - set the configured speed
serial_init(param_get(PARAM_SERIAL_SPEED));
// set all interrupts to the same priority level
IP = 0;
// global interrupt enable
EA = 1;
// Turn on the 'radio running' LED and turn off the bootloader LED
LED_RADIO = LED_ON;
LED_BOOTLOADER = LED_OFF;
// ADC system initialise for temp sensor
AD0EN = 1; // Enable ADC0
ADC0CF = 0xF9; // Set amp0gn=1 (1:1)
ADC0AC = 0x00;
ADC0MX = 0x1B; // Set ADC0MX to temp sensor
REF0CN = 0x07; // Define reference and enable temp sensor
#ifdef _BOARD_RFD900A
// PCA0, CEX0 setup and enable.
PCA0MD = 0x88;
PCA0PWM = 0x00;
PCA0CPH3 = 0x80;
PCA0CPM3 = 0x42;
PCA0CN = 0x40;
#endif
XBR2 = 0x40; // Crossbar (GPIO) enable
}
int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
{
int result = PX4_OK;
unsigned calibration_counter = 0;
const unsigned maxcount = 2400;
/* give directions */
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
const unsigned calibration_count = (maxcount * 2) / 3;
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 (PX4_OK == px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
paramreset_successful = true;
} else {
calibration_log_critical(mavlink_log_pub, "[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) {
calibration_log_critical(mavlink_log_pub, "[cal] No airspeed sensor found");
goto error_return;
}
/* set scaling offset parameter */
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, 1);
goto error_return;
}
}
calibration_log_critical(mavlink_log_pub, "[cal] Ensure sensor is not measuring wind");
usleep(500 * 1000);
while (calibration_counter < calibration_count) {
if (calibrate_cancel_check(mavlink_log_pub, 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++;
/* any differential pressure failure a reason to abort */
if (diff_pres.error_count != 0) {
calibration_log_critical(mavlink_log_pub, "[cal] Airspeed sensor is reporting errors (%llu)", diff_pres.error_count);
calibration_log_critical(mavlink_log_pub, "[cal] Check your wiring before trying again");
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
if (calibration_counter % (calibration_count / 20) == 0) {
calibration_log_info(mavlink_log_pub, 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_log_pub);
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 (PX4_OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
calibration_log_critical(mavlink_log_pub, "[cal] airspeed offset update failed");
}
px4_close(fd_scale);
}
// Prevent a completely zero param
// since this is used to detect a missing calibration
// This value is numerically down in the noise and has
// no effect on the sensor performance.
if (fabsf(diff_pres_offset) < 0.00000001f) {
diff_pres_offset = 0.00000001f;
}
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, 1);
goto error_return;
}
} else {
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
calibration_log_info(mavlink_log_pub, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
/* wait 500 ms to ensure parameter propagated through the system */
usleep(500 * 1000);
calibration_log_critical(mavlink_log_pub, "[cal] Blow across front of pitot without touching");
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_log_pub, 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);
if (fabsf(diff_pres.differential_pressure_filtered_pa) > 50.0f) {
if (diff_pres.differential_pressure_filtered_pa > 0) {
calibration_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%d Pa)", (int)diff_pres.differential_pressure_filtered_pa);
break;
} else {
/* do not allow negative values */
calibration_log_info(mavlink_log_pub, "[cal] Negative pressure difference detected (%d Pa)", (int)diff_pres.differential_pressure_filtered_pa);
calibration_log_info(mavlink_log_pub, "[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))) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, 1);
goto error_return;
}
/* save */
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 0);
param_save_default();
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
}
if (calibration_counter % 500 == 0) {
calibration_log_info(mavlink_log_pub, "[cal] Create air pressure! (got %d, wanted: 50 Pa)", (int)diff_pres.differential_pressure_filtered_pa);
tune_neutral(true);
}
calibration_counter++;
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
}
if (calibration_counter == maxcount) {
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
tune_neutral(true);
/* Wait 2sec for the airflow to stop and ensure the driver filter has caught up, otherwise
* the followup preflight checks might fail. */
usleep(2e6);
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 = PX4_ERROR;
goto normal_return;
}
void MulticopterLandDetector::_update_params()
{
param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate);
param_get(_paramHandle.maxVelocity, &_params.maxVelocity);
param_get(_paramHandle.maxRotation, &_params.maxRotation_rad_s);
_params.maxRotation_rad_s = math::radians(_params.maxRotation_rad_s);
param_get(_paramHandle.minThrottle, &_params.minThrottle);
param_get(_paramHandle.hoverThrottle, &_params.hoverThrottle);
param_get(_paramHandle.throttleRange, &_params.throttleRange);
param_get(_paramHandle.minManThrottle, &_params.minManThrottle);
param_get(_paramHandle.freefall_acc_threshold, &_params.freefall_acc_threshold);
param_get(_paramHandle.freefall_trigger_time, &_params.freefall_trigger_time);
_freefall_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1e6f * _params.freefall_trigger_time));
param_get(_paramHandle.manual_stick_down_threshold, &_params.manual_stick_down_threshold);
param_get(_paramHandle.altitude_max, &_params.altitude_max);
param_get(_paramHandle.manual_stick_up_position_takeoff_threshold, &_params.manual_stick_up_position_takeoff_threshold);
param_get(_paramHandle.landSpeed, &_params.landSpeed);
}
int
SF1XX::init()
{
int ret = PX4_ERROR;
int hw_model;
param_get(param_find("SENS_EN_SF1XX"), &hw_model);
switch (hw_model) {
case 0:
DEVICE_LOG("disabled.");
return ret;
case 1: /* SF10/a (25m 32Hz) */
_min_distance = 0.01f;
_max_distance = 25.0f;
_conversion_interval = 31250;
break;
case 2: /* SF10/b (50m 32Hz) */
_min_distance = 0.01f;
_max_distance = 50.0f;
_conversion_interval = 31250;
break;
case 3: /* SF10/c (100m 16Hz) */
_min_distance = 0.01f;
_max_distance = 100.0f;
_conversion_interval = 62500;
break;
case 4:
/* SF11/c (120m 20Hz) */
_min_distance = 0.01f;
_max_distance = 120.0f;
_conversion_interval = 50000;
break;
case 5:
/* SF20/LW20 (100m 48-388Hz) */
_min_distance = 0.001f;
_max_distance = 100.0f;
_conversion_interval = 20834;
break;
default:
DEVICE_LOG("invalid HW model %d.", hw_model);
return ret;
}
/* do I2C init (and probe) first */
if (I2C::init() != OK) {
return ret;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
set_address(SF1XX_BASEADDR);
if (_reports == nullptr) {
return ret;
}
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
/* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report = {};
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_HIGH);
if (_distance_sensor_topic == nullptr) {
DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
}
// Select altitude register
int ret2 = measure();
if (ret2 == 0) {
ret = OK;
_sensor_ok = true;
DEVICE_LOG("(%dm %dHz) with address %d found", (int)_max_distance,
(int)(1e6f / _conversion_interval), SF1XX_BASEADDR);
}
return ret;
}
void Mavlink::mavlink_update_system(void)
{
if (!_param_initialized) {
_param_system_id = param_find("MAV_SYS_ID");
_param_component_id = param_find("MAV_COMP_ID");
_param_system_type = param_find("MAV_TYPE");
_param_use_hil_gps = param_find("MAV_USEHILGPS");
_param_forward_externalsp = param_find("MAV_FWDEXTSP");
/* test param - needs to be referenced, but is unused */
(void)param_find("MAV_TEST_PAR");
}
/* update system and component id */
int32_t system_id;
param_get(_param_system_id, &system_id);
int32_t component_id;
param_get(_param_component_id, &component_id);
/* only allow system ID and component ID updates
* after reboot - not during operation */
if (!_param_initialized) {
if (system_id > 0 && system_id < 255) {
mavlink_system.sysid = system_id;
}
if (component_id > 0 && component_id < 255) {
mavlink_system.compid = component_id;
}
_param_initialized = true;
}
/* warn users that they need to reboot to take this
* into effect
*/
if (system_id != mavlink_system.sysid) {
send_statustext_critical("Save params and reboot to change SYSID");
}
if (component_id != mavlink_system.compid) {
send_statustext_critical("Save params and reboot to change COMPID");
}
int32_t system_type;
param_get(_param_system_type, &system_type);
if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
_system_type = system_type;
}
int32_t use_hil_gps;
param_get(_param_use_hil_gps, &use_hil_gps);
_use_hil_gps = (bool)use_hil_gps;
int32_t forward_externalsp;
param_get(_param_forward_externalsp, &forward_externalsp);
_forward_externalsp = (bool)forward_externalsp;
}
int parameters_update(const struct flow_position_control2_param_handles *h, struct flow_position_control2_params *p)
{
param_get(h->pos_p, &(p->pos_p));
param_get(h->pos_i, &(p->pos_i));
param_get(h->pos_d, &(p->pos_d));
param_get(h->height_p, &(p->height_p));
param_get(h->height_i, &(p->height_i));
param_get(h->height_d, &(p->height_d));
param_get(h->height_rate, &(p->height_rate));
param_get(h->height_min, &(p->height_min));
param_get(h->height_max, &(p->height_max));
param_get(h->thrust_feedforward, &(p->thrust_feedforward));
param_get(h->limit_speed_x, &(p->limit_speed_x));
param_get(h->limit_speed_y, &(p->limit_speed_y));
param_get(h->limit_height_error, &(p->limit_height_error));
param_get(h->limit_thrust_int, &(p->limit_thrust_int));
param_get(h->limit_thrust_upper, &(p->limit_thrust_upper));
param_get(h->limit_thrust_lower, &(p->limit_thrust_lower));
param_get(h->limit_yaw_step, &(p->limit_yaw_step));
param_get(h->manual_threshold, &(p->manual_threshold));
param_get(h->setpoint_radius, &(p->setpoint_radius));
param_get(h->rc_scale_pitch, &(p->rc_scale_pitch));
param_get(h->rc_scale_roll, &(p->rc_scale_roll));
param_get(h->rc_scale_yaw, &(p->rc_scale_yaw));
param_get(h->debug, &(p->debug));
return OK;
}
int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p)
{
param_get(h->q0, &(p->q[0]));
param_get(h->q1, &(p->q[1]));
param_get(h->q2, &(p->q[2]));
param_get(h->q3, &(p->q[3]));
param_get(h->q4, &(p->q[4]));
param_get(h->q5, &(p->q[5]));
param_get(h->q6, &(p->q[6]));
param_get(h->q7, &(p->q[7]));
param_get(h->q8, &(p->q[8]));
param_get(h->q9, &(p->q[9]));
param_get(h->q10, &(p->q[10]));
param_get(h->q11, &(p->q[11]));
param_get(h->r0, &(p->r[0]));
param_get(h->r1, &(p->r[1]));
param_get(h->r2, &(p->r[2]));
param_get(h->r3, &(p->r[3]));
param_get(h->r4, &(p->r[4]));
param_get(h->r5, &(p->r[5]));
param_get(h->r6, &(p->r[6]));
param_get(h->r7, &(p->r[7]));
param_get(h->r8, &(p->r[8]));
return OK;
}
int
MulticopterAttitudeControl::parameters_update()
{
float v;
/* roll gains */
param_get(_params_handles.roll_p, &v);
_params.att_p(0) = v;
param_get(_params_handles.roll_rate_p, &v);
_params.rate_p(0) = v;
param_get(_params_handles.roll_rate_i, &v);
_params.rate_i(0) = v;
param_get(_params_handles.roll_rate_d, &v);
_params.rate_d(0) = v;
param_get(_params_handles.roll_rate_ff, &v);
_params.rate_ff(0) = v;
/* pitch gains */
param_get(_params_handles.pitch_p, &v);
_params.att_p(1) = v;
param_get(_params_handles.pitch_rate_p, &v);
_params.rate_p(1) = v;
param_get(_params_handles.pitch_rate_i, &v);
_params.rate_i(1) = v;
param_get(_params_handles.pitch_rate_d, &v);
_params.rate_d(1) = v;
param_get(_params_handles.pitch_rate_ff, &v);
_params.rate_ff(1) = v;
/* yaw gains */
param_get(_params_handles.yaw_p, &v);
_params.att_p(2) = v;
param_get(_params_handles.yaw_rate_p, &v);
_params.rate_p(2) = v;
param_get(_params_handles.yaw_rate_i, &v);
_params.rate_i(2) = v;
param_get(_params_handles.yaw_rate_d, &v);
_params.rate_d(2) = v;
param_get(_params_handles.yaw_rate_ff, &v);
_params.rate_ff(2) = v;
param_get(_params_handles.yaw_ff, &_params.yaw_ff);
/* angular rate limits */
param_get(_params_handles.roll_rate_max, &_params.roll_rate_max);
_params.mc_rate_max(0) = math::radians(_params.roll_rate_max);
param_get(_params_handles.pitch_rate_max, &_params.pitch_rate_max);
_params.mc_rate_max(1) = math::radians(_params.pitch_rate_max);
param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
_params.mc_rate_max(2) = math::radians(_params.yaw_rate_max);
/* manual attitude control scale */
param_get(_params_handles.man_roll_max, &_params.man_roll_max);
param_get(_params_handles.man_pitch_max, &_params.man_pitch_max);
param_get(_params_handles.man_yaw_max, &_params.man_yaw_max);
_params.man_roll_max = math::radians(_params.man_roll_max);
_params.man_pitch_max = math::radians(_params.man_pitch_max);
_params.man_yaw_max = math::radians(_params.man_yaw_max);
/* manual rate control scale and auto mode roll/pitch rate limits */
param_get(_params_handles.acro_roll_max, &v);
_params.acro_rate_max(0) = math::radians(v);
param_get(_params_handles.acro_pitch_max, &v);
_params.acro_rate_max(1) = math::radians(v);
param_get(_params_handles.acro_yaw_max, &v);
_params.acro_rate_max(2) = math::radians(v);
_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
return OK;
}
int preflight_check_main(int argc, char *argv[])
{
bool fail_on_error = false;
if (argc > 1 && !strcmp(argv[1], "--help")) {
warnx("usage: preflight_check [--fail-on-error]\n\tif fail on error is enabled, will return 1 on error");
exit(1);
}
if (argc > 1 && !strcmp(argv[1], "--fail-on-error")) {
fail_on_error = true;
}
bool system_ok = true;
int fd;
/* open text message output path */
int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
int ret;
/* give the system some time to sample the sensors in the background */
usleep(150000);
/* ---- MAG ---- */
close(fd);
fd = open(MAG_DEVICE_PATH, 0);
if (fd < 0) {
warn("failed to open magnetometer - start with 'hmc5883 start' or 'lsm303d start'");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: NO MAG");
system_ok = false;
goto system_eval;
}
ret = ioctl(fd, MAGIOCSELFTEST, 0);
if (ret != OK) {
warnx("magnetometer calibration missing or bad - calibrate magnetometer first");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CHECK/CAL");
system_ok = false;
goto system_eval;
}
/* ---- ACCEL ---- */
close(fd);
fd = open(ACCEL_DEVICE_PATH, 0);
ret = ioctl(fd, ACCELIOCSELFTEST, 0);
if (ret != OK) {
warnx("accel self test failed");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK/CAL");
system_ok = false;
goto system_eval;
}
/* ---- GYRO ---- */
close(fd);
fd = open(GYRO_DEVICE_PATH, 0);
ret = ioctl(fd, GYROIOCSELFTEST, 0);
if (ret != OK) {
warnx("gyro self test failed");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK/CAL");
system_ok = false;
goto system_eval;
}
/* ---- BARO ---- */
close(fd);
fd = open(BARO_DEVICE_PATH, 0);
/* ---- RC CALIBRATION ---- */
param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max,
_parameter_handles_rev, _parameter_handles_dz;
float param_min, param_max, param_trim, param_rev, param_dz;
bool rc_ok = true;
char nbuf[20];
for (int i = 0; i < 12; i++) {
/* should the channel be enabled? */
uint8_t count = 0;
/* min values */
sprintf(nbuf, "RC%d_MIN", i + 1);
_parameter_handles_min = param_find(nbuf);
param_get(_parameter_handles_min, ¶m_min);
/* trim values */
sprintf(nbuf, "RC%d_TRIM", i + 1);
_parameter_handles_trim = param_find(nbuf);
param_get(_parameter_handles_trim, ¶m_trim);
/* max values */
sprintf(nbuf, "RC%d_MAX", i + 1);
_parameter_handles_max = param_find(nbuf);
param_get(_parameter_handles_max, ¶m_max);
/* channel reverse */
sprintf(nbuf, "RC%d_REV", i + 1);
_parameter_handles_rev = param_find(nbuf);
param_get(_parameter_handles_rev, ¶m_rev);
/* channel deadzone */
sprintf(nbuf, "RC%d_DZ", i + 1);
_parameter_handles_dz = param_find(nbuf);
param_get(_parameter_handles_dz, ¶m_dz);
/* assert min..center..max ordering */
if (param_min < 500) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_max > 2500) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_trim < param_min) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_trim > param_max) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
}
/* assert deadzone is sane */
if (param_dz > 500) {
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
count++;
}
/* XXX needs inspection of all the _MAP params */
// if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) {
// mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
// /* give system time to flush error message in case there are more */
// usleep(100000);
// count++;
// }
/* sanity checks pass, enable channel */
if (count) {
mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
usleep(100000);
rc_ok = false;
}
}
/* require RC ok to keep system_ok */
system_ok &= rc_ok;
system_eval:
if (system_ok) {
/* all good, exit silently */
exit(0);
} else {
fflush(stdout);
int buzzer = open("/dev/tone_alarm", O_WRONLY);
int leds = open(LED_DEVICE_PATH, 0);
/* flip blue led into alternating amber */
led_off(leds, LED_BLUE);
led_off(leds, LED_AMBER);
led_toggle(leds, LED_BLUE);
/* display and sound error */
for (int i = 0; i < 150; i++)
{
led_toggle(leds, LED_BLUE);
led_toggle(leds, LED_AMBER);
if (i % 10 == 0) {
ioctl(buzzer, TONE_SET_ALARM, 4);
} else if (i % 5 == 0) {
ioctl(buzzer, TONE_SET_ALARM, 2);
}
usleep(100000);
}
/* stop alarm */
ioctl(buzzer, TONE_SET_ALARM, 0);
/* switch on leds */
led_on(leds, LED_BLUE);
led_on(leds, LED_AMBER);
if (fail_on_error) {
/* exit with error message */
exit(1);
} else {
/* do not emit an error code to make sure system still boots */
exit(0);
}
}
}
int get_connect12_conf(int i_res, int i_conf, PROT prot) {
CONNECT connect;
FILE *debug_fp;
int i_atom;
int j_connect, k_connect, n_connect;
int j_res, j_conf, j_atom;
int k_lig, l_connect;
int n_ligs, n_conf;
int connect_found, lig_treated;
RES *res_p;
CONF *conf_p;
ATOM *atom_p,*jatom_p, *ligs[MAX_LIGS];
int ligs_res[MAX_LIGS];
int err = 0;
res_p = &prot.res[i_res];
conf_p = &prot.res[i_res].conf[i_conf];
/* Looping over all atoms */
for (i_atom=0; i_atom<prot.res[i_res].conf[i_conf].n_atom; i_atom++) {
atom_p = &prot.res[i_res].conf[i_conf].atom[i_atom];
if (!atom_p->on) continue;
for (j_connect=0; j_connect<MAX_CONNECTED; j_connect++) {
atom_p->connect12[j_connect] = NULL;
}
/* Error checking for connectivity parameter */
memset(atom_p->connect12, 0, MAX_CONNECTED*sizeof(void *));
if( param_get("CONNECT", conf_p->confName, atom_p->name, &connect) ) {
debug_fp = fopen(env.debug_log, "a");
fprintf(debug_fp, " Error! get_connect12(): Can't find CONNECT parameter of conformer \"%s\" atom \"%s\"\n", conf_p->confName, atom_p->name);
fclose(debug_fp);
err++;
continue;
}
if( connect.n > MAX_CONNECTED ) {
debug_fp = fopen(env.debug_log, "a");
fprintf(debug_fp, " Error! get_connect12(): Error in CONNECT parameter for conformer \"%s\" atom \"%s\", number of connected atoms is bigger than array size\nCheck parameter file or fix mcce.h with a bigger MAX_CONNECTED \n", conf_p->confName, atom_p->name);
fclose(debug_fp);
return USERERR;
}
lig_treated = 0; /* for each atom, ligand connectivy is checked at most once, no matter how many connectivities this atom has */
/* Looping over each connectivity member */
for (j_connect=0; j_connect<connect.n; j_connect++) {
connect_found = 0;
/* Ligand type connectivity or not */
if (!connect.atom[j_connect].ligand) { /* NOT ligand type */
if (!connect.atom[j_connect].res_offset) { /* If within the same residue (off_set is 0) */
j_res = i_res;
if ( !param_get("IATOM", conf_p->confName, connect.atom[j_connect].name, &j_atom) ) {
/* First search atom in the same conformer */
j_conf = i_conf;
atom_p->connect12[j_connect] = &prot.res[j_res].conf[j_conf].atom[j_atom];
atom_p->connect12_res[j_connect] = j_res;
connect_found = 1;
}
else {
/* If not in same conformer, there are two situations:
1. atom is in side chain conformer, connecting to backbone;
2. atom is in backbone conformer, connecting to side chain; */
for (j_conf = 0; j_conf < prot.res[j_res].n_conf; j_conf++) {
if (j_conf == i_conf) continue;
if ( !param_get("IATOM", prot.res[j_res].conf[j_conf].confName, connect.atom[j_connect].name, &j_atom) ) {
atom_p->connect12[j_connect] = &prot.res[j_res].conf[j_conf].atom[j_atom];
atom_p->connect12_res[j_connect] = j_res;
connect_found = 1;
break;
}
}
if (!connect_found) {
char err_msg1[MAXCHAR_LINE];
char err_msg2[MAXCHAR_LINE];
sprintf(err_msg1," Error! get_connect12(): connectivity of atom \"%s %s %c%04d\" is not complete",
atom_p->name, conf_p->confName, res_p->chainID, res_p->resSeq);
sprintf(err_msg2," get_connect12(): atom %s in the same residue is not found", connect.atom[j_connect].name);
if (param_exist(err_msg1, "", "")) {
if (param_exist(err_msg2, "", "")) {
continue;
}
}
param_sav(err_msg1, "", "", "", 0);
param_sav(err_msg2, "", "", "", 0);
debug_fp = fopen(env.debug_log, "a");
fprintf(debug_fp,"%s\n",err_msg1);
fprintf(debug_fp,"%s\n",err_msg2);
fclose(debug_fp);
err++;
}
}
}
else { /* Not in the same reside. */
j_res = i_res + connect.atom[j_connect].res_offset;
if (j_res < 0 || j_res >= prot.n_res) { /* j_res is out of residue list */
char err_msg1[MAXCHAR_LINE];
char err_msg2[MAXCHAR_LINE];
sprintf(err_msg1, " Error! get_connect12(): connectivity of atom \"%s %s %c%04d\" is not complete:\n",
atom_p->name, res_p->resName, res_p->chainID, res_p->resSeq);
sprintf(err_msg2, " get_connect12(): atom %s of residue i%+d is not found \n", connect.atom[j_connect].name,connect.atom[j_connect].res_offset);
if (param_exist(err_msg1, "", "")) {
if (param_exist(err_msg2, "", "")) {
continue;
}
}
param_sav(err_msg1, "", "", "", 0);
param_sav(err_msg2, "", "", "", 0);
debug_fp = fopen(env.debug_log, "a");
fprintf(debug_fp,"%s\n",err_msg1);
fprintf(debug_fp,"%s\n",err_msg2);
fclose(debug_fp);
err++;
continue;
}
for (j_conf = 0; j_conf < prot.res[j_res].n_conf; j_conf++) {
if ( !param_get("IATOM", prot.res[j_res].conf[j_conf].confName, connect.atom[j_connect].name, &j_atom) ) {
if (!prot.res[j_res].conf[j_conf].atom[j_atom].on) {
atom_p->connect12[j_connect] = &prot.res[j_res].conf[j_conf].atom[j_atom];
atom_p->connect12_res[j_connect] = j_res;
connect_found = 1;
debug_fp = fopen(env.debug_log, "a");
fprintf(debug_fp, " WARNING! get_connect12(): atom %s of residue i%+d is in connectivity list of atom \"%s %s %c%04d\", but it's not on\n",
connect.atom[j_connect].name,connect.atom[j_connect].res_offset,atom_p->name, res_p->resName, res_p->chainID, res_p->resSeq);
fclose(debug_fp);
break;
}
if (BOND_THR > dvv(atom_p->xyz, prot.res[j_res].conf[j_conf].atom[j_atom].xyz)) {
atom_p->connect12[j_connect] = &prot.res[j_res].conf[j_conf].atom[j_atom];
atom_p->connect12_res[j_connect] = j_res;
connect_found = 1;
break;
}
}
}
if (!connect_found) {
char err_msg1[MAXCHAR_LINE];
char err_msg2[MAXCHAR_LINE];
sprintf(err_msg1, " Error! get_connect12(): connectivity of atom \"%s %s %c%04d\" is not complete:\n",
atom_p->name, res_p->resName, res_p->chainID, res_p->resSeq);
sprintf(err_msg2, " get_connect12(): atom %s of residue i%+d is not found \n", connect.atom[j_connect].name,connect.atom[j_connect].res_offset);
if (param_exist(err_msg1, "", "")) {
if (param_exist(err_msg2, "", "")) {
continue;
}
}
param_sav(err_msg1, "", "", "", 0);
param_sav(err_msg2, "", "", "", 0);
debug_fp = fopen(env.debug_log, "a");
fprintf(debug_fp,"%s\n",err_msg1);
fprintf(debug_fp,"%s\n",err_msg2);
fclose(debug_fp);
err++;
}
}
}
else { /* Ligand type connectivity */
if (lig_treated) continue;
/* Loop over all atoms and find all atom within bond threshold */
n_ligs = 0;
for (j_res=0; j_res < prot.n_res; j_res++) {
if (j_res == i_res) continue;
if ( prot.res[j_res].n_conf>2 ) n_conf=2;
else n_conf = prot.res[j_res].n_conf;
for (j_conf=0; j_conf < n_conf; j_conf++) { /* searching is localized in backbone and first side chain */
for (j_atom=0; j_atom<prot.res[j_res].conf[j_conf].n_atom; j_atom++) {
jatom_p = &prot.res[j_res].conf[j_conf].atom[j_atom];
if (!jatom_p->on) continue;
if (BOND_THR > dvv(atom_p->xyz, jatom_p->xyz)) {
n_ligs++;
ligs[n_ligs-1] = jatom_p;
ligs_res[n_ligs-1] = j_res;
}
}
}
}
/* Go over connectivity list and put connected atoms in for those atom name is defined */
for (k_connect=j_connect; k_connect<connect.n; k_connect++) {
if (!connect.atom[k_connect].ligand) continue;
if (atom_p->connect12[k_connect]) continue;
if (strchr(connect.atom[k_connect].name, '?')) continue; /* If atom name in parameter is a '?', then go to next one */
for (k_lig=0; k_lig<n_ligs; k_lig++) {
if (strcmp(ligs[k_lig]->name, connect.atom[k_connect].name)) continue;
/* Check if ligand atom is already in the connectivity list, this is for the case one atom connect to over one atom with same name */
for (l_connect=j_connect; l_connect<connect.n; l_connect++) {
if (ligs[k_lig] == atom_p->connect12[l_connect]) break;
}
if (l_connect < connect.n) continue;
/* Adding ligs[k_lig] into list */
atom_p->connect12[k_connect] = ligs[k_lig];
atom_p->connect12_res[k_connect] = ligs_res[k_lig];
break;
}
}
/* Then go over connectivity list again and put atoms in for those atom name is not defined */
for (k_connect=j_connect; k_connect<connect.n; k_connect++) {
int k_lig_add;
float lig_dist;
if (!connect.atom[k_connect].ligand) continue;
if (atom_p->connect12[k_connect]) continue;
if (!strchr(connect.atom[k_connect].name, '?')) continue; /* If atom name in parameter is not a '?', then go to next one */
k_lig_add = -1;
lig_dist = BOND_THR;
for (k_lig=0; k_lig<n_ligs; k_lig++) {
if (ligs[k_lig]->name[1] == 'H') continue; /* If it's a proton, then not considered */
/* Check if ligand atom is already in the connectivity list, this is for the case one atom connect to over one atom with same name */
for (l_connect=j_connect; l_connect<connect.n; l_connect++) {
if (ligs[k_lig] == atom_p->connect12[l_connect]) break;
}
if (l_connect < connect.n) continue;
if (dvv(atom_p->xyz,ligs[k_lig]->xyz) < lig_dist) {
lig_dist = dvv(atom_p->xyz,ligs[k_lig]->xyz);
k_lig_add = k_lig;
}
}
/* Adding ligs[k_lig] into list */
if (k_lig_add >= 0) {
atom_p->connect12[k_connect] = ligs[k_lig_add];
atom_p->connect12_res[k_connect] = ligs_res[k_lig_add];
}
}
/* Go over connectivity list to check if there is empty slot */
for (k_connect=j_connect; k_connect<connect.n; k_connect++) {
if (!connect.atom[k_connect].ligand) continue;
if (atom_p->connect12[k_connect]) continue;
char err_msg1[MAXCHAR_LINE];
sprintf(err_msg1, " Warning! get_connect12(): An empty ligand connectivity slot found for atom %s in residue %s %d to atom %s\n", atom_p->name, res_p->resName, res_p->resSeq, connect.atom[k_connect].name);
if (param_exist(err_msg1, "", "")) {
continue;
}
param_sav(err_msg1, "", "", "", 0);
debug_fp = fopen(env.debug_log, "a");
fprintf(debug_fp,"%s\n",err_msg1);
fclose(debug_fp);
}
/* Check if all ligand atoms are in the connectivity list */
for (k_lig=0; k_lig<n_ligs; k_lig++) {
if (ligs[k_lig]->name[1] == 'H') continue; /* If it's a proton, then not considered */
/* Check if ligand atom is already in the connectivity list, this is for the case one atom connect to over one atom with same name */
for (l_connect=j_connect; l_connect<connect.n; l_connect++) {
if (ligs[k_lig] == atom_p->connect12[l_connect]) break;
}
if (l_connect < connect.n) continue;
char err_msg1[MAXCHAR_LINE];
sprintf(err_msg1, " Warning! get_connect12(): An atom (%s in residue %s %d) within bond threshold to atom %s in residue %s %d is not put in the connectivity list \n",ligs[k_lig]->name, ligs[k_lig]->resName, ligs[k_lig]->resSeq, atom_p->name, res_p->resName, res_p->resSeq);
if (param_exist(err_msg1, "", "")) {
continue;
}
param_sav(err_msg1, "", "", "", 0);
debug_fp = fopen(env.debug_log, "a");
fprintf(debug_fp,"%s\n",err_msg1);
fclose(debug_fp);
}
lig_treated = 1;
/* END of treating ligand type */
}
/* If connecting to a dummy atom, it could be the case connecting to NTR or CTR */
if (!atom_p->connect12[j_connect]) continue;
if ( atom_p->connect12[j_connect]->on) continue;
if (strcmp(connect.atom[j_connect].name, " CA ") && strcmp(connect.atom[j_connect].name, " C ")) continue;
//printf(" Debugging! residue %s%4d,conformer %s, on=%d\n", res_p->resName,res_p->resSeq,conf_p->confName,atom_p->connect12[j_connect]->on);
connect_found = 0;
for (j_res=0; j_res<prot.n_res; j_res++) {
if (strcmp(prot.res[j_res].resName, "NTR"))
if (strcmp(prot.res[j_res].resName, "NTG"))
if (strcmp(prot.res[j_res].resName, "CTR")) continue;
for (j_conf=0; j_conf<prot.res[j_res].n_conf; j_conf++) {
for (j_atom=0; j_atom<prot.res[j_res].conf[j_conf].n_atom; j_atom++) {
jatom_p = &prot.res[j_res].conf[j_conf].atom[j_atom];
if (!jatom_p->on) continue;
if (strcmp(connect.atom[j_connect].name, jatom_p->name)) continue;
if (BOND_THR > dvv(atom_p->xyz, jatom_p->xyz)) {
atom_p->connect12[j_connect] = jatom_p;
atom_p->connect12_res[j_connect] = j_res;
connect_found = 1;
break;
}
}
if (connect_found) break;
}
if (connect_found) break;
}
}
/* Move NULL pointer to the end of the array */
n_connect = connect.n;
for (j_connect=0; j_connect < n_connect-1; j_connect++) {
if (!atom_p->connect12[j_connect]) {
for (k_connect=j_connect; k_connect<n_connect-1; k_connect++) {
atom_p->connect12[k_connect] = atom_p->connect12[k_connect+1];
atom_p->connect12_res[k_connect] = atom_p->connect12_res[k_connect+1];
}
atom_p->connect12[n_connect-1] = NULL;
atom_p->connect12_res[n_connect-1] = 0;
j_connect--;
n_connect--;
}
}
}
return 0;
}
int
Sensors::parameters_update()
{
bool rc_valid = true;
float tmpScaleFactor = 0.0f;
float tmpRevFactor = 0.0f;
/* rc values */
for (unsigned int i = 0; i < _rc_max_chan_count; i++) {
param_get(_parameter_handles.min[i], &(_parameters.min[i]));
param_get(_parameter_handles.trim[i], &(_parameters.trim[i]));
param_get(_parameter_handles.max[i], &(_parameters.max[i]));
param_get(_parameter_handles.rev[i], &(_parameters.rev[i]));
param_get(_parameter_handles.dz[i], &(_parameters.dz[i]));
tmpScaleFactor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
tmpRevFactor = tmpScaleFactor * _parameters.rev[i];
/* handle blowup in the scaling factor calculation */
if (!isfinite(tmpScaleFactor) ||
(tmpRevFactor < 0.000001f) ||
(tmpRevFactor > 0.2f)) {
warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, tmpScaleFactor, (int)(_parameters.rev[i]));
/* scaling factors do not make sense, lock them down */
_parameters.scaling_factor[i] = 0.0f;
rc_valid = false;
} else {
_parameters.scaling_factor[i] = tmpScaleFactor;
}
}
/* handle wrong values */
if (!rc_valid) {
warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n");
}
const char *paramerr = "FAIL PARM LOAD";
/* channel mapping */
if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) {
warnx("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) {
warnx("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) {
warnx("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) {
warnx("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_failsafe, &(_parameters.rc_map_failsafe)) != OK) {
warnx("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) {
warnx("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) {
warnx("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_posctl_sw, &(_parameters.rc_map_posctl_sw)) != OK) {
warnx("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) {
warnx("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
warnx("%s", paramerr);
}
param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1));
param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2));
param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3));
param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4));
param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5));
param_get(_parameter_handles.rc_fails_thr, &(_parameters.rc_fails_thr));
param_get(_parameter_handles.rc_assist_th, &(_parameters.rc_assist_th));
_parameters.rc_assist_inv = (_parameters.rc_assist_th < 0);
_parameters.rc_assist_th = fabs(_parameters.rc_assist_th);
param_get(_parameter_handles.rc_auto_th, &(_parameters.rc_auto_th));
_parameters.rc_auto_inv = (_parameters.rc_auto_th < 0);
_parameters.rc_auto_th = fabs(_parameters.rc_auto_th);
param_get(_parameter_handles.rc_posctl_th, &(_parameters.rc_posctl_th));
_parameters.rc_posctl_inv = (_parameters.rc_posctl_th < 0);
_parameters.rc_posctl_th = fabs(_parameters.rc_posctl_th);
param_get(_parameter_handles.rc_return_th, &(_parameters.rc_return_th));
_parameters.rc_return_inv = (_parameters.rc_return_th < 0);
_parameters.rc_return_th = fabs(_parameters.rc_return_th);
param_get(_parameter_handles.rc_loiter_th, &(_parameters.rc_loiter_th));
_parameters.rc_loiter_inv = (_parameters.rc_loiter_th < 0);
_parameters.rc_loiter_th = fabs(_parameters.rc_loiter_th);
/* update RC function mappings */
_rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
_rc.function[ROLL] = _parameters.rc_map_roll - 1;
_rc.function[PITCH] = _parameters.rc_map_pitch - 1;
_rc.function[YAW] = _parameters.rc_map_yaw - 1;
_rc.function[MODE] = _parameters.rc_map_mode_sw - 1;
_rc.function[RETURN] = _parameters.rc_map_return_sw - 1;
_rc.function[POSCTL] = _parameters.rc_map_posctl_sw - 1;
_rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1;
_rc.function[FLAPS] = _parameters.rc_map_flaps - 1;
_rc.function[AUX_1] = _parameters.rc_map_aux1 - 1;
_rc.function[AUX_2] = _parameters.rc_map_aux2 - 1;
_rc.function[AUX_3] = _parameters.rc_map_aux3 - 1;
_rc.function[AUX_4] = _parameters.rc_map_aux4 - 1;
_rc.function[AUX_5] = _parameters.rc_map_aux5 - 1;
/* gyro offsets */
param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0]));
param_get(_parameter_handles.gyro_offset[1], &(_parameters.gyro_offset[1]));
param_get(_parameter_handles.gyro_offset[2], &(_parameters.gyro_offset[2]));
param_get(_parameter_handles.gyro_scale[0], &(_parameters.gyro_scale[0]));
param_get(_parameter_handles.gyro_scale[1], &(_parameters.gyro_scale[1]));
param_get(_parameter_handles.gyro_scale[2], &(_parameters.gyro_scale[2]));
/* accel offsets */
param_get(_parameter_handles.accel_offset[0], &(_parameters.accel_offset[0]));
param_get(_parameter_handles.accel_offset[1], &(_parameters.accel_offset[1]));
param_get(_parameter_handles.accel_offset[2], &(_parameters.accel_offset[2]));
param_get(_parameter_handles.accel_scale[0], &(_parameters.accel_scale[0]));
param_get(_parameter_handles.accel_scale[1], &(_parameters.accel_scale[1]));
param_get(_parameter_handles.accel_scale[2], &(_parameters.accel_scale[2]));
/* mag offsets */
param_get(_parameter_handles.mag_offset[0], &(_parameters.mag_offset[0]));
param_get(_parameter_handles.mag_offset[1], &(_parameters.mag_offset[1]));
param_get(_parameter_handles.mag_offset[2], &(_parameters.mag_offset[2]));
/* mag scaling */
param_get(_parameter_handles.mag_scale[0], &(_parameters.mag_scale[0]));
param_get(_parameter_handles.mag_scale[1], &(_parameters.mag_scale[1]));
param_get(_parameter_handles.mag_scale[2], &(_parameters.mag_scale[2]));
/* Airspeed offset */
param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa));
param_get(_parameter_handles.diff_pres_analog_enabled, &(_parameters.diff_pres_analog_enabled));
/* scaling of ADC ticks to battery voltage */
if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
warnx("%s", paramerr);
}
/* scaling of ADC ticks to battery current */
if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) {
warnx("%s", paramerr);
}
param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation));
get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
return OK;
}
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;
}
}
/*
* Read specified number of accelerometer samples, calculate average and dispersion.
*/
calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num)
{
/* get total sensor board rotation matrix */
param_t board_rotation_h = param_find("SENS_BOARD_ROT");
param_t board_offset_x = param_find("SENS_BOARD_X_OFF");
param_t board_offset_y = param_find("SENS_BOARD_Y_OFF");
param_t board_offset_z = param_find("SENS_BOARD_Z_OFF");
float board_offset[3];
param_get(board_offset_x, &board_offset[0]);
param_get(board_offset_y, &board_offset[1]);
param_get(board_offset_z, &board_offset[2]);
math::Matrix<3, 3> board_rotation_offset;
board_rotation_offset.from_euler(M_DEG_TO_RAD_F * board_offset[0],
M_DEG_TO_RAD_F * board_offset[1],
M_DEG_TO_RAD_F * board_offset[2]);
int32_t board_rotation_int;
param_get(board_rotation_h, &(board_rotation_int));
enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
math::Matrix<3, 3> board_rotation;
get_rot_matrix(board_rotation_id, &board_rotation);
/* combine board rotation with offset rotation */
board_rotation = board_rotation_offset * board_rotation;
px4_pollfd_struct_t fds[max_accel_sens];
for (unsigned i = 0; i < max_accel_sens; i++) {
fds[i].fd = subs[i];
fds[i].events = POLLIN;
}
unsigned counts[max_accel_sens] = { 0 };
float accel_sum[max_accel_sens][3];
memset(accel_sum, 0, sizeof(accel_sum));
unsigned errcount = 0;
struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */
/* try to get latest thermal corrections */
if (orb_copy(ORB_ID(sensor_correction), sensor_correction_sub, &sensor_correction) != 0) {
/* use default values */
memset(&sensor_correction, 0, sizeof(sensor_correction));
for (unsigned i = 0; i < 3; i++) {
sensor_correction.accel_scale_0[i] = 1.0f;
sensor_correction.accel_scale_1[i] = 1.0f;
sensor_correction.accel_scale_2[i] = 1.0f;
}
}
/* use the first sensor to pace the readout, but do per-sensor counts */
while (counts[0] < samples_num) {
int poll_ret = px4_poll(&fds[0], max_accel_sens, 1000);
if (poll_ret > 0) {
for (unsigned s = 0; s < max_accel_sens; s++) {
bool changed;
orb_check(subs[s], &changed);
if (changed) {
struct accel_report arp;
orb_copy(ORB_ID(sensor_accel), subs[s], &arp);
// Apply thermal offset corrections in sensor/board frame
if (s == 0) {
accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_0[0]);
accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_0[1]);
accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_0[2]);
} else if (s == 1) {
accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_1[0]);
accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_1[1]);
accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_1[2]);
} else if (s == 2) {
accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_2[0]);
accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_2[1]);
accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_2[2]);
} else {
accel_sum[s][0] += arp.x;
accel_sum[s][1] += arp.y;
accel_sum[s][2] += arp.z;
}
counts[s]++;
}
}
} else {
errcount++;
continue;
}
if (errcount > samples_num / 10) {
return calibrate_return_error;
}
}
// rotate sensor measurements from sensor to body frame using board rotation matrix
for (unsigned i = 0; i < max_accel_sens; i++) {
math::Vector<3> accel_sum_vec(&accel_sum[i][0]);
accel_sum_vec = board_rotation * accel_sum_vec;
memcpy(&accel_sum[i][0], &accel_sum_vec.data[0], sizeof(accel_sum[i]));
}
for (unsigned s = 0; s < max_accel_sens; s++) {
for (unsigned i = 0; i < 3; i++) {
accel_avg[s][orient][i] = accel_sum[s][i] / counts[s];
}
}
return calibrate_return_ok;
}
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 Pascal", (int)diff_pres_offset);
/* wait 500 ms to ensure parameter propagated through the system */
usleep(500 * 1000);
mavlink_log_critical(mavlink_fd, "Create airflow now");
calibration_counter = 0;
const unsigned 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 % 500 == 0) {
mavlink_log_info(mavlink_fd, "Create airflow! (%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_critical(mavlink_fd, "Swap static and dynamic ports!");
mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)",
(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;
}
int
MulticopterAttitudeControl::parameters_update()
{
float v;
float roll_tc, pitch_tc;
param_get(_params_handles.roll_tc, &roll_tc);
param_get(_params_handles.pitch_tc, &pitch_tc);
/* roll gains */
param_get(_params_handles.roll_p, &v);
_params.att_p(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc);
param_get(_params_handles.roll_rate_p, &v);
_params.rate_p(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc);
param_get(_params_handles.roll_rate_i, &v);
_params.rate_i(0) = v;
param_get(_params_handles.roll_rate_d, &v);
_params.rate_d(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc);
param_get(_params_handles.roll_rate_ff, &v);
_params.rate_ff(0) = v;
/* pitch gains */
param_get(_params_handles.pitch_p, &v);
_params.att_p(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc);
param_get(_params_handles.pitch_rate_p, &v);
_params.rate_p(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc);
param_get(_params_handles.pitch_rate_i, &v);
_params.rate_i(1) = v;
param_get(_params_handles.pitch_rate_d, &v);
_params.rate_d(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc);
param_get(_params_handles.pitch_rate_ff, &v);
_params.rate_ff(1) = v;
/* yaw gains */
param_get(_params_handles.yaw_p, &v);
_params.att_p(2) = v;
param_get(_params_handles.yaw_rate_p, &v);
_params.rate_p(2) = v;
param_get(_params_handles.yaw_rate_i, &v);
_params.rate_i(2) = v;
param_get(_params_handles.yaw_rate_d, &v);
_params.rate_d(2) = v;
param_get(_params_handles.yaw_rate_ff, &v);
_params.rate_ff(2) = v;
param_get(_params_handles.yaw_ff, &_params.yaw_ff);
/* angular rate limits */
param_get(_params_handles.roll_rate_max, &_params.roll_rate_max);
_params.mc_rate_max(0) = math::radians(_params.roll_rate_max);
param_get(_params_handles.pitch_rate_max, &_params.pitch_rate_max);
_params.mc_rate_max(1) = math::radians(_params.pitch_rate_max);
param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
_params.mc_rate_max(2) = math::radians(_params.yaw_rate_max);
/* manual rate control scale and auto mode roll/pitch rate limits */
param_get(_params_handles.acro_roll_max, &v);
_params.acro_rate_max(0) = math::radians(v);
param_get(_params_handles.acro_pitch_max, &v);
_params.acro_rate_max(1) = math::radians(v);
param_get(_params_handles.acro_yaw_max, &v);
_params.acro_rate_max(2) = math::radians(v);
/* stick deflection needed in rattitude mode to control rates not angles */
param_get(_params_handles.rattitude_thres, &_params.rattitude_thres);
param_get(_params_handles.vtol_type, &_params.vtol_type);
_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
return OK;
}
int main(int argc, char** argv)
{
setlocale(LC_ALL, "");
TBSYS_LOGGER.setFileName("ups_admin.log", true);
TBSYS_LOG(INFO, "ups_admin start==================================================");
ob_init_memory_pool();
CmdLineParam clp;
parse_cmd_line(argc, argv, clp);
timeout = clp.timeout?: timeout;
MockClient client;
init_mock_client(clp.serv_addr, clp.serv_port, clp.login_type, client);
PageArena<char> allocer;
int rc = 0;
if (0 == strcmp("apply", clp.cmd_type))
{
apply(clp.ini_fname, allocer, client);
}
else if (0 == strcmp("get_clog_status", clp.cmd_type))
{
get_clog_status(client);
}
else if (0 == strcmp("get_max_log_seq", clp.cmd_type))
{
get_max_log_seq(client);
}
else if (0 == strcmp("get_clog_cursor", clp.cmd_type))
{
get_clog_cursor(client);
}
else if (0 == strcmp("get_clog_master", clp.cmd_type))
{
get_clog_master(client);
}
else if (0 == strcmp("get_log_sync_delay_stat", clp.cmd_type))
{
get_log_sync_delay_stat(client);
}
else if (0 == strcmp("get", clp.cmd_type))
{
get(clp.ini_fname, allocer, client, clp.version_range,clp.expected_result_fname,
clp.schema_fname);
}
else if (0 == strcmp("param_get", clp.cmd_type))
{
param_get(clp.ini_fname, client);
}
else if (0 == strcmp("scan", clp.cmd_type))
{
scan(clp.ini_fname, allocer, client, clp.version_range,clp.expected_result_fname,
clp.schema_fname);
}
else if (0 == strcmp("total_scan", clp.cmd_type))
{
total_scan(clp.ini_fname, allocer, client, clp.version_range);
}
else if (0 == strcmp("minor_freeze", clp.cmd_type))
{
minor_freeze(client);
}
else if (0 == strcmp("major_freeze", clp.cmd_type))
{
major_freeze(client);
}
else if (0 == strcmp("fetch_schema", clp.cmd_type))
{
fetch_schema(client, clp.timestamp);
}
else if (0 == strcmp("get_sstable_range_list", clp.cmd_type))
{
get_sstable_range_list(client, clp.timestamp, clp.session_id);
}
else if (0 == strcmp("drop", clp.cmd_type))
{
drop(client);
}
else if (0 == strcmp("dump_memtable", clp.cmd_type))
{
dump_memtable(clp.ini_fname, client);
}
else if (0 == strcmp("dump_schemas", clp.cmd_type))
{
dump_schemas(client);
}
else if (0 == strcmp("force_fetch_schema", clp.cmd_type))
{
force_fetch_schema(client);
}
else if (0 == strcmp("reload_conf", clp.cmd_type))
{
reload_conf(clp.ini_fname, client);
}
else if (0 == strcmp("memory_watch", clp.cmd_type))
{
memory_watch(client);
}
else if (0 == strcmp("memory_limit", clp.cmd_type))
{
memory_limit(clp.memory_limit, clp.memtable_limit, client);
}
else if (0 == strcmp("priv_queue_conf", clp.cmd_type))
{
priv_queue_conf(clp.priv_queue_conf, client);
}
else if (0 == strcmp("clear_active_memtable", clp.cmd_type))
{
clear_active_memtable(client);
}
else if (0 == strcmp("get_last_frozen_version", clp.cmd_type))
{
get_last_frozen_version(client);
}
else if (0 == strcmp("fetch_ups_stat_info", clp.cmd_type))
{
fetch_ups_stat_info(client);
}
else if (0 == strcmp("get_bloomfilter", clp.cmd_type))
{
get_bloomfilter(client, clp.timestamp);
}
else if (0 == strcmp("store_memtable", clp.cmd_type))
{
store_memtable(client, clp.timestamp);
}
else if (0 == strcmp("erase_sstable", clp.cmd_type))
{
erase_sstable(client);
}
else if (0 == strcmp("load_new_store", clp.cmd_type))
{
load_new_store(client);
}
else if (0 == strcmp("reload_all_store", clp.cmd_type))
{
reload_all_store(client);
}
else if (0 == strcmp("reload_store", clp.cmd_type))
{
reload_store(client, clp.timestamp);
}
else if (0 == strcmp("umount_store", clp.cmd_type))
{
umount_store(client, clp.ini_fname);
}
else if (0 == strcmp("force_report_frozen_version", clp.cmd_type))
{
force_report_frozen_version(client);
}
else if (0 == strcmp("switch_commit_log", clp.cmd_type))
{
switch_commit_log(client);
}
else if (0 == strcmp("get_table_time_stamp", clp.cmd_type))
{
get_table_time_stamp(client, clp.timestamp);
}
else if (0 == strcmp("disable_memtable_checksum", clp.cmd_type))
{
disable_memtable_checksum(client);
}
else if (0 == strcmp("enable_memtable_checksum", clp.cmd_type))
{
enable_memtable_checksum(client);
}
else if (0 == strcmp("immediately_drop_memtable", clp.cmd_type))
{
immediately_drop_memtable(client);
}
else if (0 == strcmp("delay_drop_memtable", clp.cmd_type))
{
delay_drop_memtable(client);
}
else if (0 == strcmp("minor_load_bypass", clp.cmd_type))
{
rc = minor_load_bypass(client);
}
else if (0 == strcmp("major_load_bypass", clp.cmd_type))
{
rc = major_load_bypass(client);
}
else if(0 == strcmp("list_sessions", clp.cmd_type))
{
client.list_sessions(timeout);
}
else if(0 == strcmp("kill_session", clp.cmd_type))
{
client.kill_session(timeout, clp.session_id);
}
else if (0 == strcmp("change_log_level", clp.cmd_type))
{
change_log_level(client, clp.log_level);
}
else if (0 == strcmp("get_slave_ups_info", clp.cmd_type))
{
get_slave_ups_info(client);
}
else if (0 == strcmp("print_scanner", clp.cmd_type))
{
print_scanner(clp.ini_fname);
}
else if (0 == strcmp("print_schema", clp.cmd_type))
{
print_schema(clp.ini_fname);
}
else if (0 == strcmp("sql_query", clp.cmd_type))
{
execute_sql(client, clp.sql_query);
}
else if (0 == strcmp("ups_show_sessions", clp.cmd_type))
{
ups_show_sessions(client);
}
else if (0 == strcmp("ups_kill_session", clp.cmd_type))
{
ups_kill_session(static_cast<uint32_t>(clp.session_id), client);
}
else
{
print_usage();
}
if (!clp.quickly_exit)
{
client.destroy();
TBSYS_LOG(INFO, "ups_admin end==================================================");
return rc;
}
else
{
TBSYS_LOG(INFO, "ups_admin killed==================================================");
fflush(stdout);
fflush(stderr);
_exit(rc);
}
}
int
FixedwingAttitudeControl::parameters_update()
{
param_get(_parameter_handles.tconst, &(_parameters.tconst));
param_get(_parameter_handles.p_p, &(_parameters.p_p));
param_get(_parameter_handles.p_i, &(_parameters.p_i));
param_get(_parameter_handles.p_ff, &(_parameters.p_ff));
param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos));
param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg));
param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max));
param_get(_parameter_handles.p_roll_feedforward, &(_parameters.p_roll_feedforward));
param_get(_parameter_handles.r_p, &(_parameters.r_p));
param_get(_parameter_handles.r_i, &(_parameters.r_i));
param_get(_parameter_handles.r_ff, &(_parameters.r_ff));
param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max));
param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax));
param_get(_parameter_handles.y_p, &(_parameters.y_p));
param_get(_parameter_handles.y_i, &(_parameters.y_i));
param_get(_parameter_handles.y_ff, &(_parameters.y_ff));
param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max));
param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed));
param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax));
param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max));
param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll));
param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch));
param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw));
param_get(_parameter_handles.rollsp_offset_deg, &(_parameters.rollsp_offset_deg));
param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg));
_parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg);
_parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg);
param_get(_parameter_handles.man_roll_max, &(_parameters.man_roll_max));
param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max));
_parameters.man_roll_max = math::radians(_parameters.man_roll_max);
_parameters.man_pitch_max = math::radians(_parameters.man_pitch_max);
/* pitch control parameters */
_pitch_ctrl.set_time_constant(_parameters.tconst);
_pitch_ctrl.set_k_p(_parameters.p_p);
_pitch_ctrl.set_k_i(_parameters.p_i);
_pitch_ctrl.set_k_ff(_parameters.p_ff);
_pitch_ctrl.set_integrator_max(_parameters.p_integrator_max);
_pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos));
_pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg));
_pitch_ctrl.set_roll_ff(_parameters.p_roll_feedforward);
/* roll control parameters */
_roll_ctrl.set_time_constant(_parameters.tconst);
_roll_ctrl.set_k_p(_parameters.r_p);
_roll_ctrl.set_k_i(_parameters.r_i);
_roll_ctrl.set_k_ff(_parameters.r_ff);
_roll_ctrl.set_integrator_max(_parameters.r_integrator_max);
_roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax));
/* yaw control parameters */
_yaw_ctrl.set_k_p(_parameters.y_p);
_yaw_ctrl.set_k_i(_parameters.y_i);
_yaw_ctrl.set_k_ff(_parameters.y_ff);
_yaw_ctrl.set_integrator_max(_parameters.y_integrator_max);
_yaw_ctrl.set_coordinated_min_speed(_parameters.y_coordinated_min_speed);
_yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax));
return OK;
}
int uavcan_main(int argc, char *argv[])
{
if (argc < 2) {
print_usage();
::exit(1);
}
bool fw = argc > 2 && !std::strcmp(argv[2], "fw");
if (!std::strcmp(argv[1], "start")) {
if (UavcanNode::instance()) {
if (fw && UavcanServers::instance() == nullptr) {
int rv = UavcanNode::instance()->fw_server(UavcanNode::Start);
if (rv < 0) {
warnx("Firmware Server Failed to Start %d", rv);
::exit(rv);
}
::exit(0);
}
// Already running, no error
warnx("already started");
::exit(0);
}
// Node ID
int32_t node_id = 1;
(void)param_get(param_find("UAVCAN_NODE_ID"), &node_id);
if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) {
warnx("Invalid Node ID %i", node_id);
::exit(1);
}
// CAN bitrate
int32_t bitrate = 1000000;
(void)param_get(param_find("UAVCAN_BITRATE"), &bitrate);
// Start
warnx("Node ID %u, bitrate %u", node_id, bitrate);
return UavcanNode::start(node_id, bitrate);
}
/* commands below require the app to be started */
UavcanNode *const inst = UavcanNode::instance();
if (!inst) {
errx(1, "application not running");
}
if (fw && !std::strcmp(argv[1], "update")) {
if (UavcanServers::instance() == nullptr) {
errx(1, "firmware server is not running");
}
int rv = UavcanNode::instance()->fw_server(UavcanNode::CheckFW);
::exit(rv);
}
if (fw && (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info"))) {
printf("Firmware Server is %s\n", UavcanServers::instance() ? "Running" : "Stopped");
::exit(0);
}
if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) {
inst->print_info();
::exit(0);
}
if (!std::strcmp(argv[1], "shrink")) {
inst->shrink();
::exit(0);
}
if (!std::strcmp(argv[1], "arm")) {
inst->arm_actuators(true);
::exit(0);
}
if (!std::strcmp(argv[1], "disarm")) {
inst->arm_actuators(false);
::exit(0);
}
/*
* Parameter setting commands
*
* uavcan param list <node>
* uavcan param save <node>
* uavcan param get <node> <name>
* uavcan param set <node> <name> <value>
*
*/
int node_arg = !std::strcmp(argv[1], "reset") ? 2 : 3;
if (!std::strcmp(argv[1], "param") || node_arg == 2) {
if (argc < node_arg + 1) {
errx(1, "Node id required");
}
int nodeid = atoi(argv[node_arg]);
if (nodeid == 0 || nodeid > 127 || nodeid == inst->get_node().getNodeID().get()) {
errx(1, "Invalid Node id");
}
if (node_arg == 2) {
return inst->reset_node(nodeid);
} else if (!std::strcmp(argv[2], "list")) {
return inst->list_params(nodeid);
} else if (!std::strcmp(argv[2], "save")) {
return inst->save_params(nodeid);
} else if (!std::strcmp(argv[2], "get")) {
if (argc < 5) {
errx(1, "Name required");
}
return inst->get_param(nodeid, argv[4]);
} else if (!std::strcmp(argv[2], "set")) {
if (argc < 5) {
errx(1, "Name required");
}
if (argc < 6) {
errx(1, "Value required");
}
return inst->set_param(nodeid, argv[4], argv[5]);
}
}
if (!std::strcmp(argv[1], "hardpoint")) {
if (!std::strcmp(argv[2], "set") && argc > 4) {
const int hardpoint_id = atoi(argv[3]);
const int command = atoi(argv[4]);
// Sanity check - weed out negative values, check against maximums
if (hardpoint_id >= 0 && hardpoint_id < 256 &&
command >= 0 && command < 65536) {
inst->hardpoint_controller_set((uint8_t) hardpoint_id, (uint16_t) command);
} else {
errx(1, "Invalid argument");
}
} else {
errx(1, "Invalid hardpoint command");
}
::exit(0);
}
if (!std::strcmp(argv[1], "stop")) {
if (fw) {
int rv = inst->fw_server(UavcanNode::Stop);
if (rv < 0) {
warnx("Firmware Server Failed to Stop %d", rv);
::exit(rv);
}
::exit(0);
} else {
delete inst;
::exit(0);
}
}
print_usage();
::exit(1);
}
/**
* Update parameters.
*/
int
VtolAttitudeControl::parameters_update()
{
float v;
int l;
/* idle pwm for mc mode */
param_get(_params_handles.idle_pwm_mc, &_params.idle_pwm_mc);
/* vtol motor count */
param_get(_params_handles.vtol_motor_count, &_params.vtol_motor_count);
/* vtol fw permanent stabilization */
param_get(_params_handles.vtol_fw_permanent_stab, &_params.vtol_fw_permanent_stab);
/* vtol mc mode min airspeed */
param_get(_params_handles.mc_airspeed_min, &v);
_params.mc_airspeed_min = v;
/* vtol mc mode max airspeed */
param_get(_params_handles.mc_airspeed_max, &v);
_params.mc_airspeed_max = v;
/* vtol mc mode trim airspeed */
param_get(_params_handles.mc_airspeed_trim, &v);
_params.mc_airspeed_trim = v;
/* vtol pitch trim for fw mode */
param_get(_params_handles.fw_pitch_trim, &v);
_params.fw_pitch_trim = v;
/* vtol maximum power engine can produce */
param_get(_params_handles.power_max, &v);
_params.power_max = v;
/* vtol propeller efficiency factor */
param_get(_params_handles.prop_eff, &v);
_params.prop_eff = v;
/* vtol total airspeed estimate low pass gain */
param_get(_params_handles.arsp_lp_gain, &v);
_params.arsp_lp_gain = v;
param_get(_params_handles.vtol_type, &l);
_params.vtol_type = l;
/* vtol lock elevons in multicopter */
param_get(_params_handles.elevons_mc_lock, &l);
_params.elevons_mc_lock = l;
return OK;
}
int
MulticopterAttitudeControl::parameters_update()
{
float v;
/* roll gains */
param_get(_params_handles.roll_p, &v);
_params.att_p(0) = v;
param_get(_params_handles.roll_rate_p, &v);
_params.rate_p(0) = v;
param_get(_params_handles.roll_rate_i, &v);
_params.rate_i(0) = v;
param_get(_params_handles.roll_rate_d, &v);
_params.rate_d(0) = v;
/* pitch gains */
param_get(_params_handles.pitch_p, &v);
_params.att_p(1) = v;
param_get(_params_handles.pitch_rate_p, &v);
_params.rate_p(1) = v;
param_get(_params_handles.pitch_rate_i, &v);
_params.rate_i(1) = v;
param_get(_params_handles.pitch_rate_d, &v);
_params.rate_d(1) = v;
/* yaw gains */
param_get(_params_handles.yaw_p, &v);
_params.att_p(2) = v;
param_get(_params_handles.yaw_rate_p, &v);
_params.rate_p(2) = v;
param_get(_params_handles.yaw_rate_i, &v);
_params.rate_i(2) = v;
param_get(_params_handles.yaw_rate_d, &v);
_params.rate_d(2) = v;
param_get(_params_handles.yaw_ff, &_params.yaw_ff);
param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
_params.yaw_rate_max = math::radians(_params.yaw_rate_max);
/* manual control scale */
param_get(_params_handles.man_roll_max, &_params.man_roll_max);
param_get(_params_handles.man_pitch_max, &_params.man_pitch_max);
param_get(_params_handles.man_yaw_max, &_params.man_yaw_max);
_params.man_roll_max = math::radians(_params.man_roll_max);
_params.man_pitch_max = math::radians(_params.man_pitch_max);
_params.man_yaw_max = math::radians(_params.man_yaw_max);
return OK;
}
int uavcan_main(int argc, char *argv[])
{
if (argc < 2) {
print_usage();
::exit(1);
}
if (!std::strcmp(argv[1], "start")) {
if (UavcanNode::instance()) {
// Already running, no error
warnx("already started");
::exit(0);
}
// Node ID
int32_t node_id = 1;
(void)param_get(param_find("UAVCAN_NODE_ID"), &node_id);
if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) {
warnx("Invalid Node ID %i", node_id);
::exit(1);
}
// CAN bitrate
int32_t bitrate = 1000000;
(void)param_get(param_find("UAVCAN_BITRATE"), &bitrate);
// Start
warnx("Node ID %u, bitrate %u", node_id, bitrate);
return UavcanNode::start(node_id, bitrate);
}
/* commands below require the app to be started */
UavcanNode *const inst = UavcanNode::instance();
if (!inst) {
errx(1, "application not running");
}
if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) {
inst->print_info();
::exit(0);
}
if (!std::strcmp(argv[1], "arm")) {
inst->arm_actuators(true);
::exit(0);
}
if (!std::strcmp(argv[1], "disarm")) {
inst->arm_actuators(false);
::exit(0);
}
if (!std::strcmp(argv[1], "stop")) {
delete inst;
::exit(0);
}
print_usage();
::exit(1);
}
int
param_export(int fd, bool only_unsaved)
{
struct param_wbuf_s *s = NULL;
struct bson_encoder_s encoder;
int result = -1;
param_lock();
bson_encoder_init_file(&encoder, fd);
/* no modified parameters -> we are done */
if (param_values == NULL) {
result = 0;
goto out;
}
while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != NULL) {
int32_t i;
float f;
/*
* If we are only saving values changed since last save, and this
* one hasn't, then skip it
*/
if (only_unsaved && !s->unsaved)
continue;
s->unsaved = false;
/* append the appropriate BSON type object */
switch (param_type(s->param)) {
case PARAM_TYPE_INT32:
param_get(s->param, &i);
if (bson_encoder_append_int(&encoder, param_name(s->param), i)) {
debug("BSON append failed for '%s'", param_name(s->param));
goto out;
}
break;
case PARAM_TYPE_FLOAT:
param_get(s->param, &f);
if (bson_encoder_append_double(&encoder, param_name(s->param), f)) {
debug("BSON append failed for '%s'", param_name(s->param));
goto out;
}
break;
case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
if (bson_encoder_append_binary(&encoder,
param_name(s->param),
BSON_BIN_BINARY,
param_size(s->param),
param_get_value_ptr(s->param))) {
debug("BSON append failed for '%s'", param_name(s->param));
goto out;
}
break;
default:
debug("unrecognized parameter type");
goto out;
}
}
result = 0;
out:
param_unlock();
if (result == 0)
result = bson_encoder_fini(&encoder);
return result;
}
int do_accel_calibration(int mavlink_fd)
{
int fd;
int32_t device_id[max_sens];
mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_and_console_log_info(mavlink_fd, "You need to put the system on all six sides");
sleep(3);
mavlink_and_console_log_info(mavlink_fd, "Follow the instructions on the screen");
sleep(5);
struct accel_scale accel_scale = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
int res = OK;
char str[30];
/* reset all sensors */
for (unsigned s = 0; s < max_sens; s++) {
sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
/* reset all offsets to zero and all scales to one */
fd = open(str, 0);
if (fd < 0) {
continue;
}
device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0);
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
close(fd);
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
}
}
float accel_offs[max_sens][3];
float accel_T[max_sens][3][3];
unsigned active_sensors;
if (res == OK) {
/* measure and calculate offsets & scales */
res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T, &active_sensors);
}
if (res != OK || active_sensors == 0) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
return ERROR;
}
/* measurements completed successfully, rotate calibration values */
param_t board_rotation_h = param_find("SENS_BOARD_ROT");
int32_t board_rotation_int;
param_get(board_rotation_h, &(board_rotation_int));
enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
math::Matrix<3, 3> board_rotation;
get_rot_matrix(board_rotation_id, &board_rotation);
math::Matrix<3, 3> board_rotation_t = board_rotation.transposed();
for (unsigned i = 0; i < active_sensors; i++) {
/* handle individual sensors, one by one */
math::Vector<3> accel_offs_vec(accel_offs[i]);
math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec;
math::Matrix<3, 3> accel_T_mat(accel_T[i]);
math::Matrix<3, 3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
accel_scale.x_offset = accel_offs_rotated(0);
accel_scale.x_scale = accel_T_rotated(0, 0);
accel_scale.y_offset = accel_offs_rotated(1);
accel_scale.y_scale = accel_T_rotated(1, 1);
accel_scale.z_offset = accel_offs_rotated(2);
accel_scale.z_scale = accel_T_rotated(2, 2);
bool failed = false;
/* set parameters */
(void)sprintf(str, "CAL_ACC%u_XOFF", i);
failed |= (OK != param_set(param_find(str), &(accel_scale.x_offset)));
(void)sprintf(str, "CAL_ACC%u_YOFF", i);
failed |= (OK != param_set(param_find(str), &(accel_scale.y_offset)));
(void)sprintf(str, "CAL_ACC%u_ZOFF", i);
failed |= (OK != param_set(param_find(str), &(accel_scale.z_offset)));
(void)sprintf(str, "CAL_ACC%u_XSCALE", i);
failed |= (OK != param_set(param_find(str), &(accel_scale.x_scale)));
(void)sprintf(str, "CAL_ACC%u_YSCALE", i);
failed |= (OK != param_set(param_find(str), &(accel_scale.y_scale)));
(void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
failed |= (OK != param_set(param_find(str), &(accel_scale.z_scale)));
(void)sprintf(str, "CAL_ACC%u_ID", i);
failed |= (OK != param_set(param_find(str), &(device_id[i])));
if (failed) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
return ERROR;
}
sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, i);
fd = open(str, 0);
if (fd < 0) {
mavlink_and_console_log_critical(mavlink_fd, "sensor does not exist");
res = ERROR;
} else {
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
close(fd);
}
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
}
}
if (res == OK) {
/* auto-save to EEPROM */
res = param_save_default();
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
}
mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
} else {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
}
return res;
}
int parameters_update(const struct mission_commander_flow_param_handles *h, struct mission_commander_flow_params *p)
{
param_get(h->mission_x_offset, &(p->mission_x_offset));
param_get(h->mission_y_offset, &(p->mission_y_offset));
param_get(h->mission_update_step_x, &(p->mission_update_step_x));
param_get(h->mission_update_step_y, &(p->mission_update_step_y));
param_get(h->mission_update_step_yaw, &(p->mission_update_step_yaw));
param_get(h->mission_yaw_thld, &(p->mission_yaw_thld));
param_get(h->mission_wp_radius, &(p->mission_wp_radius));
param_get(h->mission_min_front_dist, &(p->mission_min_front_dist));
param_get(h->mission_min_front_side_dist, &(p->mission_min_front_side_dist));
param_get(h->mission_min_side_dist, &(p->mission_min_side_dist));
param_get(h->mission_react_front_dist, &(p->mission_react_front_dist));
param_get(h->mission_react_front_side_dist, &(p->mission_react_front_side_dist));
param_get(h->mission_react_side_dist, &(p->mission_react_side_dist));
param_get(h->mission_use_sonar, &(p->mission_use_sonar));
param_get(h->reaction_min_react_angle, &(p->reaction_min_react_angle));
param_get(h->reaction_min_overreact_angle, &(p->reaction_min_overreact_angle));
param_get(h->reaction_min_pass_distance, &(p->reaction_min_pass_distance));
param_get(h->reaction_min_free_distance, &(p->reaction_min_free_distance));
param_get(h->debug, &(p->debug));
param_get(h->manual_threshold, &(p->manual_threshold));
param_get(h->rc_scale_pitch, &(p->rc_scale_pitch));
param_get(h->rc_scale_roll, &(p->rc_scale_roll));
param_get(h->rc_scale_yaw, &(p->rc_scale_yaw));
/* calc counters from other parameters */
p->counter_react_angle = (int)(p->reaction_min_react_angle / p->mission_update_step_yaw);
p->counter_overreact_angle = (int)(p->reaction_min_overreact_angle / p->mission_update_step_yaw);
p->counter_pass_distance = (int)(p->reaction_min_pass_distance / p->mission_update_step_x);
p->counter_free_distance = (int)(p->reaction_min_free_distance / p->mission_update_step_x);
/* fill radar control settings */
p->radarControlSettings[0] = p->mission_update_step_x; // max x-step
p->radarControlSettings[1] = p->mission_update_step_y; // TODO make a seperate y step
p->radarControlSettings[2] = p->mission_update_step_yaw; // max yaw-step
p->radarControlSettings[3] = (float) p->mission_react_side_dist; // react side distance
p->radarControlSettings[4] = (float) p->mission_react_front_side_dist; // react front side distance
p->radarControlSettings[5] = (float) p->mission_react_front_dist; // react front side distance
p->radarControlSettings[6] = (float) p->mission_min_side_dist; // min side distance
p->radarControlSettings[7] = (float) p->mission_min_front_side_dist; // min front side distance
p->radarControlSettings[8] = (float) p->mission_min_front_dist; // min front distance
p->radarControlSettings[9] = (float) p->mission_use_sonar; // boolean
return OK;
}
static void
do_show_print(void *arg, param_t param)
{
int32_t i;
float f;
const char *search_string = (const char*)arg;
const char *p_name = (const char*)param_name(param);
/* print nothing if search string is invalid and not matching */
if (!(arg == NULL)) {
/* start search */
char *ss = search_string;
char *pp = p_name;
bool mismatch = false;
/* XXX this comparison is only ok for trailing wildcards */
while (*ss != '\0' && *pp != '\0') {
if (*ss == *pp) {
ss++;
pp++;
} else if (*ss == '*') {
if (*(ss + 1) != '\0') {
warnx("* symbol only allowed at end of search string.");
exit(1);
}
pp++;
} else {
/* param not found */
return;
}
}
/* the search string must have been consumed */
if (!(*ss == '\0' || *ss == '*'))
return;
}
printf("%c %s: ",
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
param_name(param));
/*
* This case can be expanded to handle printing common structure types.
*/
switch (param_type(param)) {
case PARAM_TYPE_INT32:
if (!param_get(param, &i)) {
printf("%d\n", i);
return;
}
break;
case PARAM_TYPE_FLOAT:
if (!param_get(param, &f)) {
printf("%4.4f\n", (double)f);
return;
}
break;
case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
printf("<struct type %d size %u>\n", 0 + param_type(param), param_size(param));
return;
default:
printf("<unknown type %d>\n", 0 + param_type(param));
return;
}
printf("<error fetching parameter %d>\n", param);
}
static void
radio_init(void)
{
__pdata uint32_t freq_min, freq_max;
__pdata uint32_t channel_spacing;
__pdata uint8_t txpower;
// Do generic PHY initialisation
if (!radio_initialise()) {
panic("radio_initialise failed");
}
switch (g_board_frequency) {
case FREQ_433:
freq_min = 433050000UL;
freq_max = 434790000UL;
txpower = 10;
num_fh_channels = 10;
break;
case FREQ_470:
freq_min = 470000000UL;
freq_max = 471000000UL;
txpower = 10;
num_fh_channels = 10;
break;
case FREQ_868:
freq_min = 868000000UL;
freq_max = 869000000UL;
txpower = 10;
num_fh_channels = 10;
break;
case FREQ_915:
freq_min = 915000000UL;
freq_max = 928000000UL;
txpower = 20;
num_fh_channels = MAX_FREQ_CHANNELS;
break;
default:
freq_min = 0;
freq_max = 0;
txpower = 0;
panic("bad board frequency %d", g_board_frequency);
break;
}
if (param_get(PARAM_NUM_CHANNELS) != 0) {
num_fh_channels = param_get(PARAM_NUM_CHANNELS);
}
if (param_get(PARAM_MIN_FREQ) != 0) {
freq_min = param_get(PARAM_MIN_FREQ) * 1000UL;
}
if (param_get(PARAM_MAX_FREQ) != 0) {
freq_max = param_get(PARAM_MAX_FREQ) * 1000UL;
}
if (param_get(PARAM_TXPOWER) != 0) {
txpower = param_get(PARAM_TXPOWER);
}
// constrain power and channels
txpower = constrain(txpower, BOARD_MINTXPOWER, BOARD_MAXTXPOWER);
num_fh_channels = constrain(num_fh_channels, 1, MAX_FREQ_CHANNELS);
// double check ranges the board can do
switch (g_board_frequency) {
case FREQ_433:
freq_min = constrain(freq_min, 414000000UL, 460000000UL);
freq_max = constrain(freq_max, 414000000UL, 460000000UL);
break;
case FREQ_470:
freq_min = constrain(freq_min, 450000000UL, 490000000UL);
freq_max = constrain(freq_max, 450000000UL, 490000000UL);
break;
case FREQ_868:
freq_min = constrain(freq_min, 849000000UL, 889000000UL);
freq_max = constrain(freq_max, 849000000UL, 889000000UL);
break;
case FREQ_915:
freq_min = constrain(freq_min, 868000000UL, 935000000UL);
freq_max = constrain(freq_max, 868000000UL, 935000000UL);
break;
default:
panic("bad board frequency %d", g_board_frequency);
break;
}
if (freq_max == freq_min) {
freq_max = freq_min + 1000000UL;
}
// get the duty cycle we will use
duty_cycle = param_get(PARAM_DUTY_CYCLE);
duty_cycle = constrain(duty_cycle, 0, 100);
param_set(PARAM_DUTY_CYCLE, duty_cycle);
// get the LBT threshold we will use
lbt_rssi = param_get(PARAM_LBT_RSSI);
if (lbt_rssi != 0) {
// limit to the RSSI valid range
lbt_rssi = constrain(lbt_rssi, 25, 220);
}
param_set(PARAM_LBT_RSSI, lbt_rssi);
// sanity checks
param_set(PARAM_MIN_FREQ, freq_min/1000);
param_set(PARAM_MAX_FREQ, freq_max/1000);
param_set(PARAM_NUM_CHANNELS, num_fh_channels);
channel_spacing = (freq_max - freq_min) / (num_fh_channels+2);
// add half of the channel spacing, to ensure that we are well
// away from the edges of the allowed range
freq_min += channel_spacing/2;
// add another offset based on network ID. This means that
// with different network IDs we will have much lower
// interference
srand(param_get(PARAM_NETID));
if (num_fh_channels > 5) {
freq_min += ((unsigned long)(rand()*625)) % channel_spacing;
}
debug("freq low=%lu high=%lu spacing=%lu\n",
freq_min, freq_min+(num_fh_channels*channel_spacing),
channel_spacing);
// set the frequency and channel spacing
// change base freq based on netid
radio_set_frequency(freq_min);
// set channel spacing
radio_set_channel_spacing(channel_spacing);
// start on a channel chosen by network ID
radio_set_channel(param_get(PARAM_NETID) % num_fh_channels);
// And intilise the radio with them.
if (!radio_configure(param_get(PARAM_AIR_SPEED)) &&
!radio_configure(param_get(PARAM_AIR_SPEED)) &&
!radio_configure(param_get(PARAM_AIR_SPEED))) {
panic("radio_configure failed");
}
// report the real air data rate in parameters
param_set(PARAM_AIR_SPEED, radio_air_rate());
// setup network ID
radio_set_network_id(param_get(PARAM_NETID));
// setup transmit power
radio_set_transmit_power(txpower);
// report the real transmit power in settings
param_set(PARAM_TXPOWER, radio_get_transmit_power());
#ifdef USE_RTC
// initialise real time clock
rtc_init();
#endif
// initialise frequency hopping system
fhop_init(param_get(PARAM_NETID));
// initialise TDM system
tdm_init();
}
static void
do_compare(const char* name, const char* vals[], unsigned comparisons)
{
int32_t i;
float f;
param_t param = param_find(name);
/* set nothing if parameter cannot be found */
if (param == PARAM_INVALID) {
/* param not found */
errx(1, "Error: Parameter %s not found.", name);
}
/*
* Set parameter if type is known and conversion from string to value turns out fine
*/
int ret = 1;
switch (param_type(param)) {
case PARAM_TYPE_INT32:
if (!param_get(param, &i)) {
/* convert string */
char* end;
for (unsigned k = 0; k < comparisons; k++) {
int j = strtol(vals[k],&end,10);
if (i == j) {
printf(" %d: ", i);
ret = 0;
}
}
}
break;
case PARAM_TYPE_FLOAT:
if (!param_get(param, &f)) {
/* convert string */
char* end;
for (unsigned k = 0; k < comparisons; k++) {
float g = strtod(vals[k], &end);
if (fabsf(f - g) < 1e-7f) {
printf(" %4.4f: ", (double)f);
ret = 0;
}
}
}
break;
default:
errx(1, "<unknown / unsupported type %d>\n", 0 + param_type(param));
}
if (ret == 0) {
printf("%c %s: match\n",
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
param_name(param));
}
exit(ret);
}
void
Gimbal::update_params()
{
param_get(_params_handles.aux_mnt_chn, &_parameters.aux_mnt_chn);
param_get(_params_handles.use_mnt, &_parameters.use_mnt);
}
int
FixedwingAttitudeControl::parameters_update()
{
param_get(_parameter_handles.p_tc, &(_parameters.p_tc));
param_get(_parameter_handles.p_p, &(_parameters.p_p));
param_get(_parameter_handles.p_i, &(_parameters.p_i));
param_get(_parameter_handles.p_ff, &(_parameters.p_ff));
param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos));
param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg));
param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max));
param_get(_parameter_handles.r_tc, &(_parameters.r_tc));
param_get(_parameter_handles.r_p, &(_parameters.r_p));
param_get(_parameter_handles.r_i, &(_parameters.r_i));
param_get(_parameter_handles.r_ff, &(_parameters.r_ff));
param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max));
param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax));
param_get(_parameter_handles.y_p, &(_parameters.y_p));
param_get(_parameter_handles.y_i, &(_parameters.y_i));
param_get(_parameter_handles.y_ff, &(_parameters.y_ff));
param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max));
param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed));
param_get(_parameter_handles.y_coordinated_method, &(_parameters.y_coordinated_method));
param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax));
param_get(_parameter_handles.w_p, &(_parameters.w_p));
param_get(_parameter_handles.w_i, &(_parameters.w_i));
param_get(_parameter_handles.w_ff, &(_parameters.w_ff));
param_get(_parameter_handles.w_integrator_max, &(_parameters.w_integrator_max));
param_get(_parameter_handles.w_rmax, &(_parameters.w_rmax));
param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max));
param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll));
param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch));
param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw));
param_get(_parameter_handles.trim_steer, &(_parameters.trim_steer));
param_get(_parameter_handles.rollsp_offset_deg, &(_parameters.rollsp_offset_deg));
param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg));
_parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg);
_parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg);
param_get(_parameter_handles.man_roll_max, &(_parameters.man_roll_max));
param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max));
_parameters.man_roll_max = math::radians(_parameters.man_roll_max);
_parameters.man_pitch_max = math::radians(_parameters.man_pitch_max);
param_get(_parameter_handles.man_roll_scale, &(_parameters.man_roll_scale));
param_get(_parameter_handles.man_pitch_scale, &(_parameters.man_pitch_scale));
param_get(_parameter_handles.man_yaw_scale, &(_parameters.man_yaw_scale));
param_get(_parameter_handles.flaps_scale, &_parameters.flaps_scale);
param_get(_parameter_handles.flaperon_scale, &_parameters.flaperon_scale);
param_get(_parameter_handles.vtol_type, &_parameters.vtol_type);
/* pitch control parameters */
_pitch_ctrl.set_time_constant(_parameters.p_tc);
_pitch_ctrl.set_k_p(_parameters.p_p);
_pitch_ctrl.set_k_i(_parameters.p_i);
_pitch_ctrl.set_k_ff(_parameters.p_ff);
_pitch_ctrl.set_integrator_max(_parameters.p_integrator_max);
_pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos));
_pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg));
/* roll control parameters */
_roll_ctrl.set_time_constant(_parameters.r_tc);
_roll_ctrl.set_k_p(_parameters.r_p);
_roll_ctrl.set_k_i(_parameters.r_i);
_roll_ctrl.set_k_ff(_parameters.r_ff);
_roll_ctrl.set_integrator_max(_parameters.r_integrator_max);
_roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax));
/* yaw control parameters */
_yaw_ctrl.set_k_p(_parameters.y_p);
_yaw_ctrl.set_k_i(_parameters.y_i);
_yaw_ctrl.set_k_ff(_parameters.y_ff);
_yaw_ctrl.set_integrator_max(_parameters.y_integrator_max);
_yaw_ctrl.set_coordinated_min_speed(_parameters.y_coordinated_min_speed);
_yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method);
_yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax));
/* wheel control parameters */
_wheel_ctrl.set_k_p(_parameters.w_p);
_wheel_ctrl.set_k_i(_parameters.w_i);
_wheel_ctrl.set_k_ff(_parameters.w_ff);
_wheel_ctrl.set_integrator_max(_parameters.w_integrator_max);
_wheel_ctrl.set_max_rate(math::radians(_parameters.w_rmax));
return PX4_OK;
}