aboutsummaryrefslogtreecommitdiff
path: root/src/modules
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules')
-rw-r--r--src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp24
-rw-r--r--src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c1
-rw-r--r--src/modules/px4iofirmware/controls.c16
-rw-r--r--src/modules/px4iofirmware/protocol.h3
-rw-r--r--src/modules/px4iofirmware/registers.c4
-rw-r--r--src/modules/px4iofirmware/sbus.c2
-rw-r--r--src/modules/systemlib/ppm_decode.h1
7 files changed, 32 insertions, 19 deletions
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 3784337ac..ee0aca69e 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -49,6 +49,7 @@
* More details and acknowledgements in the referenced library headers.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <nuttx/config.h>
@@ -227,6 +228,7 @@ private:
float land_H1_virt;
float land_flare_alt_relative;
float land_thrust_lim_horizontal_distance;
+ float land_heading_hold_horizontal_distance;
} _parameters; /**< local copies of interesting parameters */
@@ -271,6 +273,7 @@ private:
param_t land_H1_virt;
param_t land_flare_alt_relative;
param_t land_thrust_lim_horizontal_distance;
+ param_t land_heading_hold_horizontal_distance;
} _parameter_handles; /**< handles for interesting parameters */
@@ -420,6 +423,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT");
_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
_parameter_handles.land_thrust_lim_horizontal_distance = param_find("FW_LND_TLDIST");
+ _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
_parameter_handles.time_const = param_find("FW_T_TIME_CONST");
_parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN");
@@ -508,6 +512,7 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));
param_get(_parameter_handles.land_thrust_lim_horizontal_distance, &(_parameters.land_thrust_lim_horizontal_distance));
+ param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance));
_l1_control.set_l1_damping(_parameters.l1_damping);
_l1_control.set_l1_period(_parameters.l1_period);
@@ -822,19 +827,18 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
/* switch to heading hold for the last meters, continue heading hold after */
float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), curr_wp.getX(), curr_wp.getY());
//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
- const float heading_hold_distance = 15.0f;
- if (wp_distance < heading_hold_distance || land_noreturn_horizontal) {
+ if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) {
/* heading hold, along the line connecting this and the last waypoint */
-
- // if (mission_item_triplet.previous_valid) {
- // target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY());
- // } else {
-
- if (!land_noreturn_horizontal) //set target_bearing in first occurrence
- target_bearing = _att.yaw;
- //}
+ if (!land_noreturn_horizontal) {//set target_bearing in first occurrence
+ if (mission_item_triplet.previous_valid) {
+ target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY());
+ } else {
+ target_bearing = _att.yaw;
+ }
+ mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold");
+ }
// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
index 10a0c00fc..9f46b5170 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
@@ -120,3 +120,4 @@ PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f);
PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f);
PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f);
PARAM_DEFINE_FLOAT(FW_LND_TLDIST, 30.0f);
+PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index f630b6f2a..541eed0e1 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -50,7 +50,7 @@
#define RC_CHANNEL_HIGH_THRESH 5000
#define RC_CHANNEL_LOW_THRESH -5000
-static bool ppm_input(uint16_t *values, uint16_t *num_values);
+static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
static perf_counter_t c_gather_dsm;
static perf_counter_t c_gather_sbus;
@@ -94,7 +94,7 @@ controls_tick() {
* other. Don't do that.
*/
- /* receive signal strenght indicator (RSSI). 0 = no connection, 1000: perfect connection */
+ /* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */
uint16_t rssi = 0;
perf_begin(c_gather_dsm);
@@ -108,7 +108,7 @@ controls_tick() {
else
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11;
- rssi = 1000;
+ rssi = 255;
}
perf_end(c_gather_dsm);
@@ -125,11 +125,11 @@ controls_tick() {
* disable the PPM decoder completely if we have S.bus signal.
*/
perf_begin(c_gather_ppm);
- bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count);
+ bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_status[PX4IO_P_STATUS_RC_DATA]);
if (ppm_updated) {
/* XXX sample RSSI properly here */
- rssi = 1000;
+ rssi = 255;
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
}
@@ -321,7 +321,7 @@ controls_tick() {
}
static bool
-ppm_input(uint16_t *values, uint16_t *num_values)
+ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
{
bool result = false;
@@ -345,6 +345,10 @@ ppm_input(uint16_t *values, uint16_t *num_values)
/* clear validity */
ppm_last_valid_decode = 0;
+ /* store PPM frame length */
+ if (num_values)
+ *frame_len = ppm_frame_length;
+
/* good if we got any channels */
result = (*num_values > 0);
}
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index c10f0167c..e5bef6eb3 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -128,7 +128,8 @@
#define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */
#define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */
#define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */
-#define PX4IO_P_STATUS_NRSSI 9 /* [2] Normalized RSSI value, 0: no reception, 1000: perfect reception */
+#define PX4IO_P_STATUS_NRSSI 9 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */
+#define PX4IO_P_STATUS_RC_DATA 10 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */
/* array of post-mix actuator outputs, -10000..10000 */
#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 916b893c4..7ef1aa309 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -89,7 +89,9 @@ uint16_t r_page_status[] = {
[PX4IO_P_STATUS_IBATT] = 0,
[PX4IO_P_STATUS_VSERVO] = 0,
[PX4IO_P_STATUS_VRSSI] = 0,
- [PX4IO_P_STATUS_PRSSI] = 0
+ [PX4IO_P_STATUS_PRSSI] = 0,
+ [PX4IO_P_STATUS_NRSSI] = 0,
+ [PX4IO_P_STATUS_RC_DATA] = 0
};
/**
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 6aca2bd11..11ccd7356 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -280,7 +280,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint
*rssi = 100; // XXX magic number indicating bad signal, but not a signal loss (yet)
}
- *rssi = 1000;
+ *rssi = 255;
return true;
}
diff --git a/src/modules/systemlib/ppm_decode.h b/src/modules/systemlib/ppm_decode.h
index 6c5e15345..5a1ad84da 100644
--- a/src/modules/systemlib/ppm_decode.h
+++ b/src/modules/systemlib/ppm_decode.h
@@ -57,6 +57,7 @@ __BEGIN_DECLS
* PPM decoder state
*/
__EXPORT extern uint16_t ppm_buffer[PPM_MAX_CHANNELS]; /**< decoded PPM channel values */
+__EXPORT extern uint16_t ppm_frame_length; /**< length of the decoded PPM frame (includes gap) */
__EXPORT extern unsigned ppm_decoded_channels; /**< count of decoded channels */
__EXPORT extern hrt_abstime ppm_last_valid_decode; /**< timestamp of the last valid decode */
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-13 04:46:05 +0000