fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 06:01:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Massive improvements in state machine, still tracing wrong throttle scaling in manual input path

Browse Source
This commit is contained in:
Lorenz Meier
2012-08-13 18:53:37 +02:00
parent 56b3b46f75
commit 9014577aff
10 changed files with 268 additions and 167 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/ardrone_control/Makefile
+1 -1
View File
@@ -37,7 +37,7 @@
APPNAME = ardrone_control APPNAME = ardrone_control
PRIORITY = SCHED_PRIORITY_MAX - 15 PRIORITY = SCHED_PRIORITY_MAX - 15
STACKSIZE = 2048 STACKSIZE = 3096
# explicit list of sources - not everything is built currently # explicit list of sources - not everything is built currently
CSRCS = ardrone_control.c ardrone_motor_control.c ardrone_control_helper.c rate_control.c attitude_control.c pid.c CSRCS = ardrone_control.c ardrone_motor_control.c ardrone_control_helper.c rate_control.c attitude_control.c pid.c
apps/ardrone_control/ardrone_control.c
+7 -2
View File
@@ -187,6 +187,9 @@ int ardrone_control_main(int argc, char *argv[])
while (1) { while (1) {
/* get a local copy of the vehicle state */
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
if (state.state_machine == SYSTEM_STATE_MANUAL || if (state.state_machine == SYSTEM_STATE_MANUAL ||
state.state_machine == SYSTEM_STATE_GROUND_READY || state.state_machine == SYSTEM_STATE_GROUND_READY ||
state.state_machine == SYSTEM_STATE_STABILIZED || state.state_machine == SYSTEM_STATE_STABILIZED ||
@@ -205,8 +208,6 @@ int ardrone_control_main(int argc, char *argv[])
/* hardcore, last-resort safety checking */ /* hardcore, last-resort safety checking */
//if (status->rc_signal_lost) { //if (status->rc_signal_lost) {
/* get a local copy of the vehicle state */
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
/* get a local copy of manual setpoint */ /* get a local copy of manual setpoint */
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
/* get a local copy of attitude */ /* get a local copy of attitude */
@@ -217,8 +218,12 @@ int ardrone_control_main(int argc, char *argv[])
att_sp.roll_body = -manual.roll * M_PI_F / 8.0f; att_sp.roll_body = -manual.roll * M_PI_F / 8.0f;
att_sp.pitch_body = -manual.pitch * M_PI_F / 8.0f; att_sp.pitch_body = -manual.pitch * M_PI_F / 8.0f;
att_sp.yaw_body = -manual.yaw * M_PI_F; att_sp.yaw_body = -manual.yaw * M_PI_F;
att_sp.thrust = manual.throttle/2.0f;
control_attitude(ardrone_write, &att_sp, &att, &state); control_attitude(ardrone_write, &att_sp, &att, &state);
} else {
/* invalid mode, complain */
if (counter % 200 == 0) printf("[multirotor control] INVALID CONTROL MODE, locking down propulsion\n");
} }
} }
apps/ardrone_control/attitude_control.c
+15 -16
View File
@@ -83,7 +83,7 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
static float pid_yawspeed_lim; static float pid_yawspeed_lim;
static float pid_att_lim; static float pid_att_lim;
static bool initialized; static bool initialized = false;
/* initialize the pid controllers when the function is called for the first time */ /* initialize the pid controllers when the function is called for the first time */
if (initialized == false) { if (initialized == false) {
@@ -117,8 +117,8 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
PID_MODE_DERIVATIV_SET, 157); PID_MODE_DERIVATIV_SET, 157);
pid_yawpos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_LIM]; pid_yawpos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_LIM];
pid_yawspeed_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM]; pid_yawspeed_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM];
pid_att_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM]; pid_att_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM];
initialized = true; initialized = true;
} }
@@ -150,8 +150,8 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU]); global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU]);
pid_yawpos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_LIM]; pid_yawpos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_LIM];
pid_yawspeed_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM]; pid_yawspeed_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM];
pid_att_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM]; pid_att_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM];
} }
/*Calculate Controllers*/ /*Calculate Controllers*/
@@ -188,17 +188,13 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
float motor_thrust; float motor_thrust;
// FLYING MODES // FLYING MODES
if (status->state_machine == SYSTEM_STATE_MANUAL) { if (status->state_machine == SYSTEM_STATE_MANUAL ||
motor_thrust = att_sp->thrust; status->state_machine == SYSTEM_STATE_GROUND_READY ||
} else if (status->state_machine == SYSTEM_STATE_GROUND_READY ||
status->state_machine == SYSTEM_STATE_STABILIZED || status->state_machine == SYSTEM_STATE_STABILIZED ||
status->state_machine == SYSTEM_STATE_AUTO || status->state_machine == SYSTEM_STATE_AUTO ||
status->state_machine == SYSTEM_STATE_MISSION_ABORT) { status->state_machine == SYSTEM_STATE_MISSION_ABORT ||
motor_thrust = att_sp->thrust; //TODO status->state_machine == SYSTEM_STATE_EMCY_LANDING) {
motor_thrust = att_sp->thrust;
} else if (status->state_machine == SYSTEM_STATE_EMCY_LANDING) {
motor_thrust = att_sp->thrust; //TODO
} else if (status->state_machine == SYSTEM_STATE_EMCY_CUTOFF) { } else if (status->state_machine == SYSTEM_STATE_EMCY_CUTOFF) {
/* immediately cut off motors */ /* immediately cut off motors */
@@ -209,6 +205,8 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
motor_thrust = 0.0f; motor_thrust = 0.0f;
} }
printf("mot0: %3.1f\n", motor_thrust);
/* compensate thrust vector for roll / pitch contributions */ /* compensate thrust vector for roll / pitch contributions */
motor_thrust *= zcompensation; motor_thrust *= zcompensation;
@@ -244,6 +242,8 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
roll_controller.saturated = 1; roll_controller.saturated = 1;
} }
printf("mot1: %3.1f\n", motor_thrust);
float output_band = 0.0f; float output_band = 0.0f;
float band_factor = 0.75f; float band_factor = 0.75f;
const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */ const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */
@@ -332,12 +332,11 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
/* send motors via UART */ /* send motors via UART */
if (motor_skip_counter % 5 == 0) { if (motor_skip_counter % 5 == 0) {
if (motor_skip_counter % 50) printf("mot: %1.3f-%i-%i-%i-%i\n", motor_thrust, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]); if (motor_skip_counter % 50 == 0) printf("mot: %3.1f-%i-%i-%i-%i\n", motor_thrust, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
uint8_t buf[5] = {1, 2, 3, 4, 5}; uint8_t buf[5] = {1, 2, 3, 4, 5};
ar_get_motor_packet(buf, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]); ar_get_motor_packet(buf, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
write(ardrone_write, buf, sizeof(buf)); write(ardrone_write, buf, sizeof(buf));
} }
motor_skip_counter++; motor_skip_counter++;
} }
apps/commander/commander.c
+47 -47
View File
@@ -230,11 +230,11 @@ void cal_bsort(int16_t a[], int n)
} }
} }
void do_mag_calibration(int status_pub, struct vehicle_status_s *current_status) void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
{ {
/* set to mag calibration mode */ /* set to mag calibration mode */
current_status->preflight_mag_calibration = true; status->preflight_mag_calibration = true;
state_machine_publish(status_pub, current_status); state_machine_publish(status_pub, status, mavlink_fd);
int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
struct sensor_combined_s raw; struct sensor_combined_s raw;
@@ -312,8 +312,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *current_status)
} }
/* disable calibration mode */ /* disable calibration mode */
current_status->preflight_mag_calibration = false; status->preflight_mag_calibration = false;
state_machine_publish(status_pub, current_status); state_machine_publish(status_pub, status, mavlink_fd);
/* sort values */ /* sort values */
cal_bsort(mag_minima[0], peak_samples); cal_bsort(mag_minima[0], peak_samples);
@@ -393,7 +393,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *current_status)
free(mag_minima[2]); free(mag_minima[2]);
char offset_output[50]; char offset_output[50];
sprintf(offset_output, "[commander] mag calibration finished, offsets: x:%d, y:%d, z:%d", mag_offset[0], mag_offset[1], mag_offset[2]); sprintf(offset_output, "[commander] mag cal: x:%8.4f y:%8.4f z:%8.4f", (double)mag_offset[0], (double)mag_offset[1], (double)mag_offset[2]);
mavlink_log_info(mavlink_fd, offset_output); mavlink_log_info(mavlink_fd, offset_output);
close(sub_sensor_combined); close(sub_sensor_combined);
@@ -436,7 +436,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *current_status
global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_ZOFFSET] = gyro_offset[2]; global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_ZOFFSET] = gyro_offset[2];
char offset_output[50]; char offset_output[50];
sprintf(offset_output, "[commander] gyro calibration finished, offsets: x:%d, y:%d, z:%d", (int)gyro_offset[0], (int)gyro_offset[1], (int)gyro_offset[2]); sprintf(offset_output, "[commander] gyro cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
mavlink_log_info(mavlink_fd, offset_output); mavlink_log_info(mavlink_fd, offset_output);
close(sub_sensor_combined); close(sub_sensor_combined);
@@ -458,7 +458,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
switch (cmd->command) { switch (cmd->command) {
case MAV_CMD_DO_SET_MODE: case MAV_CMD_DO_SET_MODE:
{ {
if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, (uint8_t)cmd->param1)) { if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) {
result = MAV_RESULT_ACCEPTED; result = MAV_RESULT_ACCEPTED;
} else { } else {
result = MAV_RESULT_DENIED; result = MAV_RESULT_DENIED;
@@ -466,18 +466,17 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
} }
break; break;
case MAV_CMD_COMPONENT_ARM_DISARM: case MAV_CMD_COMPONENT_ARM_DISARM: {
{
/* request to arm */ /* request to arm */
if ((int)cmd->param1 == 1) { if ((int)cmd->param1 == 1) {
if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
result = MAV_RESULT_ACCEPTED; result = MAV_RESULT_ACCEPTED;
} else { } else {
result = MAV_RESULT_DENIED; result = MAV_RESULT_DENIED;
} }
/* request to disarm */ /* request to disarm */
} else if ((int)cmd->param1 == 0) { } else if ((int)cmd->param1 == 0) {
if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
result = MAV_RESULT_ACCEPTED; result = MAV_RESULT_ACCEPTED;
} else { } else {
result = MAV_RESULT_DENIED; result = MAV_RESULT_DENIED;
@@ -487,19 +486,19 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
break; break;
/* request for an autopilot reboot */ /* request for an autopilot reboot */
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: {
{ if ((int)cmd->param1 == 1) {
// if ((int)cmd->param1 == 1) { if (OK == do_state_update(status_pub, current_vehicle_status, mavlink_fd, SYSTEM_STATE_REBOOT)) {
// if (OK == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_HALT)) { /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */
// result = MAV_RESULT_ACCEPTED;//TODO: this has no effect result = MAV_RESULT_ACCEPTED;
// } else { } else {
// result = MAV_RESULT_DENIED; /* system may return here */
// } result = MAV_RESULT_DENIED;
// } }
}
} }
break; break;
//
// /* request to land */ // /* request to land */
// case MAV_CMD_NAV_LAND: // case MAV_CMD_NAV_LAND:
// { // {
@@ -524,18 +523,18 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
// break; // break;
// //
/* preflight calibration */ /* preflight calibration */
case MAV_CMD_PREFLIGHT_CALIBRATION: { case MAV_CMD_PREFLIGHT_CALIBRATION: {
bool handled = false; bool handled = false;
/* gyro calibration */ /* gyro calibration */
if ((int)(cmd->param1) == 1) { if ((int)(cmd->param1) == 1) {
/* transition to calibration state */ /* transition to calibration state */
do_state_update(status_pub, &current_status, SYSTEM_STATE_PREFLIGHT); do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
mavlink_log_info(mavlink_fd, "[commander] starting gyro calibration"); mavlink_log_info(mavlink_fd, "[commander] starting gyro calibration");
do_gyro_calibration(status_pub, &current_status); do_gyro_calibration(status_pub, &current_status);
do_state_update(status_pub, &current_status, SYSTEM_STATE_STANDBY); do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
result = MAV_RESULT_ACCEPTED; result = MAV_RESULT_ACCEPTED;
} else { } else {
mavlink_log_critical(mavlink_fd, "[commander] REJECTING gyro calibration"); mavlink_log_critical(mavlink_fd, "[commander] REJECTING gyro calibration");
@@ -547,12 +546,12 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
/* magnetometer calibration */ /* magnetometer calibration */
if ((int)(cmd->param2) == 1) { if ((int)(cmd->param2) == 1) {
/* transition to calibration state */ /* transition to calibration state */
do_state_update(status_pub, &current_status, SYSTEM_STATE_PREFLIGHT); do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
mavlink_log_info(mavlink_fd, "[commander] starting mag calibration"); mavlink_log_info(mavlink_fd, "[commander] starting mag calibration");
do_mag_calibration(status_pub, &current_status); do_mag_calibration(status_pub, &current_status);
do_state_update(status_pub, &current_status, SYSTEM_STATE_STANDBY); do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
result = MAV_RESULT_ACCEPTED; result = MAV_RESULT_ACCEPTED;
} else { } else {
mavlink_log_critical(mavlink_fd, "[commander] REJECTING mag calibration"); mavlink_log_critical(mavlink_fd, "[commander] REJECTING mag calibration");
@@ -571,7 +570,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
break; break;
/* preflight parameter load / store */ /* preflight parameter load / store */
case MAV_CMD_PREFLIGHT_STORAGE: { case MAV_CMD_PREFLIGHT_STORAGE: {
/* Read all parameters from EEPROM to RAM */ /* Read all parameters from EEPROM to RAM */
if (((int)cmd->param1) == 0) { if (((int)cmd->param1) == 0) {
@@ -610,7 +609,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
} }
break; break;
default: { default: {
mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported command"); mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported command");
result = MAV_RESULT_UNSUPPORTED; result = MAV_RESULT_UNSUPPORTED;
} }
@@ -625,8 +624,6 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
/* send acknowledge command */ /* send acknowledge command */
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_command_ack_pack(0, 0, &msg, cmd->command, result); mavlink_msg_command_ack_pack(0, 0, &msg, cmd->command, result);
//global_data_send_mavlink_message_out(&msg);
} }
} }
@@ -736,6 +733,8 @@ enum BAT_CHEM {
* *
* @return the estimated remaining capacity in 0..1 * @return the estimated remaining capacity in 0..1
*/ */
float battery_remaining_estimate_voltage(int cells, int chemistry, float voltage);
float battery_remaining_estimate_voltage(int cells, int chemistry, float voltage) float battery_remaining_estimate_voltage(int cells, int chemistry, float voltage)
{ {
float ret = 0; float ret = 0;
@@ -783,17 +782,19 @@ int commander_main(int argc, char *argv[])
fprintf(stderr, "[commander] ERROR: Failed to open MAVLink log stream, start mavlink app first.\n"); fprintf(stderr, "[commander] ERROR: Failed to open MAVLink log stream, start mavlink app first.\n");
} }
/* make sure we are in preflight state */
memset(&current_status, 0, sizeof(current_status));
current_status.state_machine = SYSTEM_STATE_PREFLIGHT;
/* 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);
state_machine_publish(stat_pub, &current_status, mavlink_fd);
if (stat_pub < 0) { if (stat_pub < 0) {
printf("[commander] ERROR: orb_advertise for topic vehicle_status failed.\n"); printf("[commander] ERROR: orb_advertise for topic vehicle_status failed.\n");
exit(ERROR); exit(ERROR);
} }
/* make sure we are in preflight state */
//do_state_update(stat_pub, &current_status, (commander_state_machine_t)SYSTEM_STATE_PREFLIGHT);
mavlink_log_info(mavlink_fd, "[commander] system is running"); mavlink_log_info(mavlink_fd, "[commander] system is running");
/* load EEPROM parameters */ /* load EEPROM parameters */
@@ -854,11 +855,10 @@ int commander_main(int argc, char *argv[])
struct sensor_combined_s sensors; struct sensor_combined_s sensors;
memset(&sensors, 0, sizeof(sensors)); memset(&sensors, 0, sizeof(sensors));
uint8_t vehicle_state_previous = current_status.state_machine; // uint8_t vehicle_state_previous = current_status.state_machine;
float voltage_previous = 0.0f; float voltage_previous = 0.0f;
uint64_t last_idle_time = 0; uint64_t last_idle_time = 0;
unsigned int signal_loss_counter = 0;
/* now initialized */ /* now initialized */
commander_initialized = true; commander_initialized = true;
@@ -891,7 +891,7 @@ 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 & MAV_MODE_FLAG_HIL_ENABLED)) { if ((current_status.mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
/* armed */ /* armed */
led_toggle(LED_AMBER); led_toggle(LED_AMBER);
@@ -994,7 +994,7 @@ int commander_main(int argc, char *argv[])
// //
// //
// if (flight_env == PX4_FLIGHT_ENVIRONMENT_TESTING) //simulate position fix for quick indoor tests // if (flight_env == PX4_FLIGHT_ENVIRONMENT_TESTING) //simulate position fix for quick indoor tests
update_state_machine_got_position_fix(stat_pub, &current_status); update_state_machine_got_position_fix(stat_pub, &current_status, mavlink_fd);
/* end: check gps */ /* end: check gps */
/* Check battery voltage */ /* Check battery voltage */
@@ -1017,7 +1017,7 @@ int commander_main(int argc, char *argv[])
if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
critical_battery_voltage_actions_done = true; critical_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY! CIRITICAL BATTERY!"); mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY! CIRITICAL BATTERY!");
state_machine_emergency(stat_pub, &current_status); state_machine_emergency(stat_pub, &current_status, mavlink_fd);
} }
critical_voltage_counter++; critical_voltage_counter++;
@@ -1044,7 +1044,7 @@ int commander_main(int argc, char *argv[])
/* check if left stick is in lower left position --> switch to standby state */ /* check if left stick is in lower left position --> switch to standby state */
if (rc_yaw_scale < -STICK_ON_OFF_LIMIT && rc_throttle_scale < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values if (rc_yaw_scale < -STICK_ON_OFF_LIMIT && rc_throttle_scale < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
update_state_machine_disarm(stat_pub, &current_status); update_state_machine_disarm(stat_pub, &current_status, mavlink_fd);
stick_on_counter = 0; stick_on_counter = 0;
} else { } else {
@@ -1056,7 +1056,7 @@ int commander_main(int argc, char *argv[])
/* check if left stick is in lower right position --> arm */ /* check if left stick is in lower right position --> arm */
if (rc_yaw_scale > STICK_ON_OFF_LIMIT && rc_throttle_scale < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values if (rc_yaw_scale > STICK_ON_OFF_LIMIT && rc_throttle_scale < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
update_state_machine_arm(stat_pub, &current_status); update_state_machine_arm(stat_pub, &current_status, mavlink_fd);
stick_on_counter = 0; stick_on_counter = 0;
} else { } else {
@@ -1067,13 +1067,13 @@ int commander_main(int argc, char *argv[])
//printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count); //printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count);
if (mode_switch_rc_value > STICK_ON_OFF_LIMIT) { if (mode_switch_rc_value > STICK_ON_OFF_LIMIT) {
update_state_machine_mode_manual(stat_pub, &current_status); update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
} else if (mode_switch_rc_value < -STICK_ON_OFF_LIMIT) { } else if (mode_switch_rc_value < -STICK_ON_OFF_LIMIT) {
update_state_machine_mode_auto(stat_pub, &current_status); update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
} else { } else {
update_state_machine_mode_stabilized(stat_pub, &current_status); update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
} }
/* Publish RC signal */ /* Publish RC signal */
@@ -1112,11 +1112,11 @@ int commander_main(int argc, char *argv[])
current_status.preflight_gyro_calibration == false && current_status.preflight_gyro_calibration == false &&
current_status.preflight_mag_calibration == false) { current_status.preflight_mag_calibration == false) {
/* All ok, no calibration going on, go to standby */ /* All ok, no calibration going on, go to standby */
do_state_update(stat_pub, &current_status, SYSTEM_STATE_STANDBY); do_state_update(stat_pub, &current_status, SYSTEM_STATE_STANDBY, mavlink_fd);
} }
/* Store old modes to detect and act on state transitions */ /* Store old modes to detect and act on state transitions */
vehicle_state_previous = current_status.state_machine; // vehicle_state_previous = current_status.state_machine;
voltage_previous = current_status.voltage_battery; voltage_previous = current_status.voltage_battery;
fflush(stdout); fflush(stdout);
apps/commander/state_machine_helper.c
+83 -64
View File
@@ -45,6 +45,7 @@
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.h>
#include <systemlib/systemlib.h> #include <systemlib/systemlib.h>
#include <arch/board/up_hrt.h> #include <arch/board/up_hrt.h>
#include <mavlink/mavlink_log.h>
static const char* system_state_txt[] = { static const char* system_state_txt[] = {
"SYSTEM_STATE_PREFLIGHT", "SYSTEM_STATE_PREFLIGHT",
@@ -61,10 +62,13 @@ static const char* system_state_txt[] = {
}; };
/**
void do_state_update(int status_pub, struct vehicle_status_s *current_status, commander_state_machine_t new_state) * Transition from one state to another
*/
int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state)
{ {
int invalid_state = false; int invalid_state = false;
int ret = ERROR;
commander_state_machine_t old_state = current_status->state_machine; commander_state_machine_t old_state = current_status->state_machine;
@@ -74,10 +78,10 @@ void do_state_update(int status_pub, struct vehicle_status_s *current_status, co
uint8_t flight_environment_parameter = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]); uint8_t flight_environment_parameter = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING); ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING);
} else { } else {
do_state_update(status_pub, current_status, (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; return;
@@ -89,63 +93,63 @@ void do_state_update(int status_pub, struct vehicle_status_s *current_status, co
//TODO: add emcy landing code here //TODO: add emcy landing code here
fprintf(stderr, "[commander] EMERGENCY LANDING!\n"); fprintf(stderr, "[commander] EMERGENCY LANDING!\n");
//global_data_send_mavlink_statustext_message_out("Commander: state: emergency landing", MAV_SEVERITY_INFO); mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY LANDING!");
break; break;
case SYSTEM_STATE_EMCY_CUTOFF: case SYSTEM_STATE_EMCY_CUTOFF:
/* Tell the controller to cutoff the motors (thrust = 0), make sure that this is not overwritten by another app and stays at 0 */ /* Tell the controller to cutoff the motors (thrust = 0) */
//TODO: add emcy cutoff code here
fprintf(stderr, "[commander] EMERGENCY MOTOR CUTOFF!\n"); fprintf(stderr, "[commander] EMERGENCY MOTOR CUTOFF!\n");
//global_data_send_mavlink_statustext_message_out("Commander: state: emergency cutoff", MAV_SEVERITY_INFO); mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY MOTOR CUTOFF!");
break; break;
case SYSTEM_STATE_GROUND_ERROR: case SYSTEM_STATE_GROUND_ERROR:
fprintf(stderr, "[commander] GROUND ERROR, locking down propulsion system\n"); fprintf(stderr, "[commander] GROUND ERROR, locking down propulsion system\n");
//global_data_send_mavlink_statustext_message_out("Commander: state: ground error", MAV_SEVERITY_INFO); mavlink_log_critical(mavlink_fd, "[commander] GROUND ERROR, locking down propulsion system");
break; break;
case SYSTEM_STATE_PREFLIGHT: case SYSTEM_STATE_PREFLIGHT:
//global_data_send_mavlink_statustext_message_out("Commander: state: preflight", MAV_SEVERITY_INFO);
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;
mavlink_log_info(mavlink_fd, "[commander] Switched to PREFLIGHT state");
} else { } else {
invalid_state = true; invalid_state = true;
mavlink_log_critical(mavlink_fd, "[commander] REFUSED to switch to PREFLIGHT state");
} }
break; break;
case SYSTEM_STATE_REBOOT: case SYSTEM_STATE_REBOOT:
usleep(500000); if (current_status->state_machine == SYSTEM_STATE_STANDBY
reboot(); || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
invalid_state = false;
mavlink_log_critical(mavlink_fd, "[commander] REBOOTING SYSTEM");
usleep(500000);
reboot();
/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
} else {
invalid_state = true;
mavlink_log_critical(mavlink_fd, "[commander] REFUSED to REBOOT");
}
break; break;
case SYSTEM_STATE_STANDBY: case SYSTEM_STATE_STANDBY:
//global_data_send_mavlink_statustext_message_out("Commander: state: standby", MAV_SEVERITY_INFO); mavlink_log_critical(mavlink_fd, "[commander] Switched to STANDBY state");
break; break;
case SYSTEM_STATE_GROUND_READY: case SYSTEM_STATE_GROUND_READY:
//global_data_send_mavlink_statustext_message_out("Commander: state: armed", MAV_SEVERITY_INFO); mavlink_log_critical(mavlink_fd, "[commander] Switched to GROUND READY state");
//if in manual mode switch to manual state
// if (current_status->remote_manual) {
// printf("[commander] manual mode\n");
// do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
// return;
// }
break; break;
case SYSTEM_STATE_AUTO: case SYSTEM_STATE_AUTO:
//global_data_send_mavlink_statustext_message_out("Commander: state: auto", MAV_SEVERITY_INFO); mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / AUTO mode");
break; break;
case SYSTEM_STATE_STABILIZED: case SYSTEM_STATE_STABILIZED:
//global_data_send_mavlink_statustext_message_out("Commander: state: stabilized", MAV_SEVERITY_INFO); mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / STABILIZED mode");
break; break;
case SYSTEM_STATE_MANUAL: case SYSTEM_STATE_MANUAL:
//global_data_send_mavlink_statustext_message_out("Commander: state: manual", MAV_SEVERITY_INFO); mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / MANUAL mode");
break; break;
default: default:
@@ -155,11 +159,11 @@ void do_state_update(int status_pub, struct vehicle_status_s *current_status, co
if (invalid_state == false || old_state != new_state) { if (invalid_state == false || old_state != new_state) {
current_status->state_machine = new_state; current_status->state_machine = new_state;
state_machine_publish(status_pub, current_status); state_machine_publish(status_pub, current_status, mavlink_fd);
} }
} }
void state_machine_publish(int status_pub, struct vehicle_status_s *current_status) { void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) {
/* publish the new state */ /* publish the new state */
current_status->counter++; current_status->counter++;
current_status->timestamp = hrt_absolute_time(); current_status->timestamp = hrt_absolute_time();
@@ -171,26 +175,26 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
/* /*
* Private functions, update the state machine * Private functions, update the state machine
*/ */
void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status) void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{ {
fprintf(stderr, "[commander] EMERGENCY HANDLER\n"); fprintf(stderr, "[commander] EMERGENCY HANDLER\n");
/* Depending on the current state go to one of the error states */ /* Depending on the current state go to one of the error states */
if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) { if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR);
} else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) { } else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) {
do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT);
} else { } else {
fprintf(stderr, "[commander] Unknown system state: #%d\n", current_status->state_machine); fprintf(stderr, "[commander] Unknown system state: #%d\n", current_status->state_machine);
} }
} }
void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors
{ {
if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself
state_machine_emergency_always_critical(status_pub, current_status); state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd);
} else { } else {
//global_data_send_mavlink_statustext_message_out("[commander] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); //global_data_send_mavlink_statustext_message_out("[commander] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL);
@@ -399,28 +403,28 @@ void state_machine_emergency(int status_pub, struct vehicle_status_s *current_st
/* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ /* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status) void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{ {
/* Depending on the current state switch state */ /* Depending on the current state switch state */
if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
} }
} }
void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status) void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{ {
/* Depending on the current state switch state */ /* Depending on the current state switch state */
if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_AUTO) { if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_AUTO) {
state_machine_emergency(status_pub, current_status); state_machine_emergency(status_pub, current_status, mavlink_fd);
} }
} }
void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status) void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{ {
// XXX CHANGE BACK // 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, (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) { } /*else if (current_status->state_machine == SYSTEM_STATE_AUTO) {
printf("[commander] landing\n"); printf("[commander] landing\n");
@@ -428,52 +432,67 @@ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_s
}*/ }*/
} }
void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status) void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{ {
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
printf("[commander] going standby\n"); printf("[commander] going standby\n");
do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
} else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { } else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
printf("[commander] MISSION ABORT!\n"); printf("[commander] MISSION ABORT!\n");
do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
} }
} }
void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status) void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{ {
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
current_status->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) { if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
printf("[commander] manual mode\n"); printf("[commander] manual mode\n");
do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_MANUAL); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
} }
} }
void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status) void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{ {
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_STABILIZED;
current_status->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) { if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
printf("[commander] stabilized mode\n"); printf("[commander] stabilized mode\n");
do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STABILIZED); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED);
} }
} }
void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status) void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{ {
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
current_status->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) { if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) {
printf("[commander] auto mode\n"); printf("[commander] auto mode\n");
do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_AUTO); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
} }
} }
uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, uint8_t mode) uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
{ {
printf("in update state request\n"); printf("in update state request\n");
uint8_t ret = 1; uint8_t ret = 1;
current_status->mode |= MAV_MODE_FLAG_SAFETY_ARMED; current_status->mode |= VEHICLE_MODE_FLAG_SAFETY_ARMED;
/* Set manual input enabled flag */ /* Set manual input enabled flag */
current_status->mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; current_status->mode |= VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED;
do_state_update(status_pub, current_status, (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);
/* 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->mode & VEHICLE_MODE_FLAG_SAFETY_ARMED) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
@@ -481,10 +500,10 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
// 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 */ /* Set armed flag */
current_status->mode |= MAV_MODE_FLAG_SAFETY_ARMED; current_status->mode |= VEHICLE_MODE_FLAG_SAFETY_ARMED;
/* Set manual input enabled flag */ /* Set manual input enabled flag */
current_status->mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; current_status->mode |= VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED;
do_state_update(status_pub, current_status, (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");
} }
@@ -495,7 +514,7 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
/* 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 */ /* Clear armed flag, leave manual input enabled */
// current_status->mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; // current_status->mode &= ~VEHICLE_MODE_FLAG_SAFETY_ARMED;
// do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY); // do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
// ret = OK; // ret = OK;
// printf("[commander] disarming due to command request\n"); // printf("[commander] disarming due to command request\n");
@@ -503,16 +522,16 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
//} //}
/* Switch on HIL if in standby */ /* Switch on HIL if in standby */
if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & MAV_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 |= MAV_MODE_FLAG_HIL_ENABLED; current_status->mode |= VEHICLE_MODE_FLAG_HIL_ENABLED;
ret = OK; ret = OK;
state_machine_publish(status_pub, current_status); 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 & MAV_MODE_FLAG_HIL_ENABLED) && !(mode & MAV_MODE_FLAG_HIL_ENABLED)) { if ((current_status->mode & VEHICLE_MODE_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;
} }
@@ -520,7 +539,7 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
return ret; return ret;
} }
uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations
{ {
commander_state_machine_t current_system_state = current_status->state_machine; commander_state_machine_t current_system_state = current_status->state_machine;
@@ -539,7 +558,7 @@ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_
if (current_system_state == SYSTEM_STATE_STANDBY || current_system_state == SYSTEM_STATE_PREFLIGHT) { if (current_system_state == SYSTEM_STATE_STANDBY || current_system_state == SYSTEM_STATE_PREFLIGHT) {
printf("system will reboot\n"); printf("system will reboot\n");
//global_data_send_mavlink_statustext_message_out("Rebooting autopilot.. ", MAV_SEVERITY_INFO); //global_data_send_mavlink_statustext_message_out("Rebooting autopilot.. ", MAV_SEVERITY_INFO);
do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_REBOOT); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT);
ret = 0; ret = 0;
} }
@@ -549,7 +568,7 @@ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_
printf("try to switch to auto/takeoff\n"); printf("try to switch to auto/takeoff\n");
if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_MANUAL) { if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_MANUAL) {
do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_AUTO); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
printf("state: auto\n"); printf("state: auto\n");
ret = 0; ret = 0;
} }
@@ -560,7 +579,7 @@ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_
printf("try to switch to manual\n"); printf("try to switch to manual\n");
if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_AUTO) { if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_AUTO) {
do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_MANUAL); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
printf("state: manual\n"); printf("state: manual\n");
ret = 0; ret = 0;
} }
apps/commander/state_machine_helper.h
+72 -15
View File
@@ -46,8 +46,6 @@
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.h>
#include <v1.0/common/mavlink.h>
/** /**
* Switch to new state with no checking. * Switch to new state with no checking.
@@ -56,8 +54,13 @@
* the function does not question the state change, this must be done before * the function does not question the state change, this must be done before
* The function performs actions that are connected with the new state (buzzer, reboot, ...) * The function performs actions that are connected with the new state (buzzer, reboot, ...)
* *
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*
* @return 0 (macro OK) or 1 on error (macro ERROR)
*/ */
void do_state_update(int status_pub, struct vehicle_status_s *current_status, commander_state_machine_t new_state); int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state);
/* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR */ /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR */
// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); // void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
@@ -72,43 +75,75 @@ void do_state_update(int status_pub, struct vehicle_status_s *current_status, co
/** /**
* Handle state machine if got position fix * Handle state machine if got position fix
*
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*/ */
void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status); void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
/** /**
* Handle state machine if position fix lost * Handle state machine if position fix lost
*
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*/ */
void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status); void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
/** /**
* Handle state machine if user wants to arm * Handle state machine if user wants to arm
*
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*/ */
void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status); void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
/** /**
* Handle state machine if user wants to disarm * Handle state machine if user wants to disarm
*
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*/ */
void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status); void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
/** /**
* Handle state machine if mode switch is manual * Handle state machine if mode switch is manual
*
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*/ */
void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status); void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
/** /**
* Handle state machine if mode switch is stabilized * Handle state machine if mode switch is stabilized
*
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*/ */
void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status); void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
/** /**
* Handle state machine if mode switch is auto * Handle state machine if mode switch is auto
*
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*/ */
void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status); void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
/** /**
* Publish current state information * Publish current state information
*
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*/ */
void state_machine_publish(int status_pub, struct vehicle_status_s *current_status); void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
/* /*
@@ -119,17 +154,39 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
/** /**
* Handles *incoming request* to switch to a specific state, if state change is successful returns 0 * Handles *incoming request* to switch to a specific state, if state change is successful returns 0
*
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*/ */
uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, uint8_t mode); uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode);
/** /**
* Handles *incoming request* to switch to a specific custom state, if state change is successful returns 0 * Handles *incoming request* to switch to a specific custom state, if state change is successful returns 0
*
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*/ */
uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, uint8_t custom_mode); uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode);
/**
* Always switches to critical mode under any circumstances.
*
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*/
void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status); /**
void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status); * Switches to emergency if required.
*
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*/
void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
apps/mavlink/mavlink.c
+28 -19
View File
@@ -314,7 +314,14 @@ int set_hil_on_off(uint8_t vehicle_mode)
void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, uint8_t *mavlink_state, uint8_t *mavlink_mode) void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, uint8_t *mavlink_state, uint8_t *mavlink_mode)
{ {
//TODO: Make this correct /* reset MAVLink mode bitfield */
*mavlink_mode = 0;
/* set mode flags independent of system state */
if (c_status->control_manual_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
}
switch (c_status->state_machine) { switch (c_status->state_machine) {
case SYSTEM_STATE_PREFLIGHT: case SYSTEM_STATE_PREFLIGHT:
if (c_status->preflight_gyro_calibration || c_status->preflight_mag_calibration) { if (c_status->preflight_gyro_calibration || c_status->preflight_mag_calibration) {
@@ -579,8 +586,10 @@ static void *uorb_receiveloop(void *arg)
/* XXX this is seriously bad - should be an emergency */ /* XXX this is seriously bad - should be an emergency */
} else { } else {
int ifds = 0;
/* --- SENSORS RAW VALUE --- */ /* --- SENSORS RAW VALUE --- */
if (fds[0].revents & POLLIN) { if (fds[ifds++].revents & POLLIN) {
/* copy sensors raw data into local buffer */ /* copy sensors raw data into local buffer */
orb_copy(ORB_ID(sensor_combined), sensor_sub, &buf.raw); orb_copy(ORB_ID(sensor_combined), sensor_sub, &buf.raw);
@@ -596,7 +605,7 @@ static void *uorb_receiveloop(void *arg)
} }
/* --- ATTITUDE VALUE --- */ /* --- ATTITUDE VALUE --- */
if (fds[1].revents & POLLIN) { if (fds[ifds++].revents & POLLIN) {
/* copy attitude data into local buffer */ /* copy attitude data into local buffer */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &buf.att); orb_copy(ORB_ID(vehicle_attitude), att_sub, &buf.att);
@@ -608,7 +617,7 @@ static void *uorb_receiveloop(void *arg)
} }
/* --- GPS VALUE --- */ /* --- GPS VALUE --- */
if (fds[2].revents & POLLIN) { if (fds[ifds++].revents & POLLIN) {
/* copy gps data into local buffer */ /* copy gps data into local buffer */
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &buf.gps); orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &buf.gps);
/* GPS position */ /* GPS position */
@@ -622,7 +631,7 @@ static void *uorb_receiveloop(void *arg)
} }
// /* --- ARDRONE CONTROL OUTPUTS --- */ // /* --- ARDRONE CONTROL OUTPUTS --- */
// if (fds[3].revents & POLLIN) { // if (fds[ifds++].revents & POLLIN) {
// /* copy ardrone control data into local buffer */ // /* copy ardrone control data into local buffer */
// orb_copy(ORB_ID(ardrone_control), ar_sub, &buf.ar_control); // orb_copy(ORB_ID(ardrone_control), ar_sub, &buf.ar_control);
// uint64_t timestamp = buf.ar_control.timestamp; // uint64_t timestamp = buf.ar_control.timestamp;
@@ -642,7 +651,7 @@ static void *uorb_receiveloop(void *arg)
// } // }
/* --- SYSTEM STATUS --- */ /* --- SYSTEM STATUS --- */
if (fds[4].revents & POLLIN) { if (fds[ifds++].revents & POLLIN) {
/* 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 */
@@ -658,7 +667,7 @@ static void *uorb_receiveloop(void *arg)
} }
/* --- RC CHANNELS --- */ /* --- RC CHANNELS --- */
if (fds[5].revents & POLLIN) { if (fds[ifds++].revents & POLLIN) {
/* copy rc channels into local buffer */ /* copy rc channels into local buffer */
orb_copy(ORB_ID(rc_channels), rc_sub, &rc); orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
/* Channels are sent in MAVLink main loop at a fixed interval */ /* Channels are sent in MAVLink main loop at a fixed interval */
@@ -666,7 +675,7 @@ static void *uorb_receiveloop(void *arg)
} }
/* --- FIXED WING CONTROL CHANNELS --- */ /* --- FIXED WING CONTROL CHANNELS --- */
if (fds[6].revents & POLLIN) { if (fds[ifds++].revents & POLLIN) {
/* copy fixed wing control into local buffer */ /* copy fixed wing control into local buffer */
orb_copy(ORB_ID(fixedwing_control), fw_sub, &fw_control); orb_copy(ORB_ID(fixedwing_control), fw_sub, &fw_control);
/* send control output via MAVLink */ /* send control output via MAVLink */
@@ -731,7 +740,7 @@ static void *uorb_receiveloop(void *arg)
} }
/* --- VEHICLE GLOBAL POSITION --- */ /* --- VEHICLE GLOBAL POSITION --- */
if (fds[7].revents & POLLIN) { if (fds[ifds++].revents & POLLIN) {
/* copy global position data into local buffer */ /* copy global position data into local buffer */
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
uint64_t timestamp = global_pos.timestamp; uint64_t timestamp = global_pos.timestamp;
@@ -749,14 +758,14 @@ static void *uorb_receiveloop(void *arg)
} }
/* --- VEHICLE LOCAL POSITION --- */ /* --- VEHICLE LOCAL POSITION --- */
if (fds[8].revents & POLLIN) { if (fds[ifds++].revents & POLLIN) {
/* copy local position data into local buffer */ /* copy local position data into local buffer */
orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
mavlink_msg_local_position_ned_send(MAVLINK_COMM_0, local_pos.timestamp / 1000, local_pos.x, local_pos.y, local_pos.z, local_pos.vx, local_pos.vy, local_pos.vz); mavlink_msg_local_position_ned_send(MAVLINK_COMM_0, local_pos.timestamp / 1000, local_pos.x, local_pos.y, local_pos.z, local_pos.vx, local_pos.vy, local_pos.vz);
} }
/* --- VEHICLE GLOBAL SETPOINT --- */ /* --- VEHICLE GLOBAL SETPOINT --- */
if (fds[9].revents & POLLIN) { if (fds[ifds++].revents & POLLIN) {
/* copy local position data into local buffer */ /* copy local position data into local buffer */
orb_copy(ORB_ID(vehicle_global_position_setpoint), spg_sub, &buf.global_sp); orb_copy(ORB_ID(vehicle_global_position_setpoint), spg_sub, &buf.global_sp);
uint8_t coordinate_frame = MAV_FRAME_GLOBAL; uint8_t coordinate_frame = MAV_FRAME_GLOBAL;
@@ -765,14 +774,14 @@ static void *uorb_receiveloop(void *arg)
} }
/* --- VEHICLE LOCAL SETPOINT --- */ /* --- VEHICLE LOCAL SETPOINT --- */
if (fds[10].revents & POLLIN) { if (fds[ifds++].revents & POLLIN) {
/* copy local position data into local buffer */ /* copy local position data into local buffer */
orb_copy(ORB_ID(vehicle_local_position_setpoint), spl_sub, &buf.local_sp); orb_copy(ORB_ID(vehicle_local_position_setpoint), spl_sub, &buf.local_sp);
mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0, MAV_FRAME_LOCAL_NED, buf.local_sp.x, buf.local_sp.y, buf.local_sp.z, buf.local_sp.yaw); mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0, MAV_FRAME_LOCAL_NED, buf.local_sp.x, buf.local_sp.y, buf.local_sp.z, buf.local_sp.yaw);
} }
/* --- VEHICLE ATTITUDE SETPOINT --- */ /* --- VEHICLE ATTITUDE SETPOINT --- */
if (fds[11].revents & POLLIN) { if (fds[ifds++].revents & POLLIN) {
/* copy local position data into local buffer */ /* copy local position data into local buffer */
orb_copy(ORB_ID(vehicle_attitude_setpoint), spa_sub, &buf.att_sp); orb_copy(ORB_ID(vehicle_attitude_setpoint), spa_sub, &buf.att_sp);
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, buf.att_sp.timestamp/1000, buf.att_sp.roll_tait_bryan, buf.att_sp.pitch_tait_bryan, buf.att_sp.yaw_tait_bryan, buf.att_sp.thrust); mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, buf.att_sp.timestamp/1000, buf.att_sp.roll_tait_bryan, buf.att_sp.pitch_tait_bryan, buf.att_sp.yaw_tait_bryan, buf.att_sp.thrust);
@@ -1148,15 +1157,15 @@ int mavlink_main(int argc, char *argv[])
//default values for arguments //default values for arguments
char *uart_name = "/dev/ttyS0"; char *uart_name = "/dev/ttyS0";
int baudrate = 115200; int baudrate = 57600;
const char *commandline_usage = "\tusage: %s -d <devicename> -b <baudrate> [-e/--exit-allowed]\n"; const char *commandline_usage = "\tusage: %s -d <devicename> -b <baudrate> [-e/--exit-allowed]\n\t\tdefault: -d %s -b %i\n";
/* read program arguments */ /* read program arguments */
int i; int i;
for (i = 1; i < argc; i++) { /* argv[0] is "mavlink" */ for (i = 1; i < argc; i++) { /* argv[0] is "mavlink" */
if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) {
printf(commandline_usage, argv[0]); printf(commandline_usage, argv[0], uart_name, baudrate);
return 0; return 0;
} }
@@ -1166,7 +1175,7 @@ int mavlink_main(int argc, char *argv[])
uart_name = argv[i + 1]; uart_name = argv[i + 1];
} else { } else {
printf(commandline_usage, argv[0]); printf(commandline_usage, argv[0], uart_name, baudrate);
return 0; return 0;
} }
} }
@@ -1177,7 +1186,7 @@ int mavlink_main(int argc, char *argv[])
baudrate = atoi(argv[i + 1]); baudrate = atoi(argv[i + 1]);
} else { } else {
printf(commandline_usage, argv[0]); printf(commandline_usage, argv[0], uart_name, baudrate);
return 0; return 0;
} }
} }
@@ -1223,7 +1232,7 @@ int mavlink_main(int argc, char *argv[])
pthread_attr_t uorb_attr; pthread_attr_t uorb_attr;
pthread_attr_init(&uorb_attr); pthread_attr_init(&uorb_attr);
/* Set stack size, needs more than 2048 bytes */ /* Set stack size, needs more than 2048 bytes */
pthread_attr_setstacksize(&uorb_attr, 4096); pthread_attr_setstacksize(&uorb_attr, 5096);
pthread_create(&uorb_receive_thread, &uorb_attr, uorb_receiveloop, NULL); pthread_create(&uorb_receive_thread, &uorb_attr, uorb_receiveloop, NULL);
/* initialize waypoint manager */ /* initialize waypoint manager */
apps/sensors/Makefile
+1 -1
View File
@@ -37,6 +37,6 @@
APPNAME = sensors APPNAME = sensors
PRIORITY = SCHED_PRIORITY_MAX-5 PRIORITY = SCHED_PRIORITY_MAX-5
STACKSIZE = 2560 STACKSIZE = 2048
include $(APPDIR)/mk/app.mk include $(APPDIR)/mk/app.mk
apps/sensors/sensors.c
+5 -1
View File
@@ -398,7 +398,7 @@ int sensors_main(int argc, char *argv[])
int manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); int manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
if (manual_control_pub < 0) { if (manual_control_pub < 0) {
printf("[sensors] ERROR: orb_advertise for topic manual_control_setpoint failed.\n"); fprintf(stderr, "[sensors] ERROR: orb_advertise for topic manual_control_setpoint failed.\n");
} }
/* advertise the rc topic */ /* advertise the rc topic */
@@ -406,6 +406,10 @@ int sensors_main(int argc, char *argv[])
memset(&rc, 0, sizeof(rc)); memset(&rc, 0, sizeof(rc));
int rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); int rc_pub = orb_advertise(ORB_ID(rc_channels), &rc);
if (rc_pub < 0) {
fprintf(stderr, "[sensors] ERROR: orb_advertise for topic rc_channels failed.\n");
}
/* subscribe to system status */ /* subscribe to system status */
struct vehicle_status_s vstatus; struct vehicle_status_s vstatus;
memset(&vstatus, 0, sizeof(vstatus)); memset(&vstatus, 0, sizeof(vstatus));
apps/uORB/topics/vehicle_status.h
+9 -1
View File
@@ -86,6 +86,12 @@ enum VEHICLE_MODE_FLAG {
VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1
}; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ }; /**< 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
};
/** /**
* state machine / state of vehicle. * state machine / state of vehicle.
* *
@@ -98,7 +104,9 @@ struct vehicle_status_s
uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ uint16_t counter; /**< incremented by the writing thread everytime new data is stored */
uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
commander_state_machine_t 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 */
uint8_t mode; uint8_t mode;
bool control_manual_enabled; /**< true if manual input is mixed in */ bool control_manual_enabled; /**< true if manual input is mixed in */