dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'px4io-i2c' of github.com:PX4/Firmware into px4io-i2c-nuttx

This commit is contained in:
Lorenz Meier 2013-02-17 16:33:59 +01:00
parent 5d92e0195f 038037d676
commit d677512981
5 changed files with 207 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
apps/commander/commander.c
View File

@ -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, &param_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");
}

159
apps/drivers/px4io/px4io.cpp
View File

@ -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,6 +351,100 @@ PX4IO::init()
if (_max_rc_input > RC_INPUT_MAX_CHANNELS)
_max_rc_input = RC_INPUT_MAX_CHANNELS;
/*
* 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.
*/
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, &reg, 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 |
@ -345,6 +458,8 @@ PX4IO::init()
return ret;
}
}
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)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) {

15
apps/px4io/registers.c
View File

@ -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:

5
apps/uORB/topics/vehicle_status.h
View File

@ -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 */
@ -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) */
};
/**