dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander: make sure the power_button_state topic is advertised on startup 

Otherwise the publication from IRQ context won't work
This commit is contained in:
Beat Küng 2017-07-24 14:33:38 +02:00 committed by Lorenz Meier
parent 8923664f30
commit 007b6dd8d7
1 changed files with 16 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/modules/commander/commander.cpp
View File

@ -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;