dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander: whitespace only 

Trailing whitespace all over.
This commit is contained in:
Julian Oes 2016-03-07 22:50:16 +01:00
parent f24b2a701f
commit 98e407696e
3 changed files with 76 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/modules/commander/accelerometer_calibration.cpp
View File

@ -263,7 +263,7 @@ int do_accel_calibration(int mavlink_fd)
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 || (OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary)));
@ -285,7 +285,7 @@ int do_accel_calibration(int mavlink_fd)
(void)sprintf(str, "CAL_ACC%u_ID", i);
failed |= (OK != param_set_no_notification(param_find(str), &(device_id[i])));
#endif
if (failed) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, i);
return ERROR;
@ -325,7 +325,7 @@ int do_accel_calibration(int mavlink_fd)
} else {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
}
/* give this message enough time to propagate */
usleep(600000);
@ -336,30 +336,30 @@ static calibrate_return accel_calibration_worker(detect_orientation_return orien
{
const unsigned samples_num = 750;
accel_worker_data_t* worker_data = (accel_worker_data_t*)(data);
mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Hold still, measuring %s side", detect_orientation_str(orientation));
read_accelerometer_avg(worker_data->subs, worker_data->accel_ref, orientation, samples_num);
mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side result: [%8.4f %8.4f %8.4f]", detect_orientation_str(orientation),
(double)worker_data->accel_ref[0][orientation][0],
(double)worker_data->accel_ref[0][orientation][1],
(double)worker_data->accel_ref[0][orientation][2]);
worker_data->done_count++;
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, 17 * worker_data->done_count);
return calibrate_return_ok;
}
calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors)
{
calibrate_return result = calibrate_return_ok;
*active_sensors = 0;
accel_worker_data_t worker_data;
worker_data.mavlink_fd = mavlink_fd;
worker_data.done_count = 0;
@ -378,7 +378,7 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel
result = calibrate_return_error;
break;
}
/* store initial timestamp - used to infer which sensors are active */
struct accel_report arp = {};
(void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);

44
src/modules/commander/gyro_calibration.cpp
View File

@ -84,58 +84,58 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
const unsigned calibration_count = 5000;
struct gyro_report gyro_report;
unsigned poll_errcount = 0;
px4_pollfd_struct_t fds[max_gyros];
for (unsigned s = 0; s < max_gyros; s++) {
fds[s].fd = worker_data->gyro_sensor_sub[s];
fds[s].events = POLLIN;
}
memset(&worker_data->gyro_report_0, 0, sizeof(worker_data->gyro_report_0));
memset(&worker_data->gyro_scale, 0, sizeof(worker_data->gyro_scale));
/* use first gyro to pace, but count correctly per-gyro for statistics */
while (calibration_counter[0] < calibration_count) {
if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) {
return calibrate_return_cancelled;
}
int poll_ret = px4_poll(&fds[0], max_gyros, 1000);
if (poll_ret > 0) {
for (unsigned s = 0; s < max_gyros; s++) {
bool changed;
orb_check(worker_data->gyro_sensor_sub[s], &changed);
if (changed) {
orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &gyro_report);
if (s == 0) {
orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &worker_data->gyro_report_0);
}
worker_data->gyro_scale[s].x_offset += gyro_report.x;
worker_data->gyro_scale[s].y_offset += gyro_report.y;
worker_data->gyro_scale[s].z_offset += gyro_report.z;
calibration_counter[s]++;
}
if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) {
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter[0] * 100) / calibration_count);
}
}
} else {
poll_errcount++;
}
if (poll_errcount > 1000) {
mavlink_and_console_log_critical(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG);
return calibrate_return_error;
}
}
for (unsigned s = 0; s < max_gyros; s++) {
if (worker_data->device_id[s] != 0 && calibration_counter[s] < calibration_count / 2) {
mavlink_and_console_log_critical(worker_data->mavlink_fd, "[cal] ERROR: missing data, sensor %d", s)
@ -158,7 +158,7 @@ int do_gyro_calibration(int mavlink_fd)
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
worker_data.mavlink_fd = mavlink_fd;
struct gyro_scale gyro_scale_zero = {
0.0f, // x offset
1.0f, // x scale
@ -167,7 +167,7 @@ int do_gyro_calibration(int mavlink_fd)
0.0f, // z offset
1.0f, // z scale
};
int device_prio_max = 0;
int32_t device_id_primary = 0;
@ -190,7 +190,7 @@ int do_gyro_calibration(int mavlink_fd)
return ERROR;
}
#endif
// Reset all offsets to 0 and scales to 1
(void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale));
#ifndef __PX4_QURT
@ -207,9 +207,9 @@ int do_gyro_calibration(int mavlink_fd)
}
}
#endif
}
for (unsigned s = 0; s < max_gyros; s++) {
worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
@ -228,7 +228,7 @@ int do_gyro_calibration(int mavlink_fd)
unsigned try_count = 0;
unsigned max_tries = 20;
res = ERROR;
do {
// Calibrate gyro and ensure user didn't move
calibrate_return cal_return = gyro_calibration_worker(cancel_sub, &worker_data);
@ -274,7 +274,7 @@ int do_gyro_calibration(int mavlink_fd)
}
calibrate_cancel_unsubscribe(cancel_sub);
for (unsigned s = 0; s < max_gyros; s++) {
px4_close(worker_data.gyro_sensor_sub[s]);
}
@ -289,7 +289,7 @@ int do_gyro_calibration(int mavlink_fd)
for (unsigned s = 0; s < max_gyros; s++) {
if (worker_data.device_id[s] != 0) {
char str[30];
(void)sprintf(str, "CAL_GYRO%u_XOFF", s);
failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].x_offset)));
(void)sprintf(str, "CAL_GYRO%u_YOFF", s);
@ -353,6 +353,6 @@ int do_gyro_calibration(int mavlink_fd)
/* give this message enough time to propagate */
usleep(600000);
return res;
}

