diff options
| author | Julian Oes <joes@student.ethz.ch> | 2012-11-15 11:55:55 -0800 |
|---|---|---|
| committer | Julian Oes <joes@student.ethz.ch> | 2012-11-15 11:55:55 -0800 |
| commit | 33e750602ab384069b08ca17ca6589c08177f7a6 (patch) | |
| tree | 8f2dbfe7779fc74436ea4ba243e2929c09d5b2ac /apps/commander/state_machine_helper.c | |
| parent | b7c6a11e6739d217e5df1e79b7f80399ff1fd8f8 (diff) | |
| parent | 3016ae72a3b3b7d7bf1df937fd62a14f53eace6f (diff) | |
| download | px4-firmware-33e750602ab384069b08ca17ca6589c08177f7a6.tar.gz px4-firmware-33e750602ab384069b08ca17ca6589c08177f7a6.tar.bz2 px4-firmware-33e750602ab384069b08ca17ca6589c08177f7a6.zip | |
Merge remote-tracking branch 'upstream/master' into io
Fixed Conflicts:
apps/multirotor_att_control/multirotor_att_control_main.c
rc loss failsafe throttle tested
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 9c7db8fca..bf50ebad2 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]); } |