forked from Archive/PX4-Autopilot
Merge branch 'px4io-i2c' of github.com:PX4/Firmware into px4io-i2c-nuttx
This commit is contained in:
Lorenz Meier
commit
d677512981
|
|
@ -1260,6 +1260,8 @@ int commander_thread_main(int argc, char *argv[])
|
|||
param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms);
|
||||
|
||||
param_t _param_sys_type = param_find("MAV_TYPE");
|
||||
param_t _param_system_id = param_find("MAV_SYS_ID");
|
||||
param_t _param_component_id = param_find("MAV_COMP_ID");
|
||||
|
||||
/* welcome user */
|
||||
warnx("I am in command now!\n");
|
||||
|
|
@ -1444,6 +1446,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
/* parameters changed */
|
||||
orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed);
|
||||
|
||||
|
||||
/* update parameters */
|
||||
if (!current_status.flag_system_armed) {
|
||||
if (param_get(_param_sys_type, &(current_status.system_type)) != OK) {
|
||||
|
|
@ -1460,6 +1463,10 @@ int commander_thread_main(int argc, char *argv[])
|
|||
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));
|
||||
|
||||
} else {
|
||||
warnx("ARMED, rejecting sys type change\n");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -76,6 +76,7 @@
|
|||
#include <uORB/topics/actuator_controls_effective.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/rc_channels.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
|
|
@ -243,6 +244,24 @@ private:
|
|||
*/
|
||||
int mixer_send(const char *buf, unsigned buflen);
|
||||
|
||||
/**
|
||||
* Handle a status update from IO.
|
||||
*
|
||||
* Publish IO status information if necessary.
|
||||
*
|
||||
* @param status The status register
|
||||
*/
|
||||
int io_handle_status(uint16_t status);
|
||||
|
||||
/**
|
||||
* Handle an alarm update from IO.
|
||||
*
|
||||
* Publish IO alarm information if necessary.
|
||||
*
|
||||
* @param alarm The status register
|
||||
*/
|
||||
int io_handle_alarms(uint16_t alarms);
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
|
@ -332,17 +351,113 @@ PX4IO::init()
|
|||
if (_max_rc_input > RC_INPUT_MAX_CHANNELS)
|
||||
_max_rc_input = RC_INPUT_MAX_CHANNELS;
|
||||
|
||||
/* dis-arm IO before touching anything */
|
||||
io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING,
|
||||
PX4IO_P_SETUP_ARMING_ARM_OK |
|
||||
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK |
|
||||
PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0);
|
||||
/*
|
||||
* Check for IO flight state - if FMU was flagged to be in
|
||||
* armed state, FMU is recovering from an in-air reset.
|
||||
* Read back status and request the commander to arm
|
||||
* in this case.
|
||||
*/
|
||||
|
||||
/* publish RC config to IO */
|
||||
ret = io_set_rc_config();
|
||||
if (ret != OK) {
|
||||
log("failed to update RC input config");
|
||||
uint16_t reg;
|
||||
|
||||
/* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */
|
||||
ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, ®, sizeof(reg));
|
||||
if (ret != OK)
|
||||
return ret;
|
||||
|
||||
if (reg & PX4IO_P_SETUP_ARMING_ARM_OK) {
|
||||
|
||||
/* 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));
|
||||
/* fill with initial values, clear updated flag */
|
||||
vehicle_status_s status;
|
||||
uint64_t try_start_time = hrt_absolute_time();
|
||||
bool updated = false;
|
||||
|
||||
/* keep checking for an update, ensure we got a recent state,
|
||||
not something that was published a long time ago. */
|
||||
do {
|
||||
orb_check(vstatus_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
/* got data, copy and exit loop */
|
||||
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status);
|
||||
break;
|
||||
}
|
||||
|
||||
/* wait 10 ms */
|
||||
usleep(10000);
|
||||
|
||||
/* abort after 10s */
|
||||
if ((hrt_absolute_time() - try_start_time)/1000 > 10000) {
|
||||
log("failed to recover from in-air restart (1), aborting IO driver init.");
|
||||
return 1;
|
||||
}
|
||||
|
||||
} while (true);
|
||||
|
||||
/* send command to arm system via command API */
|
||||
vehicle_command_s cmd;
|
||||
/* request arming */
|
||||
cmd.param1 = 1.0f;
|
||||
cmd.param2 = 0;
|
||||
cmd.param3 = 0;
|
||||
cmd.param4 = 0;
|
||||
cmd.param5 = 0;
|
||||
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;
|
||||
/* ask to confirm command */
|
||||
cmd.confirmation = 1;
|
||||
|
||||
/* send command once */
|
||||
(void)orb_advertise(ORB_ID(vehicle_command), &cmd);
|
||||
|
||||
/* spin here until IO's state has propagated into the system */
|
||||
do {
|
||||
orb_check(vstatus_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status);
|
||||
}
|
||||
|
||||
/* wait 10 ms */
|
||||
usleep(10000);
|
||||
|
||||
/* abort after 10s */
|
||||
if ((hrt_absolute_time() - try_start_time)/1000 > 10000) {
|
||||
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);
|
||||
|
||||
/* regular boot, no in-air restart, init IO */
|
||||
} else {
|
||||
|
||||
|
||||
/* dis-arm IO before touching anything */
|
||||
io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING,
|
||||
PX4IO_P_SETUP_ARMING_ARM_OK |
|
||||
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK |
|
||||
PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0);
|
||||
|
||||
/* publish RC config to IO */
|
||||
ret = io_set_rc_config();
|
||||
if (ret != OK) {
|
||||
log("failed to update RC input config");
|
||||
return ret;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
|
||||
|
|
@ -646,6 +761,42 @@ PX4IO::io_set_rc_config()
|
|||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
PX4IO::io_handle_status(uint16_t status)
|
||||
{
|
||||
int ret = 1;
|
||||
/**
|
||||
* WARNING: This section handles in-air resets.
|
||||
*/
|
||||
|
||||
/* check for IO reset - force it back to armed if necessary */
|
||||
if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED)) {
|
||||
/* set the arming flag */
|
||||
ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED);
|
||||
|
||||
/* set new status */
|
||||
_status = status;
|
||||
_status &= PX4IO_P_STATUS_FLAGS_ARMED;
|
||||
} else {
|
||||
ret = 0;
|
||||
|
||||
/* set new status */
|
||||
_status = status;
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
PX4IO::io_handle_alarms(uint16_t alarms)
|
||||
{
|
||||
|
||||
/* XXX handle alarms */
|
||||
|
||||
|
||||
/* set new alarms state */
|
||||
_alarms = alarms;
|
||||
}
|
||||
|
||||
int
|
||||
PX4IO::io_get_status()
|
||||
{
|
||||
|
|
@ -657,12 +808,8 @@ PX4IO::io_get_status()
|
|||
if (ret != OK)
|
||||
return ret;
|
||||
|
||||
_status = regs[0];
|
||||
_alarms = regs[1];
|
||||
|
||||
/* XXX handle status */
|
||||
|
||||
/* XXX handle alarms */
|
||||
io_handle_status(regs[0]);
|
||||
io_handle_alarms(regs[1]);
|
||||
|
||||
/* only publish if battery has a valid minimum voltage */
|
||||
if (regs[2] > 3300) {
|
||||
|
|
|
|||
|
|
@ -64,9 +64,9 @@
|
|||
* packed.
|
||||
*/
|
||||
|
||||
#define PX4IO_CONTROL_CHANNELS 8
|
||||
#define PX4IO_INPUT_CHANNELS 12
|
||||
#define PX4IO_RELAY_CHANNELS 4
|
||||
#define PX4IO_CONTROL_CHANNELS 8
|
||||
#define PX4IO_INPUT_CHANNELS 12
|
||||
#define PX4IO_RELAY_CHANNELS 4
|
||||
|
||||
/* Per C, this is safe for all 2's complement systems */
|
||||
#define REG_TO_SIGNED(_reg) ((int16_t)(_reg))
|
||||
|
|
@ -76,7 +76,7 @@
|
|||
#define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f))
|
||||
|
||||
/* static configuration page */
|
||||
#define PX4IO_PAGE_CONFIG 0
|
||||
#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_BOOTLOADER_VERSION 2 /* get this how? */
|
||||
|
|
@ -88,11 +88,11 @@
|
|||
#define PX4IO_P_CONFIG_RELAY_COUNT 8 /* harcoded # of relay outputs */
|
||||
|
||||
/* dynamic status page */
|
||||
#define PX4IO_PAGE_STATUS 1
|
||||
#define PX4IO_PAGE_STATUS 1
|
||||
#define PX4IO_P_STATUS_FREEMEM 0
|
||||
#define PX4IO_P_STATUS_CPULOAD 1
|
||||
|
||||
#define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */
|
||||
#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_OVERRIDE (1 << 1) /* in manual override */
|
||||
#define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */
|
||||
|
|
@ -103,7 +103,7 @@
|
|||
#define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 7) /* raw PWM from FMU is bypassing the mixer */
|
||||
#define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */
|
||||
|
||||
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
|
||||
#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 */
|
||||
#define PX4IO_P_STATUS_ALARMS_TEMPERATURE (1 << 1) /* board temperature is high */
|
||||
#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* servo current limit was exceeded */
|
||||
|
|
@ -115,29 +115,29 @@
|
|||
#define PX4IO_P_STATUS_IBATT 5 /* battery current in cA */
|
||||
|
||||
/* array of post-mix actuator outputs, -10000..10000 */
|
||||
#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
|
||||
/* array of PWM servo output values, microseconds */
|
||||
#define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
#define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
|
||||
/* array of raw RC input values, microseconds */
|
||||
#define PX4IO_PAGE_RAW_RC_INPUT 4
|
||||
#define PX4IO_PAGE_RAW_RC_INPUT 4
|
||||
#define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */
|
||||
#define PX4IO_P_RAW_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT channels from here */
|
||||
|
||||
/* array of scaled RC input values, -10000..10000 */
|
||||
#define PX4IO_PAGE_RC_INPUT 5
|
||||
#define PX4IO_PAGE_RC_INPUT 5
|
||||
#define PX4IO_P_RC_VALID 0 /* bitmask of valid controls */
|
||||
#define PX4IO_P_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT controls from here */
|
||||
|
||||
/* array of raw ADC values */
|
||||
#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */
|
||||
#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */
|
||||
|
||||
/* setup page */
|
||||
#define PX4IO_PAGE_SETUP 100
|
||||
#define PX4IO_PAGE_SETUP 100
|
||||
#define PX4IO_P_SETUP_FEATURES 0
|
||||
|
||||
#define PX4IO_P_SETUP_ARMING 1 /* arming controls */
|
||||
#define PX4IO_P_SETUP_ARMING 1 /* arming controls */
|
||||
#define PX4IO_P_SETUP_ARMING_ARM_OK (1 << 0) /* OK to arm */
|
||||
#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */
|
||||
#define PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK (1 << 3) /* OK to perform position / vector control (= position lock) */
|
||||
|
|
@ -152,13 +152,13 @@
|
|||
#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
|
||||
|
||||
/* autopilot control values, -10000..10000 */
|
||||
#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */
|
||||
#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */
|
||||
|
||||
/* raw text load to the mixer parser - ignores offset */
|
||||
#define PX4IO_PAGE_MIXERLOAD 102
|
||||
#define PX4IO_PAGE_MIXERLOAD 102
|
||||
|
||||
/* R/C channel config */
|
||||
#define PX4IO_PAGE_RC_CONFIG 103 /* R/C input configuration */
|
||||
#define PX4IO_PAGE_RC_CONFIG 103 /* R/C input configuration */
|
||||
#define PX4IO_P_RC_CONFIG_MIN 0 /* lowest input value */
|
||||
#define PX4IO_P_RC_CONFIG_CENTER 1 /* center input value */
|
||||
#define PX4IO_P_RC_CONFIG_MAX 2 /* highest input value */
|
||||
|
|
@ -170,10 +170,10 @@
|
|||
#define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */
|
||||
|
||||
/* PWM output - overrides mixer */
|
||||
#define PX4IO_PAGE_DIRECT_PWM 104 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
#define PX4IO_PAGE_DIRECT_PWM 104 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
|
||||
/* PWM failsafe values - zero disables the output */
|
||||
#define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
#define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
|
||||
/**
|
||||
* As-needed mixer data upload.
|
||||
|
|
@ -187,8 +187,8 @@ struct px4io_mixdata {
|
|||
#define F2I_MIXER_MAGIC 0x6d74
|
||||
|
||||
uint8_t action;
|
||||
#define F2I_MIXER_ACTION_RESET 0
|
||||
#define F2I_MIXER_ACTION_APPEND 1
|
||||
#define F2I_MIXER_ACTION_RESET 0
|
||||
#define F2I_MIXER_ACTION_APPEND 1
|
||||
|
||||
char text[0]; /* actual text size may vary */
|
||||
};
|
||||
|
|
|
|||
|
|
@ -290,11 +290,20 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
case PX4IO_P_SETUP_ARMING:
|
||||
|
||||
value &= PX4IO_P_SETUP_ARMING_VALID;
|
||||
|
||||
/*
|
||||
* Update arming state - disarm if no longer OK.
|
||||
* This builds on the requirement that the FMU driver
|
||||
* asks about the FMU arming state on initialization,
|
||||
* 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_ARM_OK) && !(value & PX4IO_P_SETUP_ARMING_ARM_OK)) {
|
||||
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
|
||||
}
|
||||
|
||||
r_setup_arming = value;
|
||||
|
||||
/* update arming state - disarm if no longer OK */
|
||||
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && !(value & PX4IO_P_SETUP_ARMING_ARM_OK))
|
||||
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
|
||||
break;
|
||||
|
||||
case PX4IO_P_SETUP_PWM_RATES:
|
||||
|
|
|
|||
|
|
@ -156,7 +156,9 @@ struct vehicle_status_s
|
|||
enum VEHICLE_FLIGHT_MODE flight_mode; /**< current flight mode, as defined by mode switch */
|
||||
enum VEHICLE_MANUAL_CONTROL_MODE manual_control_mode; /**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */
|
||||
enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; /**< current stabilization mode */
|
||||
int32_t system_type; /**< system type, inspired by MAVLinks VEHICLE_TYPE enum */
|
||||
int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */
|
||||
int32_t system_id; /**< system id, inspired by MAVLink's system ID field */
|
||||
int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */
|
||||
|
||||
/* system flags - these represent the state predicates */
|
||||
|
||||
|
|
@ -171,7 +173,7 @@ struct vehicle_status_s
|
|||
bool flag_control_position_enabled; /**< true if position is controlled */
|
||||
|
||||
bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */
|
||||
bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */
|
||||
bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */
|
||||
bool flag_preflight_accel_calibration;
|
||||
|
||||
bool rc_signal_found_once;
|
||||
|
|
@ -209,6 +211,7 @@ struct vehicle_status_s
|
|||
bool flag_auto_flight_mode_ok; /**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */
|
||||
bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */
|
||||
bool flag_valid_launch_position; /**< indicates a valid launch position */
|
||||
bool flag_valid_home_position; /**< indicates a valid home position (a valid home position is not always a valid launch) */
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
|||
Loading…
Reference in New Issue