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

Merge branch 'master' of github.com:PX4/Firmware into battload

This commit is contained in:
Lorenz Meier 2014-07-20 16:21:59 +02:00
parent 5d2489880c 2685e3cfa4
commit e21cb64553
16 changed files with 295 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
mavlink/include/mavlink/v1.0

@ -1 +1 @@
Subproject commit d1ebe85eb6bb06d0078f3e0b8144adb131263628
Subproject commit 04b1ad5b284d5e916858ca9f928e93d97bbf6ad9

3
src/drivers/drv_pwm_output.h
View File

@ -202,6 +202,9 @@ ORB_DECLARE(output_pwm);
/** force safety switch off (to disable use of safety switch) */
#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23)
/** force failsafe mode (failsafe values are set immediately even if failsafe condition not met) */
#define PWM_SERVO_SET_FORCE_FAILSAFE _IOC(_PWM_SERVO_BASE, 24)
/*
*
*

23
src/drivers/px4io/px4io.cpp
View File

@ -1149,6 +1149,12 @@ PX4IO::io_set_arming_state()
clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
}
if (armed.force_failsafe) {
set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
} else {
clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
}
if (armed.ready_to_arm) {
set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
@ -2002,7 +2008,7 @@ PX4IO::print_status(bool extended_status)
((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "")
);
uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
printf("arming 0x%04x%s%s%s%s%s%s%s\n",
printf("arming 0x%04x%s%s%s%s%s%s%s%s\n",
arming,
((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"),
((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"),
@ -2010,7 +2016,9 @@ PX4IO::print_status(bool extended_status)
((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" : ""),
((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : ""));
((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : ""),
((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : "")
);
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
@ -2222,6 +2230,17 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
break;
case PWM_SERVO_SET_FORCE_FAILSAFE:
/* force failsafe mode instantly */
if (arg == 0) {
/* clear force failsafe flag */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE, 0);
} else {
/* set force failsafe flag */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE);
}
break;
case DSM_BIND_START:
/* only allow DSM2, DSM-X and DSM-X with more than 7 channels */

29
src/modules/commander/commander.cpp
View File

@ -546,24 +546,19 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
}
break;
#if 0
/* Flight termination */
case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
//XXX: to enable the parachute, a param needs to be set
//xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
if (armed_local->armed && cmd->param3 > 0.5 && parachute_enabled) {
transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
case VEHICLE_CMD_DO_FLIGHTTERMINATION: {
if (cmd->param1 > 0.5f) {
//XXX update state machine?
armed_local->force_failsafe = true;
warnx("forcing failsafe");
} else {
/* reject parachute depoyment not armed */
cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
armed_local->force_failsafe = false;
warnx("disabling failsafe");
}
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
}
break;
#endif
case VEHICLE_CMD_DO_SET_HOME: {
bool use_current = cmd->param1 > 0.5f;
@ -1421,8 +1416,12 @@ int commander_thread_main(int argc, char *argv[])
arming_state_changed = true;
} else if (arming_ret == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
mavlink_log_critical(mavlink_fd, "arming state transition denied");
/*
* the arming transition can be denied to a number of reasons:
* - pre-flight check failed (sensors not ok or not calibrated)
* - safety not disabled
* - system not in manual mode
*/
tune_negative(true);
}

26
src/modules/commander/state_machine_helper.cpp
View File

@ -110,8 +110,8 @@ arming_state_transition(struct vehicle_status_s *status, /// current
ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
transition_result_t ret = TRANSITION_DENIED;
arming_state_t current_arming_state = status->arming_state;
bool feedback_provided = false;
/* only check transition if the new state is actually different from the current one */
if (new_arming_state == current_arming_state) {
@ -156,13 +156,15 @@ arming_state_transition(struct vehicle_status_s *status, /// current
// Fail transition if pre-arm check fails
if (prearm_ret) {
/* the prearm check already prints the reject reason */
feedback_provided = true;
valid_transition = false;
// Fail transition if we need safety switch press
} else if (safety->safety_switch_available && !safety->safety_off) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch!");
mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!");
feedback_provided = true;
valid_transition = false;
}
@ -173,6 +175,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current
if (!status->condition_power_input_valid) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
feedback_provided = true;
valid_transition = false;
}
@ -182,6 +185,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current
(status->avionics_power_rail_voltage < 4.9f))) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
valid_transition = false;
}
}
@ -200,6 +204,8 @@ arming_state_transition(struct vehicle_status_s *status, /// current
/* Sensors need to be initialized for STANDBY state */
if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational.");
feedback_provided = true;
valid_transition = false;
}
@ -216,11 +222,14 @@ arming_state_transition(struct vehicle_status_s *status, /// current
}
if (ret == TRANSITION_DENIED) {
static const char *errMsg = "INVAL: %s - %s";
const char * str = "INVAL: %s - %s";
/* only print to console here by default as this is too technical to be useful during operation */
warnx(str, state_names[status->arming_state], state_names[new_arming_state]);
mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]);
/* print to MAVLink if we didn't provide any feedback yet */
if (!feedback_provided) {
mavlink_log_critical(mavlink_fd, str, state_names[status->arming_state], state_names[new_arming_state]);
}
}
return ret;
@ -648,8 +657,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE");
mavlink_log_critical(mavlink_fd, "hold still while arming");
mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE, hold still");
/* this is frickin' fatal */
failed = true;
goto system_eval;

