diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-13 18:53:37 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-13 18:53:37 +0200 |
commit | 9014577aff02233e890d1f8eefc06471fca8b6d2 (patch) | |
tree | bd589542e278bad3e4978d2098587ffc76c29681 | |
parent | 56b3b46f75c0b434932eecba2ac7207f84e2342e (diff) | |
download | px4-firmware-9014577aff02233e890d1f8eefc06471fca8b6d2.tar.gz px4-firmware-9014577aff02233e890d1f8eefc06471fca8b6d2.tar.bz2 px4-firmware-9014577aff02233e890d1f8eefc06471fca8b6d2.zip |
Massive improvements in state machine, still tracing wrong throttle scaling in manual input path
-rw-r--r-- | apps/ardrone_control/Makefile | 2 | ||||
-rw-r--r-- | apps/ardrone_control/ardrone_control.c | 9 | ||||
-rw-r--r-- | apps/ardrone_control/attitude_control.c | 31 | ||||
-rw-r--r-- | apps/commander/commander.c | 94 | ||||
-rw-r--r-- | apps/commander/state_machine_helper.c | 147 | ||||
-rw-r--r-- | apps/commander/state_machine_helper.h | 87 | ||||
-rw-r--r-- | apps/mavlink/mavlink.c | 47 | ||||
-rw-r--r-- | apps/sensors/Makefile | 2 | ||||
-rw-r--r-- | apps/sensors/sensors.c | 6 | ||||
-rw-r--r-- | apps/uORB/topics/vehicle_status.h | 10 |
10 files changed, 268 insertions, 167 deletions
diff --git a/apps/ardrone_control/Makefile b/apps/ardrone_control/Makefile index 23e2d49fb..2038e982b 100644 --- a/apps/ardrone_control/Makefile +++ b/apps/ardrone_control/Makefile @@ -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 diff --git a/apps/ardrone_control/ardrone_control.c b/apps/ardrone_control/ardrone_control.c index e9a48bbfe..d5c31dc70 100644 --- a/apps/ardrone_control/ardrone_control.c +++ b/apps/ardrone_control/ardrone_control.c @@ -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"); } } diff --git a/apps/ardrone_control/attitude_control.c b/apps/ardrone_control/attitude_control.c index 034a9c5a6..37edbf0e5 100644 --- a/apps/ardrone_control/attitude_control.c +++ b/apps/ardrone_control/attitude_control.c @@ -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++; - } diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 68bb36b2d..19fc81f63 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -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, ¤t_status, SYSTEM_STATE_PREFLIGHT); + do_state_update(status_pub, ¤t_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, ¤t_status); - do_state_update(status_pub, ¤t_status, SYSTEM_STATE_STANDBY); + do_state_update(status_pub, ¤t_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, ¤t_status, SYSTEM_STATE_PREFLIGHT); + do_state_update(status_pub, ¤t_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, ¤t_status); - do_state_update(status_pub, ¤t_status, SYSTEM_STATE_STANDBY); + do_state_update(status_pub, ¤t_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(¤t_status, 0, sizeof(current_status)); + current_status.state_machine = SYSTEM_STATE_PREFLIGHT; + /* advertise to ORB */ stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); + state_machine_publish(stat_pub, ¤t_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, ¤t_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, ¤t_status); + update_state_machine_got_position_fix(stat_pub, ¤t_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, ¤t_status); + state_machine_emergency(stat_pub, ¤t_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, ¤t_status); + update_state_machine_disarm(stat_pub, ¤t_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, ¤t_status); + update_state_machine_arm(stat_pub, ¤t_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, ¤t_status); + update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); } else if (mode_switch_rc_value < -STICK_ON_OFF_LIMIT) { - update_state_machine_mode_auto(stat_pub, ¤t_status); + update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); } else { - update_state_machine_mode_stabilized(stat_pub, ¤t_status); + update_state_machine_mode_stabilized(stat_pub, ¤t_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, ¤t_status, SYSTEM_STATE_STANDBY); + do_state_update(stat_pub, ¤t_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); diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index ebae1c61c..a62b1437a 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -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; } diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index 540182e4f..4908c799a 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -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); diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 6745c0102..1e6156557 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -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 */ diff --git a/apps/sensors/Makefile b/apps/sensors/Makefile index 0bc5b52d6..dadc2993d 100644 --- a/apps/sensors/Makefile +++ b/apps/sensors/Makefile @@ -37,6 +37,6 @@ APPNAME = sensors PRIORITY = SCHED_PRIORITY_MAX-5 -STACKSIZE = 2560 +STACKSIZE = 2048 include $(APPDIR)/mk/app.mk diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c index 098db4456..d847c4ffc 100644 --- a/apps/sensors/sensors.c +++ b/apps/sensors/sensors.c @@ -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)); diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 9f72c8f78..8b2b6525c 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -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 */ |