Browse Source

Removed [cal] references from calibration_log_critical() routines.

sbg
Robbie Sharma 7 years ago committed by Beat Küng
parent
commit
ab51a41793
  1. 8
      src/modules/commander/accelerometer_calibration.cpp
  2. 4
      src/modules/commander/airspeed_calibration.cpp
  3. 2
      src/modules/commander/calibration_routines.cpp
  4. 16
      src/modules/commander/gyro_calibration.cpp
  5. 12
      src/modules/commander/mag_calibration.cpp

8
src/modules/commander/accelerometer_calibration.cpp

@ -469,7 +469,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub @@ -469,7 +469,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
// Warn that we will not calibrate more than max_accels accelerometers
if (orb_accel_count > max_accel_sens) {
calibration_log_critical(mavlink_log_pub, "[cal] Detected %u accels, but will calibrate only %u", orb_accel_count, max_accel_sens);
calibration_log_critical(mavlink_log_pub, "Detected %u accels, but will calibrate only %u", orb_accel_count, max_accel_sens);
}
for (unsigned cur_accel = 0; cur_accel < orb_accel_count && cur_accel < max_accel_sens; cur_accel++) {
@ -507,7 +507,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub @@ -507,7 +507,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
}
if(!found_cur_accel) {
calibration_log_critical(mavlink_log_pub, "[cal] Accel #%u (ID %u) no matching uORB devid", cur_accel, device_id[cur_accel]);
calibration_log_critical(mavlink_log_pub, "Accel #%u (ID %u) no matching uORB devid", cur_accel, device_id[cur_accel]);
result = calibrate_return_error;
break;
}
@ -522,7 +522,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub @@ -522,7 +522,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
device_id_primary = device_id[cur_accel];
}
} else {
calibration_log_critical(mavlink_log_pub, "[cal] Accel #%u no device id, abort", cur_accel);
calibration_log_critical(mavlink_log_pub, "Accel #%u no device id, abort", cur_accel);
result = calibrate_return_error;
break;
}
@ -554,7 +554,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub @@ -554,7 +554,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
if (result != calibrate_return_ok) {
calibration_log_critical(mavlink_log_pub, "[cal] ERROR: calibration calculation error");
calibration_log_critical(mavlink_log_pub, "ERROR: calibration calculation error");
break;
}
}

4
src/modules/commander/airspeed_calibration.cpp

@ -109,9 +109,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) @@ -109,9 +109,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
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, refer to the following:");
calibration_log_critical(mavlink_log_pub, "http://px4.io/docs/sensor-selection/");
calibration_log_critical(mavlink_log_pub, "http://px4.io/docs/vtols-without-airspeed-sensor/");
calibration_log_critical(mavlink_log_pub, "[cal] No airspeed sensor found");
goto error_return;
}

2
src/modules/commander/calibration_routines.cpp

@ -664,7 +664,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, @@ -664,7 +664,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,
return DETECT_ORIENTATION_RIGHTSIDE_UP; // [ 0, 0, -g ]
}
calibration_log_critical(mavlink_log_pub, "[cal] ERROR: invalid orientation");
calibration_log_critical(mavlink_log_pub, "ERROR: invalid orientation");
return DETECT_ORIENTATION_ERROR; // Can't detect orientation
}

16
src/modules/commander/gyro_calibration.cpp

