fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 16:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Code cleanup and ifdefs required for qurt build

Browse Source 
Code that was previously out of tree that was #if 0, is now #ifdef __PX4_QURT.
These changes were required for flight using the qurt build.

Changes include code cleanup for shmem_posix.c.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois
2016-01-21 20:41:42 -08:00
committed by Julian Oes
parent 8634452d6d
commit fea910d45a
5 changed files with 87 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/accelerometer_calibration.cpp
+8
View File
@@ -175,7 +175,9 @@ typedef struct {
int do_accel_calibration(int mavlink_fd)
{
#ifndef __PX4_QURT
int fd;
#endif
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
@@ -192,6 +194,7 @@ int do_accel_calibration(int mavlink_fd)
char str[30];
#ifndef __PX4_QURT
/* reset all sensors */
for (unsigned s = 0; s < max_accel_sens; s++) {
sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
@@ -211,6 +214,7 @@ int do_accel_calibration(int mavlink_fd)
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s);
}
}
#endif
float accel_offs[max_accel_sens][3];
float accel_T[max_accel_sens][3][3];
@@ -277,14 +281,17 @@ int do_accel_calibration(int mavlink_fd)
failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.y_scale)));
(void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.z_scale)));
#ifndef __PX4_QURT
(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;
}
#ifndef __PX4_QURT
sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, i);
fd = px4_open(str, 0);
@@ -299,6 +306,7 @@ int do_accel_calibration(int mavlink_fd)
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, i);
}
#endif
}
if (res == OK) {
src/modules/commander/gyro_calibration.cpp
+14 -1
View File
@@ -173,7 +173,7 @@ int do_gyro_calibration(int mavlink_fd)
for (unsigned s = 0; s < max_gyros; s++) {
char str[30];
#ifndef __PX4_QURT
// Reset gyro ids to unavailable
worker_data.device_id[s] = 0;
(void)sprintf(str, "CAL_GYRO%u_ID", s);
@@ -182,9 +182,18 @@ int do_gyro_calibration(int mavlink_fd)
mavlink_and_console_log_critical(mavlink_fd, "[cal] Unable to reset CAL_GYRO%u_ID", s);
return ERROR;
}
#else
(void)sprintf(str, "CAL_GYRO%u_ID", s);
res = param_get(param_find(str), &(worker_data.device_id[s]));
if (res != OK) {
mavlink_log_critical(mavlink_fd, "[cal] Unable to get CAL_GYRO%u_ID", s);
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
sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
int fd = px4_open(str, 0);
if (fd >= 0) {
@@ -197,6 +206,7 @@ int do_gyro_calibration(int mavlink_fd)
return ERROR;
}
}
#endif
}
@@ -286,6 +296,8 @@ int do_gyro_calibration(int mavlink_fd)
failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].y_offset)));
(void)sprintf(str, "CAL_GYRO%u_ZOFF", s);
failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].z_offset)));
#ifndef __PX4_QURT
(void)sprintf(str, "CAL_GYRO%u_ID", s);
failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.device_id[s])));
@@ -304,6 +316,7 @@ int do_gyro_calibration(int mavlink_fd)
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, 1);
}
#endif
}
}
src/modules/commander/mag_calibration.cpp
+17
View File
@@ -101,6 +101,7 @@ int do_mag_calibration(int mavlink_fd)
{
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
#ifndef __PX4_QURT
struct mag_scale mscale_null = {
0.0f,
1.0f,
@@ -109,6 +110,7 @@ int do_mag_calibration(int mavlink_fd)
0.0f,
1.0f,
};
#endif
int result = OK;
@@ -121,6 +123,7 @@ int do_mag_calibration(int mavlink_fd)
}
for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {
#ifndef __PX4_QURT
// Reset mag id to mag not available
(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
result = param_set_no_notification(param_find(str), &(device_ids[cur_mag]));
@@ -128,7 +131,17 @@ int do_mag_calibration(int mavlink_fd)
mavlink_and_console_log_info(mavlink_fd, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag);
break;
}
#else
(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
result = param_get(param_find(str), &(device_ids[cur_mag]));
if (result != OK) {
mavlink_and_console_log_info(mavlink_fd, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag);
break;
}
#endif
/* for calibration, commander will run on apps, so orb messages are used to get info from dsp */
#ifndef __PX4_QURT
// Attempt to open mag
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
int fd = px4_open(str, O_RDONLY);
@@ -158,6 +171,7 @@ int do_mag_calibration(int mavlink_fd)
}
px4_close(fd);
#endif
}
// Calibrate all mags at the same time
@@ -615,8 +629,11 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
bool failed = false;
/* set parameters */
#ifndef __PX4_QURT
(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag])));
#endif
(void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_offset)));
(void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag);