fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 23:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

support orbit command in fixed wing mode

Browse Source 
Signed-off-by: RomanBapst <bapstroman@gmail.com>

commander: support orbit mode for fixed wings

Signed-off-by: RomanBapst <bapstroman@gmail.com>

FwPositionControl: publish orbit status

Signed-off-by: RomanBapst <bapstroman@gmail.com>

commander:reject orbit mode while doing a vtol transition

Signed-off-by: RomanBapst <bapstroman@gmail.com>

FixedWingPositionControl: explicitly cast waypoint for Orbit status

FixedwingPositionControl: fill missing orbit_status fields

navigator_main: handle reposition/orbit corner cases

- set orbit rotation direction correctly
- send mavlink message when orbit is rejected

FixedWingPositionControl: correctly report rotation direction in orbit_status

navigator: hack to not break orbit while doing altitude changes

Signed-off-by: RomanBapst <bapstroman@gmail.com>

navigator: set cruise throttle for orbit command

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2021-10-25 09:48:27 +02:00
committed by Silvan Fuhrer
parent 93eb0162e5
commit d84b0296d2
7 changed files with 135 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+17
View File
@@ -763,6 +763,10 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
_att_sp.pitch_reset_integral = false;
_att_sp.yaw_reset_integral = false;
if (pos_sp_curr.valid && pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
publishOrbitStatus(pos_sp_curr);
}
const uint8_t position_sp_type = handle_setpoint_type(pos_sp_curr.type, pos_sp_curr);
_type = position_sp_type; // for publication
@@ -2200,6 +2204,19 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
tecs_status_publish();
}
void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_sp)
{
orbit_status_s orbit_status{};
orbit_status.timestamp = hrt_absolute_time();
orbit_status.radius = static_cast<float>(pos_sp.loiter_direction) * pos_sp.loiter_radius;
orbit_status.frame = 0; // MAV_FRAME::MAV_FRAME_GLOBAL
orbit_status.x = static_cast<double>(pos_sp.lat);
orbit_status.y = static_cast<double>(pos_sp.lon);
orbit_status.z = pos_sp.alt;
orbit_status.yaw_behaviour = orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE;
_orbit_status_pub.publish(orbit_status);
}
int FixedwingPositionControl::task_spawn(int argc, char *argv[])
{
bool vtol = false;