@ -193,7 +193,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data) @@ -193,7 +193,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
for (unsigned s = 0; s < max_gyros; s++) {
if (worker_data->device_id[s] != 0 && calibration_counter[s] < calibration_count / 2) {
calibration_log_critical(worker_data->mavlink_log_pub, "[cal] ERROR: missing data, sensor %d", s)
calibration_log_critical(worker_data->mavlink_log_pub, "ERROR: missing data, sensor %d", s)
return calibrate_return_error;
}
@ -237,7 +237,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) @@ -237,7 +237,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
(void)sprintf(str, "CAL_GYRO%u_ID", s);
res = param_set_no_notification(param_find(str), &(worker_data.device_id[s]));
if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, "[cal] Unable to reset CAL_GYRO%u_ID", s);
calibration_log_critical(mavlink_log_pub, "Unable to reset CAL_GYRO%u_ID", s);
return PX4_ERROR;
}
@ -297,7 +297,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) @@ -297,7 +297,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
// Warn that we will not calibrate more than max_gyros gyroscopes
if (orb_gyro_count > max_gyros) {
calibration_log_critical(mavlink_log_pub, "[cal] Detected %u gyros, but will calibrate only %u", orb_gyro_count, max_gyros);
calibration_log_critical(mavlink_log_pub, "Detected %u gyros, but will calibrate only %u", orb_gyro_count, max_gyros);
}
for (unsigned cur_gyro = 0; cur_gyro < orb_gyro_count && cur_gyro < max_gyros; cur_gyro++) {
@ -333,7 +333,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) @@ -333,7 +333,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
}
if(!found_cur_gyro) {
calibration_log_critical(mavlink_log_pub, "[cal] Gyro #%u (ID %u) no matching uORB devid", cur_gyro, worker_data.device_id[cur_gyro]);
calibration_log_critical(mavlink_log_pub, "Gyro #%u (ID %u) no matching uORB devid", cur_gyro, worker_data.device_id[cur_gyro]);
res = calibrate_return_error;
break;
}
@ -348,7 +348,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) @@ -348,7 +348,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
device_id_primary = worker_data.device_id[cur_gyro];
}
} else {
calibration_log_critical(mavlink_log_pub, "[cal] Gyro #%u no device id, abort", cur_gyro);
calibration_log_critical(mavlink_log_pub, "Gyro #%u no device id, abort", cur_gyro);
}
}
@ -386,7 +386,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) @@ -386,7 +386,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
fabsf(ydiff) > maxoff ||
fabsf(zdiff) > maxoff) {
calibration_log_critical(mavlink_log_pub, "[cal] motion, retrying..");
calibration_log_critical(mavlink_log_pub, "motion, retrying..");
res = PX4_ERROR;
} else {
@ -398,7 +398,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) @@ -398,7 +398,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
} while (res == PX4_ERROR && try_count <= max_tries);
if (try_count >= max_tries) {
calibration_log_critical(mavlink_log_pub, "[cal] ERROR: Motion during calibration");
calibration_log_critical(mavlink_log_pub, "ERROR: Motion during calibration");
res = PX4_ERROR;
}
@ -494,7 +494,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) @@ -494,7 +494,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
}
if (failed) {
calibration_log_critical(mavlink_log_pub, "[cal] ERROR: failed to set offset params");
calibration_log_critical(mavlink_log_pub, "ERROR: failed to set offset params");
res = PX4_ERROR;
}
}

12
src/modules/commander/mag_calibration.cpp

@ -575,7 +575,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) @@ -575,7 +575,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
worker_data.z[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
if (worker_data.x[cur_mag] == nullptr || worker_data.y[cur_mag] == nullptr || worker_data.z[cur_mag] == nullptr) {
calibration_log_critical(mavlink_log_pub, "[cal] ERROR: out of memory");
calibration_log_critical(mavlink_log_pub, "ERROR: out of memory");
result = calibrate_return_error;
}
}
@ -589,7 +589,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) @@ -589,7 +589,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
// Warn that we will not calibrate more than max_mags magnetometers
if (orb_mag_count > max_mags) {
calibration_log_critical(mavlink_log_pub, "[cal] Detected %u mags, but will calibrate only %u", orb_mag_count, max_mags);
calibration_log_critical(mavlink_log_pub, "Detected %u mags, but will calibrate only %u", orb_mag_count, max_mags);
}
for (unsigned cur_mag = 0; cur_mag < orb_mag_count && cur_mag < max_mags; cur_mag++) {
@ -625,7 +625,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) @@ -625,7 +625,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
}
if(!found_cur_mag) {
calibration_log_critical(mavlink_log_pub, "[cal] Mag #%u (ID %u) no matching uORB devid", cur_mag, device_ids[cur_mag]);
calibration_log_critical(mavlink_log_pub, "Mag #%u (ID %u) no matching uORB devid", cur_mag, device_ids[cur_mag]);
result = calibrate_return_error;
break;
}
@ -641,7 +641,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) @@ -641,7 +641,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
}
} else {
calibration_log_critical(mavlink_log_pub, "[cal] Mag #%u no device id, abort", cur_mag);
calibration_log_critical(mavlink_log_pub, "Mag #%u no device id, abort", cur_mag);
result = calibrate_return_error;
break;
}
@ -804,13 +804,13 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) @@ -804,13 +804,13 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
fd_mag = px4_open(str, 0);
if (fd_mag < 0) {
calibration_log_critical(mavlink_log_pub, "[cal] ERROR: unable to open mag device #%u", cur_mag);
calibration_log_critical(mavlink_log_pub, "ERROR: unable to open mag device #%u", cur_mag);
result = calibrate_return_error;
}
if (result == calibrate_return_ok) {
if (px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != PX4_OK) {
calibration_log_critical(mavlink_log_pub, "[cal] ERROR: failed to get current calibration #%u", cur_mag);
calibration_log_critical(mavlink_log_pub, "ERROR: failed to get current calibration #%u", cur_mag);
result = calibrate_return_error;
}
}

Loading…
Cancel
Save