forked from Archive/PX4-Autopilot
Merge remote-tracking branch 'upstream/io_fixes' into new_state_machine
Conflicts: src/drivers/px4io/px4io.cpp src/modules/commander/commander.c src/modules/commander/state_machine_helper.c src/modules/commander/state_machine_helper.h src/modules/px4iofirmware/mixer.cpp src/modules/uORB/topics/actuator_controls.h src/modules/uORB/topics/vehicle_status.h
This commit is contained in:
Julian Oes
commit
202792294a
|
|
@ -505,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++) {
|
||||
|
|
|
|||
|
|
@ -81,6 +81,7 @@
|
|||
#include <uORB/topics/rc_channels.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/safety.h>
|
||||
#include <debug.h>
|
||||
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
|
@ -172,6 +173,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
|
||||
|
|
@ -345,6 +347,7 @@ PX4IO::PX4IO() :
|
|||
_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),
|
||||
|
|
@ -398,6 +401,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);
|
||||
|
|
@ -423,21 +460,24 @@ 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, ®, 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
|
||||
|
|
@ -446,7 +486,7 @@ PX4IO::init()
|
|||
|
||||
int safety_sub = orb_subscribe(ORB_ID(actuator_safety));
|
||||
/* fill with initial values, clear updated flag */
|
||||
struct actuator_safety_s armed;
|
||||
struct actuator_safety_s safety;
|
||||
uint64_t try_start_time = hrt_absolute_time();
|
||||
bool updated = false;
|
||||
|
||||
|
|
@ -457,7 +497,7 @@ PX4IO::init()
|
|||
|
||||
if (updated) {
|
||||
/* got data, copy and exit loop */
|
||||
orb_copy(ORB_ID(actuator_safety), safety_sub, &armed);
|
||||
orb_copy(ORB_ID(actuator_safety), safety_sub, &safety);
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
@ -491,27 +531,33 @@ PX4IO::init()
|
|||
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(safety_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(actuator_safety), safety_sub, &armed);
|
||||
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 (!armed.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 {
|
||||
|
|
@ -549,7 +595,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;
|
||||
}
|
||||
|
|
@ -899,14 +945,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 */
|
||||
|
|
@ -921,6 +967,25 @@ PX4IO::io_handle_status(uint16_t status)
|
|||
_status = status;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get and handle the safety status
|
||||
*/
|
||||
struct safety_s safety;
|
||||
safety.timestamp = hrt_absolute_time();
|
||||
|
||||
if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
|
||||
safety.status = SAFETY_STATUS_UNLOCKED;
|
||||
} else {
|
||||
safety.status = SAFETY_STATUS_SAFE;
|
||||
}
|
||||
|
||||
/* lazily publish the safety status */
|
||||
if (_to_safety > 0) {
|
||||
orb_publish(ORB_ID(safety), _to_safety, &safety);
|
||||
} else {
|
||||
_to_safety = orb_advertise(ORB_ID(safety), &safety);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
|
@ -984,6 +1049,7 @@ PX4IO::io_get_status()
|
|||
_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
|
@ -1221,6 +1287,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;
|
||||
|
|
@ -1311,7 +1378,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" : ""),
|
||||
|
|
@ -1672,6 +1740,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");
|
||||
|
||||
|
|
@ -1720,7 +1793,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);
|
||||
|
|
|
|||
|
|
@ -79,6 +79,7 @@
|
|||
#include <uORB/topics/actuator_safety.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/differential_pressure.h>
|
||||
#include <uORB/topics/safety.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include <drivers/drv_led.h>
|
||||
|
|
@ -1229,6 +1230,10 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
/* set battery warning flag */
|
||||
current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
|
||||
/* set safety device detection flag */
|
||||
|
||||
/* XXX do we need this? */
|
||||
//current_status.flag_safety_present = false;
|
||||
|
||||
// XXX for now just set sensors as initialized
|
||||
current_status.condition_system_sensors_initialized = true;
|
||||
|
|
@ -1352,7 +1357,6 @@ int commander_thread_main(int argc, char *argv[])
|
|||
memset(&battery, 0, sizeof(battery));
|
||||
battery.voltage_v = 0.0f;
|
||||
|
||||
|
||||
// uint8_t vehicle_state_previous = current_status.state_machine;
|
||||
float voltage_previous = 0.0f;
|
||||
|
||||
|
|
@ -1374,6 +1378,39 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
/* Get current values */
|
||||
bool new_data;
|
||||
|
||||
/* update parameters */
|
||||
orb_check(param_changed_sub, &new_data);
|
||||
|
||||
if (new_data || param_init_forced) {
|
||||
param_init_forced = false;
|
||||
/* parameters changed */
|
||||
orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed);
|
||||
|
||||
|
||||
/* update parameters */
|
||||
if (!safety.armed) {
|
||||
if (param_get(_param_sys_type, &(current_status.system_type)) != OK) {
|
||||
warnx("failed setting new system type");
|
||||
}
|
||||
|
||||
/* disable manual override for all systems that rely on electronic stabilization */
|
||||
if (current_status.system_type == VEHICLE_TYPE_QUADROTOR ||
|
||||
current_status.system_type == VEHICLE_TYPE_HEXAROTOR ||
|
||||
current_status.system_type == VEHICLE_TYPE_OCTOROTOR) {
|
||||
current_status.flag_external_manual_override_ok = false;
|
||||
|
||||
} else {
|
||||
current_status.flag_external_manual_override_ok = true;
|
||||
}
|
||||
|
||||
/* check and update system / component ID */
|
||||
param_get(_param_system_id, &(current_status.system_id));
|
||||
param_get(_param_component_id, &(current_status.component_id));
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
orb_check(sp_man_sub, &new_data);
|
||||
|
||||
if (new_data) {
|
||||
|
|
@ -1410,7 +1447,6 @@ int commander_thread_main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
|
||||
|
||||
/* update parameters */
|
||||
orb_check(param_changed_sub, &new_data);
|
||||
|
||||
|
|
@ -1686,7 +1722,9 @@ int commander_thread_main(int argc, char *argv[])
|
|||
// state_changed = true;
|
||||
// }
|
||||
|
||||
if (orb_check(gps_sub, &new_data)) {
|
||||
orb_check(ORB_ID(vehicle_gps_position), &new_data);
|
||||
if (new_data) {
|
||||
|
||||
|
||||
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
|
||||
|
||||
|
|
|
|||
|
|
@ -54,9 +54,25 @@
|
|||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include "state_machine_helper.h"
|
||||
#include "commander.h"
|
||||
|
||||
bool is_multirotor(const struct vehicle_status_s *current_status)
|
||||
{
|
||||
return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
|
||||
(current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
|
||||
(current_status->system_type == VEHICLE_TYPE_OCTOROTOR) ||
|
||||
(current_status->system_type == VEHICLE_TYPE_TRICOPTER));
|
||||
}
|
||||
|
||||
bool is_rotary_wing(const struct vehicle_status_s *current_status)
|
||||
{
|
||||
return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER)
|
||||
|| (current_status->system_type == VEHICLE_TYPE_COAXIAL);
|
||||
}
|
||||
|
||||
int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd) {
|
||||
|
||||
|
||||
int ret = ERROR;
|
||||
|
||||
/* only check transition if the new state is actually different from the current one */
|
||||
|
|
@ -717,7 +733,6 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
|
|||
// }
|
||||
|
||||
|
||||
|
||||
///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
|
||||
//
|
||||
//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
|
||||
|
|
|
|||
|
|
@ -49,8 +49,13 @@
|
|||
#include <uORB/topics/actuator_safety.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
|
||||
|
||||
void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
|
||||
|
||||
bool is_multirotor(const struct vehicle_status_s *current_status);
|
||||
|
||||
bool is_rotary_wing(const struct vehicle_status_s *current_status);
|
||||
|
||||
//int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state);
|
||||
|
||||
void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
|
||||
|
|
|
|||
|
|
@ -659,7 +659,7 @@ uorb_receive_thread(void *arg)
|
|||
|
||||
/* handle the poll result */
|
||||
if (poll_ret == 0) {
|
||||
mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s");
|
||||
/* silent */
|
||||
|
||||
} else if (poll_ret < 0) {
|
||||
mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data");
|
||||
|
|
|
|||
|
|
@ -62,7 +62,7 @@ extern "C" {
|
|||
/*
|
||||
* Time that the ESCs need to initialize
|
||||
*/
|
||||
#define ESC_INIT_TIME_US 2000000
|
||||
#define ESC_INIT_TIME_US 1000000
|
||||
|
||||
/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
|
||||
#define ROLL 0
|
||||
|
|
@ -74,8 +74,9 @@ extern "C" {
|
|||
/* current servo arm/disarm state */
|
||||
static bool mixer_servos_armed = false;
|
||||
|
||||
static uint64_t time_armed;
|
||||
static bool init_complete = false;
|
||||
static uint64_t esc_init_time;
|
||||
static bool esc_init_active = false;
|
||||
static bool esc_init_done = false;
|
||||
|
||||
/* selected control values and count for mixing */
|
||||
enum mixer_source {
|
||||
|
|
@ -106,7 +107,7 @@ mixer_tick(void)
|
|||
if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
|
||||
isr_debug(1, "AP RX timeout");
|
||||
}
|
||||
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM);
|
||||
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK);
|
||||
r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
|
||||
|
||||
} else {
|
||||
|
|
@ -120,12 +121,11 @@ mixer_tick(void)
|
|||
* Decide which set of controls we're using.
|
||||
*/
|
||||
|
||||
/* do not mix if mixer is invalid or if RAW_PWM mode is on and FMU is good */
|
||||
/* do not mix if RAW_PWM mode is on and FMU is good */
|
||||
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
|
||||
!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
|
||||
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
|
||||
|
||||
/* don't actually mix anything - we already have raw PWM values or
|
||||
not a valid mixer. */
|
||||
/* don't actually mix anything - we already have raw PWM values */
|
||||
source = MIX_NONE;
|
||||
|
||||
} else {
|
||||
|
|
@ -175,28 +175,42 @@ mixer_tick(void)
|
|||
float outputs[IO_SERVO_COUNT];
|
||||
unsigned mixed;
|
||||
|
||||
/* after arming, some ESCs need an initalization period, count the time from here */
|
||||
if (mixer_servos_armed && !esc_init_done && !esc_init_active) {
|
||||
esc_init_time = hrt_absolute_time();
|
||||
esc_init_active = true;
|
||||
isr_debug(1, "start counting now");
|
||||
}
|
||||
|
||||
/* after waiting long enough for the ESC initialization, we can disable the ESC initialization phase */
|
||||
if (!esc_init_done && esc_init_active && mixer_servos_armed && (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US)) {
|
||||
esc_init_active = false;
|
||||
esc_init_done = true;
|
||||
isr_debug(1, "time is up");
|
||||
}
|
||||
|
||||
/* mix */
|
||||
mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
|
||||
|
||||
if (!init_complete && mixer_servos_armed && (hrt_elapsed_time(&time_armed) > ESC_INIT_TIME_US)) {
|
||||
init_complete = true;
|
||||
}
|
||||
|
||||
/* scale to PWM and update the servo outputs as required */
|
||||
for (unsigned i = 0; i < mixed; i++) {
|
||||
|
||||
/* save actuator values for FMU readback */
|
||||
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
|
||||
|
||||
/* scale to control range after init time */
|
||||
if (init_complete) {
|
||||
/* XXX maybe this check for an armed FMU could be achieved a little less messy */
|
||||
if (source == MIX_FMU && !(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)) {
|
||||
r_page_servos[i] = r_page_servo_failsafe[i];
|
||||
}
|
||||
/* during ESC initialization, use low PWM */
|
||||
else if (esc_init_active) {
|
||||
r_page_servos[i] = (outputs[i] * 600 + 1500);
|
||||
|
||||
/* afterwards use min and max values */
|
||||
} else {
|
||||
r_page_servos[i] = (outputs[i]
|
||||
* (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2
|
||||
+ (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2);
|
||||
|
||||
/* but use init range from 900 to 2100 right after arming */
|
||||
} else {
|
||||
r_page_servos[i] = (outputs[i] * 600 + 1500);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -214,26 +228,31 @@ mixer_tick(void)
|
|||
* here.
|
||||
*/
|
||||
bool should_arm = (
|
||||
/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
|
||||
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
|
||||
/* there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
|
||||
/* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) &&
|
||||
/* FMU is available or FMU is not available but override is an option */
|
||||
((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ))
|
||||
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
|
||||
/* and either FMU is armed */ ( ( (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
|
||||
/* and there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK))) ||
|
||||
|
||||
/* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) )
|
||||
);
|
||||
|
||||
|
||||
if (should_arm && !mixer_servos_armed) {
|
||||
/* need to arm, but not armed */
|
||||
up_pwm_servo_arm(true);
|
||||
mixer_servos_armed = true;
|
||||
time_armed = hrt_absolute_time();
|
||||
init_complete = false;
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
|
||||
isr_debug(5, "> armed");
|
||||
|
||||
} else if (!should_arm && mixer_servos_armed) {
|
||||
/* armed but need to disarm */
|
||||
up_pwm_servo_arm(false);
|
||||
mixer_servos_armed = false;
|
||||
init_complete = false;
|
||||
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
|
||||
isr_debug(5, "> disarmed");
|
||||
|
||||
esc_init_active = false;
|
||||
isr_debug(1, "disarming, and init aborted");
|
||||
}
|
||||
|
||||
if (mixer_servos_armed) {
|
||||
|
|
@ -288,9 +307,8 @@ static unsigned mixer_text_length = 0;
|
|||
void
|
||||
mixer_handle_text(const void *buffer, size_t length)
|
||||
{
|
||||
/* do not allow a mixer change while fully armed */
|
||||
if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
|
||||
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
|
||||
/* do not allow a mixer change while outputs armed */
|
||||
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -367,6 +385,7 @@ mixer_set_failsafe()
|
|||
* Check if a custom failsafe value has been written,
|
||||
* or if the mixer is not ok and bail out.
|
||||
*/
|
||||
|
||||
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ||
|
||||
!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
|
||||
return;
|
||||
|
|
|
|||
|
|
@ -75,10 +75,13 @@
|
|||
#define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f)
|
||||
#define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f))
|
||||
|
||||
#define PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC 2
|
||||
#define PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC 2
|
||||
|
||||
/* static configuration page */
|
||||
#define PX4IO_PAGE_CONFIG 0
|
||||
#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */
|
||||
#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */
|
||||
#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers */
|
||||
#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers */
|
||||
#define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */
|
||||
#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */
|
||||
#define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */
|
||||
|
|
@ -93,7 +96,7 @@
|
|||
#define PX4IO_P_STATUS_CPULOAD 1
|
||||
|
||||
#define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */
|
||||
#define PX4IO_P_STATUS_FLAGS_ARMED (1 << 0) /* arm-ok and locally armed */
|
||||
#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED (1 << 0) /* arm-ok and locally armed */
|
||||
#define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */
|
||||
#define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */
|
||||
#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */
|
||||
|
|
@ -105,6 +108,7 @@
|
|||
#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */
|
||||
#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */
|
||||
#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */
|
||||
#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */
|
||||
|
||||
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
|
||||
#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */
|
||||
|
|
|
|||
|
|
@ -57,8 +57,8 @@ static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t alt
|
|||
* Static configuration parameters.
|
||||
*/
|
||||
static const uint16_t r_page_config[] = {
|
||||
[PX4IO_P_CONFIG_PROTOCOL_VERSION] = 1, /* XXX hardcoded magic number */
|
||||
[PX4IO_P_CONFIG_SOFTWARE_VERSION] = 1, /* XXX hardcoded magic number */
|
||||
[PX4IO_P_CONFIG_PROTOCOL_VERSION] = PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC,
|
||||
[PX4IO_P_CONFIG_SOFTWARE_VERSION] = PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC,
|
||||
[PX4IO_P_CONFIG_BOOTLOADER_VERSION] = 3, /* XXX hardcoded magic number */
|
||||
[PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */
|
||||
[PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS,
|
||||
|
|
@ -146,7 +146,8 @@ volatile uint16_t r_page_setup[] =
|
|||
#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \
|
||||
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
|
||||
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
|
||||
PX4IO_P_SETUP_ARMING_IO_ARM_OK)
|
||||
PX4IO_P_SETUP_ARMING_IO_ARM_OK) | \
|
||||
PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM
|
||||
#define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1)
|
||||
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
|
||||
|
||||
|
|
@ -377,9 +378,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
* so that an in-air reset of FMU can not lead to a
|
||||
* lockup of the IO arming state.
|
||||
*/
|
||||
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
|
||||
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
|
||||
}
|
||||
|
||||
// XXX do not reset IO's safety state by FMU for now
|
||||
// if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
|
||||
// r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
|
||||
// }
|
||||
|
||||
r_setup_arming = value;
|
||||
|
||||
|
|
@ -427,9 +430,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
|
||||
case PX4IO_PAGE_RC_CONFIG: {
|
||||
|
||||
/* do not allow a RC config change while fully armed */
|
||||
if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
|
||||
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
|
||||
/* do not allow a RC config change while outputs armed */
|
||||
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -110,7 +110,7 @@ safety_check_button(void *arg)
|
|||
* state machine, keep ARM_COUNTER_THRESHOLD the same
|
||||
* length in all cases of the if/else struct below.
|
||||
*/
|
||||
if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
|
||||
if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
|
||||
(r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) {
|
||||
|
||||
if (counter < ARM_COUNTER_THRESHOLD) {
|
||||
|
|
@ -118,18 +118,18 @@ safety_check_button(void *arg)
|
|||
|
||||
} else if (counter == ARM_COUNTER_THRESHOLD) {
|
||||
/* switch to armed state */
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED;
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
|
||||
counter++;
|
||||
}
|
||||
|
||||
} else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
|
||||
} else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
|
||||
|
||||
if (counter < ARM_COUNTER_THRESHOLD) {
|
||||
counter++;
|
||||
|
||||
} else if (counter == ARM_COUNTER_THRESHOLD) {
|
||||
/* change to disarmed state and notify the FMU */
|
||||
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
|
||||
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
|
||||
counter++;
|
||||
}
|
||||
|
||||
|
|
@ -140,7 +140,7 @@ safety_check_button(void *arg)
|
|||
/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
|
||||
uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM;
|
||||
|
||||
if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) {
|
||||
if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
|
||||
if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) {
|
||||
pattern = LED_PATTERN_IO_FMU_ARMED;
|
||||
|
||||
|
|
|
|||
|
|
@ -168,3 +168,7 @@ ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s);
|
|||
|
||||
#include "topics/debug_key_value.h"
|
||||
ORB_DEFINE(debug_key_value, struct debug_key_value_s);
|
||||
|
||||
/* status of the system safety device */
|
||||
#include "topics/safety.h"
|
||||
ORB_DEFINE(safety, struct safety_s);
|
||||
|
|
|
|||
|
|
@ -0,0 +1,60 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file safety.h
|
||||
*
|
||||
* Status of an attached safety device
|
||||
*/
|
||||
|
||||
#ifndef TOPIC_SAFETY_H
|
||||
#define TOPIC_SAFETY_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include "../uORB.h"
|
||||
|
||||
enum SAFETY_STATUS {
|
||||
SAFETY_STATUS_NOT_PRESENT,
|
||||
SAFETY_STATUS_SAFE,
|
||||
SAFETY_STATUS_UNLOCKED
|
||||
};
|
||||
|
||||
struct safety_s {
|
||||
uint64_t timestamp; /**< output timestamp in us since system boot */
|
||||
enum SAFETY_STATUS status;
|
||||
};
|
||||
|
||||
/* actuator output sets; this list can be expanded as more drivers emerge */
|
||||
ORB_DECLARE(safety);
|
||||
|
||||
#endif /* TOPIC_SAFETY_H */
|
||||
|
|
@ -180,7 +180,8 @@ pwm_main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
/* iterate remaining arguments */
|
||||
unsigned channel = 0;
|
||||
unsigned nchannels = 0;
|
||||
unsigned channel[8] = {0};
|
||||
while (argc--) {
|
||||
const char *arg = argv[0];
|
||||
argv++;
|
||||
|
|
@ -204,13 +205,15 @@ pwm_main(int argc, char *argv[])
|
|||
}
|
||||
unsigned pwm_value = strtol(arg, &ep, 0);
|
||||
if (*ep == '\0') {
|
||||
ret = ioctl(fd, PWM_SERVO_SET(channel), pwm_value);
|
||||
if (ret != OK)
|
||||
err(1, "PWM_SERVO_SET(%d)", channel);
|
||||
channel++;
|
||||
if (nchannels > sizeof(channel) / sizeof(channel[0]))
|
||||
err(1, "too many pwm values (max %d)", sizeof(channel) / sizeof(channel[0]));
|
||||
|
||||
channel[nchannels] = pwm_value;
|
||||
nchannels++;
|
||||
|
||||
continue;
|
||||
}
|
||||
usage("unrecognised option");
|
||||
usage("unrecognized option");
|
||||
}
|
||||
|
||||
/* print verbose info */
|
||||
|
|
@ -250,5 +253,38 @@ pwm_main(int argc, char *argv[])
|
|||
}
|
||||
fflush(stdout);
|
||||
}
|
||||
|
||||
/* perform PWM output */
|
||||
if (nchannels) {
|
||||
|
||||
/* Open console directly to grab CTRL-C signal */
|
||||
int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
|
||||
if (!console)
|
||||
err(1, "failed opening console");
|
||||
|
||||
warnx("Press CTRL-C or 'c' to abort.");
|
||||
|
||||
while (1) {
|
||||
for (int i = 0; i < nchannels; i++) {
|
||||
ret = ioctl(fd, PWM_SERVO_SET(i), channel[i]);
|
||||
if (ret != OK)
|
||||
err(1, "PWM_SERVO_SET(%d)", i);
|
||||
}
|
||||
|
||||
/* abort on user request */
|
||||
char c;
|
||||
if (read(console, &c, 1) == 1) {
|
||||
if (c == 0x03 || c == 0x63 || c == 'q') {
|
||||
warnx("User abort\n");
|
||||
close(console);
|
||||
exit(0);
|
||||
}
|
||||
}
|
||||
|
||||
/* rate limit to ~ 20 Hz */
|
||||
usleep(50000);
|
||||
}
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
Loading…
Reference in New Issue