Browse Source

commander app sets an airspeed_valid flag in the vehicle status

sbg
Thomas Gubler 12 years ago
parent
commit
6f1d7dc7de
  1. 24
      apps/commander/commander.c
  2. 1
      apps/uORB/topics/vehicle_status.h

24
apps/commander/commander.c

@ -1475,6 +1475,11 @@ int commander_thread_main(int argc, char *argv[]) @@ -1475,6 +1475,11 @@ int commander_thread_main(int argc, char *argv[])
struct sensor_combined_s sensors;
memset(&sensors, 0, sizeof(sensors));
int differential_pressure_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s differential_pressure;
memset(&differential_pressure, 0, sizeof(differential_pressure));
uint64_t last_differential_pressure_time = 0;
/* Subscribe to command topic */
int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
struct vehicle_command_s cmd;
@ -1528,6 +1533,13 @@ int commander_thread_main(int argc, char *argv[]) @@ -1528,6 +1533,13 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
}
orb_check(differential_pressure_sub, &new_data);
if (new_data) {
orb_copy(ORB_ID(differential_pressure), differential_pressure_sub, &differential_pressure);
last_differential_pressure_time = differential_pressure.timestamp;
}
orb_check(cmd_sub, &new_data);
if (new_data) {
@ -1714,6 +1726,7 @@ int commander_thread_main(int argc, char *argv[]) @@ -1714,6 +1726,7 @@ int commander_thread_main(int argc, char *argv[])
bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok;
bool global_pos_valid = current_status.flag_global_position_valid;
bool local_pos_valid = current_status.flag_local_position_valid;
bool airspeed_valid = current_status.flag_airspeed_valid;
/* check for global or local position updates, set a timeout of 2s */
if (hrt_absolute_time() - last_global_position_time < 2000000) {
@ -1732,6 +1745,14 @@ int commander_thread_main(int argc, char *argv[]) @@ -1732,6 +1745,14 @@ int commander_thread_main(int argc, char *argv[])
current_status.flag_local_position_valid = false;
}
/* Check for valid airspeed/differential pressure measurements */
if (hrt_absolute_time() - last_differential_pressure_time < 2000000) {
current_status.flag_airspeed_valid = true;
} else {
current_status.flag_airspeed_valid = false;
}
/*
* Consolidate global position and local position valid flags
* for vector flight mode.
@ -1747,7 +1768,8 @@ int commander_thread_main(int argc, char *argv[]) @@ -1747,7 +1768,8 @@ int commander_thread_main(int argc, char *argv[])
/* consolidate state change, flag as changed if required */
if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok ||
global_pos_valid != current_status.flag_global_position_valid ||
local_pos_valid != current_status.flag_local_position_valid) {
local_pos_valid != current_status.flag_local_position_valid ||
airspeed_valid != current_status.flag_airspeed_valid) {
state_changed = true;
}

1
apps/uORB/topics/vehicle_status.h

@ -210,6 +210,7 @@ struct vehicle_status_s @@ -210,6 +210,7 @@ struct vehicle_status_s
bool flag_auto_flight_mode_ok; /**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */
bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */
bool flag_valid_launch_position; /**< indicates a valid launch position */
bool flag_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */
};
/**

Loading…
Cancel
Save