diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-14 15:18:16 +0100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-14 15:18:16 +0100 |
| commit | c4bf3ea3ed81acdce9f972534c56e2dc68135d19 (patch) | |
| tree | ca738352001a60d6eb2a104295b57c872f85014d /apps/commander/state_machine_helper.c | |
| parent | 3eb36bbd2145a2932e5169e1ae6b676bf636debe (diff) | |
| download | px4-firmware-c4bf3ea3ed81acdce9f972534c56e2dc68135d19.tar.gz px4-firmware-c4bf3ea3ed81acdce9f972534c56e2dc68135d19.tar.bz2 px4-firmware-c4bf3ea3ed81acdce9f972534c56e2dc68135d19.zip | |
better system status reporting, work in progress
Diffstat (limited to 'apps/commander/state_machine_helper.c')
| -rw-r--r-- | apps/commander/state_machine_helper.c | 18 |
1 files changed, 18 insertions, 0 deletions
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 891efe9d7..657c9af9a 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -213,6 +213,24 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat /* publish the new state */ current_status->counter++; current_status->timestamp = hrt_absolute_time(); + + /* assemble state vector based on flag values */ + if (current_status->flag_control_rates_enabled) { + current_status->onboard_control_sensors_present |= 0x400; + } else { + current_status->onboard_control_sensors_present &= ~0x400; + } + current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; + current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; + current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; + current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; + + current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0; + current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; + current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; + current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; + current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; + orb_publish(ORB_ID(vehicle_status), status_pub, current_status); printf("[commander] new state: %s\n", system_state_txt[current_status->state_machine]); } |