From 007b6dd8d72b905520696fbd6b65f6e68dbf8747 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=83=C2=BCng?= Date: Mon, 24 Jul 2017 14:33:38 +0200 Subject: [PATCH] commander: make sure the power_button_state topic is advertised on startup Otherwise the publication from IRQ context won't work --- src/modules/commander/commander.cpp | 20 ++++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cf51f8201f..0f241eea56 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -361,8 +361,11 @@ static int power_button_state_notification_cb(board_power_button_state_notificat return ret; } - int instance; - orb_publish_auto(ORB_ID(power_button_state), &power_button_state_pub, &button_state, &instance, ORB_PRIO_DEFAULT); + if (power_button_state_pub != nullptr) { + orb_publish(ORB_ID(power_button_state), power_button_state_pub, &button_state); + } else { + PX4_ERR("power_button_state_pub not properly initialized"); + } return ret; } @@ -1423,6 +1426,17 @@ int commander_thread_main(int argc, char *argv[]) PX4_WARN("Buzzer init failed"); } + int power_button_state_sub = orb_subscribe(ORB_ID(power_button_state)); + { + // we need to do an initial publication to make sure uORB allocates the buffer, which cannot happen + // in IRQ context. + power_button_state_s button_state; + button_state.timestamp = 0; + button_state.event = 0xff; + power_button_state_pub = orb_advertise(ORB_ID(power_button_state), &button_state); + orb_copy(ORB_ID(power_button_state), power_button_state_sub, &button_state); + } + if (board_register_power_state_notification_cb(power_button_state_notification_cb) != 0) { PX4_ERR("Failed to register power notification callback"); } @@ -1667,8 +1681,6 @@ int commander_thread_main(int argc, char *argv[]) struct system_power_s system_power; memset(&system_power, 0, sizeof(system_power)); - int power_button_state_sub = orb_subscribe(ORB_ID(power_button_state)); - /* Subscribe to actuator controls (outputs) */ int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); struct actuator_controls_s actuator_controls;