zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

State machine cleanup, introduced variable rates for MAVLink depending on the baud rate

sbg
Lorenz Meier 13 years ago
parent
e707574f73
commit
dc484c1d21
7 changed files with 142 additions and 76 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 10
      apps/commander/commander.c
  2. 90
      apps/commander/state_machine_helper.c
  3. 76
      apps/mavlink/mavlink.c
  4. 2
      apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
  5. 2
      apps/sensors/sensors.c
  6. 6
      apps/systemcmds/top/top.c
  7. 32
      apps/uORB/topics/vehicle_status.h

10
apps/commander/commander.c
Unescape View File

@ -95,10 +95,7 @@ static int leds;
static int buzzer; static int buzzer;
static int mavlink_fd; static int mavlink_fd;
static bool commander_initialized = false; static bool commander_initialized = false;
static struct vehicle_status_s current_status = { static struct vehicle_status_s current_status; /**< Main state machine */
.state_machine = SYSTEM_STATE_PREFLIGHT,
.mode = 0
}; /**< Main state machine */
static int stat_pub; static int stat_pub;
static uint16_t nofix_counter = 0; static uint16_t nofix_counter = 0;
@ -798,6 +795,7 @@ int commander_main(int argc, char *argv[])
/* make sure we are in preflight state */ /* make sure we are in preflight state */
memset(&current_status, 0, sizeof(current_status)); memset(&current_status, 0, sizeof(current_status));
current_status.state_machine = SYSTEM_STATE_PREFLIGHT; current_status.state_machine = SYSTEM_STATE_PREFLIGHT;
current_status.flag_system_armed = false;
/* advertise to ORB */ /* advertise to ORB */
stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status); stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
@ -905,8 +903,8 @@ int commander_main(int argc, char *argv[])
} }
/* toggle error led at 5 Hz in HIL mode */ /* toggle error led at 5 Hz in HIL mode */
if ((current_status.mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { if (current_status.flag_hil_enabled) {
/* armed */ /* hil enabled */
led_toggle(LED_AMBER); led_toggle(LED_AMBER);
} else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) {

90
apps/commander/state_machine_helper.c
Unescape View File

@ -83,14 +83,14 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
} else { } else {
ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF); ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF);
} }
return;
} }
break; break;
case SYSTEM_STATE_EMCY_LANDING: case SYSTEM_STATE_EMCY_LANDING:
/* Tell the controller to land */ /* 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"); fprintf(stderr, "[commander] EMERGENCY LANDING!\n");
mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY LANDING!"); 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: case SYSTEM_STATE_EMCY_CUTOFF:
/* Tell the controller to cutoff the motors (thrust = 0) */ /* 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"); fprintf(stderr, "[commander] EMERGENCY MOTOR CUTOFF!\n");
mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY MOTOR CUTOFF!"); mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY MOTOR CUTOFF!");
break; break;
case SYSTEM_STATE_GROUND_ERROR: 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"); fprintf(stderr, "[commander] GROUND ERROR, locking down propulsion system\n");
mavlink_log_critical(mavlink_fd, "[commander] GROUND ERROR, locking down propulsion system"); mavlink_log_critical(mavlink_fd, "[commander] GROUND ERROR, locking down propulsion system");
break; break;
@ -110,7 +120,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
case SYSTEM_STATE_PREFLIGHT: case SYSTEM_STATE_PREFLIGHT:
if (current_status->state_machine == SYSTEM_STATE_STANDBY if (current_status->state_machine == SYSTEM_STATE_STANDBY
|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { || 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"); mavlink_log_critical(mavlink_fd, "[commander] Switched to PREFLIGHT state");
} else { } else {
invalid_state = true; 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 if (current_status->state_machine == SYSTEM_STATE_STANDBY
|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
invalid_state = false; invalid_state = false;
/* set system flags according to state */
current_status->flag_system_armed = false;
mavlink_log_critical(mavlink_fd, "[commander] REBOOTING SYSTEM"); mavlink_log_critical(mavlink_fd, "[commander] REBOOTING SYSTEM");
usleep(500000); usleep(500000);
reboot(); reboot();
@ -133,22 +146,47 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
break; break;
case SYSTEM_STATE_STANDBY: 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"); mavlink_log_critical(mavlink_fd, "[commander] Switched to STANDBY state");
break; break;
case SYSTEM_STATE_GROUND_READY: 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"); mavlink_log_critical(mavlink_fd, "[commander] Switched to GROUND READY state");
break; break;
case SYSTEM_STATE_AUTO: 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"); mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / AUTO mode");
break; break;
case SYSTEM_STATE_STABILIZED: 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"); mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / STABILIZED mode");
break; break;
case SYSTEM_STATE_MANUAL: 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"); mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / MANUAL mode");
break; 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) 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) { if (current_status->state_machine == SYSTEM_STATE_STANDBY) {
printf("[commander] arming\n"); printf("[commander] arming\n");
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); 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) 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; int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; 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 (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) { 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; int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_STABILIZED; 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 (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) { 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; int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO; 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 (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) { 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"); printf("in update state request\n");
uint8_t ret = 1; 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 */ /* 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 */ /* only arm in standby state */
// XXX REMOVE // XXX REMOVE
if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { 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); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
ret = OK; ret = OK;
printf("[commander] arming due to command request\n"); 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 */ /* 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 */ /* only disarm in ground ready */
//if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
/* Clear armed flag, leave manual input enabled */ do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
// current_status->mode &= ~VEHICLE_MODE_FLAG_SAFETY_ARMED; ret = OK;
// do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY); printf("[commander] disarming due to command request\n");
// ret = OK; }
// printf("[commander] disarming due to command request\n"); }
//}
//}
/* Switch on HIL if in standby */ /* Switch on HIL if in standby */
if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
/* Enable HIL on request */ /* Enable HIL on request */
current_status->mode |= VEHICLE_MODE_FLAG_HIL_ENABLED; current_status->flag_hil_enabled = true;
ret = OK; ret = OK;
state_machine_publish(status_pub, current_status, mavlink_fd); state_machine_publish(status_pub, current_status, mavlink_fd);
printf("[commander] Enabling HIL\n"); printf("[commander] Enabling HIL\n");
} }
/* NEVER actually switch off HIL without reboot */ /* 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"); fprintf(stderr, "[commander] DENYING request to switch of HIL. Please power cycle (safety reasons)\n");
ret = ERROR; ret = ERROR;
} }

76
apps/mavlink/mavlink.c
Unescape View File

@ -92,7 +92,6 @@ static mavlink_status_t status;
static pthread_t receive_thread; static pthread_t receive_thread;
static pthread_t uorb_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 */ /* Allocate storage space for waypoints */
mavlink_wpm_storage wpm_s; mavlink_wpm_storage wpm_s;
@ -125,6 +124,7 @@ static struct vehicle_command_s vcmd;
static int pub_hil_global_pos = -1; static int pub_hil_global_pos = -1;
static int ardrone_motors_pub = -1; static int ardrone_motors_pub = -1;
static int cmd_pub = -1; static int cmd_pub = -1;
static int sensor_sub = -1;
static int global_pos_sub = -1; static int global_pos_sub = -1;
static int local_pos_sub = -1; static int local_pos_sub = -1;
static int flow_pub = -1; static int flow_pub = -1;
@ -154,7 +154,7 @@ void handleMessage(mavlink_message_t *msg);
/** /**
* Enable / disable Hardware in the Loop simulation mode. * 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. * 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; int ret = OK;
/* Enable HIL */ /* Enable HIL */
if ((vehicle_mode & MAV_MODE_FLAG_HIL_ENABLED) && !mavlink_hil_enabled) { if (hil_enabled && !mavlink_hil_enabled) {
//printf("\n HIL ON \n"); //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; mavlink_hil_enabled = false;
(void)close(pub_hil_attitude); (void)close(pub_hil_attitude);
(void)close(pub_hil_global_pos); (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; *mavlink_mode = 0;
/* set mode flags independent of system state */ /* 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; *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
} }
@ -432,6 +432,28 @@ static void *receiveloop(void *arg)
return NULL; 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. * Listen for uORB topics and send via MAVLink.
* *
@ -466,7 +488,7 @@ static void *uorb_receiveloop(void *arg)
/* --- SENSORS RAW VALUE --- */ /* --- SENSORS RAW VALUE --- */
/* subscribe to ORB for sensors raw */ /* 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 */ orb_set_interval(sensor_sub, 100); /* 10Hz updates */
fds[fdsc_count].fd = sensor_sub; fds[fdsc_count].fd = sensor_sub;
fds[fdsc_count].events = POLLIN; fds[fdsc_count].events = POLLIN;
@ -662,11 +684,11 @@ static void *uorb_receiveloop(void *arg)
/* immediately communicate state changes back to user */ /* immediately communicate state changes back to user */
orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); orb_copy(ORB_ID(vehicle_status), status_sub, &v_status);
/* enable or disable HIL */ /* 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 */ /* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0; 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); get_mavlink_mode_and_state(&v_status, &mavlink_state, &mavlink_mode);
/* send heartbeat */ /* send heartbeat */
@ -691,7 +713,7 @@ static void *uorb_receiveloop(void *arg)
fw_control.attitude_control_output[3]); fw_control.attitude_control_output[3]);
/* Only send in HIL mode */ /* 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 */ /* 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 // 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 */ /* check if topic is advertised */
if (flow_pub <= 0) { 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) { 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. */ /* reate the device node that's used for sending text log messages, etc. */
register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL); 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 */ /* default values for arguments */
char *uart_name = "/dev/ttyS0"; char *uart_name = "/dev/ttyS0";
int baudrate = 57600; int baudrate = 57600;
@ -1283,9 +1301,23 @@ int mavlink_main(int argc, char *argv[])
uint16_t counter = 0; uint16_t counter = 0;
int lowspeed_counter = 0; int lowspeed_counter = 0;
/**< Subscribe to system state and RC channels */ /* all subscriptions are now active, set up initial guess about rate limits */
// int status_sub = orb_subscribe(ORB_ID(vehicle_status)); if (baudrate >= 921600) {
// int rc_sub = orb_subscribe(ORB_ID(rc_channels)); /* 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) { while (1) {
@ -1305,7 +1337,7 @@ int mavlink_main(int argc, char *argv[])
if (lowspeed_counter == 10) { if (lowspeed_counter == 10) {
/* translate the current syste state to mavlink state and mode */ /* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0; 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); get_mavlink_mode_and_state(&v_status, &mavlink_state, &mavlink_mode);
/* send heartbeat */ /* send heartbeat */

2
apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
Unescape View File

@ -245,7 +245,7 @@ int attitude_estimator_bm_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
/* switching from non-HIL to HIL mode */ /* 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); //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; hil_enabled = true;
publishing = false; publishing = false;
int ret = close(pub_att); int ret = close(pub_att);

2
apps/sensors/sensors.c
Unescape View File

@ -452,7 +452,7 @@ int sensors_main(int argc, char *argv[])
/* switching from non-HIL to HIL mode */ /* 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); //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; hil_enabled = true;
publishing = false; publishing = false;
int ret = close(sensor_pub); int ret = close(sensor_pub);

6
apps/systemcmds/top/top.c
Unescape View File

@ -135,9 +135,9 @@ int top_main(int argc, char *argv[])
memset(header_spaces, ' ', CONFIG_TASK_NAME_SIZE); memset(header_spaces, ' ', CONFIG_TASK_NAME_SIZE);
header_spaces[CONFIG_TASK_NAME_SIZE] = '\0'; header_spaces[CONFIG_TASK_NAME_SIZE] = '\0';
#if CONFIG_RR_INTERVAL > 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 #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 #endif
} else { } else {
@ -190,7 +190,7 @@ int top_main(int argc, char *argv[])
runtime_spaces = ""; 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 */ /* Print scheduling info with RR time slice */
#if CONFIG_RR_INTERVAL > 0 #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); 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);

32
apps/uORB/topics/vehicle_status.h
Unescape View File

@ -87,9 +87,16 @@ enum VEHICLE_MODE_FLAG {
}; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ }; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */
enum VEHICLE_FLIGHT_MODE { enum VEHICLE_FLIGHT_MODE {
VEHICLE_FLIGHT_MODE_MANUAL, VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, same as VEHICLE_FLIGHT_MODE_ATTITUDE for multirotors */
VEHICLE_FLIGHT_MODE_STABILIZED, VEHICLE_FLIGHT_MODE_ATTITUDE, /**< attitude or rate stabilization, as defined by VEHICLE_ATTITUDE_MODE */
VEHICLE_FLIGHT_MODE_AUTO 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 */ 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_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 flag_control_rates_enabled; /**< true if rates are stabilized */
bool control_rates_enabled; /**< true if rates are stabilized */ // bool flag_control_attitude_enabled; *< true if attitude stabilization is mixed in
bool control_attitude_enabled; /**< true if attitude stabilization is mixed in */ // bool control_speed_enabled; /**< true if speed (implies direction) is controlled */
bool control_speed_enabled; /**< true if speed (implies direction) is controlled */ // bool control_position_enabled; /**< true if position is controlled */
bool control_position_enabled; /**< true if position is controlled */
bool preflight_gyro_calibration; /**< true if gyro calibration is requested */ bool preflight_gyro_calibration; /**< true if gyro calibration is requested */
bool preflight_mag_calibration; /**< true if mag calibration is requested */ bool preflight_mag_calibration; /**< true if mag calibration is requested */

Write Preview
Loading…
Cancel
Save