forked from Archive/PX4-Autopilot
Merge branch 'release_v1.0.0' into beta
This commit is contained in:
Lorenz Meier
commit
722a5f61f6
|
|
@ -4,9 +4,9 @@
|
|||
[submodule "NuttX"]
|
||||
path = NuttX
|
||||
url = git://github.com/PX4/NuttX.git
|
||||
[submodule "uavcan"]
|
||||
[submodule "libuavcan"]
|
||||
path = src/lib/uavcan
|
||||
url = git://github.com/pavel-kirienko/uavcan.git
|
||||
url = git://github.com/UAVCAN/libuavcan.git
|
||||
[submodule "Tools/genmsg"]
|
||||
path = Tools/genmsg
|
||||
url = https://github.com/ros/genmsg.git
|
||||
|
|
|
|||
|
|
@ -27,13 +27,13 @@ for the elevons.
|
|||
|
||||
M: 2
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 0 7500 7500 0 -10000 10000
|
||||
S: 0 1 8000 8000 0 -10000 10000
|
||||
S: 0 0 8500 8500 0 -10000 10000
|
||||
S: 0 1 9500 9500 0 -10000 10000
|
||||
|
||||
M: 2
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 0 7500 7500 0 -10000 10000
|
||||
S: 0 1 -8000 -8000 0 -10000 10000
|
||||
S: 0 0 8500 8500 0 -10000 10000
|
||||
S: 0 1 -9500 -9500 0 -10000 10000
|
||||
|
||||
Output 2
|
||||
--------
|
||||
|
|
@ -51,19 +51,3 @@ range. Inputs below zero are treated as zero.
|
|||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 3 0 20000 -10000 -10000 10000
|
||||
|
||||
Inputs to the mixer come from channel group 2 (payload), channels 0
|
||||
(bay servo 1), 1 (bay servo 2) and 3 (drop release).
|
||||
-----------------------------------------------------
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 2 0 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 2 1 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 2 2 -10000 -10000 0 -10000 10000
|
||||
|
|
|
|||
|
|
@ -147,6 +147,9 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage
|
|||
|
||||
#define INAIR_RESTART_HOLDOFF_INTERVAL 2000000
|
||||
|
||||
#define HIL_ID_MIN 1000
|
||||
#define HIL_ID_MAX 1999
|
||||
|
||||
enum MAV_MODE_FLAG {
|
||||
MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */
|
||||
MAV_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */
|
||||
|
|
@ -253,6 +256,15 @@ void *commander_low_prio_loop(void *arg);
|
|||
|
||||
void answer_command(struct vehicle_command_s &cmd, unsigned result);
|
||||
|
||||
/**
|
||||
* check whether autostart ID is in the reserved range for HIL setups
|
||||
*/
|
||||
bool is_hil_setup(int id);
|
||||
|
||||
bool is_hil_setup(int id) {
|
||||
return (id >= HIL_ID_MIN) && (id <= HIL_ID_MAX);
|
||||
}
|
||||
|
||||
|
||||
int commander_main(int argc, char *argv[])
|
||||
{
|
||||
|
|
@ -437,6 +449,12 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
|
|||
{
|
||||
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
|
||||
|
||||
// For HIL platforms, require that simulated sensors are connected
|
||||
if (is_hil_setup(autostart_id) && status.hil_state != vehicle_status_s::HIL_STATE_ON) {
|
||||
mavlink_and_console_log_critical(mavlink_fd_local, "HIL platform: Connect to simulator before arming");
|
||||
return TRANSITION_DENIED;
|
||||
}
|
||||
|
||||
// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
|
||||
// output appropriate error messages if the state cannot transition.
|
||||
arming_res = arming_state_transition(&status, &safety, arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, &armed,
|
||||
|
|
@ -481,10 +499,14 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
|
|||
transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd);
|
||||
|
||||
// Transition the arming state
|
||||
arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
|
||||
bool cmd_arm = base_mode & MAV_MODE_FLAG_SAFETY_ARMED;
|
||||
|
||||
arming_ret = arm_disarm(cmd_arm, mavlink_fd, "set mode command");
|
||||
|
||||
/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
|
||||
if (arming_ret == TRANSITION_CHANGED && hrt_absolute_time() > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) {
|
||||
if (cmd_arm && (arming_ret == TRANSITION_CHANGED) &&
|
||||
(hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL))) {
|
||||
|
||||
commander_set_home_position(*home_pub, *home, *local_pos, *global_pos);
|
||||
}
|
||||
|
||||
|
|
@ -1156,7 +1178,11 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
// Run preflight check
|
||||
param_get(_param_autostart_id, &autostart_id);
|
||||
if (autostart_id > 1999) {
|
||||
if (is_hil_setup(autostart_id)) {
|
||||
// HIL configuration selected: real sensors will be disabled
|
||||
status.condition_system_sensors_initialized = false;
|
||||
set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
|
||||
} else {
|
||||
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !status.rc_input_mode, !status.circuit_breaker_engaged_gpsfailure_check);
|
||||
if (!status.condition_system_sensors_initialized) {
|
||||
set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune
|
||||
|
|
@ -1164,11 +1190,6 @@ int commander_thread_main(int argc, char *argv[])
|
|||
else {
|
||||
set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
|
||||
}
|
||||
} else {
|
||||
// HIL configuration selected: real sensors will be disabled
|
||||
warnx("HIL configuration; autostart_id: %d, onboard IMU sensors disabled", autostart_id);
|
||||
status.condition_system_sensors_initialized = false;
|
||||
set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
|
||||
}
|
||||
|
||||
commander_boot_timestamp = hrt_absolute_time();
|
||||
|
|
@ -1339,14 +1360,14 @@ int commander_thread_main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
/* provide RC and sensor status feedback to the user */
|
||||
if (autostart_id > 1999) {
|
||||
(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed,
|
||||
!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check);
|
||||
} else {
|
||||
if (is_hil_setup(autostart_id)) {
|
||||
/* HIL configuration: check only RC input */
|
||||
warnx("new telemetry link connected: checking RC status");
|
||||
(void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false,
|
||||
!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false);
|
||||
} else {
|
||||
/* check sensors also */
|
||||
(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed,
|
||||
!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -2024,11 +2045,11 @@ int commander_thread_main(int argc, char *argv[])
|
|||
}
|
||||
}
|
||||
|
||||
//Get current timestamp
|
||||
/* Get current timestamp */
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
//First time home position update
|
||||
if (!status.condition_home_position_valid) {
|
||||
/* First time home position update - but only if disarmed */
|
||||
if (!status.condition_home_position_valid && !armed.armed) {
|
||||
commander_set_home_position(home_pub, _home, local_position, global_position);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -494,7 +494,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
|
|||
case vehicle_status_s::MAIN_STATE_ALTCTL:
|
||||
case vehicle_status_s::MAIN_STATE_POSCTL:
|
||||
/* require RC for all manual modes */
|
||||
if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) {
|
||||
if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed && !status->condition_landed) {
|
||||
status->failsafe = true;
|
||||
|
||||
if (status->condition_global_position_valid && status->condition_home_position_valid) {
|
||||
|
|
|
|||
|
|
@ -72,6 +72,8 @@ static constexpr float rc = 10.0f; // RC time constant of 1st order LPF in secon
|
|||
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)
|
||||
static constexpr unsigned GYRO_SWITCH_HYSTERESIS = 5; ///< Ignore the first few gyro failures (which amounts to a few milliseconds)
|
||||
static constexpr unsigned ACCEL_SWITCH_HYSTERESIS = 5; ///< Ignore the first few accel failures (which amounts to a few milliseconds)
|
||||
|
||||
/**
|
||||
* estimator app start / stop handling function
|
||||
|
|
@ -631,7 +633,10 @@ void AttitudePositionEstimatorEKF::task_main()
|
|||
// gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds
|
||||
// maintain heavily filtered values for both baro and gps altitude
|
||||
// Assume the filtered output should be identical for both sensors
|
||||
_baro_gps_offset = _baro_alt_filt - _gps_alt_filt;
|
||||
|
||||
if (_gpsIsGood) {
|
||||
_baro_gps_offset = _baro_alt_filt - _gps_alt_filt;
|
||||
}
|
||||
// if (hrt_elapsed_time(&_last_debug_print) >= 5e6) {
|
||||
// _last_debug_print = hrt_absolute_time();
|
||||
// perf_print_counter(_perf_baro);
|
||||
|
|
@ -658,6 +663,8 @@ void AttitudePositionEstimatorEKF::task_main()
|
|||
|
||||
_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
|
||||
|
||||
_filter_ref_offset = -_baro.altitude;
|
||||
|
||||
warnx("filter ref off: baro_alt: %8.4f", (double)_filter_ref_offset);
|
||||
|
||||
} else {
|
||||
|
|
@ -1219,7 +1226,7 @@ void AttitudePositionEstimatorEKF::pollData()
|
|||
if (isfinite(_sensor_combined.gyro_rad_s[0]) &&
|
||||
isfinite(_sensor_combined.gyro_rad_s[1]) &&
|
||||
isfinite(_sensor_combined.gyro_rad_s[2]) &&
|
||||
(_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) {
|
||||
(_sensor_combined.gyro_errcount <= (_sensor_combined.gyro1_errcount + GYRO_SWITCH_HYSTERESIS))) {
|
||||
|
||||
_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
|
||||
_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
|
||||
|
|
@ -1258,7 +1265,7 @@ void AttitudePositionEstimatorEKF::pollData()
|
|||
int last_accel_main = _accel_main;
|
||||
|
||||
/* fail over to the 2nd accel if we know the first is down */
|
||||
if (_sensor_combined.accelerometer_errcount <= _sensor_combined.accelerometer1_errcount) {
|
||||
if (_sensor_combined.accelerometer_errcount <= (_sensor_combined.accelerometer1_errcount + ACCEL_SWITCH_HYSTERESIS)) {
|
||||
_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
|
||||
_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
|
||||
_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
|
||||
|
|
|
|||
|
|
@ -230,9 +230,9 @@ Mission::update_offboard_mission()
|
|||
* however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
|
||||
dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
|
||||
|
||||
failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing,
|
||||
failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing,
|
||||
dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
|
||||
_navigator->get_home_position()->alt);
|
||||
_navigator->get_home_position()->alt, _navigator->home_position_valid());
|
||||
|
||||
} else {
|
||||
warnx("offboard mission update failed");
|
||||
|
|
@ -374,7 +374,7 @@ Mission::set_mission_items()
|
|||
} else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) {
|
||||
/* if mission type changed, notify */
|
||||
if (_mission_type != MISSION_TYPE_OFFBOARD) {
|
||||
mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running");
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running");
|
||||
}
|
||||
_mission_type = MISSION_TYPE_OFFBOARD;
|
||||
} else {
|
||||
|
|
|
|||
|
|
@ -63,42 +63,43 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab
|
|||
}
|
||||
|
||||
|
||||
bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
|
||||
bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
|
||||
{
|
||||
bool failed = false;
|
||||
/* Init if not done yet */
|
||||
init();
|
||||
|
||||
/* Open mavlink fd */
|
||||
if (_mavlink_fd < 0) {
|
||||
/* try to open the mavlink log device every once in a while */
|
||||
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
|
||||
}
|
||||
_mavlink_fd = mavlink_fd;
|
||||
|
||||
// check if all mission item commands are supported
|
||||
failed |= !checkMissionItemValidity(dm_current, nMissionItems);
|
||||
|
||||
|
||||
if (isRotarywing)
|
||||
failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt);
|
||||
else
|
||||
failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt);
|
||||
if (isRotarywing) {
|
||||
failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid);
|
||||
} else {
|
||||
failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid);
|
||||
}
|
||||
|
||||
if (!failed) {
|
||||
mavlink_log_info(_mavlink_fd, "Mission checked and ready.");
|
||||
}
|
||||
|
||||
return !failed;
|
||||
}
|
||||
|
||||
bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
|
||||
bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
|
||||
{
|
||||
|
||||
/* Perform checks and issue feedback to the user for all checks */
|
||||
bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
|
||||
bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt);
|
||||
bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid);
|
||||
|
||||
/* Mission is only marked as feasible if all checks return true */
|
||||
return (resGeofence && resHomeAltitude);
|
||||
}
|
||||
|
||||
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
|
||||
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
|
||||
{
|
||||
/* Update fixed wing navigation capabilites */
|
||||
updateNavigationCapabilities();
|
||||
|
|
@ -107,7 +108,7 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre
|
|||
/* Perform checks and issue feedback to the user for all checks */
|
||||
bool resLanding = checkFixedWingLanding(dm_current, nMissionItems);
|
||||
bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
|
||||
bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt);
|
||||
bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid);
|
||||
|
||||
/* Mission is only marked as feasible if all checks return true */
|
||||
return (resLanding && resGeofence && resHomeAltitude);
|
||||
|
|
@ -136,7 +137,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
|
|||
return true;
|
||||
}
|
||||
|
||||
bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error)
|
||||
bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool throw_error)
|
||||
{
|
||||
/* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */
|
||||
for (size_t i = 0; i < nMissionItems; i++) {
|
||||
|
|
@ -152,6 +153,12 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
|
|||
}
|
||||
}
|
||||
|
||||
/* always reject relative alt without home set */
|
||||
if (missionitem.altitude_is_relative && !home_valid) {
|
||||
mavlink_log_critical(_mavlink_fd, "Rejecting Mission: No home pos, WP %d uses rel alt", i);
|
||||
return false;
|
||||
}
|
||||
|
||||
/* calculate the global waypoint altitude */
|
||||
float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude;
|
||||
|
||||
|
|
@ -197,7 +204,6 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
|
|||
return false;
|
||||
}
|
||||
}
|
||||
mavlink_log_info(_mavlink_fd, "Mission ready.");
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -61,16 +61,16 @@ private:
|
|||
|
||||
/* Checks for all airframes */
|
||||
bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
|
||||
bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error = false);
|
||||
bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool throw_error = false);
|
||||
bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems);
|
||||
|
||||
/* Checks specific to fixedwing airframes */
|
||||
bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt);
|
||||
bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid);
|
||||
bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems);
|
||||
void updateNavigationCapabilities();
|
||||
|
||||
/* Checks specific to rotarywing airframes */
|
||||
bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt);
|
||||
bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid);
|
||||
public:
|
||||
|
||||
MissionFeasibilityChecker();
|
||||
|
|
@ -79,8 +79,8 @@ public:
|
|||
/*
|
||||
* Returns true if mission is feasible and false otherwise
|
||||
*/
|
||||
bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
|
||||
float home_alt);
|
||||
bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
|
||||
float home_alt, bool home_valid);
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -134,6 +134,7 @@ public:
|
|||
struct vehicle_gps_position_s* get_gps_position() { return &_gps_pos; }
|
||||
struct sensor_combined_s* get_sensor_combined() { return &_sensor_combined; }
|
||||
struct home_position_s* get_home_position() { return &_home_pos; }
|
||||
bool home_position_valid() { return _home_position_set; }
|
||||
struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
|
||||
struct mission_result_s* get_mission_result() { return &_mission_result; }
|
||||
struct geofence_result_s* get_geofence_result() { return &_geofence_result; }
|
||||
|
|
|
|||
|
|
@ -209,7 +209,10 @@ Navigator::home_position_update()
|
|||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
|
||||
_home_position_set = true;
|
||||
|
||||
if (_home_pos.timestamp > 0) {
|
||||
_home_position_set = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -311,13 +311,6 @@ 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;
|
||||
|
|
@ -360,6 +353,13 @@ mixer_callback(uintptr_t handle,
|
|||
return -1;
|
||||
}
|
||||
|
||||
/* limit output */
|
||||
if (control > 1.0f) {
|
||||
control = 1.0f;
|
||||
} else if (control < -1.0f) {
|
||||
control = -1.0f;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue