aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/modules/fw_pos_control_l1/mtecs/mTecs.cpp10
-rw-r--r--src/modules/fw_pos_control_l1/mtecs/mTecs.h1
-rw-r--r--src/modules/fw_pos_control_l1/mtecs/mTecs_params.c7
-rw-r--r--src/modules/navigator/mission_feasibility_checker.cpp9
-rw-r--r--src/modules/navigator/navigator_main.cpp2
-rw-r--r--src/modules/sdlog2/sdlog2.c26
-rw-r--r--src/modules/sdlog2/sdlog2_messages.h3
-rw-r--r--src/modules/uORB/topics/position_setpoint_triplet.h2
-rw-r--r--src/modules/uORB/topics/tecs_status.h1
9 files changed, 43 insertions, 18 deletions
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
index 2e37d166e..749f57a2b 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -59,6 +59,7 @@ mTecs::mTecs() :
_controlAltitude(this, "FPA", true),
_controlAirSpeed(this, "ACC"),
_flightPathAngleLowpass(this, "FPA_LP"),
+ _altitudeLowpass(this, "ALT_LP"),
_airspeedLowpass(this, "A_LP"),
_airspeedDerivative(this, "AD"),
_throttleSp(0.0f),
@@ -93,18 +94,23 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
/* time measurement */
updateTimeMeasurement();
+ /* Filter altitude */
+ float altitudeFiltered = _altitudeLowpass.update(altitude);
+
+
/* calculate flight path angle setpoint from altitude setpoint */
- float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude);
+ float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitudeFiltered);
/* Debug output */
if (_counter % 10 == 0) {
debug("***");
- debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp);
+ debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, altitude filtered %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)altitudeFiltered, (double)flightPathAngleSp);
}
/* Write part of the status message */
_status.altitudeSp = altitudeSp;
_status.altitude = altitude;
+ _status.altitudeFiltered = altitudeFiltered;
/* use flightpath angle setpoint for total energy control */
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
index efa89a5d3..ae6867d38 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
@@ -115,6 +115,7 @@ protected:
/* Other calculation Blocks */
control::BlockLowPass _flightPathAngleLowpass; /**< low pass filter for the flight path angle */
+ control::BlockLowPass _altitudeLowpass; /**< low pass filter for altitude */
control::BlockLowPass _airspeedLowpass; /**< low pass filter for airspeed */
control::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
index 5b9238780..4ca31fe20 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
@@ -175,6 +175,13 @@ PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f);
PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f);
/**
+ * Lowpass (cutoff freq.) for altitude
+ *
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_ALT_LP, 1.0f);
+
+/**
* Lowpass (cutoff freq.) for the flight path angle
*
* @group mTECS
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
index dd7f4c801..606521f20 100644
--- a/src/modules/navigator/mission_feasibility_checker.cpp
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -135,12 +135,15 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
}
}
- if (home_alt > missionitem.altitude) {
+ /* calculate the global waypoint altitude */
+ float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude;
+
+ if (home_alt > wp_alt) {
if (throw_error) {
- mavlink_log_info(_mavlink_fd, "Waypoint %d below home", i);
+ mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i);
return false;
} else {
- mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i);
+ mavlink_log_critical(_mavlink_fd, "Warning: Waypoint %d below home", i);
return true;
}
}
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 93cbfd7b1..331a9a728 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -458,7 +458,7 @@ void
Navigator::publish_position_setpoint_triplet()
{
/* update navigation state */
- /* TODO: set nav_state */
+ _pos_sp_triplet.nav_state = _vstatus.nav_state;
/* lazily publish the position setpoint triplet only once available */
if (_pos_sp_triplet_pub > 0) {
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index f534c0f4c..31861c3fc 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1432,17 +1432,20 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- GLOBAL POSITION SETPOINT --- */
if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) {
- log_msg.msg_type = LOG_GPSP_MSG;
- log_msg.body.log_GPSP.nav_state = 0; /* TODO: Fix this */
- log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d);
- log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d);
- log_msg.body.log_GPSP.alt = buf.triplet.current.alt;
- log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw;
- log_msg.body.log_GPSP.type = buf.triplet.current.type;
- log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius;
- log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction;
- log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min;
- LOGBUFFER_WRITE_AND_COUNT(GPSP);
+
+ if (buf.triplet.current.valid) {
+ log_msg.msg_type = LOG_GPSP_MSG;
+ log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state;
+ log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d);
+ log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d);
+ log_msg.body.log_GPSP.alt = buf.triplet.current.alt;
+ log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw;
+ log_msg.body.log_GPSP.type = buf.triplet.current.type;
+ log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius;
+ log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction;
+ log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min;
+ LOGBUFFER_WRITE_AND_COUNT(GPSP);
+ }
}
/* --- VICON POSITION --- */
@@ -1595,6 +1598,7 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.msg_type = LOG_TECS_MSG;
log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp;
log_msg.body.log_TECS.altitude = buf.tecs_status.altitude;
+ log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitudeFiltered;
log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp;
log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle;
log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered;
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index b14ef04cc..853a3811f 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -334,6 +334,7 @@ struct log_GS1B_s {
struct log_TECS_s {
float altitudeSp;
float altitude;
+ float altitudeFiltered;
float flightPathAngleSp;
float flightPathAngle;
float flightPathAngleFiltered;
@@ -454,7 +455,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
- LOG_FORMAT(TECS, "ffffffffffffffB", "AltSP,Alt,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
+ LOG_FORMAT(TECS, "fffffffffffffffB", "ASP,A,AF,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"),
/* system-level messages, ID >= 0x80 */
diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
index 673c0e491..4a1932180 100644
--- a/src/modules/uORB/topics/position_setpoint_triplet.h
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -95,6 +95,8 @@ struct position_setpoint_triplet_s
struct position_setpoint_s previous;
struct position_setpoint_s current;
struct position_setpoint_s next;
+
+ unsigned nav_state; /**< report the navigation state */
};
/**
diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h
index c4d0c1874..33055018c 100644
--- a/src/modules/uORB/topics/tecs_status.h
+++ b/src/modules/uORB/topics/tecs_status.h
@@ -66,6 +66,7 @@ struct tecs_status_s {
float altitudeSp;
float altitude;
+ float altitudeFiltered;
float flightPathAngleSp;
float flightPathAngle;
float flightPathAngleFiltered;
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-18 05:56:57 +0000