fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixups after merge from master

Browse Source 
MuORB was missing the orb_exists() function added to uORB.cpp

gyro_calibration.cpp still had some merge conflicts that had not been resolved.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois 2015-04-28 12:28:10 -07:00
parent 190814bc97
commit 2446dfec16
2 changed files with 30 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -85,7 +85,7 @@ static calibrate_return gyro_calibration_worker(detect_orientation_return orient
struct gyro_report gyro_report;
unsigned poll_errcount = 0;
struct pollfd fds[max_gyros];
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;
@ -182,25 +182,16 @@ int do_gyro_calibration(int mavlink_fd)
// Reset all offsets to 0 and scales to 1
(void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale));
sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
int fd = open(str, 0);
int fd = px4_open(str, 0);
if (fd >= 0) {
worker_data.device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0);
res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero);
close(fd);
px4_close(fd);
<<<<<<< HEAD
for (unsigned s = 0; s < max_gyros; s++) {
px4_close(sub_sensor_gyro[s]);
gyro_scale[s].x_offset /= calibration_counter[s];
gyro_scale[s].y_offset /= calibration_counter[s];
gyro_scale[s].z_offset /= calibration_counter[s];
=======
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s);
return ERROR;
}
>>>>>>> upstream/master
}
}
@ -223,7 +214,7 @@ int do_gyro_calibration(int mavlink_fd)
calibrate_cancel_unsubscribe(cancel_sub);
for (unsigned s = 0; s < max_gyros; s++) {
close(worker_data.gyro_sensor_sub[s]);
px4_close(worker_data.gyro_sensor_sub[s]);
}
if (cal_return == calibrate_return_cancelled) {
@ -242,15 +233,9 @@ int do_gyro_calibration(int mavlink_fd)
/* maximum allowable calibration error in radians */
const float maxoff = 0.0055f;
<<<<<<< HEAD
if (!PX4_ISFINITE(gyro_scale[0].x_offset) ||
!PX4_ISFINITE(gyro_scale[0].y_offset) ||
!PX4_ISFINITE(gyro_scale[0].z_offset) ||
=======
if (!isfinite(worker_data.gyro_scale[0].x_offset) ||
!isfinite(worker_data.gyro_scale[0].y_offset) ||
!isfinite(worker_data.gyro_scale[0].z_offset) ||
>>>>>>> upstream/master
if (!PX4_ISFINITE(worker_data.gyro_scale[0].x_offset) ||
!PX4_ISFINITE(worker_data.gyro_scale[0].y_offset) ||
!PX4_ISFINITE(worker_data.gyro_scale[0].z_offset) ||
fabsf(xdiff) > maxoff ||
fabsf(ydiff) > maxoff ||
fabsf(zdiff) > maxoff) {
@ -278,43 +263,15 @@ int do_gyro_calibration(int mavlink_fd)
/* apply new scaling and offsets */
(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
int fd = open(str, 0);
int fd = px4_open(str, 0);
if (fd < 0) {
failed = true;
continue;
}
<<<<<<< HEAD
/* if any reasonable amount of data is missing, skip */
if (calibration_counter[s] < calibration_count / 2) {
continue;
}
(void)sprintf(str, "CAL_GYRO%u_XOFF", s);
failed |= (OK != param_set_no_notification(param_find(str), &(gyro_scale[s].x_offset)));
(void)sprintf(str, "CAL_GYRO%u_YOFF", s);
failed |= (OK != param_set_no_notification(param_find(str), &(gyro_scale[s].y_offset)));
(void)sprintf(str, "CAL_GYRO%u_ZOFF", s);
failed |= (OK != param_set_no_notification(param_find(str), &(gyro_scale[s].z_offset)));
(void)sprintf(str, "CAL_GYRO%u_ID", s);
failed |= (OK != param_set_no_notification(param_find(str), &(device_id[s])));
/* apply new scaling and offsets */
(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
int fd = px4_open(str, 0);
if (fd < 0) {
failed = true;
continue;
}
res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale[s]);
px4_close(fd);
=======
res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&worker_data.gyro_scale[s]);
close(fd);
>>>>>>> upstream/master
res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&worker_data.gyro_scale[s]);
px4_close(fd);
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG);

20
src/modules/uORB/MuORB.cpp
View File

@ -42,6 +42,7 @@
#include <drivers/device/device.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
@ -1235,6 +1236,25 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve
} // namespace
int
orb_exists(const struct orb_metadata *meta, int instance)
{
/*
* Generate the path to the node and try to open it.
*/
char path[orb_maxpath];
int inst = instance;
int ret = node_mkpath(path, PUBSUB, meta, &inst);
if (ret != OK) {
errno = -ret;
return ERROR;
}
struct stat buffer;
return stat(path, &buffer);
}
orb_advert_t
orb_advertise(const struct orb_metadata *meta, const void *data)
{