aboutsummaryrefslogtreecommitdiff
path: root/src/drivers
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers')
-rw-r--r--src/drivers/ardrone_interface/ardrone_interface.c13
-rw-r--r--src/drivers/blinkm/blinkm.cpp68
-rw-r--r--src/drivers/hil/hil.cpp17
-rw-r--r--src/drivers/mkblctrl/mkblctrl.cpp19
-rw-r--r--src/drivers/px4fmu/fmu.cpp27
-rw-r--r--src/drivers/px4io/px4io.cpp336
-rw-r--r--src/drivers/stm32/tone_alarm/tone_alarm.cpp29
7 files changed, 398 insertions, 111 deletions
diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c
index 735bdb41a..187820372 100644
--- a/src/drivers/ardrone_interface/ardrone_interface.c
+++ b/src/drivers/ardrone_interface/ardrone_interface.c
@@ -55,6 +55,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_safety.h>
#include <systemlib/systemlib.h>
@@ -247,13 +248,13 @@ int ardrone_interface_thread_main(int argc, char *argv[])
memset(&state, 0, sizeof(state));
struct actuator_controls_s actuator_controls;
memset(&actuator_controls, 0, sizeof(actuator_controls));
- struct actuator_armed_s armed;
- armed.armed = false;
+ struct actuator_safety_s safety;
+ safety.armed = false;
/* subscribe to attitude, motor setpoints and system state */
int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
- int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+ int safety_sub = orb_subscribe(ORB_ID(actuator_safety));
printf("[ardrone_interface] Motors initialized - ready.\n");
fflush(stdout);
@@ -322,16 +323,14 @@ int ardrone_interface_thread_main(int argc, char *argv[])
} else {
/* MAIN OPERATION MODE */
- /* get a local copy of the vehicle state */
- orb_copy(ORB_ID(vehicle_status), state_sub, &state);
/* get a local copy of the actuator controls */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
- orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
+ orb_copy(ORB_ID(actuator_safety), safety_sub, &safety);
/* for now only spin if armed and immediately shut down
* if in failsafe
*/
- if (armed.armed && !armed.lockdown) {
+ if (safety.armed && !safety.lockdown) {
ardrone_mixing_and_output(ardrone_write, &actuator_controls);
} else {
diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp
index 3fabfd9a5..a11f88477 100644
--- a/src/drivers/blinkm/blinkm.cpp
+++ b/src/drivers/blinkm/blinkm.cpp
@@ -116,6 +116,7 @@
#include <poll.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_safety.h>
#include <uORB/topics/vehicle_gps_position.h>
static const float MAX_CELL_VOLTAGE = 4.3f;
@@ -376,6 +377,7 @@ BlinkM::led()
static int vehicle_status_sub_fd;
static int vehicle_gps_position_sub_fd;
+ static int actuator_safety_sub_fd;
static int num_of_cells = 0;
static int detected_cells_runcount = 0;
@@ -386,6 +388,7 @@ BlinkM::led()
static int led_interval = 1000;
static int no_data_vehicle_status = 0;
+ static int no_data_actuator_safety = 0;
static int no_data_vehicle_gps_position = 0;
static bool topic_initialized = false;
@@ -398,6 +401,9 @@ BlinkM::led()
vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
orb_set_interval(vehicle_status_sub_fd, 1000);
+ actuator_safety_sub_fd = orb_subscribe(ORB_ID(actuator_safety));
+ orb_set_interval(actuator_safety_sub_fd, 1000);
+
vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
orb_set_interval(vehicle_gps_position_sub_fd, 1000);
@@ -452,12 +458,14 @@ BlinkM::led()
if (led_thread_runcount == 15) {
/* obtained data for the first file descriptor */
struct vehicle_status_s vehicle_status_raw;
+ struct actuator_safety_s actuator_safety;
struct vehicle_gps_position_s vehicle_gps_position_raw;
memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw));
memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw));
bool new_data_vehicle_status;
+ bool new_data_actuator_safety;
bool new_data_vehicle_gps_position;
orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
@@ -471,6 +479,17 @@ BlinkM::led()
no_data_vehicle_status = 3;
}
+ orb_check(actuator_safety_sub_fd, &new_data_actuator_safety);
+
+ if (new_data_actuator_safety) {
+ orb_copy(ORB_ID(actuator_safety), actuator_safety_sub_fd, &actuator_safety);
+ no_data_actuator_safety = 0;
+ } else {
+ no_data_actuator_safety++;
+ if(no_data_vehicle_status >= 3)
+ no_data_vehicle_status = 3;
+ }
+
orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position);
if (new_data_vehicle_gps_position) {
@@ -486,15 +505,15 @@ BlinkM::led()
/* get number of used satellites in navigation */
num_of_used_sats = 0;
- //for(int satloop=0; satloop<20; satloop++) {
- for(int satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) {
- if(vehicle_gps_position_raw.satellite_used[satloop] == 1) {
+
+ for (unsigned satloop = 0; satloop < (sizeof(vehicle_gps_position_raw.satellite_used) / sizeof(vehicle_gps_position_raw.satellite_used[0])); satloop++) {
+ if (vehicle_gps_position_raw.satellite_used[satloop] == 1) {
num_of_used_sats++;
}
}
- if(new_data_vehicle_status || no_data_vehicle_status < 3){
- if(num_of_cells == 0) {
+ if (new_data_vehicle_status || no_data_vehicle_status < 3) {
+ if (num_of_cells == 0) {
/* looking for lipo cells that are connected */
printf("<blinkm> checking cells\n");
for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) {
@@ -530,7 +549,7 @@ BlinkM::led()
} else {
/* no battery warnings here */
- if(vehicle_status_raw.flag_system_armed == false) {
+ if(actuator_safety.armed == false) {
/* system not armed */
led_color_1 = LED_RED;
led_color_2 = LED_RED;
@@ -555,26 +574,27 @@ BlinkM::led()
led_blink = LED_BLINK;
/* handle 4th led - flightmode indicator */
- switch((int)vehicle_status_raw.flight_mode) {
- case VEHICLE_FLIGHT_MODE_MANUAL:
- led_color_4 = LED_AMBER;
- break;
-
- case VEHICLE_FLIGHT_MODE_STAB:
- led_color_4 = LED_YELLOW;
- break;
-
- case VEHICLE_FLIGHT_MODE_HOLD:
- led_color_4 = LED_BLUE;
- break;
-
- case VEHICLE_FLIGHT_MODE_AUTO:
- led_color_4 = LED_GREEN;
- break;
- }
+#warning fix this
+// switch((int)vehicle_status_raw.flight_mode) {
+// case VEHICLE_FLIGHT_MODE_MANUAL:
+// led_color_4 = LED_AMBER;
+// break;
+//
+// case VEHICLE_FLIGHT_MODE_STAB:
+// led_color_4 = LED_YELLOW;
+// break;
+//
+// case VEHICLE_FLIGHT_MODE_HOLD:
+// led_color_4 = LED_BLUE;
+// break;
+//
+// case VEHICLE_FLIGHT_MODE_AUTO:
+// led_color_4 = LED_GREEN;
+// break;
+// }
if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
- /* handling used sat´s */
+ /* handling used sat�s */
if(num_of_used_sats >= 7) {
led_color_1 = LED_OFF;
led_color_2 = LED_OFF;
diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp
index bd027ce0b..15fbf8c88 100644
--- a/src/drivers/hil/hil.cpp
+++ b/src/drivers/hil/hil.cpp
@@ -75,6 +75,7 @@
#include <systemlib/mixer/mixer.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_safety.h>
#include <uORB/topics/actuator_outputs.h>
#include <systemlib/err.h>
@@ -108,7 +109,7 @@ private:
int _current_update_rate;
int _task;
int _t_actuators;
- int _t_armed;
+ int _t_safety;
orb_advert_t _t_outputs;
unsigned _num_outputs;
bool _primary_pwm_device;
@@ -161,7 +162,7 @@ HIL::HIL() :
_current_update_rate(0),
_task(-1),
_t_actuators(-1),
- _t_armed(-1),
+ _t_safety(-1),
_t_outputs(0),
_num_outputs(0),
_primary_pwm_device(false),
@@ -321,8 +322,8 @@ HIL::task_main()
/* force a reset of the update rate */
_current_update_rate = 0;
- _t_armed = orb_subscribe(ORB_ID(actuator_armed));
- orb_set_interval(_t_armed, 200); /* 5Hz update rate */
+ _t_safety = orb_subscribe(ORB_ID(actuator_safety));
+ orb_set_interval(_t_safety, 200); /* 5Hz update rate */
/* advertise the mixed control outputs */
actuator_outputs_s outputs;
@@ -334,7 +335,7 @@ HIL::task_main()
pollfd fds[2];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
- fds[1].fd = _t_armed;
+ fds[1].fd = _t_safety;
fds[1].events = POLLIN;
unsigned num_outputs;
@@ -426,15 +427,15 @@ HIL::task_main()
/* how about an arming update? */
if (fds[1].revents & POLLIN) {
- actuator_armed_s aa;
+ actuator_safety_s aa;
/* get new value */
- orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
+ orb_copy(ORB_ID(actuator_safety), _t_safety, &aa);
}
}
::close(_t_actuators);
- ::close(_t_armed);
+ ::close(_t_safety);
/* make sure servos are off */
// up_pwm_servo_deinit();
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index b54b7aba1..f187f22a6 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -74,6 +74,7 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_safety.h>
#include <systemlib/err.h>
@@ -132,7 +133,7 @@ private:
int _current_update_rate;
int _task;
int _t_actuators;
- int _t_armed;
+ int _t_actuator_safety;
unsigned int _motor;
int _px4mode;
int _frametype;
@@ -240,7 +241,7 @@ MK::MK(int bus) :
_update_rate(50),
_task(-1),
_t_actuators(-1),
- _t_armed(-1),
+ _t_actuator_safety(-1),
_t_outputs(0),
_t_actuators_effective(0),
_num_outputs(0),
@@ -496,8 +497,8 @@ MK::task_main()
/* force a reset of the update rate */
_current_update_rate = 0;
- _t_armed = orb_subscribe(ORB_ID(actuator_armed));
- orb_set_interval(_t_armed, 200); /* 5Hz update rate */
+ _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety));
+ orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */
/* advertise the mixed control outputs */
actuator_outputs_s outputs;
@@ -519,7 +520,7 @@ MK::task_main()
pollfd fds[2];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
- fds[1].fd = _t_armed;
+ fds[1].fd = _t_actuator_safety;
fds[1].events = POLLIN;
log("starting");
@@ -625,13 +626,13 @@ MK::task_main()
/* how about an arming update? */
if (fds[1].revents & POLLIN) {
- actuator_armed_s aa;
+ actuator_safety_s as;
/* get new value */
- orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
+ orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &as);
/* update PWM servo armed status if armed and not locked down */
- mk_servo_arm(aa.armed && !aa.lockdown);
+ mk_servo_arm(as.armed && !as.lockdown);
}
@@ -639,7 +640,7 @@ MK::task_main()
::close(_t_actuators);
::close(_t_actuators_effective);
- ::close(_t_armed);
+ ::close(_t_actuator_safety);
/* make sure servos are off */
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 5147ac500..05951f74c 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -69,6 +69,7 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_safety.h>
#include <systemlib/err.h>
#include <systemlib/ppm_decode.h>
@@ -104,7 +105,7 @@ private:
unsigned _current_update_rate;
int _task;
int _t_actuators;
- int _t_armed;
+ int _t_actuator_safety;
orb_advert_t _t_outputs;
orb_advert_t _t_actuators_effective;
unsigned _num_outputs;
@@ -174,7 +175,7 @@ PX4FMU::PX4FMU() :
_current_update_rate(0),
_task(-1),
_t_actuators(-1),
- _t_armed(-1),
+ _t_actuator_safety(-1),
_t_outputs(0),
_t_actuators_effective(0),
_num_outputs(0),
@@ -376,8 +377,8 @@ PX4FMU::task_main()
/* force a reset of the update rate */
_current_update_rate = 0;
- _t_armed = orb_subscribe(ORB_ID(actuator_armed));
- orb_set_interval(_t_armed, 200); /* 5Hz update rate */
+ _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety));
+ orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */
/* advertise the mixed control outputs */
actuator_outputs_s outputs;
@@ -396,7 +397,7 @@ PX4FMU::task_main()
pollfd fds[2];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
- fds[1].fd = _t_armed;
+ fds[1].fd = _t_actuator_safety;
fds[1].events = POLLIN;
unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4;
@@ -499,13 +500,13 @@ PX4FMU::task_main()
/* how about an arming update? */
if (fds[1].revents & POLLIN) {
- actuator_armed_s aa;
+ actuator_safety_s as;
/* get new value */
- orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
+ orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &as);
/* update PWM servo armed status if armed and not locked down */
- bool set_armed = aa.armed && !aa.lockdown;
+ bool set_armed = as.armed && !as.lockdown;
if (set_armed != _armed) {
_armed = set_armed;
up_pwm_servo_arm(set_armed);
@@ -535,7 +536,7 @@ PX4FMU::task_main()
::close(_t_actuators);
::close(_t_actuators_effective);
- ::close(_t_armed);
+ ::close(_t_actuator_safety);
/* make sure servos are off */
up_pwm_servo_deinit();
@@ -1021,12 +1022,12 @@ fake(int argc, char *argv[])
if (handle < 0)
errx(1, "advertise failed");
- actuator_armed_s aa;
+ actuator_safety_s as;
- aa.armed = true;
- aa.lockdown = false;
+ as.armed = true;
+ as.lockdown = false;
- handle = orb_advertise(ORB_ID(actuator_armed), &aa);
+ handle = orb_advertise(ORB_ID(actuator_safety), &as);
if (handle < 0)
errx(1, "advertise failed 2");
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 19163cebe..df2f18270 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -75,6 +75,7 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_safety.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/rc_channels.h>
@@ -125,6 +126,21 @@ public:
int set_failsafe_values(const uint16_t *vals, unsigned len);
/**
+ * Set the minimum PWM signals when armed
+ */
+ int set_min_values(const uint16_t *vals, unsigned len);
+
+ /**
+ * Set the maximum PWM signal when armed
+ */
+ int set_max_values(const uint16_t *vals, unsigned len);
+
+ /**
+ * Set an idle PWM signal that is active right after startup, even when SAFETY_SAFE
+ */
+ int set_idle_values(const uint16_t *vals, unsigned len);
+
+ /**
* Print the current status of IO
*/
void print_status();
@@ -152,7 +168,7 @@ private:
/* subscribed topics */
int _t_actuators; ///< actuator controls topic
- int _t_armed; ///< system armed control topic
+ int _t_actuator_safety; ///< system armed control topic
int _t_vstatus; ///< system / vehicle status
int _t_param; ///< parameter update topic
@@ -161,6 +177,7 @@ private:
orb_advert_t _to_actuators_effective; ///< effective actuator controls topic
orb_advert_t _to_outputs; ///< mixed servo outputs topic
orb_advert_t _to_battery; ///< battery status / voltage
+ orb_advert_t _to_safety; ///< status of safety
actuator_outputs_s _outputs; ///< mixed outputs
actuator_controls_effective_s _controls_effective; ///< effective controls
@@ -327,13 +344,14 @@ PX4IO::PX4IO() :
_status(0),
_alarms(0),
_t_actuators(-1),
- _t_armed(-1),
+ _t_actuator_safety(-1),
_t_vstatus(-1),
_t_param(-1),
_to_input_rc(0),
_to_actuators_effective(0),
_to_outputs(0),
_to_battery(0),
+ _to_safety(0),
_primary_pwm_device(false),
_battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor
_battery_amp_bias(0),
@@ -387,6 +405,40 @@ PX4IO::init()
*/
_retries = 2;
+ /* get IO's last seen FMU state */
+ int val = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
+ if (val == _io_reg_get_error) {
+ mavlink_log_emergency(_mavlink_fd, "[IO] ERROR! FAILED READING STATE");
+ }
+ uint16_t arming = val;
+
+ /* get basic software version */
+ /* basic configuration */
+ usleep(5000);
+
+ unsigned proto_version = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
+ unsigned sw_version = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION);
+
+ if (proto_version != PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC) {
+ mavlink_log_emergency(_mavlink_fd, "[IO] ERROR! PROTO VER MISMATCH: v%u vs v%u\n",
+ proto_version,
+ PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC);
+
+ mavlink_log_emergency(_mavlink_fd, "[IO] Please update PX4IO firmware.");
+ log("protocol version mismatch (v%u on IO vs v%u on FMU)", proto_version, PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC);
+ return 1;
+ }
+
+ if (sw_version != PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC) {
+ mavlink_log_emergency(_mavlink_fd, "[IO] ERROR! SOFTWARE VER MISMATCH: v%u vs v%u\n",
+ proto_version,
+ PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC);
+
+ mavlink_log_emergency(_mavlink_fd, "[IO] Please update PX4IO firmware.");
+ log("software version mismatch (v%u on IO vs v%u on FMU)", sw_version, PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC);
+ return 1;
+ }
+
/* get some parameters */
_max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT);
_max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT);
@@ -412,41 +464,44 @@ PX4IO::init()
* in this case.
*/
- uint16_t reg;
+ printf("arming 0x%04x%s%s%s%s\n",
+ arming,
+ ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : ""),
+ ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : ""),
+ ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
+ ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""),
+ ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE CUSTOM" : ""));
- /* get IO's last seen FMU state */
- ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, &reg, sizeof(reg));
- if (ret != OK)
- return ret;
/*
* in-air restart is only tried if the IO board reports it is
* already armed, and has been configured for in-air restart
*/
- if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) &&
- (reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+ if ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) &&
+ (arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART");
+ log("INAIR RESTART RECOVERY (needs commander app running)");
/* WARNING: COMMANDER app/vehicle status must be initialized.
* If this fails (or the app is not started), worst-case IO
* remains untouched (so manual override is still available).
*/
- int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+ int safety_sub = orb_subscribe(ORB_ID(actuator_safety));
/* fill with initial values, clear updated flag */
- vehicle_status_s status;
+ struct actuator_safety_s safety;
uint64_t try_start_time = hrt_absolute_time();
bool updated = false;
- /* keep checking for an update, ensure we got a recent state,
+ /* keep checking for an update, ensure we got a arming information,
not something that was published a long time ago. */
do {
- orb_check(vstatus_sub, &updated);
+ orb_check(safety_sub, &updated);
if (updated) {
/* got data, copy and exit loop */
- orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status);
+ orb_copy(ORB_ID(actuator_safety), safety_sub, &safety);
break;
}
@@ -472,35 +527,41 @@ PX4IO::init()
cmd.param6 = 0;
cmd.param7 = 0;
cmd.command = VEHICLE_CMD_COMPONENT_ARM_DISARM;
- cmd.target_system = status.system_id;
- cmd.target_component = status.component_id;
- cmd.source_system = status.system_id;
- cmd.source_component = status.component_id;
+ // cmd.target_system = status.system_id;
+ // cmd.target_component = status.component_id;
+ // cmd.source_system = status.system_id;
+ // cmd.source_component = status.component_id;
/* ask to confirm command */
cmd.confirmation = 1;
/* send command once */
- (void)orb_advertise(ORB_ID(vehicle_command), &cmd);
+ orb_advert_t pub = orb_advertise(ORB_ID(vehicle_command), &cmd);
/* spin here until IO's state has propagated into the system */
do {
- orb_check(vstatus_sub, &updated);
+ orb_check(safety_sub, &updated);
if (updated) {
- orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status);
+ orb_copy(ORB_ID(actuator_safety), safety_sub, &safety);
}
- /* wait 10 ms */
- usleep(10000);
+ /* wait 50 ms */
+ usleep(50000);
/* abort after 5s */
- if ((hrt_absolute_time() - try_start_time)/1000 > 50000) {
+ if ((hrt_absolute_time() - try_start_time)/1000 > 2000) {
log("failed to recover from in-air restart (2), aborting IO driver init.");
return 1;
}
- /* keep waiting for state change for 10 s */
- } while (!status.flag_system_armed);
+ /* re-send if necessary */
+ if (!safety.armed) {
+ orb_publish(ORB_ID(vehicle_command), pub, &cmd);
+ log("re-sending arm cmd");
+ }
+
+ /* keep waiting for state change for 2 s */
+ } while (!safety.armed);
/* regular boot, no in-air restart, init IO */
} else {
@@ -510,7 +571,8 @@ PX4IO::init()
io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING,
PX4IO_P_SETUP_ARMING_FMU_ARMED |
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK |
- PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0);
+ PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK |
+ PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE, 0);
/* publish RC config to IO */
ret = io_set_rc_config();
@@ -538,7 +600,7 @@ PX4IO::init()
return -errno;
}
- mavlink_log_info(_mavlink_fd, "[IO] init ok");
+ mavlink_log_info(_mavlink_fd, "[IO] init ok (sw v.%u)", sw_version);
return OK;
}
@@ -564,8 +626,8 @@ PX4IO::task_main()
ORB_ID(actuator_controls_1));
orb_set_interval(_t_actuators, 20); /* default to 50Hz */
- _t_armed = orb_subscribe(ORB_ID(actuator_armed));
- orb_set_interval(_t_armed, 200); /* 5Hz update rate */
+ _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety));
+ orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */
_t_vstatus = orb_subscribe(ORB_ID(vehicle_status));
orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */
@@ -577,7 +639,7 @@ PX4IO::task_main()
pollfd fds[4];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
- fds[1].fd = _t_armed;
+ fds[1].fd = _t_actuator_safety;
fds[1].events = POLLIN;
fds[2].fd = _t_vstatus;
fds[2].events = POLLIN;
@@ -711,24 +773,65 @@ PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len)
}
int
+PX4IO::set_min_values(const uint16_t *vals, unsigned len)
+{
+ uint16_t regs[_max_actuators];
+
+ if (len > _max_actuators)
+ /* fail with error */
+ return E2BIG;
+
+ /* copy values to registers in IO */
+ return io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, vals, len);
+}
+
+int
+PX4IO::set_max_values(const uint16_t *vals, unsigned len)
+{
+ uint16_t regs[_max_actuators];
+
+ if (len > _max_actuators)
+ /* fail with error */
+ return E2BIG;
+
+ /* copy values to registers in IO */
+ return io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, vals, len);
+}
+
+int
+PX4IO::set_idle_values(const uint16_t *vals, unsigned len)
+{
+ uint16_t regs[_max_actuators];
+
+ if (len > _max_actuators)
+ /* fail with error */
+ return E2BIG;
+
+ printf("Sending IDLE values\n");
+
+ /* copy values to registers in IO */
+ return io_reg_set(PX4IO_PAGE_IDLE_PWM, 0, vals, len);
+}
+
+
+int
PX4IO::io_set_arming_state()
{
- actuator_armed_s armed; ///< system armed state
+ actuator_safety_s safety; ///< system armed state
vehicle_status_s vstatus; ///< overall system state
- orb_copy(ORB_ID(actuator_armed), _t_armed, &armed);
+ orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &safety);
orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus);
uint16_t set = 0;
uint16_t clear = 0;
- if (armed.armed && !armed.lockdown) {
+ if (safety.armed && !safety.lockdown) {
set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
} else {
clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
}
-
- if (armed.ready_to_arm) {
+ if (safety.ready_to_arm) {
set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
} else {
clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
@@ -861,14 +964,14 @@ PX4IO::io_handle_status(uint16_t status)
*/
/* check for IO reset - force it back to armed if necessary */
- if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED)
+ if (_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF && !(status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
&& !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
/* set the arming flag */
- ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED | PX4IO_P_STATUS_FLAGS_ARM_SYNC);
+ ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_SAFETY_OFF | PX4IO_P_STATUS_FLAGS_ARM_SYNC);
/* set new status */
_status = status;
- _status &= PX4IO_P_STATUS_FLAGS_ARMED;
+ _status &= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
} else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
/* set the sync flag */
@@ -883,6 +986,29 @@ PX4IO::io_handle_status(uint16_t status)
_status = status;
}
+ /**
+ * Get and handle the safety status
+ */
+ struct actuator_safety_s safety;
+ safety.timestamp = hrt_absolute_time();
+
+ orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &safety);
+
+ if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
+ safety.safety_off = true;
+ safety.safety_switch_available = true;
+ } else {
+ safety.safety_off = false;
+ safety.safety_switch_available = true;
+ }
+
+ /* lazily publish the safety status */
+ if (_to_safety > 0) {
+ orb_publish(ORB_ID(actuator_safety), _to_safety, &safety);
+ } else {
+ _to_safety = orb_advertise(ORB_ID(actuator_safety), &safety);
+ }
+
return ret;
}
@@ -912,7 +1038,7 @@ PX4IO::io_get_status()
io_handle_status(regs[0]);
io_handle_alarms(regs[1]);
-
+
/* only publish if battery has a valid minimum voltage */
if (regs[2] > 3300) {
battery_status_s battery_status;
@@ -946,6 +1072,7 @@ PX4IO::io_get_status()
_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
}
}
+
return ret;
}
@@ -1183,6 +1310,7 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t
uint16_t value;
ret = io_reg_get(page, offset, &value, 1);
+
if (ret)
return ret;
value &= ~clearbits;
@@ -1273,7 +1401,8 @@ PX4IO::print_status()
uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n",
flags,
- ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""),
+ ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""),
+ ((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"),
((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"),
((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""),
@@ -1335,12 +1464,14 @@ PX4IO::print_status()
/* setup and state */
printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES));
uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
- printf("arming 0x%04x%s%s%s%s\n",
+ printf("arming 0x%04x%s%s%s%s%s%s\n",
arming,
((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : ""),
((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
- ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""));
+ ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""),
+ ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""),
+ ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""));
printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE),
@@ -1367,7 +1498,10 @@ PX4IO::print_status()
}
printf("failsafe");
for (unsigned i = 0; i < _max_actuators; i++)
- printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i));
+ printf(" %u\n", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i));
+ printf("\nidle values");
+ for (unsigned i = 0; i < _max_actuators; i++)
+ printf(" %u", io_reg_get(PX4IO_PAGE_IDLE_PWM, i));
printf("\n");
}
@@ -1634,6 +1768,11 @@ test(void)
if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count))
err(1, "failed to get servo count");
+ /* tell IO that its ok to disable its safety with the switch */
+ ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
+ if (ret != OK)
+ err(1, "PWM_SERVO_SET_ARM_OK");
+
if (ioctl(fd, PWM_SERVO_ARM, 0))
err(1, "failed to arm servos");
@@ -1682,7 +1821,7 @@ test(void)
/* Check if user wants to quit */
char c;
if (read(console, &c, 1) == 1) {
- if (c == 0x03 || c == 0x63) {
+ if (c == 0x03 || c == 0x63 || c == 'q') {
warnx("User abort\n");
close(console);
exit(0);
@@ -1792,6 +1931,111 @@ px4io_main(int argc, char *argv[])
exit(0);
}
+ if (!strcmp(argv[1], "min")) {
+
+ if (argc < 3) {
+ errx(1, "min command needs at least one channel value (PWM)");
+ }
+
+ if (g_dev != nullptr) {
+
+ /* set values for first 8 channels, fill unassigned channels with 900. */
+ uint16_t min[8];
+
+ for (int i = 0; i < sizeof(min) / sizeof(min[0]); i++)
+ {
+ /* set channel to commanline argument or to 900 for non-provided channels */
+ if (argc > i + 2) {
+ min[i] = atoi(argv[i+2]);
+ if (min[i] < 900 || min[i] > 1200) {
+ errx(1, "value out of range of 900 < value < 1200. Aborting.");
+ }
+ } else {
+ /* a zero value will the default */
+ min[i] = 0;
+ }
+ }
+
+ int ret = g_dev->set_min_values(min, sizeof(min) / sizeof(min[0]));
+
+ if (ret != OK)
+ errx(ret, "failed setting min values");
+ } else {
+ errx(1, "not loaded");
+ }
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "max")) {
+
+ if (argc < 3) {
+ errx(1, "max command needs at least one channel value (PWM)");
+ }
+
+ if (g_dev != nullptr) {
+
+ /* set values for first 8 channels, fill unassigned channels with 2100. */
+ uint16_t max[8];
+
+ for (int i = 0; i < sizeof(max) / sizeof(max[0]); i++)
+ {
+ /* set channel to commanline argument or to 2100 for non-provided channels */
+ if (argc > i + 2) {
+ max[i] = atoi(argv[i+2]);
+ if (max[i] < 1800 || max[i] > 2100) {
+ errx(1, "value out of range of 1800 < value < 2100. Aborting.");
+ }
+ } else {
+ /* a zero value will the default */
+ max[i] = 0;
+ }
+ }
+
+ int ret = g_dev->set_max_values(max, sizeof(max) / sizeof(max[0]));
+
+ if (ret != OK)
+ errx(ret, "failed setting max values");
+ } else {
+ errx(1, "not loaded");
+ }
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "idle")) {
+
+ if (argc < 3) {
+ errx(1, "max command needs at least one channel value (PWM)");
+ }
+
+ if (g_dev != nullptr) {
+
+ /* set values for first 8 channels, fill unassigned channels with 0. */
+ uint16_t idle[8];
+
+ for (int i = 0; i < sizeof(idle) / sizeof(idle[0]); i++)
+ {
+ /* set channel to commanline argument or to 0 for non-provided channels */
+ if (argc > i + 2) {
+ idle[i] = atoi(argv[i+2]);
+ if (idle[i] < 900 || idle[i] > 2100) {
+ errx(1, "value out of range of 900 < value < 2100. Aborting.");
+ }
+ } else {
+ /* a zero value will the default */
+ idle[i] = 0;
+ }
+ }
+
+ int ret = g_dev->set_idle_values(idle, sizeof(idle) / sizeof(idle[0]));
+
+ if (ret != OK)
+ errx(ret, "failed setting idle values");
+ } else {
+ errx(1, "not loaded");
+ }
+ exit(0);
+ }
+
if (!strcmp(argv[1], "recovery")) {
if (g_dev != nullptr) {
@@ -1919,5 +2163,5 @@ px4io_main(int argc, char *argv[])
monitor();
out:
- errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe' or 'update'");
+ errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe', 'min, 'max', 'idle' or 'update'");
}
diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
index 167ef30a8..e1409a815 100644
--- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (C) 2013 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
@@ -241,6 +241,8 @@ private:
static const unsigned _default_ntunes;
static const uint8_t _note_tab[];
+ unsigned _default_tune_number; // number of currently playing default tune (0 for none)
+
const char *_user_tune;
const char *_tune; // current tune string
@@ -456,6 +458,11 @@ const char * const ToneAlarm::_default_tunes[] = {
"O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8"
"O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16"
"O2E2P64",
+ "MNT75L1O2G", //arming warning
+ "MBNT100a8", //battery warning slow
+ "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"
+ "a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"
+ "a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8", //battery warning fast // XXX why is there a break before a repetition
};
const unsigned ToneAlarm::_default_ntunes = sizeof(_default_tunes) / sizeof(_default_tunes[0]);
@@ -471,6 +478,7 @@ extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]);
ToneAlarm::ToneAlarm() :
CDev("tone_alarm", "/dev/tone_alarm"),
+ _default_tune_number(0),
_user_tune(nullptr),
_tune(nullptr),
_next(nullptr)
@@ -803,8 +811,12 @@ tune_error:
// stop (and potentially restart) the tune
tune_end:
stop_note();
- if (_repeat)
+ if (_repeat) {
start_tune(_tune);
+ } else {
+ _tune = nullptr;
+ _default_tune_number = 0;
+ }
return;
}
@@ -873,8 +885,17 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg)
_tune = nullptr;
_next = nullptr;
} else {
- // play the selected tune
- start_tune(_default_tunes[arg - 1]);
+ /* don't interrupt alarms unless they are repeated */
+ if (_tune != nullptr && !_repeat) {
+ /* abort and let the current tune finish */
+ result = -EBUSY;
+ } else if (_repeat && _default_tune_number == arg) {
+ /* requested repeating tune already playing */
+ } else {
+ // play the selected tune
+ _default_tune_number = arg;
+ start_tune(_default_tunes[arg - 1]);
+ }
}
} else {
result = -EINVAL;
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-30 17:15:05 +0000