aboutsummaryrefslogtreecommitdiff
path: root/src/modules/commander
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/commander')
-rw-r--r--src/modules/commander/airspeed_calibration.cpp14
-rw-r--r--src/modules/commander/commander.cpp17
-rw-r--r--src/modules/commander/px4_custom_mode.h2
-rw-r--r--src/modules/commander/state_machine_helper.cpp53
4 files changed, 65 insertions, 21 deletions
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp
index 6039d92a7..c8c7a42e7 100644
--- a/src/modules/commander/airspeed_calibration.cpp
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -64,9 +64,9 @@ int do_airspeed_calibration(int mavlink_fd)
{
/* give directions */
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
- mavlink_log_info(mavlink_fd, "don't move system");
+ mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind");
- const int calibration_count = 2500;
+ const int calibration_count = 2000;
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s diff_pres;
@@ -85,13 +85,15 @@ int do_airspeed_calibration(int mavlink_fd)
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) {
- warn("WARNING: failed to set scale / offsets for airspeed sensor");
- mavlink_log_critical(mavlink_fd, "could not reset dpress sensor");
+ warn("FAILED to set scale / offsets for airspeed");
+ mavlink_log_critical(mavlink_fd, "dpress reset failed");
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
return ERROR;
}
@@ -107,7 +109,7 @@ int do_airspeed_calibration(int mavlink_fd)
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
- diff_pres_offset += diff_pres.differential_pressure_pa;
+ diff_pres_offset += diff_pres.differential_pressure_raw_pa;
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0)
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 57faafcfd..e39cc5fe7 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -118,7 +118,7 @@ extern struct system_load_s system_load;
#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */
-#define RC_TIMEOUT 100000
+#define RC_TIMEOUT 500000
#define DIFFPRESS_TIMEOUT 2000000
#define PRINT_INTERVAL 5000000
@@ -384,6 +384,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED;
bool ret = false;
+ /* only handle commands that are meant to be handled by this system and component */
+ if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components
+ return false;
+ }
+
/* only handle high-priority commands here */
/* request to set different system mode */
@@ -539,11 +544,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
break;
default:
- /* warn about unsupported commands */
+ /* Warn about unsupported commands, this makes sense because only commands
+ * to this component ID (or all) are passed by mavlink. */
answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
break;
}
-
if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
/* already warned about unsupported commands in "default" case */
answer_command(*cmd, result);
@@ -1039,9 +1044,9 @@ int commander_thread_main(int argc, char *argv[])
&& global_position.global_valid) {
/* copy position data to uORB home message, store it locally as well */
- home.lat = global_position.lat;
- home.lon = global_position.lon;
- home.alt = global_position.alt;
+ home.lat = gps_position.lat / (double)1e7;
+ home.lon = gps_position.lon / (double)1e7;
+ home.alt = gps_position.alt;
warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt);
mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt);
diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h
index b60a7e0b9..2144d3460 100644
--- a/src/modules/commander/px4_custom_mode.h
+++ b/src/modules/commander/px4_custom_mode.h
@@ -8,6 +8,8 @@
#ifndef PX4_CUSTOM_MODE_H_
#define PX4_CUSTOM_MODE_H_
+#include <stdint.h>
+
enum PX4_CUSTOM_MAIN_MODE {
PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
PX4_CUSTOM_MAIN_MODE_SEATBELT,
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 359cc62cd..f09d586c7 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -44,6 +44,7 @@
#include <stdbool.h>
#include <dirent.h>
#include <fcntl.h>
+#include <string.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
@@ -290,10 +291,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
bool valid_transition = false;
int ret = ERROR;
- warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state);
-
if (current_status->hil_state == new_state) {
- warnx("Hil state not changed");
valid_transition = true;
} else {
@@ -321,23 +319,60 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
/* list directory */
DIR *d;
- struct dirent *direntry;
d = opendir("/dev");
if (d) {
+ struct dirent *direntry;
+ char devname[24];
+
while ((direntry = readdir(d)) != NULL) {
- int sensfd = ::open(direntry->d_name, 0);
- int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0);
+ /* skip serial ports */
+ if (!strncmp("tty", direntry->d_name, 3)) {
+ continue;
+ }
+ /* skip mtd devices */
+ if (!strncmp("mtd", direntry->d_name, 3)) {
+ continue;
+ }
+ /* skip ram devices */
+ if (!strncmp("ram", direntry->d_name, 3)) {
+ continue;
+ }
+ /* skip MMC devices */
+ if (!strncmp("mmc", direntry->d_name, 3)) {
+ continue;
+ }
+ /* skip mavlink */
+ if (!strcmp("mavlink", direntry->d_name)) {
+ continue;
+ }
+ /* skip console */
+ if (!strcmp("console", direntry->d_name)) {
+ continue;
+ }
+ /* skip null */
+ if (!strcmp("null", direntry->d_name)) {
+ continue;
+ }
+
+ snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name);
+
+ int sensfd = ::open(devname, 0);
+
+ if (sensfd < 0) {
+ warn("failed opening device %s", devname);
+ continue;
+ }
+
+ int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
close(sensfd);
- printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL");
+ printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
}
closedir(d);
- warnx("directory listing ok (FS mounted and readable)");
-
} else {
/* failed opening dir */
warnx("FAILED LISTING DEVICE ROOT DIRECTORY");
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-09 13:13:42 +0000