Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

New follow-me flight task #18026

Merged
merged 3 commits into from
Jun 16, 2022
Merged

New follow-me flight task #18026

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
31375ba
New follow-me flight task
potaito Jul 20, 2021
93f6bfa
FollowMe : Replace First order target position filter with Second ord…
junwoo091400 Feb 25, 2022
b1205b8
boards: disable modules to save flash
dagar Jun 16, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,7 @@ param set-default COM_RC_LOSS_T 3

# ekf2
param set-default EKF2_AID_MASK 33
param set-default EKF2_TERR_MASK 1
param set-default EKF2_BARO_DELAY 0
param set-default EKF2_BARO_NOISE 2.0

Expand Down
10 changes: 4 additions & 6 deletions boards/cubepilot/cubeorange/default.px4board
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@ CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0"
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1"
CONFIG_DRIVERS_ADC_ADS1115=y
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_COMMON_BAROMETERS=y
CONFIG_DRIVERS_BAROMETER_MS5611=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
Expand Down Expand Up @@ -42,6 +42,7 @@ CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
CONFIG_MODULES_BATTERY_STATUS=y
CONFIG_MODULES_CAMERA_FEEDBACK=y
CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_DATAMAN=y
CONFIG_MODULES_EKF2=y
CONFIG_MODULES_ESC_BATTERY=y
Expand All @@ -50,6 +51,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y
CONFIG_MODULES_GYRO_CALIBRATION=y
CONFIG_MODULES_GYRO_FFT=y
CONFIG_MODULES_LAND_DETECTOR=y
Expand All @@ -65,17 +67,15 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
CONFIG_MODULES_MC_POS_CONTROL=y
CONFIG_MODULES_MC_RATE_CONTROL=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_NAVIGATOR=y
CONFIG_MODULES_RC_UPDATE=y
CONFIG_MODULES_ROVER_POS_CONTROL=y
CONFIG_MODULES_SENSORS=y
CONFIG_MODULES_SIH=y
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
CONFIG_MODULES_GIMBAL=y
CONFIG_MODULES_VTOL_ATT_CONTROL=y
CONFIG_SYSTEMCMDS_BL_UPDATE=y
CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
CONFIG_SYSTEMCMDS_BL_UPDATE=y
CONFIG_SYSTEMCMDS_DMESG=y
CONFIG_SYSTEMCMDS_DUMPFILE=y
CONFIG_SYSTEMCMDS_GPIO=y
Expand All @@ -93,7 +93,6 @@ CONFIG_SYSTEMCMDS_PWM=y
CONFIG_SYSTEMCMDS_REBOOT=y
CONFIG_SYSTEMCMDS_REFLECT=y
CONFIG_SYSTEMCMDS_SD_BENCH=y
CONFIG_SYSTEMCMDS_SD_STRESS=y
CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
CONFIG_SYSTEMCMDS_TOP=y
CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
Expand All @@ -102,4 +101,3 @@ CONFIG_SYSTEMCMDS_UORB=y
CONFIG_SYSTEMCMDS_USB_CONNECTED=y
CONFIG_SYSTEMCMDS_VER=y
CONFIG_SYSTEMCMDS_WORK_QUEUE=y
CONFIG_EXAMPLES_FAKE_GPS=y
2 changes: 1 addition & 1 deletion boards/cubepilot/cubeorange/test.px4board
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,11 +5,11 @@ CONFIG_DRIVERS_PCA9685=n
CONFIG_DRIVERS_PCA9685_PWM_OUT=n
CONFIG_DRIVERS_RPM=n
CONFIG_DRIVERS_UAVCAN=n
CONFIG_EXAMPLES_FAKE_GPS=n
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
CONFIG_MODULES_ROVER_POS_CONTROL=n
CONFIG_MODULES_TEMPERATURE_COMPENSATION=n
CONFIG_BOARD_TESTING=y
CONFIG_DRIVERS_TEST_PPM=y
CONFIG_SYSTEMCMDS_MICROBENCH=y
CONFIG_SYSTEMCMDS_SD_STRESS=y
1 change: 1 addition & 0 deletions boards/holybro/kakutef7/default.px4board
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
CONFIG_BOARD_ARCHITECTURE="cortex-m7"
CONFIG_BOARD_CONSTRAINED_FLASH=y
CONFIG_BOARD_NO_HELP=y
CONFIG_BOARD_EXTERNAL_METADATA=y
CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3"
Expand Down
2 changes: 2 additions & 0 deletions boards/px4/fmu-v5/uavcanv0periph.px4board
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
CONFIG_COMMON_BAROMETERS=n
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
CONFIG_COMMON_HYGROMETERS=n
CONFIG_COMMON_TELEMETRY=n
Expand Down Expand Up @@ -32,3 +33,4 @@ CONFIG_SYSTEMCMDS_SD_STRESS=n
CONFIG_SYSTEMCMDS_SERIAL_TEST=n
CONFIG_SYSTEMCMDS_TOPIC_LISTENER=n
CONFIG_BOARD_UAVCAN_PERIPHERALS="cuav_can-gps-v1_default"
CONFIG_DRIVERS_BAROMETER_MS5611=y
3 changes: 3 additions & 0 deletions boards/px4/fmu-v5x/rtps.px4board
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,3 +1,4 @@
CONFIG_COMMON_BAROMETERS=n
CONFIG_COMMON_TELEMETRY=n
CONFIG_DRIVERS_OSD=n
CONFIG_EXAMPLES_FAKE_GPS=n
Expand All @@ -8,5 +9,7 @@ CONFIG_SYSTEMCMDS_REFLECT=n
CONFIG_SYSTEMCMDS_SD_BENCH=n
CONFIG_SYSTEMCMDS_SD_STRESS=n
CONFIG_SYSTEMCMDS_SERIAL_TEST=n
CONFIG_DRIVERS_BAROMETER_BMP388=y
CONFIG_DRIVERS_BAROMETER_MS5611=y
CONFIG_MODULES_MICRODDS_CLIENT=y
CONFIG_MODULES_MICRORTPS_BRIDGE=y
1 change: 0 additions & 1 deletion boards/px4/fmu-v6x/default.px4board
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,6 @@ CONFIG_DRIVERS_PWM_OUT_SIM=y
CONFIG_DRIVERS_PX4IO=y
CONFIG_DRIVERS_RC_INPUT=y
CONFIG_DRIVERS_SAFETY_BUTTON=y
CONFIG_COMMON_TELEMETRY=y
CONFIG_DRIVERS_TONE_ALARM=y
CONFIG_DRIVERS_UAVCAN=y
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
Expand Down
2 changes: 2 additions & 0 deletions msg/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -81,7 +81,9 @@ set(msg_files
estimator_aid_source_2d.msg
estimator_aid_source_3d.msg
event.msg
follow_target_status.msg
follow_target.msg
follow_target_estimator.msg
failure_detector_status.msg
generator_status.msg
geofence_result.msg
Expand Down
19 changes: 11 additions & 8 deletions msg/follow_target.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,8 +1,11 @@
uint64 timestamp # time since system start (microseconds)
float64 lat # target position (deg * 1e7)
float64 lon # target position (deg * 1e7)
float32 alt # target position
float32 vy # target vel in y
float32 vx # target vel in x
float32 vz # target vel in z
uint8 est_cap # target reporting capabilities
uint64 timestamp # time since system start (microseconds)

float64 lat # target position (deg * 1e7)
float64 lon # target position (deg * 1e7)
float32 alt # target position

float32 vy # target vel in y
float32 vx # target vel in x
float32 vz # target vel in z

uint8 est_cap # target reporting capabilities
16 changes: 16 additions & 0 deletions msg/follow_target_estimator.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,16 @@
uint64 timestamp # time since system start (microseconds)
uint64 last_filter_reset_timestamp # time of last filter reset (microseconds)

bool valid # True if estimator states are okay to be used
bool stale # True if estimator stopped receiving follow_target messages for some time. The estimate can still be valid, though it might be inaccurate.

float64 lat_est # Estimated target latitude
float64 lon_est # Estimated target longitude
float32 alt_est # Estimated target altitude

float32[3] pos_est # Estimated target NED position (m)
float32[3] vel_est # Estimated target NED velocity (m/s)
float32[3] acc_est # Estimated target NED acceleration (m^2/s)

uint64 prediction_count
uint64 fusion_count
12 changes: 12 additions & 0 deletions msg/follow_target_status.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,12 @@
uint64 timestamp # [microseconds] time since system start

float32 tracked_target_course # [rad] Tracked target course in NED local frame (North is course zero)
float32 follow_angle # [rad] Current follow angle setting

float32 orbit_angle_setpoint # [rad] Current orbit angle setpoint from the smooth trajectory generator
float32 angular_rate_setpoint # [rad/s] Angular rate commanded from Jerk-limited Orbit Angle trajectory for Orbit Angle

float32[3] desired_position_raw # [m] Raw 'idealistic' desired drone position if a drone could teleport from place to places
Copy link
Member

@MaEtUgR MaEtUgR Jun 15, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Those are 34 more fields to log which adds flash for every field name. For comparison that's roughly the same amount as the entire estimator_status message of ekf.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Right. I think a lot of those were necessary for debugging purposes, but can be removed now.

Copy link
Contributor Author

@potaito potaito Jun 15, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think we can at least remove

  • pos_est_filtered
  • vel_est_filtered
  • raw_orbit_angle_setpoint
  • desired_position_raw

Maybe also the gimbal message can be removed if it's logged elsewhere already (vmount?)

@junwoo091400 what's tracked_target_course? Is that the estimated target course?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes it's the estimated target course!

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Some of the inner fields were removed & was incorporated into the commit!

I left the desired_position_raw in there as it gives a intuitive sense on what the 'correct' follow position is, but I am also up for removing it 😉


bool in_emergency_ascent # [bool] True when doing emergency ascent (when distance to ground is below safety altitude)
float32 gimbal_pitch # [rad] Gimbal pitch commanded to track target in the center of the frame
5 changes: 4 additions & 1 deletion src/modules/commander/Commander.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3376,7 +3376,6 @@ Commander::update_control_mode()
break;

case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
Expand Down Expand Up @@ -3444,6 +3443,10 @@ Commander::update_control_mode()

break;

case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:

// Follow Target supports RC adjustment, so disable auto control mode to disable
// the Flight Task from exiting itself when RC stick movement is detected.
case vehicle_status_s::NAVIGATION_STATE_ORBIT:
_vehicle_control_mode.flag_control_manual_enabled = false;
_vehicle_control_mode.flag_control_auto_enabled = false;
Expand Down
15 changes: 15 additions & 0 deletions src/modules/commander/state_machine_helper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -484,7 +484,22 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
// nothing to do - everything done in check_invalid_pos_nav_state

} else if (status.data_link_lost && data_link_loss_act_configured && !landed && is_armed) {
// failsafe: datalink is lost
// Trigger RTL
set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, 0);

enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_datalink);

} else if (status.rc_signal_lost && status_flags.rc_signal_found_once && !data_link_loss_act_configured && is_armed) {
// Trigger failsafe on RC loss only if RC was present once before
// Otherwise fly without RC, as follow-target only depends on the datalink
enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc);

set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, 0);

} else {
// no failsafe, RC is not mandatory for follow_target
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET;
}

Expand Down
24 changes: 8 additions & 16 deletions src/modules/flight_mode_manager/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,24 +36,10 @@
# Prepare flight tasks
###########################################

# add upstream flight tasks (they are being handled differently from the core inside the python script)
list(APPEND flight_tasks_to_add
Orbit
)
# remove possible duplicates
list(REMOVE_DUPLICATES flight_tasks_to_add)

# remove flight tasks depending on target
if(flight_tasks_to_remove)
list(REMOVE_ITEM flight_tasks_to_add
${flight_tasks_to_remove}
)
endif()

# add core flight tasks to list
set(flight_tasks_all)
list(APPEND flight_tasks_all
Auto
AutoFollowMe
Descend
Failsafe
ManualAcceleration
Expand All @@ -62,9 +48,15 @@ list(APPEND flight_tasks_all
ManualPosition
ManualPositionSmoothVel
Transition
${flight_tasks_to_add}
)

if(NOT px4_constrained_flash_build)
list(APPEND flight_tasks_all
AutoFollowTarget
Orbit
)
endif()

# set the files to be generated
set(files_to_generate
FlightTasks_generated.hpp
Expand Down
6 changes: 5 additions & 1 deletion src/modules/flight_mode_manager/FlightModeManager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -200,7 +200,11 @@ void FlightModeManager::start_flight_task()
// Auto-follow me
if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) {
should_disable_task = false;
FlightTaskError error = switchTask(FlightTaskIndex::AutoFollowMe);
FlightTaskError error = FlightTaskError::InvalidTask;

#if !defined(CONSTRAINED_FLASH)
error = switchTask(FlightTaskIndex::AutoFollowTarget);
#endif // !CONSTRAINED_FLASH

if (error != FlightTaskError::NoError) {
if (prev_failure_count == 0) {
Expand Down
49 changes: 0 additions & 49 deletions src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp
View file
Open in desktop

This file was deleted.

50 changes: 0 additions & 50 deletions src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.hpp
View file
Open in desktop

This file was deleted.

Loading