diff options
Diffstat (limited to 'apps')
-rw-r--r-- | apps/commander/commander.c | 10 | ||||
-rw-r--r-- | apps/commander/state_machine_helper.c | 90 | ||||
-rw-r--r-- | apps/mavlink/mavlink.c | 76 | ||||
-rw-r--r-- | apps/px4/attitude_estimator_bm/attitude_estimator_bm.c | 2 | ||||
-rw-r--r-- | apps/sensors/sensors.c | 2 | ||||
-rw-r--r-- | apps/systemcmds/top/top.c | 6 | ||||
-rw-r--r-- | apps/uORB/topics/vehicle_status.h | 32 |
7 files changed, 142 insertions, 76 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 47829ddf5..149a662fd 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -95,10 +95,7 @@ static int leds; static int buzzer; static int mavlink_fd; static bool commander_initialized = false; -static struct vehicle_status_s current_status = { - .state_machine = SYSTEM_STATE_PREFLIGHT, - .mode = 0 -}; /**< Main state machine */ +static struct vehicle_status_s current_status; /**< Main state machine */ static int stat_pub; static uint16_t nofix_counter = 0; @@ -798,6 +795,7 @@ int commander_main(int argc, char *argv[]) /* make sure we are in preflight state */ memset(¤t_status, 0, sizeof(current_status)); current_status.state_machine = SYSTEM_STATE_PREFLIGHT; + current_status.flag_system_armed = false; /* advertise to ORB */ stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); @@ -905,8 +903,8 @@ int commander_main(int argc, char *argv[]) } /* toggle error led at 5 Hz in HIL mode */ - if ((current_status.mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { - /* armed */ + if (current_status.flag_hil_enabled) { + /* hil enabled */ led_toggle(LED_AMBER); } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index b44d2150c..3209ee728 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -83,14 +83,14 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con } else { ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF); } - - return; } break; case SYSTEM_STATE_EMCY_LANDING: /* Tell the controller to land */ - //TODO: add emcy landing code here + + /* set system flags according to state */ + current_status->flag_system_armed = true; fprintf(stderr, "[commander] EMERGENCY LANDING!\n"); mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY LANDING!"); @@ -98,11 +98,21 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con case SYSTEM_STATE_EMCY_CUTOFF: /* Tell the controller to cutoff the motors (thrust = 0) */ + + /* set system flags according to state */ + current_status->flag_system_armed = false; + fprintf(stderr, "[commander] EMERGENCY MOTOR CUTOFF!\n"); mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY MOTOR CUTOFF!"); break; case SYSTEM_STATE_GROUND_ERROR: + + /* set system flags according to state */ + + /* prevent actuators from arming */ + current_status->flag_system_armed = false; + fprintf(stderr, "[commander] GROUND ERROR, locking down propulsion system\n"); mavlink_log_critical(mavlink_fd, "[commander] GROUND ERROR, locking down propulsion system"); break; @@ -110,7 +120,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con case SYSTEM_STATE_PREFLIGHT: if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - invalid_state = false; + /* set system flags according to state */ + current_status->flag_system_armed = true; mavlink_log_critical(mavlink_fd, "[commander] Switched to PREFLIGHT state"); } else { invalid_state = true; @@ -122,6 +133,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { invalid_state = false; + /* set system flags according to state */ + current_status->flag_system_armed = false; mavlink_log_critical(mavlink_fd, "[commander] REBOOTING SYSTEM"); usleep(500000); reboot(); @@ -133,22 +146,47 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con break; case SYSTEM_STATE_STANDBY: + /* set system flags according to state */ + + /* standby enforces disarmed */ + current_status->flag_system_armed = false; + mavlink_log_critical(mavlink_fd, "[commander] Switched to STANDBY state"); break; case SYSTEM_STATE_GROUND_READY: + + /* set system flags according to state */ + + /* ground ready has motors / actuators armed */ + current_status->flag_system_armed = true; + mavlink_log_critical(mavlink_fd, "[commander] Switched to GROUND READY state"); break; case SYSTEM_STATE_AUTO: + + /* set system flags according to state */ + + /* auto is airborne and in auto mode, motors armed */ + current_status->flag_system_armed = true; + mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / AUTO mode"); break; case SYSTEM_STATE_STABILIZED: + + /* set system flags according to state */ + current_status->flag_system_armed = true; + mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / STABILIZED mode"); break; case SYSTEM_STATE_MANUAL: + + /* set system flags according to state */ + current_status->flag_system_armed = true; + mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / MANUAL mode"); break; @@ -424,15 +462,10 @@ void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { - // XXX CHANGE BACK if (current_status->state_machine == SYSTEM_STATE_STANDBY) { printf("[commander] arming\n"); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); - } /*else if (current_status->state_machine == SYSTEM_STATE_AUTO) { - - printf("[commander] landing\n"); - do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); - }*/ + } } void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) @@ -451,7 +484,7 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c { int old_mode = current_status->flight_mode; current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; - current_status->control_manual_enabled = true; + current_status->flag_control_manual_enabled = true; if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { @@ -464,7 +497,7 @@ void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_ { int old_mode = current_status->flight_mode; current_status->flight_mode = VEHICLE_FLIGHT_MODE_STABILIZED; - current_status->control_manual_enabled = true; + current_status->flag_control_manual_enabled = true; if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { @@ -477,7 +510,7 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur { int old_mode = current_status->flight_mode; current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO; - current_status->control_manual_enabled = true; + current_status->flag_control_manual_enabled = true; if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) { @@ -492,20 +525,11 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_ printf("in update state request\n"); uint8_t ret = 1; - current_status->mode |= VEHICLE_MODE_FLAG_SAFETY_ARMED; - /* Set manual input enabled flag */ - current_status->mode |= VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED; - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); - /* vehicle is disarmed, mode requests arming */ - if (!(current_status->mode & VEHICLE_MODE_FLAG_SAFETY_ARMED) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { /* only arm in standby state */ // XXX REMOVE if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - /* Set armed flag */ - current_status->mode |= VEHICLE_MODE_FLAG_SAFETY_ARMED; - /* Set manual input enabled flag */ - current_status->mode |= VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED; do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); ret = OK; printf("[commander] arming due to command request\n"); @@ -513,28 +537,26 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_ } /* vehicle is armed, mode requests disarming */ - //if ((current_status->mode & VEHICLE_MODE_FLAG_SAFETY_ARMED) && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { /* only disarm in ground ready */ - //if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { - /* Clear armed flag, leave manual input enabled */ - // current_status->mode &= ~VEHICLE_MODE_FLAG_SAFETY_ARMED; - // do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - // ret = OK; - // printf("[commander] disarming due to command request\n"); - //} - //} + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); + ret = OK; + printf("[commander] disarming due to command request\n"); + } + } /* Switch on HIL if in standby */ if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { /* Enable HIL on request */ - current_status->mode |= VEHICLE_MODE_FLAG_HIL_ENABLED; + current_status->flag_hil_enabled = true; ret = OK; state_machine_publish(status_pub, current_status, mavlink_fd); printf("[commander] Enabling HIL\n"); } /* NEVER actually switch off HIL without reboot */ - if ((current_status->mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { + if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { fprintf(stderr, "[commander] DENYING request to switch of HIL. Please power cycle (safety reasons)\n"); ret = ERROR; } diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 9fc987910..026af14ae 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -92,7 +92,6 @@ static mavlink_status_t status; static pthread_t receive_thread; static pthread_t uorb_receive_thread; -static uint16_t mavlink_message_intervals[256]; /**< intervals at which to send MAVLink packets */ /* Allocate storage space for waypoints */ mavlink_wpm_storage wpm_s; @@ -125,6 +124,7 @@ static struct vehicle_command_s vcmd; static int pub_hil_global_pos = -1; static int ardrone_motors_pub = -1; static int cmd_pub = -1; +static int sensor_sub = -1; static int global_pos_sub = -1; static int local_pos_sub = -1; static int flow_pub = -1; @@ -154,7 +154,7 @@ void handleMessage(mavlink_message_t *msg); /** * Enable / disable Hardware in the Loop simulation mode. */ -int set_hil_on_off(uint8_t vehicle_mode); +int set_hil_on_off(bool hil_enabled); /** * Translate the custom state into standard mavlink modes and state. @@ -281,12 +281,12 @@ extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float pa -int set_hil_on_off(uint8_t vehicle_mode) +int set_hil_on_off(bool hil_enabled) { int ret = OK; /* Enable HIL */ - if ((vehicle_mode & MAV_MODE_FLAG_HIL_ENABLED) && !mavlink_hil_enabled) { + if (hil_enabled && !mavlink_hil_enabled) { //printf("\n HIL ON \n"); @@ -308,7 +308,7 @@ int set_hil_on_off(uint8_t vehicle_mode) } } - if (!(vehicle_mode & MAV_MODE_FLAG_HIL_ENABLED) && mavlink_hil_enabled) { + if (!hil_enabled && mavlink_hil_enabled) { mavlink_hil_enabled = false; (void)close(pub_hil_attitude); (void)close(pub_hil_global_pos); @@ -326,7 +326,7 @@ void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, uint8_t *mavlink_mode = 0; /* set mode flags independent of system state */ - if (c_status->control_manual_enabled) { + if (c_status->flag_control_manual_enabled) { *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } @@ -432,6 +432,28 @@ static void *receiveloop(void *arg) return NULL; } +static int set_mavlink_interval_limit(int mavlink_msg_id, int min_interval) +{ + int ret = OK; + + switch (mavlink_msg_id) { + case MAVLINK_MSG_ID_SCALED_IMU: + /* senser sub triggers scaled IMU */ + orb_set_interval(sensor_sub, min_interval); + break; + case MAVLINK_MSG_ID_RAW_IMU: + /* senser sub triggers RAW IMU */ + orb_set_interval(sensor_sub, min_interval); + break; + default: + /* not found */ + ret = ERROR; + break; + } + + return ret; +} + /** * Listen for uORB topics and send via MAVLink. * @@ -466,7 +488,7 @@ static void *uorb_receiveloop(void *arg) /* --- SENSORS RAW VALUE --- */ /* subscribe to ORB for sensors raw */ - int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); orb_set_interval(sensor_sub, 100); /* 10Hz updates */ fds[fdsc_count].fd = sensor_sub; fds[fdsc_count].events = POLLIN; @@ -662,11 +684,11 @@ static void *uorb_receiveloop(void *arg) /* immediately communicate state changes back to user */ orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); /* enable or disable HIL */ - set_hil_on_off(v_status.mode); + set_hil_on_off(v_status.flag_hil_enabled); /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; - uint8_t mavlink_mode = v_status.mode; + uint8_t mavlink_mode = 0; get_mavlink_mode_and_state(&v_status, &mavlink_state, &mavlink_mode); /* send heartbeat */ @@ -691,7 +713,7 @@ static void *uorb_receiveloop(void *arg) fw_control.attitude_control_output[3]); /* Only send in HIL mode */ - if (v_status.mode & MAV_MODE_FLAG_HIL_ENABLED) { + if (v_status.flag_hil_enabled) { /* Send the desired attitude from RC or from the autonomous controller */ // XXX it should not depend on a RC setting, but on a system_state value @@ -898,10 +920,11 @@ void handleMessage(mavlink_message_t *msg) /* check if topic is advertised */ if (flow_pub <= 0) { - flow_pub = orb_advertise(ORB_ID(optical_flow), &flow); + flow_pub = orb_advertise(ORB_ID(optical_flow), &f); + } else { + /* publish */ + orb_publish(ORB_ID(optical_flow), flow_pub, &f); } - /* publish */ - orb_publish(ORB_ID(optical_flow), flow_pub, &flow); } if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) { @@ -1192,11 +1215,6 @@ int mavlink_main(int argc, char *argv[]) /* reate the device node that's used for sending text log messages, etc. */ register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL); - /* Send attitude at 10 Hz / every 100 ms */ - mavlink_message_intervals[MAVLINK_MSG_ID_ATTITUDE] = 100; - /* Send raw sensor values at 10 Hz / every 100 ms */ - mavlink_message_intervals[MAVLINK_MSG_ID_RAW_IMU] = 100; - /* default values for arguments */ char *uart_name = "/dev/ttyS0"; int baudrate = 57600; @@ -1283,9 +1301,23 @@ int mavlink_main(int argc, char *argv[]) uint16_t counter = 0; int lowspeed_counter = 0; - /**< Subscribe to system state and RC channels */ - // int status_sub = orb_subscribe(ORB_ID(vehicle_status)); - // int rc_sub = orb_subscribe(ORB_ID(rc_channels)); + /* all subscriptions are now active, set up initial guess about rate limits */ + if (baudrate >= 921600) { + /* 1000 Hz / 1 ms */ + set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 1); + } else if (baudrate >= 460800) { + /* 250 Hz / 4 ms */ + set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 4); + } else if (baudrate >= 115200) { + /* 50 Hz / 20 ms */ + set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 20); + } else if (baudrate >= 57600) { + /* 10 Hz / 100 ms */ + set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 100); + } else { + /* very low baud rate, limit to 1 Hz / 1000 ms */ + set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 1000); + } while (1) { @@ -1305,7 +1337,7 @@ int mavlink_main(int argc, char *argv[]) if (lowspeed_counter == 10) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; - uint8_t mavlink_mode = v_status.mode; + uint8_t mavlink_mode = 0; get_mavlink_mode_and_state(&v_status, &mavlink_state, &mavlink_mode); /* send heartbeat */ diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c index c6869a07e..7385171a2 100644 --- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c +++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c @@ -245,7 +245,7 @@ int attitude_estimator_bm_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); /* switching from non-HIL to HIL mode */ //printf("[attitude_estimator_bm] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled); - if ((vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !hil_enabled) { + if (vstatus.flag_hil_enabled && !hil_enabled) { hil_enabled = true; publishing = false; int ret = close(pub_att); diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c index aa006f3a6..c44ff8a2a 100644 --- a/apps/sensors/sensors.c +++ b/apps/sensors/sensors.c @@ -452,7 +452,7 @@ int sensors_main(int argc, char *argv[]) /* switching from non-HIL to HIL mode */ //printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled); - if ((vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !hil_enabled) { + if (vstatus.flag_hil_enabled && !hil_enabled) { hil_enabled = true; publishing = false; int ret = close(sensor_pub); diff --git a/apps/systemcmds/top/top.c b/apps/systemcmds/top/top.c index cbbeb8cc0..e6248dd43 100644 --- a/apps/systemcmds/top/top.c +++ b/apps/systemcmds/top/top.c @@ -135,9 +135,9 @@ int top_main(int argc, char *argv[]) memset(header_spaces, ' ', CONFIG_TASK_NAME_SIZE); header_spaces[CONFIG_TASK_NAME_SIZE] = '\0'; #if CONFIG_RR_INTERVAL > 0 - printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tMIN STACK USE\tCURR (BASE) PRIO\tRR SLICE\n", header_spaces); + printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tMIN STACK USE\tCURR (BASE) PRIO\tRR SLICE\n", header_spaces); #else - printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tMIN STACK USE\tCURR (BASE) PRIO\n", header_spaces); + printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tMIN STACK USE\tCURR (BASE) PRIO\n", header_spaces); #endif } else { @@ -190,7 +190,7 @@ int top_main(int argc, char *argv[]) runtime_spaces = ""; } - printf("\033[K % 2d\t%s%s\t% 8lld ms%s \t % 2d.%03d \t % 6d B", (int)system_load.tasks[i].tcb->pid, system_load.tasks[i].tcb->name, spaces, (system_load.tasks[i].total_runtime / 1000), runtime_spaces, (int)(curr_loads[i] * 100), (int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100), (uint32_t)system_load.tasks[i].tcb->adj_stack_ptr - (uint32_t)system_load.tasks[i].tcb->xcp.regs[REG_R13]); + printf("\033[K % 2d\t%s%s % 8lld ms%s \t % 2d.%03d \t % 6d B", (int)system_load.tasks[i].tcb->pid, system_load.tasks[i].tcb->name, spaces, (system_load.tasks[i].total_runtime / 1000), runtime_spaces, (int)(curr_loads[i] * 100), (int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100), (uint32_t)system_load.tasks[i].tcb->adj_stack_ptr - (uint32_t)system_load.tasks[i].tcb->xcp.regs[REG_R13]); /* Print scheduling info with RR time slice */ #if CONFIG_RR_INTERVAL > 0 printf("\t%d\t(%d)\t\t%d\n", (int)system_load.tasks[i].tcb->sched_priority, (int)system_load.tasks[i].tcb->base_priority, (int)system_load.tasks[i].tcb->timeslice); diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 2cd179da0..c378b05f1 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -87,9 +87,16 @@ enum VEHICLE_MODE_FLAG { }; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ enum VEHICLE_FLIGHT_MODE { - VEHICLE_FLIGHT_MODE_MANUAL, - VEHICLE_FLIGHT_MODE_STABILIZED, - VEHICLE_FLIGHT_MODE_AUTO + VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, same as VEHICLE_FLIGHT_MODE_ATTITUDE for multirotors */ + VEHICLE_FLIGHT_MODE_ATTITUDE, /**< attitude or rate stabilization, as defined by VEHICLE_ATTITUDE_MODE */ + VEHICLE_FLIGHT_MODE_STABILIZED, /**< attitude or rate stabilization plus velocity or position stabilization */ + VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */ +}; + +enum VEHICLE_ATTITUDE_MODE { + VEHICLE_ATTITUDE_MODE_DIRECT, /**< no attitude control, direct stick input mixing (only fixed wing) */ + VEHICLE_ATTITUDE_MODE_RATES, /**< body rates control mode */ + VEHICLE_ATTITUDE_MODE_ATTITUDE /**< tait-bryan attitude control mode */ }; /** @@ -106,14 +113,21 @@ struct vehicle_status_s commander_state_machine_t state_machine; /**< Current flight state, main state machine */ enum VEHICLE_FLIGHT_MODE flight_mode; /**< Current flight mode, as defined by mode switch */ + enum VEHICLE_ATTITUDE_MODE attitute_mode; /**< Current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */ + + // uint8_t mode; + + + /* system flags - these represent the state predicates */ - uint8_t mode; + bool flag_system_armed; /**< True is motors / actuators are armed */ + bool flag_control_manual_enabled; /**< true if manual input is mixed in */ + bool flag_hil_enabled; /**< True if hardware in the loop simulation is enabled */ - bool control_manual_enabled; /**< true if manual input is mixed in */ - bool control_rates_enabled; /**< true if rates are stabilized */ - bool control_attitude_enabled; /**< true if attitude stabilization is mixed in */ - bool control_speed_enabled; /**< true if speed (implies direction) is controlled */ - bool control_position_enabled; /**< true if position is controlled */ + // bool flag_control_rates_enabled; /**< true if rates are stabilized */ + // bool flag_control_attitude_enabled; *< true if attitude stabilization is mixed in + // bool control_speed_enabled; /**< true if speed (implies direction) is controlled */ + // bool control_position_enabled; /**< true if position is controlled */ bool preflight_gyro_calibration; /**< true if gyro calibration is requested */ bool preflight_mag_calibration; /**< true if mag calibration is requested */ |