11
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -212,9 +212,6 @@ private:
float throttle_land_max;
float heightrate_p;
float speedrate_p;
float land_slope_angle;
float land_H1_virt;
float land_flare_alt_relative;
@ -242,9 +239,6 @@ private:
param_t throttle_land_max;
param_t heightrate_p;
param_t speedrate_p;
param_t land_slope_angle;
param_t land_H1_virt;
param_t land_flare_alt_relative;
@ -494,9 +488,6 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max));
param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p));
param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p));
param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle));
param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));
@ -756,9 +747,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (!_was_pos_control_mode) {
/* reset integrators */
if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators();
_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
}
}
_was_pos_control_mode = true;

1
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
View File

@ -49,6 +49,7 @@ namespace fwPosctrl {
mTecs::mTecs() :
SuperBlock(NULL, "MT"),
/* Parameters */
_mTecsEnabled(this, "ENABLED"),
_airspeedMin(this, "FW_AIRSPD_MIN", false),
/* Publications */
_status(&getPublications(), ORB_ID(tecs_status)),

2
src/modules/fw_pos_control_l1/mtecs/mTecs.h
View File

@ -90,12 +90,14 @@ public:
void resetDerivatives(float airspeed);
/* Accessors */
bool getEnabled() { return _mTecsEnabled.get() > 0; }
float getThrottleSetpoint() { return _throttleSp; }
float getPitchSetpoint() { return _pitchSp; }
float airspeedLowpassUpdate(float input) { return _airspeedLowpass.update(input); }
protected:
/* parameters */
control::BlockParamInt _mTecsEnabled; /**< 1 if mTecs is enabled */
control::BlockParamFloat _airspeedMin; /**< minimal airspeed */
/* Publications */

11
src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
View File

@ -46,6 +46,17 @@
* Controller parameters, accessible via MAVLink
*/
/**
* mTECS enabled
*
* Set to 1 to enable mTECS
*
* @min 0
* @max 1
* @group mTECS
*/
PARAM_DEFINE_INT32(MT_ENABLED, 1);
/**
* Total Energy Rate Control Feedforward
* Maps the total energy rate setpoint to the throttle setpoint

174
src/modules/position_estimator_inav/position_estimator_inav_params.c
View File

@ -40,22 +40,196 @@
#include "position_estimator_inav_params.h"
/**
* Z axis weight for barometer
*
* Weight (cutoff frequency) for barometer altitude measurements.
*
* @min 0.0
* @max 10.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f);
/**
* Z axis weight for GPS
*
* Weight (cutoff frequency) for GPS altitude measurements. GPS altitude data is very noisy and should be used only as slow correction for baro offset.
*
* @min 0.0
* @max 10.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f);
/**
* Z axis weight for sonar
*
* Weight (cutoff frequency) for sonar measurements.
*
* @min 0.0
* @max 10.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f);
/**
* XY axis weight for GPS position
*
* Weight (cutoff frequency) for GPS position measurements.
*
* @min 0.0
* @max 10.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f);
/**
* XY axis weight for GPS velocity
*
* Weight (cutoff frequency) for GPS velocity measurements.
*
* @min 0.0
* @max 10.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f);
/**
* XY axis weight for optical flow
*
* Weight (cutoff frequency) for optical flow (velocity) measurements.
*
* @min 0.0
* @max 10.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f);
/**
* XY axis weight for resetting velocity
*
* When velocity sources lost slowly decrease estimated horizontal velocity with this weight.
*
* @min 0.0
* @max 10.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f);
/**
* XY axis weight factor for GPS when optical flow available
*
* When optical flow data available, multiply GPS weights (for position and velocity) by this factor.
*
* @min 0.0
* @max 1.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f);
/**
* Accelerometer bias estimation weight
*
* Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable.
*
* @min 0.0
* @max 0.1
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f);
/**
* Optical flow scale factor
*
* Factor to convert raw optical flow (in pixels) to radians [rad/px].
*
* @min 0.0
* @max 1.0
* @unit rad/px
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f);
/**
* Minimal acceptable optical flow quality
*
* 0 - lowest quality, 1 - best quality.
*
* @min 0.0
* @max 1.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f);
/**
* Weight for sonar filter
*
* Sonar filter detects spikes on sonar measurements and used to detect new surface level.
*
* @min 0.0
* @max 1.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f);
/**
* Sonar maximal error for new surface
*
* If sonar measurement error is larger than this value it skiped (spike) or accepted as new surface level (if offset is stable).
*
* @min 0.0
* @max 1.0
* @unit m
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
/**
* Land detector time
*
* Vehicle assumed landed if no altitude changes happened during this time on low throttle.
*
* @min 0.0
* @max 10.0
* @unit s
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f);
/**
* Land detector altitude dispersion threshold
*
* Dispersion threshold for triggering land detector.
*
* @min 0.0
* @max 10.0
* @unit m
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f);
/**
* Land detector throttle threshold
*
* Value should be lower than minimal hovering thrust. Half of it is good choice.
*
* @min 0.0
* @max 1.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f);
/**
* GPS delay
*
* GPS delay compensation
*
* @min 0.0
* @max 1.0
* @unit s
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f);
int parameters_init(struct position_estimator_inav_param_handles *h)

9
src/modules/px4iofirmware/mixer.cpp
View File

@ -111,7 +111,7 @@ mixer_tick(void)
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
}
/* default to failsafe mixing */
/* default to failsafe mixing - it will be forced below if flag is set */
source = MIX_FAILSAFE;
/*
@ -154,6 +154,13 @@ mixer_tick(void)
}
}
/*
* Check if we should force failsafe - and do it if we have to
*/
if (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) {
source = MIX_FAILSAFE;
}
/*
* Set failsafe status flag depending on mixing source
*/

5
src/modules/px4iofirmware/protocol.h
View File

@ -179,6 +179,7 @@
#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */
#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 6) /* Disable the IO-internal evaluation of the RC */
#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 7) /* If set, the system operates normally, but won't actuate any servos */
#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 8) /* If set, the system will always output the failsafe values */
#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */
@ -253,6 +254,10 @@ enum { /* DSM bind states */
/* PWM failsafe values - zero disables the output */
#define PX4IO_PAGE_FAILSAFE_PWM 55 /**< 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM failsafe values - zero disables the output */
#define PX4IO_PAGE_SENSORS 56 /**< Sensors connected to PX4IO */
#define PX4IO_P_SENSORS_ALTITUDE 0 /**< Altitude of an external sensor (HoTT or S.BUS2) */
/* Debug and test page - not used in normal operation */
#define PX4IO_PAGE_TEST 127
#define PX4IO_P_TEST_LED 0 /**< set the amber LED on/off */

3
src/modules/px4iofirmware/registers.c
View File

@ -189,7 +189,8 @@ volatile uint16_t r_page_setup[] =
PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
PX4IO_P_SETUP_ARMING_LOCKDOWN)
PX4IO_P_SETUP_ARMING_LOCKDOWN | \
PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE)
#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)

