PX4 / PX4-Autopilot

PX4 Autopilot Software
https://px4.io
BSD 3-Clause "New" or "Revised" License
8.29k stars 13.43k forks source link

"no offboard signal" even though trajectory_setpoint and offboard_control_mode get published in uorb #22456

Open NXPBenjaminK opened 10 months ago

NXPBenjaminK commented 10 months ago

Describe the bug

Hi! I have written a node, which publishes velocity offboard-commands using the uxrce-dds-bridge. The trajectory_setpoint and offboard_control_mode messages get transmitted and are visible in uorb (see below) However when i try to switch to offboard mode, i get the following error (see sreenshot below): "Critical: Switching to mode 'Offboard' is currently not possible No offboard signal"

Arming the vehicle via uxrce_dds bridge while in offboard-mode and with skipping of the pre-flight checks causes the vehicle to immediatly go into failsafe with the same error.

Using the px4_ros_com offboard_control node causes the same problem.

The relevant part of my code:

VelocityController::VelocityController(void) : Node("px4_offboard_control") {
    controll_mode_msg.position = false;
    controll_mode_msg.velocity = true;
    pub_offboard_control_mode = this->create_publisher<px4_msgs::msg::OffboardControlMode>("/fmu/in/offboard_control_mode", 10);
    pub_trajectory_setpoint = this->create_publisher<px4_msgs::msg::TrajectorySetpoint>("/fmu/in/trajectory_setpoint", 10);
    ...
}
...
void VelocityController::publish_trajectory_setpoint(void){
    publish_offboard_control_mode();
    timestamp = this->get_clock()->now().nanoseconds() / 1000;
    publish(trajectory_msg);
}
...
void VelocityController::publish_offboard_control_mode()
{
    controll_mode_msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
    pub_offboard_control_mode->publish(controll_mode_msg);
}

To Reproduce

Expected behavior

Rover should slowly drive West

Screenshot / Media

screen uorb_msgs qgc output

Flight Log

https://logs.px4.io/plot_app?log=3c4983fe-dae7-4ba4-b823-88a2fa28ce32

Software Version

https://github.com/PX4/PX4-Autopilot/commit/6a34b63b60251aa15703a66ad9f4479943868f16

HW arch: NXP_MR_CANHUBK3 HW type: MR-CANHUBK3-ADAP HW version: 0x000 HW revision: 0x000 PX4 git-hash: 6a34b63b60251aa15703a66ad9f4479943868f16 PX4 version: 1.15.0 0 (17760256) PX4 git-branch: main OS: NuttX OS version: Release 11.0.0 (184549631) OS git-hash: e7da5ac0e660238cb353948b2a9118579727267a Build datetime: Nov 28 2023 16:44:37 Build uri: localhost Build variant: fmu Toolchain: GNU GCC, 9.3.1 20200408 (release) PX4GUID: 00086702193221071965ffffffffffffffff MCU: S32K344, rev. 0

Flight controller

NXP_MR_CANHUBK3

Vehicle type

Rover

How are the different components wired up (including port information)

No response

Additional context

No response

DronecodeBot commented 10 months ago

This issue has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/px4-community-q-a-november-29-2023/35461/1

NXPBenjaminK commented 10 months ago

I have now teted it on a drone with an px4_fmu6xrt controller and have the same issue there. Also when trying the example from the new dynamic mode system it works for ~3 seconds and then the fmu crashes completly and needs to be power cycled (on both the rover and the drone).

This is the output over the serial consol when trying the dynamic mode:

WARN [health_and_arming_checks] No response from 0, flagging unresponsive WARN [failsafe] Failsafe activated INFO [commander] Disarmed by auto preflight disarming INFO [logger] closed logfile, bytes written: 412989 INFO [commander] Connection to ground station lost

Here is a flight log of the dynamic mode test on a drone: https://review.px4.io/plot_app?log=d631e501-5910-41fc-ba1e-36deab1da9cc

dk7xe commented 10 months ago

important to get that solved cause we like to test UWB based indoor localization and navigation from ROS2. @bkueng

NXPBenjaminK commented 10 months ago

I found the issue: We are running these vehicles indoors without a gps signal and apparently for velocity control there is still gps needed. I tested it with attitude control and it now works (the "old" offboard mode).

@hamishwillee maybe this could be a little clearer in the px4 docu

beniaminopozzan commented 10 months ago

Hi @NXPBenjaminK , I'm glad that you found the root of the issue.

Yeah, looking into it the error message is quite unclear https://github.com/PX4/PX4-Autopilot/blob/1c25d65a1ecd9fb1c405c60ecf75791c93d0754e/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp#L126-L136 as it combines the condition "no stepoint" and "no valid required estimate". Thank you for raising this.

Regarding the need of gps: https://github.com/PX4/PX4-Autopilot/blob/1c25d65a1ecd9fb1c405c60ecf75791c93d0754e/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp#L38-L67 if you only have the velocity flag set, then you need a valid velocity estimate. I does not need to come from a GPS.

Finally, the documentation https://docs.px4.io/main/en/flight_modes/offboard.html#ros-2-messages says that velocity is needed :)