diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index e76349642f..be3c2e1375 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -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); diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 0776894fb7..08261c9890 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -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; } diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 451e14d202..74d6dbadf0 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -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); } } diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index b7194461e6..c44f29a404 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -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; } diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 1bb25104fa..596064f15f 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -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)]); diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index a649920320..64ba0f93c4 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -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 */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index b38d374b9a..f0c7bde5dd 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -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]) /* diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 977bc59fff..6f1341c347 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -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; }