Merge branch 'release_v1.0.0' of github.com:PX4/Firmware into beta

This commit is contained in:
Lorenz Meier
2015-06-11 09:03:01 +02:00
8 changed files with 88 additions and 21 deletions
+29 -1
View File
@@ -1114,6 +1114,27 @@ PX4IO::task_main()
}
(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_REVERSE, pwm_invert_mask);
float trim_val;
param_t trim_parm;
trim_parm = param_find("TRIM_ROLL");
if (trim_parm != PARAM_INVALID) {
param_get(trim_parm, &trim_val);
(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_ROLL, FLOAT_TO_REG(trim_val));
}
trim_parm = param_find("TRIM_PITCH");
if (trim_parm != PARAM_INVALID) {
param_get(trim_parm, &trim_val);
(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_PITCH, FLOAT_TO_REG(trim_val));
}
trim_parm = param_find("TRIM_YAW");
if (trim_parm != PARAM_INVALID) {
param_get(trim_parm, &trim_val);
(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_YAW, FLOAT_TO_REG(trim_val));
}
}
}
@@ -2127,7 +2148,14 @@ PX4IO::print_status(bool extended_status)
for (unsigned i = 0; i < _max_actuators; i++) {
printf("%s", (pwm_invert_mask & (1 << i)) ? "x" : "_");
}
printf("]\n");
printf("]");
float trim_roll = REG_TO_FLOAT(io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_ROLL));
float trim_pitch = REG_TO_FLOAT(io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_PITCH));
float trim_yaw = REG_TO_FLOAT(io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_YAW));
printf(" trims: r: %8.4f p: %8.4f y: %8.4f\n",
(double)trim_roll, (double)trim_pitch, (double)trim_yaw);
uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT);
printf("%d raw R/C inputs", raw_inputs);
+11 -7
View File
@@ -56,7 +56,7 @@ static const int ERROR = -1;
int do_trim_calibration(int mavlink_fd)
{
int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
usleep(200000);
usleep(400000);
struct manual_control_setpoint_s sp;
bool changed;
orb_check(sub_man, &changed);
@@ -70,18 +70,22 @@ int do_trim_calibration(int mavlink_fd)
/* set parameters */
float p = sp.y;
param_set(param_find("TRIM_ROLL"), &p);
p = sp.x;
param_set(param_find("TRIM_PITCH"), &p);
int p1r = param_set(param_find("TRIM_ROLL"), &p);
/*
we explicitly swap sign here because the trim is added to the actuator controls
which are moving in an inverse sense to manual pitch inputs
*/
p = -sp.x;
int p2r = param_set(param_find("TRIM_PITCH"), &p);
p = sp.r;
param_set(param_find("TRIM_YAW"), &p);
int p3r = param_set(param_find("TRIM_YAW"), &p);
/* store to permanent storage */
/* auto-save */
int save_ret = param_save_default();
if (save_ret != 0) {
mavlink_log_critical(mavlink_fd, "TRIM: SAVE FAIL");
if (save_ret != 0 || p1r != 0 || p2r != 0 || p3r != 0) {
mavlink_log_critical(mavlink_fd, "TRIM: PARAM SET FAIL");
close(sub_man);
return ERROR;
}
@@ -71,6 +71,7 @@ static uint64_t IMUusec = 0;
static constexpr float rc = 10.0f; // RC time constant of 1st order LPF in seconds
static constexpr uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; ///< units: microseconds
static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we signal a total GPS failure
static constexpr unsigned MAG_SWITCH_HYSTERESIS = 10; ///< Ignore the first few mag failures (which amounts to a few milliseconds)
/**
* estimator app start / stop handling function
@@ -1488,7 +1489,7 @@ void AttitudePositionEstimatorEKF::pollData()
/* fail over to the 2nd mag if we know the first is down */
if (hrt_elapsed_time(&_sensor_combined.magnetometer_timestamp) < mag_timeout_us &&
_sensor_combined.magnetometer_errcount <= _sensor_combined.magnetometer1_errcount &&
_sensor_combined.magnetometer_errcount <= (_sensor_combined.magnetometer1_errcount + MAG_SWITCH_HYSTERESIS) &&
mag0.length() > 0.1f) {
_ekf->magData.x = mag0.x;
_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
@@ -1516,7 +1517,7 @@ void AttitudePositionEstimatorEKF::pollData()
}
if (last_mag_main != _mag_main) {
mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Switched from #%d to %d", last_mag_main, _mag_main);
mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Failover from unit %d to unit %d", last_mag_main, _mag_main);
}
}
@@ -861,8 +861,8 @@ FixedwingAttitudeControl::task_main()
if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) {
roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
} else {
roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
+ _parameters.rollsp_offset_rad;
roll_sp = (_manual.y * _parameters.man_roll_max)
+ _parameters.rollsp_offset_rad;
}
pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
@@ -883,8 +883,7 @@ FixedwingAttitudeControl::task_main()
/*
* Velocity should be controlled and manual is enabled.
*/
roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
+ _parameters.rollsp_offset_rad;
roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad;
pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
throttle_sp = _att_sp.thrust;
@@ -911,10 +910,8 @@ FixedwingAttitudeControl::task_main()
* the intended attitude setpoint. Later, after the rate control step the
* trim is added again to the control signal.
*/
roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
+ _parameters.rollsp_offset_rad;
pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch)
+ _parameters.pitchsp_offset_rad;
roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad;
pitch_sp = -(_manual.x * _parameters.man_pitch_max) + _parameters.pitchsp_offset_rad;
/* allow manual control of rudder deflection */
yaw_manual = _manual.r;
throttle_sp = _manual.z;
@@ -1081,9 +1078,9 @@ FixedwingAttitudeControl::task_main()
} else {
/* manual/direct control */
_actuators.control[0] = _manual.y;
_actuators.control[1] = -_manual.x;
_actuators.control[2] = _manual.r;
_actuators.control[0] = _manual.y + _parameters.trim_roll;
_actuators.control[1] = -_manual.x + _parameters.trim_pitch;
_actuators.control[2] = _manual.r + _parameters.trim_yaw;
_actuators.control[3] = _manual.z;
_actuators.control[4] = _manual.flaps;
}
+21
View File
@@ -311,6 +311,13 @@ mixer_callback(uintptr_t handle,
case MIX_FMU:
if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS ) {
control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
if (control_group == 0 && control_index == 0) {
control += REG_TO_FLOAT(r_setup_trim_roll);
} else if (control_group == 0 && control_index == 1) {
control += REG_TO_FLOAT(r_setup_trim_pitch);
} else if (control_group == 0 && control_index == 2) {
control += REG_TO_FLOAT(r_setup_trim_yaw);
}
break;
}
return -1;
@@ -318,6 +325,13 @@ mixer_callback(uintptr_t handle,
case MIX_OVERRIDE:
if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) {
control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
if (control_group == 0 && control_index == 0) {
control += REG_TO_FLOAT(r_setup_trim_roll);
} else if (control_group == 0 && control_index == 1) {
control += REG_TO_FLOAT(r_setup_trim_pitch);
} else if (control_group == 0 && control_index == 2) {
control += REG_TO_FLOAT(r_setup_trim_yaw);
}
break;
}
return -1;
@@ -326,6 +340,13 @@ mixer_callback(uintptr_t handle,
/* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */
if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) {
control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
if (control_group == 0 && control_index == 0) {
control += REG_TO_FLOAT(r_setup_trim_roll);
} else if (control_group == 0 && control_index == 1) {
control += REG_TO_FLOAT(r_setup_trim_pitch);
} else if (control_group == 0 && control_index == 2) {
control += REG_TO_FLOAT(r_setup_trim_yaw);
}
break;
} else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) {
control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
+3
View File
@@ -234,6 +234,9 @@ enum { /* DSM bind states */
#define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */
#define PX4IO_P_SETUP_PWM_REVERSE 15 /**< Bitmask to reverse PWM channels 1-8 */
#define PX4IO_P_SETUP_TRIM_ROLL 16 /**< Roll trim, in actuator units */
#define PX4IO_P_SETUP_TRIM_PITCH 17 /**< Pitch trim, in actuator units */
#define PX4IO_P_SETUP_TRIM_YAW 18 /**< Yaw trim, in actuator units */
/* autopilot control values, -10000..10000 */
#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */
+4
View File
@@ -113,6 +113,10 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
#define r_setup_pwm_reverse r_page_setup[PX4IO_P_SETUP_PWM_REVERSE]
#define r_setup_trim_roll r_page_setup[PX4IO_P_SETUP_TRIM_ROLL]
#define r_setup_trim_pitch r_page_setup[PX4IO_P_SETUP_TRIM_PITCH]
#define r_setup_trim_yaw r_page_setup[PX4IO_P_SETUP_TRIM_YAW]
#define r_control_values (&r_page_controls[0])
/*
+9
View File
@@ -174,6 +174,9 @@ volatile uint16_t r_page_setup[] =
[PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0,
[PX4IO_P_SETUP_PWM_REVERSE] = 0,
[PX4IO_P_SETUP_TRIM_ROLL] = 0,
[PX4IO_P_SETUP_TRIM_PITCH] = 0,
[PX4IO_P_SETUP_TRIM_YAW] = 0
};
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
@@ -627,6 +630,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
r_page_setup[PX4IO_P_SETUP_PWM_REVERSE] = value;
break;
case PX4IO_P_SETUP_TRIM_ROLL:
case PX4IO_P_SETUP_TRIM_PITCH:
case PX4IO_P_SETUP_TRIM_YAW:
r_page_setup[offset] = value;
break;
default:
return -1;
}