84
src/modules/commander/mag_calibration.cpp
View File

@ -114,7 +114,7 @@ int do_mag_calibration(int mavlink_fd)
#endif
int result = OK;
// Determine which mags are available and reset each
char str[30];
@ -122,7 +122,7 @@ int do_mag_calibration(int mavlink_fd)
for (size_t i=0; i<max_mags; i++) {
device_ids[i] = 0; // signals no mag
}
for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {
#ifndef __PX4_QURT
// Reset mag id to mag not available
@ -183,7 +183,7 @@ int do_mag_calibration(int mavlink_fd)
// Cancel message already displayed, we're done here
result = ERROR;
break;
case calibrate_return_ok:
/* auto-save to EEPROM */
result = param_save_default();
@ -199,7 +199,7 @@ int do_mag_calibration(int mavlink_fd)
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
}
// Fall through
default:
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
break;
@ -208,7 +208,7 @@ int do_mag_calibration(int mavlink_fd)
/* give this message enough time to propagate */
usleep(600000);
return result;
}
@ -237,11 +237,11 @@ static unsigned progress_percentage(mag_worker_data_t* worker_data) {
static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
{
calibrate_return result = calibrate_return_ok;
unsigned int calibration_counter_side;
mag_worker_data_t* worker_data = (mag_worker_data_t*)(data);
mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Rotate vehicle around the detected orientation");
mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds);
@ -309,20 +309,20 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
}
px4_close(sub_gyro);
uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds;
unsigned poll_errcount = 0;
calibration_counter_side = 0;
while (hrt_absolute_time() < calibration_deadline &&
calibration_counter_side < worker_data->calibration_points_perside) {
if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) {
result = calibrate_return_cancelled;
break;
}
// Wait clocking for new data on all mags
px4_pollfd_struct_t fds[max_mags];
size_t fd_count = 0;
@ -334,7 +334,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
}
}
int poll_ret = px4_poll(fds, fd_count, 1000);
if (poll_ret > 0) {
int prev_count[max_mags];
@ -354,7 +354,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
worker_data->x[cur_mag], worker_data->y[cur_mag], worker_data->z[cur_mag],
worker_data->calibration_counter_total[cur_mag],
calibration_sides * worker_data->calibration_points_perside);
worker_data->x[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.x;
worker_data->y[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.y;
worker_data->z[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.z;
@ -380,21 +380,21 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
} else {
poll_errcount++;
}
if (poll_errcount > worker_data->calibration_points_perside * 3) {
result = calibrate_return_error;
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG);
break;
}
}
if (result == calibrate_return_ok) {
mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side done, rotate to a different side", detect_orientation_str(orientation));
worker_data->done_count++;
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, progress_percentage(worker_data));
}
return result;
}
@ -403,7 +403,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
calibrate_return result = calibrate_return_ok;
mag_worker_data_t worker_data;
worker_data.mavlink_fd = mavlink_fd;
worker_data.done_count = 0;
worker_data.calibration_points_perside = calibration_total_points / calibration_sides;
@ -417,11 +417,11 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = false;
worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = false;
worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = false;
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
// Initialize to no subscription
worker_data.sub_mag[cur_mag] = -1;
// Initialize to no memory allocated
worker_data.x[cur_mag] = NULL;
worker_data.y[cur_mag] = NULL;
@ -430,9 +430,9 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
}
const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside;
char str[30];
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
worker_data.x[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
worker_data.y[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
@ -443,7 +443,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
}
}
// Setup subscriptions to mag sensors
if (result == calibrate_return_ok) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
@ -467,21 +467,21 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
}
}
}
// Limit update rate to get equally spaced measurements over time (in ms)
if (result == calibrate_return_ok) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
// Mag in this slot is available
unsigned int orb_interval_msecs = (worker_data.calibration_interval_perside_useconds / 1000) / worker_data.calibration_points_perside;
//mavlink_and_console_log_info(mavlink_fd, "Orb interval %u msecs", orb_interval_msecs);
orb_set_interval(worker_data.sub_mag[cur_mag], orb_interval_msecs);
}
}
}
if (result == calibrate_return_ok) {
int cancel_sub = calibrate_cancel_subscribe();
@ -493,34 +493,34 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
true); // true: lenient still detection
calibrate_cancel_unsubscribe(cancel_sub);
}
// Close subscriptions
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (worker_data.sub_mag[cur_mag] >= 0) {
px4_close(worker_data.sub_mag[cur_mag]);
}
}
// Calculate calibration values for each mag
float sphere_x[max_mags];
float sphere_y[max_mags];
float sphere_z[max_mags];
float sphere_radius[max_mags];
// Sphere fit the data to get calibration values
if (result == calibrate_return_ok) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
// Mag in this slot is available and we should have values for it to calibrate
sphere_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag],
worker_data.calibration_counter_total[cur_mag],
100, 0.0f,
&sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag],
&sphere_radius[cur_mag]);
if (!PX4_ISFINITE(sphere_x[cur_mag]) || !PX4_ISFINITE(sphere_y[cur_mag]) || !PX4_ISFINITE(sphere_z[cur_mag])) {
mavlink_and_console_log_emergency(mavlink_fd, "ERROR: Retry calibration (sphere NaN, #%u)", cur_mag);
result = calibrate_return_error;
@ -580,14 +580,14 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
printf(">>>>>>>\n");
}
}
// Data points are no longer needed
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
free(worker_data.x[cur_mag]);
free(worker_data.y[cur_mag]);
free(worker_data.z[cur_mag]);
}
if (result == calibrate_return_ok) {
(void)param_set_no_notification(param_find("CAL_MAG_PRIME"), &(device_id_primary));
@ -596,16 +596,16 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
if (device_ids[cur_mag] != 0) {
int fd_mag = -1;
struct mag_scale mscale;
// Set new scale
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
fd_mag = px4_open(str, 0);
if (fd_mag < 0) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: unable to open mag device #%u", cur_mag);
result = calibrate_return_error;
}
if (result == calibrate_return_ok) {
if (px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != OK) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: failed to get current calibration #%u", cur_mag);
@ -623,7 +623,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
result = calibrate_return_error;
}
}
// Mag device no longer needed
if (fd_mag >= 0) {
px4_close(fd_mag);
@ -631,7 +631,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
if (result == calibrate_return_ok) {
bool failed = false;
/* set parameters */
#ifndef __PX4_QURT