mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-27 23:23:33 +08:00
Merge branch 'release_v1.0.0' into beta
This commit is contained in:
@@ -6,5 +6,30 @@
|
|||||||
|
|
||||||
sh /etc/init.d/rc.fw_defaults
|
sh /etc/init.d/rc.fw_defaults
|
||||||
|
|
||||||
|
if [ $AUTOCNF == yes ]
|
||||||
|
then
|
||||||
|
param set BAT_N_CELLS 3
|
||||||
|
param set FW_AIRSPD_MAX 20
|
||||||
|
param set FW_AIRSPD_MIN 12
|
||||||
|
param set FW_AIRSPD_TRIM 14
|
||||||
|
param set FW_ATT_TC 0.3
|
||||||
|
param set FW_L1_DAMPING 0.74
|
||||||
|
param set FW_L1_PERIOD 16
|
||||||
|
param set FW_LND_ANG 15
|
||||||
|
param set FW_LND_FLALT 5
|
||||||
|
param set FW_LND_HHDIST 15
|
||||||
|
param set FW_LND_HVIRT 13
|
||||||
|
param set FW_LND_TLALT 5
|
||||||
|
param set FW_THR_LND_MAX 0
|
||||||
|
param set FW_PR_FF 0.35
|
||||||
|
param set FW_PR_I 0.1
|
||||||
|
param set FW_PR_IMAX 0.4
|
||||||
|
param set FW_PR_P 0.2
|
||||||
|
param set FW_RR_FF 0.6
|
||||||
|
param set FW_RR_I 0.1
|
||||||
|
param set FW_RR_IMAX 0.2
|
||||||
|
param set FW_RR_P 0.3
|
||||||
|
fi
|
||||||
|
|
||||||
set HIL yes
|
set HIL yes
|
||||||
set MIXER AERT
|
set MIXER AERT
|
||||||
|
|||||||
@@ -7,4 +7,12 @@
|
|||||||
|
|
||||||
sh /etc/init.d/rc.fw_defaults
|
sh /etc/init.d/rc.fw_defaults
|
||||||
|
|
||||||
|
if [ $AUTOCNF == yes ]
|
||||||
|
then
|
||||||
|
param set NAV_LOITER_RAD 150
|
||||||
|
param set FW_AIRSPD_MAX 30
|
||||||
|
param set FW_AIRSPD_MIN 13
|
||||||
|
param set FW_AIRSPD_TRIM 15
|
||||||
|
fi
|
||||||
|
|
||||||
set MIXER FX79
|
set MIXER FX79
|
||||||
|
|||||||
@@ -9,26 +9,29 @@ sh /etc/init.d/rc.fw_defaults
|
|||||||
|
|
||||||
if [ $AUTOCNF == yes ]
|
if [ $AUTOCNF == yes ]
|
||||||
then
|
then
|
||||||
|
param set FW_AIRSPD_MAX 20
|
||||||
# TODO: these are the X5 default parameters, update them to the caipi
|
param set FW_AIRSPD_MIN 12
|
||||||
|
param set FW_AIRSPD_TRIM 16
|
||||||
param set FW_AIRSPD_MIN 15
|
|
||||||
param set FW_AIRSPD_TRIM 20
|
|
||||||
param set FW_AIRSPD_MAX 40
|
|
||||||
param set FW_ATT_TC 0.3
|
param set FW_ATT_TC 0.3
|
||||||
param set FW_L1_DAMPING 0.74
|
param set FW_L1_DAMPING 0.74
|
||||||
param set FW_L1_PERIOD 15
|
param set FW_L1_PERIOD 16
|
||||||
param set FW_PR_FF 0.3
|
param set FW_LND_ANG 15
|
||||||
param set FW_PR_I 0
|
param set FW_LND_FLALT 5
|
||||||
param set FW_PR_IMAX 0.2
|
param set FW_LND_HHDIST 15
|
||||||
param set FW_PR_P 0.03
|
param set FW_LND_HVIRT 13
|
||||||
param set FW_P_ROLLFF 0
|
param set FW_LND_TLALT 5
|
||||||
param set FW_RR_FF 0.3
|
param set FW_THR_LND_MAX 0
|
||||||
param set FW_RR_I 0
|
param set FW_PR_FF 0.35
|
||||||
|
param set FW_PR_I 0.01
|
||||||
|
param set FW_PR_IMAX 0.4
|
||||||
|
param set FW_PR_P 0.08
|
||||||
|
param set FW_RR_FF 0.6
|
||||||
|
param set FW_RR_I 0.01
|
||||||
param set FW_RR_IMAX 0.2
|
param set FW_RR_IMAX 0.2
|
||||||
param set FW_RR_P 0.03
|
param set FW_RR_P 0.04
|
||||||
param set FW_R_LIM 60
|
|
||||||
param set FW_R_RMAX 0
|
|
||||||
fi
|
fi
|
||||||
|
|
||||||
set MIXER Q
|
set MIXER caipi
|
||||||
|
# Provide ESC a constant 1000 us pulse
|
||||||
|
set PWM_OUT 4
|
||||||
|
set PWM_DISARMED 1000
|
||||||
|
|||||||
@@ -0,0 +1,51 @@
|
|||||||
|
Delta-wing mixer
|
||||||
|
===========================
|
||||||
|
|
||||||
|
Designed for TBS Caipirinha
|
||||||
|
|
||||||
|
This file defines mixers suitable for controlling a delta wing aircraft using
|
||||||
|
PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU
|
||||||
|
servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is
|
||||||
|
assumed to be unused.
|
||||||
|
|
||||||
|
Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
|
||||||
|
(roll), 1 (pitch) and 3 (thrust).
|
||||||
|
|
||||||
|
See the README for more information on the scaler format.
|
||||||
|
|
||||||
|
Elevon mixers
|
||||||
|
-------------
|
||||||
|
Three scalers total (output, roll, pitch).
|
||||||
|
|
||||||
|
On the assumption that the two elevon servos are physically reversed, the pitch
|
||||||
|
input is inverted between the two servos.
|
||||||
|
|
||||||
|
The scaling factor for roll inputs is adjusted to implement differential travel
|
||||||
|
for the elevons.
|
||||||
|
|
||||||
|
M: 2
|
||||||
|
O: 10000 10000 -3000 -10000 10000
|
||||||
|
S: 0 0 -9000 -9000 0 -10000 10000
|
||||||
|
S: 0 1 8000 8000 0 -10000 10000
|
||||||
|
|
||||||
|
M: 2
|
||||||
|
O: 10000 10000 3000 -10000 10000
|
||||||
|
S: 0 0 -9000 -9000 0 -10000 10000
|
||||||
|
S: 0 1 -8000 -8000 0 -10000 10000
|
||||||
|
|
||||||
|
Output 2
|
||||||
|
--------
|
||||||
|
This mixer is empty.
|
||||||
|
|
||||||
|
Z:
|
||||||
|
|
||||||
|
Motor speed mixer
|
||||||
|
-----------------
|
||||||
|
Two scalers total (output, thrust).
|
||||||
|
|
||||||
|
This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
|
||||||
|
range. Inputs below zero are treated as zero.
|
||||||
|
|
||||||
|
M: 1
|
||||||
|
O: 10000 10000 0 -10000 10000
|
||||||
|
S: 0 3 0 20000 -10000 -10000 10000
|
||||||
@@ -0,0 +1,5 @@
|
|||||||
|
uint64 timestamp # timestamp this topic was emitted
|
||||||
|
float32 turn_distance # the optimal distance to a waypoint to switch to the next
|
||||||
|
float32 landing_horizontal_slope_displacement
|
||||||
|
float32 landing_slope_angle_rad
|
||||||
|
float32 landing_flare_length
|
||||||
@@ -1009,7 +1009,7 @@ PX4IO::task_main()
|
|||||||
orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd);
|
orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd);
|
||||||
|
|
||||||
// Check for a DSM pairing command
|
// Check for a DSM pairing command
|
||||||
if (((int)cmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) && ((int)cmd.param1 == 0)) {
|
if (((uint32_t)cmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) && ((int)cmd.param1 == 0)) {
|
||||||
dsm_bind_ioctl((int)cmd.param2);
|
dsm_bind_ioctl((int)cmd.param2);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -314,10 +314,16 @@ TRONE::probe()
|
|||||||
uint8_t who_am_i=0;
|
uint8_t who_am_i=0;
|
||||||
|
|
||||||
const uint8_t cmd = TRONE_WHO_AM_I_REG;
|
const uint8_t cmd = TRONE_WHO_AM_I_REG;
|
||||||
if (transfer(&cmd, 1, &who_am_i, 1) == OK && who_am_i == TRONE_WHO_AM_I_REG_VAL) {
|
|
||||||
// it is responding correctly to a WHO_AM_I
|
// set the I2C bus address
|
||||||
return measure();
|
set_address(TRONE_BASEADDR);
|
||||||
}
|
|
||||||
|
// can't use a single transfer as TROne need a bit of time for internal processing
|
||||||
|
if (transfer(&cmd, 1, nullptr, 0) == OK) {
|
||||||
|
if ( transfer(nullptr, 0, &who_am_i, 1) == OK && who_am_i == TRONE_WHO_AM_I_REG_VAL){
|
||||||
|
return measure();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
debug("WHO_AM_I byte mismatch 0x%02x should be 0x%02x\n",
|
debug("WHO_AM_I byte mismatch 0x%02x should be 0x%02x\n",
|
||||||
(unsigned)who_am_i,
|
(unsigned)who_am_i,
|
||||||
|
|||||||
@@ -1582,6 +1582,8 @@ int commander_thread_main(int argc, char *argv[])
|
|||||||
low_battery_voltage_actions_done = true;
|
low_battery_voltage_actions_done = true;
|
||||||
if (armed.armed) {
|
if (armed.armed) {
|
||||||
mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
|
mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
|
||||||
|
} else {
|
||||||
|
mavlink_log_critical(mavlink_fd, "LOW BATTERY, TAKEOFF DISCOURAGED");
|
||||||
}
|
}
|
||||||
status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW;
|
status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW;
|
||||||
status_changed = true;
|
status_changed = true;
|
||||||
@@ -2054,11 +2056,13 @@ int commander_thread_main(int argc, char *argv[])
|
|||||||
set_tune(TONE_ARMING_WARNING_TUNE);
|
set_tune(TONE_ARMING_WARNING_TUNE);
|
||||||
arm_tune_played = true;
|
arm_tune_played = true;
|
||||||
|
|
||||||
} else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
|
} else if ((status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
|
||||||
|
status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
|
||||||
/* play tune on battery critical */
|
/* play tune on battery critical */
|
||||||
set_tune(TONE_BATTERY_WARNING_FAST_TUNE);
|
set_tune(TONE_BATTERY_WARNING_FAST_TUNE);
|
||||||
|
|
||||||
} else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status.failsafe) {
|
} else if ((status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
|
||||||
|
(status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status.failsafe)) {
|
||||||
/* play tune on battery warning or failsafe */
|
/* play tune on battery warning or failsafe */
|
||||||
set_tune(TONE_BATTERY_WARNING_SLOW_TUNE);
|
set_tune(TONE_BATTERY_WARNING_SLOW_TUNE);
|
||||||
|
|
||||||
|
|||||||
@@ -948,7 +948,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
|
|||||||
* which makes the corner positions unreachable.
|
* which makes the corner positions unreachable.
|
||||||
* scale yaw up and clip it to overcome this.
|
* scale yaw up and clip it to overcome this.
|
||||||
*/
|
*/
|
||||||
rc.values[2] = man.r / 1.2f + 1500;
|
rc.values[2] = man.r / 1.1f + 1500;
|
||||||
if (rc.values[2] > 2000) {
|
if (rc.values[2] > 2000) {
|
||||||
rc.values[2] = 2000;
|
rc.values[2] = 2000;
|
||||||
} else if (rc.values[2] < 1000) {
|
} else if (rc.values[2] < 1000) {
|
||||||
@@ -956,7 +956,12 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
|
|||||||
}
|
}
|
||||||
|
|
||||||
/* throttle */
|
/* throttle */
|
||||||
rc.values[3] = man.z + 1000;
|
rc.values[3] = man.z / 0.9f + 1000;
|
||||||
|
if (rc.values[3] > 2000) {
|
||||||
|
rc.values[3] = 2000;
|
||||||
|
} else if (rc.values[3] < 1000) {
|
||||||
|
rc.values[3] = 1000;
|
||||||
|
}
|
||||||
|
|
||||||
rc.values[4] = decode_switch_pos_n(man.buttons, 0) * 1000 + 1000;
|
rc.values[4] = decode_switch_pos_n(man.buttons, 0) * 1000 + 1000;
|
||||||
rc.values[5] = decode_switch_pos_n(man.buttons, 1) * 1000 + 1000;
|
rc.values[5] = decode_switch_pos_n(man.buttons, 1) * 1000 + 1000;
|
||||||
|
|||||||
@@ -81,7 +81,7 @@ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1);
|
|||||||
* @max 1000
|
* @max 1000
|
||||||
* @group Mission
|
* @group Mission
|
||||||
*/
|
*/
|
||||||
PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500);
|
PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 900);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Altitude setpoint mode
|
* Altitude setpoint mode
|
||||||
@@ -94,7 +94,7 @@ PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500);
|
|||||||
* @max 1
|
* @max 1
|
||||||
* @group Mission
|
* @group Mission
|
||||||
*/
|
*/
|
||||||
PARAM_DEFINE_INT32(MIS_ALTMODE, 0);
|
PARAM_DEFINE_INT32(MIS_ALTMODE, 1);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Multirotor only. Yaw setpoint mode.
|
* Multirotor only. Yaw setpoint mode.
|
||||||
|
|||||||
@@ -64,6 +64,10 @@ static int _dsm_fd;
|
|||||||
|
|
||||||
static uint16_t rc_value_override = 0;
|
static uint16_t rc_value_override = 0;
|
||||||
|
|
||||||
|
#ifdef ADC_RSSI
|
||||||
|
static unsigned _rssi_adc_counts = 0;
|
||||||
|
#endif
|
||||||
|
|
||||||
bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated)
|
bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated)
|
||||||
{
|
{
|
||||||
perf_begin(c_gather_dsm);
|
perf_begin(c_gather_dsm);
|
||||||
@@ -71,17 +75,22 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
|
|||||||
uint8_t n_bytes = 0;
|
uint8_t n_bytes = 0;
|
||||||
uint8_t *bytes;
|
uint8_t *bytes;
|
||||||
*dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes);
|
*dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes);
|
||||||
|
|
||||||
if (*dsm_updated) {
|
if (*dsm_updated) {
|
||||||
r_raw_rc_count = temp_count & 0x7fff;
|
r_raw_rc_count = temp_count & 0x7fff;
|
||||||
if (temp_count & 0x8000)
|
|
||||||
|
if (temp_count & 0x8000) {
|
||||||
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
|
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
|
||||||
else
|
|
||||||
|
} else {
|
||||||
r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
|
r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
|
||||||
|
}
|
||||||
|
|
||||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
|
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
|
||||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
|
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
perf_end(c_gather_dsm);
|
perf_end(c_gather_dsm);
|
||||||
|
|
||||||
/* get data from FD and attempt to parse with DSM and ST24 libs */
|
/* get data from FD and attempt to parse with DSM and ST24 libs */
|
||||||
@@ -94,7 +103,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
|
|||||||
/* set updated flag if one complete packet was parsed */
|
/* set updated flag if one complete packet was parsed */
|
||||||
st24_rssi = RC_INPUT_RSSI_MAX;
|
st24_rssi = RC_INPUT_RSSI_MAX;
|
||||||
*st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count,
|
*st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count,
|
||||||
&st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
|
&st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
|
||||||
}
|
}
|
||||||
|
|
||||||
if (*st24_updated) {
|
if (*st24_updated) {
|
||||||
@@ -121,7 +130,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
|
|||||||
/* set updated flag if one complete packet was parsed */
|
/* set updated flag if one complete packet was parsed */
|
||||||
sumd_rssi = RC_INPUT_RSSI_MAX;
|
sumd_rssi = RC_INPUT_RSSI_MAX;
|
||||||
*sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count,
|
*sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count,
|
||||||
&sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
|
&sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
|
||||||
}
|
}
|
||||||
|
|
||||||
if (*sumd_updated) {
|
if (*sumd_updated) {
|
||||||
@@ -170,7 +179,8 @@ controls_init(void)
|
|||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
controls_tick() {
|
controls_tick()
|
||||||
|
{
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Gather R/C control inputs from supported sources.
|
* Gather R/C control inputs from supported sources.
|
||||||
@@ -184,19 +194,24 @@ controls_tick() {
|
|||||||
uint16_t rssi = 0;
|
uint16_t rssi = 0;
|
||||||
|
|
||||||
#ifdef ADC_RSSI
|
#ifdef ADC_RSSI
|
||||||
|
|
||||||
if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) {
|
if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) {
|
||||||
unsigned counts = adc_measure(ADC_RSSI);
|
unsigned counts = adc_measure(ADC_RSSI);
|
||||||
if (counts != 0xffff) {
|
|
||||||
/* use 1:1 scaling on 3.3V ADC input */
|
|
||||||
unsigned mV = counts * 3300 / 4096;
|
|
||||||
|
|
||||||
/* scale to 0..253 and lowpass */
|
if (counts != 0xffff) {
|
||||||
rssi = (rssi * 0.99f) + ((mV / (3300 / RC_INPUT_RSSI_MAX)) * 0.01f);
|
/* low pass*/
|
||||||
|
_rssi_adc_counts = (_rssi_adc_counts * 0.998f) + (counts * 0.002f);
|
||||||
|
/* use 1:1 scaling on 3.3V, 12-Bit ADC input */
|
||||||
|
unsigned mV = _rssi_adc_counts * 3300 / 4095;
|
||||||
|
/* scale to 0..100 (RC_INPUT_RSSI_MAX == 100) */
|
||||||
|
rssi = (mV * RC_INPUT_RSSI_MAX / 3300);
|
||||||
|
|
||||||
if (rssi > RC_INPUT_RSSI_MAX) {
|
if (rssi > RC_INPUT_RSSI_MAX) {
|
||||||
rssi = RC_INPUT_RSSI_MAX;
|
rssi = RC_INPUT_RSSI_MAX;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
/* zero RSSI if signal is lost */
|
/* zero RSSI if signal is lost */
|
||||||
@@ -207,21 +222,26 @@ controls_tick() {
|
|||||||
perf_begin(c_gather_dsm);
|
perf_begin(c_gather_dsm);
|
||||||
bool dsm_updated, st24_updated, sumd_updated;
|
bool dsm_updated, st24_updated, sumd_updated;
|
||||||
(void)dsm_port_input(&rssi, &dsm_updated, &st24_updated, &sumd_updated);
|
(void)dsm_port_input(&rssi, &dsm_updated, &st24_updated, &sumd_updated);
|
||||||
|
|
||||||
if (dsm_updated) {
|
if (dsm_updated) {
|
||||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
|
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (st24_updated) {
|
if (st24_updated) {
|
||||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24;
|
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (sumd_updated) {
|
if (sumd_updated) {
|
||||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD;
|
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD;
|
||||||
}
|
}
|
||||||
|
|
||||||
perf_end(c_gather_dsm);
|
perf_end(c_gather_dsm);
|
||||||
|
|
||||||
perf_begin(c_gather_sbus);
|
perf_begin(c_gather_sbus);
|
||||||
|
|
||||||
bool sbus_failsafe, sbus_frame_drop;
|
bool sbus_failsafe, sbus_frame_drop;
|
||||||
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS);
|
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop,
|
||||||
|
PX4IO_RC_INPUT_CHANNELS);
|
||||||
|
|
||||||
if (sbus_updated) {
|
if (sbus_updated) {
|
||||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
|
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
|
||||||
@@ -231,17 +251,19 @@ controls_tick() {
|
|||||||
if (sbus_frame_drop) {
|
if (sbus_frame_drop) {
|
||||||
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
|
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
|
||||||
sbus_rssi = RC_INPUT_RSSI_MAX / 2;
|
sbus_rssi = RC_INPUT_RSSI_MAX / 2;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
|
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (sbus_failsafe) {
|
if (sbus_failsafe) {
|
||||||
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
|
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
|
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* set RSSI to an emulated value if ADC RSSI is off */
|
/* set RSSI to an emulated value if ADC RSSI is off */
|
||||||
if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
|
if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
|
||||||
rssi = sbus_rssi;
|
rssi = sbus_rssi;
|
||||||
}
|
}
|
||||||
@@ -257,17 +279,20 @@ controls_tick() {
|
|||||||
*/
|
*/
|
||||||
perf_begin(c_gather_ppm);
|
perf_begin(c_gather_ppm);
|
||||||
bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);
|
bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);
|
||||||
|
|
||||||
if (ppm_updated) {
|
if (ppm_updated) {
|
||||||
|
|
||||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
|
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
|
||||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
|
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
|
||||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
|
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
|
||||||
}
|
}
|
||||||
|
|
||||||
perf_end(c_gather_ppm);
|
perf_end(c_gather_ppm);
|
||||||
|
|
||||||
/* limit number of channels to allowable data size */
|
/* limit number of channels to allowable data size */
|
||||||
if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
|
if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) {
|
||||||
r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
|
r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
|
||||||
|
}
|
||||||
|
|
||||||
/* store RSSI */
|
/* store RSSI */
|
||||||
r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi;
|
r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi;
|
||||||
@@ -308,10 +333,13 @@ controls_tick() {
|
|||||||
/*
|
/*
|
||||||
* 1) Constrain to min/max values, as later processing depends on bounds.
|
* 1) Constrain to min/max values, as later processing depends on bounds.
|
||||||
*/
|
*/
|
||||||
if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
|
if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) {
|
||||||
raw = conf[PX4IO_P_RC_CONFIG_MIN];
|
raw = conf[PX4IO_P_RC_CONFIG_MIN];
|
||||||
if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
|
}
|
||||||
|
|
||||||
|
if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) {
|
||||||
raw = conf[PX4IO_P_RC_CONFIG_MAX];
|
raw = conf[PX4IO_P_RC_CONFIG_MAX];
|
||||||
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* 2) Scale around the mid point differently for lower and upper range.
|
* 2) Scale around the mid point differently for lower and upper range.
|
||||||
@@ -330,10 +358,12 @@ controls_tick() {
|
|||||||
* DO NOT REMOVE OR ALTER STEP 1!
|
* DO NOT REMOVE OR ALTER STEP 1!
|
||||||
*/
|
*/
|
||||||
if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
|
if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
|
||||||
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
|
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(
|
||||||
|
conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
|
||||||
|
|
||||||
} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
|
} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
|
||||||
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
|
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(
|
||||||
|
conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
/* in the configured dead zone, output zero */
|
/* in the configured dead zone, output zero */
|
||||||
@@ -347,6 +377,7 @@ controls_tick() {
|
|||||||
|
|
||||||
/* and update the scaled/mapped version */
|
/* and update the scaled/mapped version */
|
||||||
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
|
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
|
||||||
|
|
||||||
if (mapped < PX4IO_CONTROL_CHANNELS) {
|
if (mapped < PX4IO_CONTROL_CHANNELS) {
|
||||||
|
|
||||||
/* invert channel if pitch - pulling the lever down means pitching up by convention */
|
/* invert channel if pitch - pulling the lever down means pitching up by convention */
|
||||||
@@ -360,6 +391,7 @@ controls_tick() {
|
|||||||
if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) ||
|
if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) ||
|
||||||
((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) {
|
((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) {
|
||||||
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
|
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
|
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
|
||||||
}
|
}
|
||||||
@@ -389,6 +421,7 @@ controls_tick() {
|
|||||||
/* if we have enough channels (5) to control the vehicle, the mapping is ok */
|
/* if we have enough channels (5) to control the vehicle, the mapping is ok */
|
||||||
if (assigned_channels > 4) {
|
if (assigned_channels > 4) {
|
||||||
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
|
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
|
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
|
||||||
}
|
}
|
||||||
@@ -408,9 +441,9 @@ controls_tick() {
|
|||||||
|
|
||||||
/* clear the input-kind flags here */
|
/* clear the input-kind flags here */
|
||||||
r_status_flags &= ~(
|
r_status_flags &= ~(
|
||||||
PX4IO_P_STATUS_FLAGS_RC_PPM |
|
PX4IO_P_STATUS_FLAGS_RC_PPM |
|
||||||
PX4IO_P_STATUS_FLAGS_RC_DSM |
|
PX4IO_P_STATUS_FLAGS_RC_DSM |
|
||||||
PX4IO_P_STATUS_FLAGS_RC_SBUS);
|
PX4IO_P_STATUS_FLAGS_RC_SBUS);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -427,8 +460,8 @@ controls_tick() {
|
|||||||
if (rc_input_lost) {
|
if (rc_input_lost) {
|
||||||
/* Clear the RC input status flag, clear manual override flag */
|
/* Clear the RC input status flag, clear manual override flag */
|
||||||
r_status_flags &= ~(
|
r_status_flags &= ~(
|
||||||
PX4IO_P_STATUS_FLAGS_OVERRIDE |
|
PX4IO_P_STATUS_FLAGS_OVERRIDE |
|
||||||
PX4IO_P_STATUS_FLAGS_RC_OK);
|
PX4IO_P_STATUS_FLAGS_RC_OK);
|
||||||
|
|
||||||
/* flag raw RC as lost */
|
/* flag raw RC as lost */
|
||||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_RC_OK);
|
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_RC_OK);
|
||||||
@@ -451,9 +484,9 @@ controls_tick() {
|
|||||||
* Override is enabled if either the hardcoded channel / value combination
|
* Override is enabled if either the hardcoded channel / value combination
|
||||||
* is selected, or the AP has requested it.
|
* is selected, or the AP has requested it.
|
||||||
*/
|
*/
|
||||||
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) &&
|
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) &&
|
||||||
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
|
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
|
||||||
!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
|
!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
|
||||||
|
|
||||||
bool override = false;
|
bool override = false;
|
||||||
|
|
||||||
@@ -464,8 +497,9 @@ controls_tick() {
|
|||||||
* requested override.
|
* requested override.
|
||||||
*
|
*
|
||||||
*/
|
*/
|
||||||
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH))
|
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) {
|
||||||
override = true;
|
override = true;
|
||||||
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
if the FMU is dead then enable override if we have a
|
if the FMU is dead then enable override if we have a
|
||||||
@@ -473,20 +507,23 @@ controls_tick() {
|
|||||||
*/
|
*/
|
||||||
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
|
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
|
||||||
(r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) &&
|
(r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) &&
|
||||||
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
|
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
|
||||||
override = true;
|
override = true;
|
||||||
|
}
|
||||||
|
|
||||||
if (override) {
|
if (override) {
|
||||||
|
|
||||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
|
r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
|
||||||
|
|
||||||
/* mix new RC input control values to servos */
|
/* mix new RC input control values to servos */
|
||||||
if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated)
|
if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated) {
|
||||||
mixer_tick();
|
mixer_tick();
|
||||||
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
|
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
|
||||||
}
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
|
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
|
||||||
}
|
}
|
||||||
@@ -512,8 +549,10 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
|
|||||||
|
|
||||||
/* PPM data exists, copy it */
|
/* PPM data exists, copy it */
|
||||||
*num_values = ppm_decoded_channels;
|
*num_values = ppm_decoded_channels;
|
||||||
if (*num_values > PX4IO_RC_INPUT_CHANNELS)
|
|
||||||
|
if (*num_values > PX4IO_RC_INPUT_CHANNELS) {
|
||||||
*num_values = PX4IO_RC_INPUT_CHANNELS;
|
*num_values = PX4IO_RC_INPUT_CHANNELS;
|
||||||
|
}
|
||||||
|
|
||||||
for (unsigned i = 0; ((i < *num_values) && (i < PPM_MAX_CHANNELS)); i++) {
|
for (unsigned i = 0; ((i < *num_values) && (i < PPM_MAX_CHANNELS)); i++) {
|
||||||
values[i] = ppm_buffer[i];
|
values[i] = ppm_buffer[i];
|
||||||
|
|||||||
@@ -1,70 +0,0 @@
|
|||||||
/****************************************************************************
|
|
||||||
*
|
|
||||||
* Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
|
|
||||||
*
|
|
||||||
* Redistribution and use in source and binary forms, with or without
|
|
||||||
* modification, are permitted provided that the following conditions
|
|
||||||
* are met:
|
|
||||||
*
|
|
||||||
* 1. Redistributions of source code must retain the above copyright
|
|
||||||
* notice, this list of conditions and the following disclaimer.
|
|
||||||
* 2. Redistributions in binary form must reproduce the above copyright
|
|
||||||
* notice, this list of conditions and the following disclaimer in
|
|
||||||
* the documentation and/or other materials provided with the
|
|
||||||
* distribution.
|
|
||||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
|
||||||
* used to endorse or promote products derived from this software
|
|
||||||
* without specific prior written permission.
|
|
||||||
*
|
|
||||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
||||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
||||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
||||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
||||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
||||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
||||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
|
||||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
|
||||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
||||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
||||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
||||||
* POSSIBILITY OF SUCH DAMAGE.
|
|
||||||
*
|
|
||||||
****************************************************************************/
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @file navigation_capabilities.h
|
|
||||||
*
|
|
||||||
* Definition of navigation capabilities uORB topic.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#ifndef TOPIC_NAVIGATION_CAPABILITIES_H_
|
|
||||||
#define TOPIC_NAVIGATION_CAPABILITIES_H_
|
|
||||||
|
|
||||||
#include "../uORB.h"
|
|
||||||
#include <stdint.h>
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @addtogroup topics
|
|
||||||
* @{
|
|
||||||
*/
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Airspeed
|
|
||||||
*/
|
|
||||||
struct navigation_capabilities_s {
|
|
||||||
float turn_distance; /**< the optimal distance to a waypoint to switch to the next */
|
|
||||||
|
|
||||||
/* Landing parameters: see fw_pos_control_l1/landingslope.h */
|
|
||||||
float landing_horizontal_slope_displacement;
|
|
||||||
float landing_slope_angle_rad;
|
|
||||||
float landing_flare_length;
|
|
||||||
};
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @}
|
|
||||||
*/
|
|
||||||
|
|
||||||
/* register this as object request broker structure */
|
|
||||||
ORB_DECLARE(navigation_capabilities);
|
|
||||||
|
|
||||||
#endif
|
|
||||||
Reference in New Issue
Block a user