1
src/modules/uORB/topics/actuator_armed.h
View File

@ -56,6 +56,7 @@ struct actuator_armed_s {
bool armed; /**< Set to true if system is armed */
bool ready_to_arm; /**< Set to true if system is ready to be armed */
bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
bool force_failsafe; /**< Set to true if the actuators are forced to the failsafe position */
};
/**

1
src/modules/uORB/topics/vehicle_command.h
View File

@ -76,6 +76,7 @@ enum VEHICLE_CMD {
VEHICLE_CMD_DO_REPEAT_RELAY = 182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */
VEHICLE_CMD_DO_SET_SERVO = 183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */
VEHICLE_CMD_DO_REPEAT_SERVO = 184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */
VEHICLE_CMD_DO_FLIGHTTERMINATION=185, /* Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| */
VEHICLE_CMD_DO_CONTROL_VIDEO = 200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */
VEHICLE_CMD_DO_LAST = 240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */
VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */

22
src/systemcmds/pwm/pwm.c
View File

@ -635,8 +635,28 @@ pwm_main(int argc, char *argv[])
}
exit(0);
} else if (!strcmp(argv[1], "forcefail")) {
if (argc < 3) {
errx(1, "arg missing [on|off]");
} else {
if (!strcmp(argv[2], "on")) {
/* force failsafe */
ret = ioctl(fd, PWM_SERVO_SET_FORCE_FAILSAFE, 1);
} else {
/* force failsafe */
ret = ioctl(fd, PWM_SERVO_SET_FORCE_FAILSAFE, 0);
}
usage("specify arm|disarm|rate|failsafe|disarmed|min|max|test|info");
if (ret != OK) {
warnx("FAILED setting forcefail %s", argv[2]);
}
}
exit(0);
}
usage("specify arm|disarm|rate|failsafe|disarmed|min|max|test|info|forcefail");
return 0;
}