mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-18 20:49:05 +08:00
State machine cleanup, introduced variable rates for MAVLink depending on the baud rate
This commit is contained in:
Lorenz Meier
parent
e707574f73
commit
dc484c1d21
@ -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)) {
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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 */
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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;
|
||||
// uint8_t mode;
|
||||
|
||||
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 */
|
||||
|
||||
/* system flags - these represent the state predicates */
|
||||
|
||||
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 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 */
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user