dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
apps/ardrone_control/Makefile
View File

@ -37,7 +37,7 @@
APPNAME = ardrone_control
PRIORITY = SCHED_PRIORITY_MAX - 15
STACKSIZE = 2048
STACKSIZE = 3096
# 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

9
apps/ardrone_control/ardrone_control.c
View File

@ -187,6 +187,9 @@ int ardrone_control_main(int argc, char *argv[])
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 ||
state.state_machine == SYSTEM_STATE_GROUND_READY ||
state.state_machine == SYSTEM_STATE_STABILIZED ||
@ -205,8 +208,6 @@ int ardrone_control_main(int argc, char *argv[])
/* hardcore, last-resort safety checking */
//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 */
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
/* 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.pitch_body = -manual.pitch * M_PI_F / 8.0f;
att_sp.yaw_body = -manual.yaw * M_PI_F;
att_sp.thrust = manual.throttle/2.0f;
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");
}
}

31
apps/ardrone_control/attitude_control.c
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_att_lim;
static bool initialized;
static bool initialized = false;
/* initialize the pid controllers when the function is called for the first time */
if (initialized == false) {
@ -117,8 +117,8 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
PID_MODE_DERIVATIV_SET, 157);
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_att_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM];
pid_yawspeed_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM];
pid_att_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM];
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]);
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_att_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM];
pid_yawspeed_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM];
pid_att_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM];
}
/*Calculate Controllers*/
@ -188,17 +188,13 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
float motor_thrust;
// FLYING MODES
if (status->state_machine == SYSTEM_STATE_MANUAL) {
motor_thrust = att_sp->thrust;
} else if (status->state_machine == SYSTEM_STATE_GROUND_READY ||
if (status->state_machine == SYSTEM_STATE_MANUAL ||
status->state_machine == SYSTEM_STATE_GROUND_READY ||
status->state_machine == SYSTEM_STATE_STABILIZED ||
status->state_machine == SYSTEM_STATE_AUTO ||
status->state_machine == SYSTEM_STATE_MISSION_ABORT) {
motor_thrust = att_sp->thrust; //TODO
} else if (status->state_machine == SYSTEM_STATE_EMCY_LANDING) {
motor_thrust = att_sp->thrust; //TODO
status->state_machine == SYSTEM_STATE_MISSION_ABORT ||
status->state_machine == SYSTEM_STATE_EMCY_LANDING) {
motor_thrust = att_sp->thrust;
} else if (status->state_machine == SYSTEM_STATE_EMCY_CUTOFF) {
/* immediately cut off motors */
@ -209,6 +205,8 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
motor_thrust = 0.0f;
}
printf("mot0: %3.1f\n", motor_thrust);
/* compensate thrust vector for roll / pitch contributions */
motor_thrust *= zcompensation;
@ -244,6 +242,8 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
roll_controller.saturated = 1;
}
printf("mot1: %3.1f\n", motor_thrust);
float output_band = 0.0f;
float band_factor = 0.75f;
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 */
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};
ar_get_motor_packet(buf, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
write(ardrone_write, buf, sizeof(buf));
}
motor_skip_counter++;
}

