aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
authorAnton Babushkin <anton.babushkin@me.com>2014-01-23 22:41:26 +0100
committerAnton Babushkin <anton.babushkin@me.com>2014-01-23 22:41:26 +0100
commit1cffa9d2f77f788f8446e0aceec60b7676a0a65f (patch)
tree6ed00299b9b3f2e096f13a4eb615ffd84e4c2564 /src
parent6acb8fa66f38d20af57b8c45cc7878257abb24d2 (diff)
downloadpx4-firmware-1cffa9d2f77f788f8446e0aceec60b7676a0a65f.tar.gz
px4-firmware-1cffa9d2f77f788f8446e0aceec60b7676a0a65f.tar.bz2
px4-firmware-1cffa9d2f77f788f8446e0aceec60b7676a0a65f.zip
position_setpoint_triplet refactoring finished
Diffstat (limited to 'src')
-rw-r--r--src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp26
-rw-r--r--src/modules/mavlink/orb_listener.c15
-rw-r--r--src/modules/mavlink/orb_topics.h3
-rw-r--r--src/modules/mavlink_onboard/orb_topics.h2
-rw-r--r--src/modules/mc_pos_control/mc_pos_control_main.cpp8
-rw-r--r--src/modules/navigator/navigator_main.cpp21
-rw-r--r--src/modules/sdlog2/sdlog2.c13
-rw-r--r--src/modules/sdlog2/sdlog2_messages.h9
-rw-r--r--src/modules/uORB/topics/position_setpoint_triplet.h9
9 files changed, 51 insertions, 55 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 3889012c9..f3d688646 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
@@ -828,7 +828,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
- _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
+ _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, math::radians(_parameters.pitch_limit_min),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
@@ -845,7 +845,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* heading hold, along the line connecting this and the last waypoint */
if (!land_noreturn_horizontal) {//set target_bearing in first occurrence
- if (pos_sp_triplet.previous_valid) {
+ if (pos_sp_triplet.previous.valid) {
target_bearing = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
} else {
target_bearing = _att.yaw;
@@ -877,7 +877,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
// /* do not go down too early */
// if (wp_distance > 50.0f) {
-// altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt;
+// altitude_error = (_global_triplet.current.alt + 25.0f) - _global_pos.alt;
// }
/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
// XXX this could make a great param
@@ -888,12 +888,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
float airspeed_approach = 1.3f * _parameters.airspeed_min;
float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length;
- float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement);
- float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement);
+ float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement);
+ float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement);
- if ( (_global_pos.alt < _pos_sp_triplet.current.altitude + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out
+ if ( (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out
/* land with minimal speed */
@@ -912,12 +912,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
- float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _pos_sp_triplet.current.altitude);
+ float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _pos_sp_triplet.current.alt);
/* avoid climbout */
if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground)
{
- flare_curve_alt = pos_sp_triplet.current.altitude;
+ flare_curve_alt = pos_sp_triplet.current.alt;
land_stayonground = true;
}
@@ -1009,7 +1009,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (altitude_error > 15.0f) {
/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
- _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.altitude, calculate_target_airspeed(1.3f * _parameters.airspeed_min),
+ _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min),
_airspeed.indicated_airspeed_m_s, eas2tas,
true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
@@ -1020,7 +1020,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else {
- _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
+ _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, math::radians(_parameters.pitch_limit_min),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
@@ -1042,7 +1042,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_loiter_hold = false;
/* reset land state */
- if (pos_sp_triplet.current.nav_cmd != NAV_CMD_LAND) {
+ if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) {
land_noreturn_horizontal = false;
land_noreturn_vertical = false;
land_stayonground = false;
@@ -1051,7 +1051,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
/* reset takeoff/launch state */
- if (pos_sp_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) {
+ if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) {
launch_detected = false;
usePreTakeoffThrust = false;
}
@@ -1283,7 +1283,7 @@ FixedwingPositionControl::task_main()
}
/* XXX check if radius makes sense here */
- float turn_distance = _l1_control.switch_distance(_pos_sp_triplet.current.acceptance_radius);
+ float turn_distance = _l1_control.switch_distance(100.0f);
/* lazily publish navigation capabilities */
if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) {
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index e491911d3..0b8ac6d3d 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -411,22 +411,17 @@ void
l_global_position_setpoint(const struct listener *l)
{
struct position_setpoint_triplet_s triplet;
- orb_copy(ORB_ID(mission_item_triplet), mavlink_subs.triplet_sub, &triplet);
+ orb_copy(ORB_ID(position_setpoint_triplet), mavlink_subs.triplet_sub, &triplet);
- uint8_t coordinate_frame = MAV_FRAME_GLOBAL;
-
- if (!triplet.current_valid)
+ if (!triplet.current.valid)
return;
- if (triplet.current.altitude_is_relative)
- coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
-
if (gcs_link)
mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0,
- coordinate_frame,
+ MAV_FRAME_GLOBAL,
(int32_t)(triplet.current.lat * 1e7d),
(int32_t)(triplet.current.lon * 1e7d),
- (int32_t)(triplet.current.altitude * 1e3f),
+ (int32_t)(triplet.current.alt * 1e3f),
(int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f));
}
@@ -804,7 +799,7 @@ uorb_receive_start(void)
orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */
/* --- GLOBAL SETPOINT VALUE --- */
- mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet));
+ mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
orb_set_interval(mavlink_subs.triplet_sub, 2000); /* 0.5 Hz updates */
/* --- LOCAL SETPOINT VALUE --- */
diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h
index 4d428406a..30ba348cf 100644
--- a/src/modules/mavlink/orb_topics.h
+++ b/src/modules/mavlink/orb_topics.h
@@ -51,9 +51,8 @@
#include <uORB/topics/offboard_control_setpoint.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
-#include <uORB/topics/mission_item_triplet.h>
+#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_vicon_position.h>
-#include <uORB/topics/mission_item_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h
index 86bfa26f2..bbc9f6e66 100644
--- a/src/modules/mavlink_onboard/orb_topics.h
+++ b/src/modules/mavlink_onboard/orb_topics.h
@@ -50,7 +50,7 @@
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_vicon_position.h>
-#include <uORB/topics/mission_item_triplet.h>
+#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/optical_flow.h>
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index bc20a4f47..d3e39e3a0 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -65,7 +65,7 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
-#include <uORB/topics/mission_item_triplet.h>
+#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
@@ -117,7 +117,7 @@ private:
int _manual_sub; /**< notification of manual control updates */
int _arming_sub; /**< arming status of outputs */
int _local_pos_sub; /**< vehicle local position */
- int _pos_sp_triplet_sub; /**< mission item triplet */
+ int _pos_sp_triplet_sub; /**< position setpoint triplet */
orb_advert_t _local_pos_sp_pub; /**< local position setpoint publication */
orb_advert_t _att_sp_pub; /**< attitude setpoint publication */
@@ -687,7 +687,7 @@ MulticopterPositionControl::task_main()
if (!_control_mode.flag_control_manual_enabled) {
/* use constant descend rate when landing, ignore altitude setpoint */
- if (_pos_sp_triplet.current_valid && _pos_sp_triplet.current.nav_cmd == NAV_CMD_LAND) {
+ if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
_vel_sp(2) = _params.land_speed;
}
@@ -789,7 +789,7 @@ MulticopterPositionControl::task_main()
float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2));
float tilt_max = _params.tilt_max;
if (!_control_mode.flag_control_manual_enabled) {
- if (_pos_sp_triplet.current_valid && _pos_sp_triplet.current.nav_cmd == NAV_CMD_LAND) {
+ if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
/* limit max tilt and min lift when landing */
tilt_max = _params.land_tilt_max;
if (thr_min < 0.0f)
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index dfd07d315..ba51b024f 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -972,10 +972,10 @@ Navigator::start_loiter()
/* use current altitude if above min altitude set by parameter */
if (_global_pos.alt < min_alt_amsl) {
- _pos_sp_triplet.current.altitude = min_alt_amsl;
+ _pos_sp_triplet.current.alt = min_alt_amsl;
mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt));
} else {
- _pos_sp_triplet.current.altitude = _global_pos.alt;
+ _pos_sp_triplet.current.alt = _global_pos.alt;
mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude");
}
@@ -987,6 +987,8 @@ Navigator::start_loiter()
}
}
+ _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
+ _pos_sp_triplet.current.loiter_direction = 1;
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = true;
_pos_sp_triplet.next.valid = false;
@@ -1052,7 +1054,7 @@ Navigator::set_mission_item()
}
/* check if we really need takeoff */
- if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - item.acceptance_radius) {
+ if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - _mission_item.acceptance_radius) {
/* force TAKEOFF if landed or waypoint altitude is more than current */
_do_takeoff = true;
@@ -1067,14 +1069,14 @@ Navigator::set_mission_item()
_pos_sp_triplet.current.yaw = NAN;
_pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF;
}
- } else if (item.nav_cmd == NAV_CMD_LAND) {
+ } else if (_mission_item.nav_cmd == NAV_CMD_LAND) {
/* will need takeoff after landing */
_need_takeoff = true;
}
}
if (_do_takeoff) {
- mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm AMSL", _pos_sp_triplet.current.altitude);
+ mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm AMSL", _pos_sp_triplet.current.alt);
} else {
if (onboard) {
mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index);
@@ -1207,7 +1209,7 @@ Navigator::set_rtl_item()
_pos_sp_triplet.next.valid = false;
- mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm", descend_alt - _home_pos.altitude);
+ mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm AMSL", _mission_item.altitude);
break;
}
case RTL_STATE_LAND: {
@@ -1258,9 +1260,12 @@ Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_
} else {
sp->lat = item->lat;
sp->lon = item->lon;
- sp->alt = item->altitude_is_relative ? item->alt + _home_pos.altitude : item->alt;
+ sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.altitude : item->altitude;
}
sp->yaw = item->yaw;
+ sp->loiter_radius = item->loiter_radius;
+ sp->loiter_direction = item->loiter_direction;
+ sp->pitch_min = item->pitch_min;
if (item->nav_cmd == NAV_CMD_TAKEOFF) {
sp->type = SETPOINT_TYPE_TAKEOFF;
} else if (item->nav_cmd == NAV_CMD_LAND) {
@@ -1320,7 +1325,7 @@ Navigator::check_mission_item_reached()
/* current relative or AMSL altitude depending on mission item altitude_is_relative flag */
float wp_alt_amsl = _mission_item.altitude;
if (_mission_item.altitude_is_relative)
- _mission_itemt.altitude += _home_pos.altitude;
+ wp_alt_amsl += _home_pos.altitude;
dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl,
(double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt,
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index de510e022..46f8ed827 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -73,7 +73,7 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/mission_item_triplet.h>
+#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
@@ -908,7 +908,7 @@ int sdlog2_thread_main(int argc, char *argv[])
fdsc_count++;
/* --- GLOBAL POSITION SETPOINT--- */
- subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet));
+ subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
fds[fdsc_count].fd = subs.triplet_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
@@ -1263,18 +1263,15 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- GLOBAL POSITION SETPOINT --- */
if (fds[ifds++].revents & POLLIN) {
- orb_copy(ORB_ID(mission_item_triplet), subs.triplet_sub, &buf.triplet);
+ orb_copy(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet);
log_msg.msg_type = LOG_GPSP_MSG;
- log_msg.body.log_GPSP.altitude_is_relative = buf.triplet.current.altitude_is_relative;
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.altitude = buf.triplet.current.altitude;
+ 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.nav_cmd = buf.triplet.current.nav_cmd;
+ 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.acceptance_radius = buf.triplet.current.acceptance_radius;
- log_msg.body.log_GPSP.time_inside = buf.triplet.current.time_inside;
log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min;
LOGBUFFER_WRITE_AND_COUNT(GPSP);
}
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 503dc0afc..98736dd21 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -210,16 +210,13 @@ struct log_GPOS_s {
/* --- GPSP - GLOBAL POSITION SETPOINT --- */
#define LOG_GPSP_MSG 17
struct log_GPSP_s {
- uint8_t altitude_is_relative;
int32_t lat;
int32_t lon;
- float altitude;
+ float alt;
float yaw;
- uint8_t nav_cmd;
+ uint8_t type;
float loiter_radius;
int8_t loiter_direction;
- float acceptance_radius;
- float time_inside;
float pitch_min;
};
@@ -307,7 +304,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
- LOG_FORMAT(GPSP, "BLLffBfbfff", "AltRel,Lat,Lon,Alt,Yaw,NavCmd,LoitR,LoitDir,AccR,TimeIn,PitMin"),
+ LOG_FORMAT(GPSP, "LLffBfbf", "Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
index a8bd6b8e3..4b57833b6 100644
--- a/src/modules/uORB/topics/position_setpoint_triplet.h
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -61,12 +61,15 @@ enum SETPOINT_TYPE
struct position_setpoint_s
{
- bool valid; /**< true if point is valid */
+ bool valid; /**< true if setpoint is valid */
+ enum SETPOINT_TYPE type; /**< setpoint type to adjust behavior of position controller */
double lat; /**< latitude, in deg */
double lon; /**< longitude, in deg */
float alt; /**< altitude AMSL, in m */
- float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN to hold current yaw */
- enum SETPOINT_TYPE type; /**< setpoint type to adjust behavior of position controller */
+ float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */
+ float loiter_radius; /**< loiter radius (only for fixed wing), in m */
+ int8_t loiter_direction; /**< loiter direction: 1 = CW, -1 = CCW */
+ float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */
};
/**