mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-23 06:06:37 +08:00
Merge branch 'release_v1.0.0' of github.com:PX4/Firmware into beta
This commit is contained in:
@@ -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);
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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)]);
|
||||
|
||||
@@ -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 */
|
||||
|
||||
@@ -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])
|
||||
|
||||
/*
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user