94
apps/commander/commander.c
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 */
current_status->preflight_mag_calibration = true;
state_machine_publish(status_pub, current_status);
status->preflight_mag_calibration = true;
state_machine_publish(status_pub, status, mavlink_fd);
int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
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 */
current_status->preflight_mag_calibration = false;
state_machine_publish(status_pub, current_status);
status->preflight_mag_calibration = false;
state_machine_publish(status_pub, status, mavlink_fd);
/* sort values */
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]);
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);
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];
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);
close(sub_sensor_combined);
@ -458,7 +458,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
switch (cmd->command) {
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;
} else {
result = MAV_RESULT_DENIED;
@ -466,18 +466,17 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
}
break;
case MAV_CMD_COMPONENT_ARM_DISARM:
{
case MAV_CMD_COMPONENT_ARM_DISARM: {
/* request to arm */
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;
} else {
result = MAV_RESULT_DENIED;
}
/* request to disarm */
} 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;
} else {
result = MAV_RESULT_DENIED;
@ -487,19 +486,19 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
break;
/* request for an autopilot reboot */
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
{
// if ((int)cmd->param1 == 1) {
// if (OK == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_HALT)) {
// result = MAV_RESULT_ACCEPTED;//TODO: this has no effect
// } else {
// result = MAV_RESULT_DENIED;
// }
// }
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: {
if ((int)cmd->param1 == 1) {
if (OK == do_state_update(status_pub, current_vehicle_status, mavlink_fd, SYSTEM_STATE_REBOOT)) {
/* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */
result = MAV_RESULT_ACCEPTED;
} else {
/* system may return here */
result = MAV_RESULT_DENIED;
}
}
}
break;
//
// /* request to land */
// case MAV_CMD_NAV_LAND:
// {
@ -524,18 +523,18 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
// break;
//
/* preflight calibration */
case MAV_CMD_PREFLIGHT_CALIBRATION: {
case MAV_CMD_PREFLIGHT_CALIBRATION: {
bool handled = false;
/* gyro calibration */
if ((int)(cmd->param1) == 1) {
/* 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) {
mavlink_log_info(mavlink_fd, "[commander] starting gyro calibration");
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;
} else {
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 */
if ((int)(cmd->param2) == 1) {
/* 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) {
mavlink_log_info(mavlink_fd, "[commander] starting mag calibration");
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;
} else {
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;
/* preflight parameter load / store */
case MAV_CMD_PREFLIGHT_STORAGE: {
case MAV_CMD_PREFLIGHT_STORAGE: {
/* Read all parameters from EEPROM to RAM */
if (((int)cmd->param1) == 0) {
@ -610,7 +609,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
}
break;
default: {
default: {
mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported command");
result = MAV_RESULT_UNSUPPORTED;
}
@ -625,8 +624,6 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
/* send acknowledge command */
mavlink_message_t msg;
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
*/
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;
@ -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");
}
/* make sure we are in preflight state */
memset(&current_status, 0, sizeof(current_status));
current_status.state_machine = SYSTEM_STATE_PREFLIGHT;
/* advertise to ORB */
stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
state_machine_publish(stat_pub, &current_status, mavlink_fd);
if (stat_pub < 0) {
printf("[commander] ERROR: orb_advertise for topic vehicle_status failed.\n");
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");
/* load EEPROM parameters */
@ -854,11 +855,10 @@ int commander_main(int argc, char *argv[])
struct sensor_combined_s 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;
uint64_t last_idle_time = 0;
unsigned int signal_loss_counter = 0;
/* now initialized */
commander_initialized = true;
@ -891,7 +891,7 @@ int commander_main(int argc, char *argv[])
}
/* 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 */
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
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 */
/* Check battery voltage */
@ -1017,7 +1017,7 @@ int commander_main(int argc, char *argv[])
if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
critical_battery_voltage_actions_done = true;
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++;
@ -1044,7 +1044,7 @@ int commander_main(int argc, char *argv[])
/* 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 (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;
} else {
@ -1056,7 +1056,7 @@ int commander_main(int argc, char *argv[])
/* 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 (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;
} 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);
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) {
update_state_machine_mode_auto(stat_pub, &current_status);
update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
} else {
update_state_machine_mode_stabilized(stat_pub, &current_status);
update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
}
/* Publish RC signal */
@ -1112,11 +1112,11 @@ int commander_main(int argc, char *argv[])
current_status.preflight_gyro_calibration == false &&
current_status.preflight_mag_calibration == false) {
/* 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 */
vehicle_state_previous = current_status.state_machine;
// vehicle_state_previous = current_status.state_machine;
voltage_previous = current_status.voltage_battery;
fflush(stdout);

147
apps/commander/state_machine_helper.c
View File

@ -45,6 +45,7 @@
#include <uORB/topics/vehicle_status.h>
#include <systemlib/systemlib.h>
#include <arch/board/up_hrt.h>
#include <mavlink/mavlink_log.h>
static const char* system_state_txt[] = {
"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 ret = ERROR;
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]);
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 {
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;
@ -89,63 +93,63 @@ void do_state_update(int status_pub, struct vehicle_status_s *current_status, co
//TODO: add emcy landing code here
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;
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 */
//TODO: add emcy cutoff code here
/* Tell the controller to cutoff the motors (thrust = 0) */
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;
case SYSTEM_STATE_GROUND_ERROR:
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;
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
|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
invalid_state = false;
mavlink_log_info(mavlink_fd, "[commander] Switched to PREFLIGHT state");
} else {
invalid_state = true;
mavlink_log_critical(mavlink_fd, "[commander] REFUSED to switch to PREFLIGHT state");
}
break;
case SYSTEM_STATE_REBOOT:
usleep(500000);
reboot();
if (current_status->state_machine == SYSTEM_STATE_STANDBY
|| 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;
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;
case SYSTEM_STATE_GROUND_READY:
//global_data_send_mavlink_statustext_message_out("Commander: state: armed", MAV_SEVERITY_INFO);
//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;
// }
mavlink_log_critical(mavlink_fd, "[commander] Switched to GROUND READY state");
break;
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;
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;
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;
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) {
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 */
current_status->counter++;
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
*/
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");
/* 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) {
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) {
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 {
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
state_machine_emergency_always_critical(status_pub, current_status);
state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd);
} 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);
@ -399,28 +403,28 @@ void state_machine_emergency(int status_pub, struct vehicle_status_s *current_st
/* 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 */
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 */
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
if (current_status->state_machine == SYSTEM_STATE_STANDBY) {
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) {
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) {
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) {
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) {
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) {
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) {
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");
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 */
current_status->mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
current_status->mode |= VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED;
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
/* vehicle is disarmed, mode requests arming */
if (!(current_status->mode & VEHICLE_MODE_FLAG_SAFETY_ARMED) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
@ -481,10 +500,10 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
// XXX REMOVE
if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
/* Set armed flag */
current_status->mode |= MAV_MODE_FLAG_SAFETY_ARMED;
current_status->mode |= VEHICLE_MODE_FLAG_SAFETY_ARMED;
/* Set manual input enabled flag */
current_status->mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
current_status->mode |= VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED;
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
ret = OK;
printf("[commander] arming due to command request\n");
}
@ -495,7 +514,7 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
/* only disarm in ground ready */
//if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
/* 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);
// ret = OK;
// 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 */
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 */
current_status->mode |= MAV_MODE_FLAG_HIL_ENABLED;
current_status->mode |= VEHICLE_MODE_FLAG_HIL_ENABLED;
ret = OK;
state_machine_publish(status_pub, current_status);
state_machine_publish(status_pub, current_status, mavlink_fd);
printf("[commander] Enabling HIL\n");
}
/* NEVER actually switch off HIL without reboot */
if ((current_status->mode & 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");
ret = ERROR;
}
@ -520,7 +539,7 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
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;
@ -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) {
printf("system will reboot\n");
//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;
}
@ -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");
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");
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");
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");
ret = 0;
}

87
apps/commander/state_machine_helper.h
View File

@ -46,8 +46,6 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <v1.0/common/mavlink.h>
/**
* 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 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 */
// 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
*
* @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
*
* @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
*
* @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
*
* @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
*
* @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
*
* @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
*
* @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
*
* @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
*
* @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
*
* @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);

47
apps/mavlink/mavlink.c
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)
{
//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) {
case SYSTEM_STATE_PREFLIGHT:
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 */
} else {
int ifds = 0;
/* --- SENSORS RAW VALUE --- */
if (fds[0].revents & POLLIN) {
if (fds[ifds++].revents & POLLIN) {
/* copy sensors raw data into local buffer */
orb_copy(ORB_ID(sensor_combined), sensor_sub, &buf.raw);
@ -596,7 +605,7 @@ static void *uorb_receiveloop(void *arg)
}
/* --- ATTITUDE VALUE --- */
if (fds[1].revents & POLLIN) {
if (fds[ifds++].revents & POLLIN) {
/* copy attitude data into local buffer */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &buf.att);
@ -608,7 +617,7 @@ static void *uorb_receiveloop(void *arg)
}
/* --- GPS VALUE --- */
if (fds[2].revents & POLLIN) {
if (fds[ifds++].revents & POLLIN) {
/* copy gps data into local buffer */
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &buf.gps);
/* GPS position */
@ -622,7 +631,7 @@ static void *uorb_receiveloop(void *arg)
}
// /* --- ARDRONE CONTROL OUTPUTS --- */
// if (fds[3].revents & POLLIN) {
// if (fds[ifds++].revents & POLLIN) {
// /* copy ardrone control data into local buffer */
// orb_copy(ORB_ID(ardrone_control), ar_sub, &buf.ar_control);
// uint64_t timestamp = buf.ar_control.timestamp;
@ -642,7 +651,7 @@ static void *uorb_receiveloop(void *arg)
// }
/* --- SYSTEM STATUS --- */
if (fds[4].revents & POLLIN) {
if (fds[ifds++].revents & POLLIN) {
/* immediately communicate state changes back to user */
orb_copy(ORB_ID(vehicle_status), status_sub, &v_status);
/* enable or disable HIL */
@ -658,7 +667,7 @@ static void *uorb_receiveloop(void *arg)
}
/* --- RC CHANNELS --- */
if (fds[5].revents & POLLIN) {
if (fds[ifds++].revents & POLLIN) {
/* copy rc channels into local buffer */
orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
/* 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 --- */
if (fds[6].revents & POLLIN) {
if (fds[ifds++].revents & POLLIN) {
/* copy fixed wing control into local buffer */
orb_copy(ORB_ID(fixedwing_control), fw_sub, &fw_control);
/* send control output via MAVLink */
@ -731,7 +740,7 @@ static void *uorb_receiveloop(void *arg)
}
/* --- VEHICLE GLOBAL POSITION --- */
if (fds[7].revents & POLLIN) {
if (fds[ifds++].revents & POLLIN) {
/* copy global position data into local buffer */
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
uint64_t timestamp = global_pos.timestamp;
@ -749,14 +758,14 @@ static void *uorb_receiveloop(void *arg)
}
/* --- VEHICLE LOCAL POSITION --- */
if (fds[8].revents & POLLIN) {
if (fds[ifds++].revents & POLLIN) {
/* copy local position data into local buffer */
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);
}
/* --- VEHICLE GLOBAL SETPOINT --- */
if (fds[9].revents & POLLIN) {
if (fds[ifds++].revents & POLLIN) {
/* copy local position data into local buffer */
orb_copy(ORB_ID(vehicle_global_position_setpoint), spg_sub, &buf.global_sp);
uint8_t coordinate_frame = MAV_FRAME_GLOBAL;
@ -765,14 +774,14 @@ static void *uorb_receiveloop(void *arg)
}
/* --- VEHICLE LOCAL SETPOINT --- */
if (fds[10].revents & POLLIN) {
if (fds[ifds++].revents & POLLIN) {
/* copy local position data into local buffer */
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);
}
/* --- VEHICLE ATTITUDE SETPOINT --- */
if (fds[11].revents & POLLIN) {
if (fds[ifds++].revents & POLLIN) {
/* copy local position data into local buffer */
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);
@ -1148,15 +1157,15 @@ int mavlink_main(int argc, char *argv[])
//default values for arguments
char *uart_name = "/dev/ttyS0";
int baudrate = 115200;
const char *commandline_usage = "\tusage: %s -d <devicename> -b <baudrate> [-e/--exit-allowed]\n";
int baudrate = 57600;
const char *commandline_usage = "\tusage: %s -d <devicename> -b <baudrate> [-e/--exit-allowed]\n\t\tdefault: -d %s -b %i\n";
/* read program arguments */
int i;
for (i = 1; i < argc; i++) { /* argv[0] is "mavlink" */
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;
}
@ -1166,7 +1175,7 @@ int mavlink_main(int argc, char *argv[])
uart_name = argv[i + 1];
} else {
printf(commandline_usage, argv[0]);
printf(commandline_usage, argv[0], uart_name, baudrate);
return 0;
}
}
@ -1177,7 +1186,7 @@ int mavlink_main(int argc, char *argv[])
baudrate = atoi(argv[i + 1]);
} else {
printf(commandline_usage, argv[0]);
printf(commandline_usage, argv[0], uart_name, baudrate);
return 0;
}
}
@ -1223,7 +1232,7 @@ int mavlink_main(int argc, char *argv[])
pthread_attr_t uorb_attr;
pthread_attr_init(&uorb_attr);
/* 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);
/* initialize waypoint manager */

2
apps/sensors/Makefile
View File

@ -37,6 +37,6 @@
APPNAME = sensors
PRIORITY = SCHED_PRIORITY_MAX-5
STACKSIZE = 2560
STACKSIZE = 2048
include $(APPDIR)/mk/app.mk

6
apps/sensors/sensors.c
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);
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 */
@ -406,6 +406,10 @@ int sensors_main(int argc, char *argv[])
memset(&rc, 0, sizeof(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 */
struct vehicle_status_s vstatus;
memset(&vstatus, 0, sizeof(vstatus));

10
apps/uORB/topics/vehicle_status.h
View File

@ -86,6 +86,12 @@ enum VEHICLE_MODE_FLAG {
VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1
}; /**< 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.
*
@ -98,7 +104,9 @@ struct vehicle_status_s
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 */
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;
bool control_manual_enabled; /**< true if manual input is mixed in */