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->flag_control_manual_enabled = true; /* set behaviour based on airframe */ if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) { /* assuming a rotary wing, set to SAS */ current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; current_status->flag_control_attitude_enabled = true; current_status->flag_control_rates_enabled = true; } else { /* assuming a fixed wing, set to direct pass-through */ current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; current_status->flag_control_attitude_enabled = false; current_status->flag_control_rates_enabled = false; } 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("[cmd] manual mode\n"); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); } }
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->flag_control_manual_enabled = true; if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) { printf("[commander] auto mode\n"); 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, const int mavlink_fd, uint8_t mode) { printf("[commander] Requested new mode: %d\n", (int)mode); uint8_t ret = 1; /* vehicle is disarmed, mode requests arming */ if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { /* only arm in standby state */ // XXX REMOVE if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { 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"); } } /* vehicle is armed, mode requests disarming */ if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { /* only disarm in ground ready */ if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); ret = OK; printf("[commander] disarming due to command request\n"); } } /* Switch on HIL if in standby and not already in HIL mode */ if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !current_status->flag_hil_enabled) { /* Enable HIL on request */ current_status->flag_hil_enabled = true; ret = OK; state_machine_publish(status_pub, current_status, mavlink_fd); publish_armed_status(current_status); printf("[commander] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); } /* NEVER actually switch off HIL without reboot */ if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { fprintf(stderr, "[commander] DENYING request to switch of HIL. Please power cycle (safety reasons)\n"); ret = ERROR; } return ret; }
void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { if (!current_status->flag_vector_flight_mode_ok) { mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE"); return; } if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) { printf("[cmd] auto mode\n"); int old_mode = current_status->flight_mode; current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO; current_status->flag_control_manual_enabled = false; current_status->flag_control_attitude_enabled = true; current_status->flag_control_rates_enabled = true; do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); } }
void update_state_machine_mode_stabilized(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_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { int old_mode = current_status->flight_mode; int old_manual_control_mode = current_status->manual_control_mode; current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; current_status->flag_control_attitude_enabled = true; current_status->flag_control_rates_enabled = true; current_status->flag_control_manual_enabled = true; if (old_mode != current_status->flight_mode || old_manual_control_mode != current_status->manual_control_mode) { printf("[cmd] att stabilized mode\n"); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); state_machine_publish(status_pub, current_status, mavlink_fd); } } }
/** * 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; switch (new_state) { case SYSTEM_STATE_MISSION_ABORT: { /* Indoor or outdoor */ // if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING); // } else { // ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF); // } } break; case SYSTEM_STATE_EMCY_LANDING: /* Tell the controller to land */ /* set system flags according to state */ current_status->flag_system_armed = true; warnx("EMERGENCY LANDING!\n"); mavlink_log_critical(mavlink_fd, "EMERGENCY LANDING!"); break; case SYSTEM_STATE_EMCY_CUTOFF: /* Tell the controller to cutoff the motors (thrust = 0) */ /* set system flags according to state */ current_status->flag_system_armed = false; warnx("EMERGENCY MOTOR CUTOFF!\n"); mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!"); break; case SYSTEM_STATE_GROUND_ERROR: /* set system flags according to state */ /* prevent actuators from arming */ current_status->flag_system_armed = false; warnx("GROUND ERROR, locking down propulsion system\n"); mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system"); break; case SYSTEM_STATE_PREFLIGHT: if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { /* set system flags according to state */ current_status->flag_system_armed = false; mavlink_log_critical(mavlink_fd, "Switched to PREFLIGHT state"); } else { invalid_state = true; mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state"); } break; case SYSTEM_STATE_REBOOT: if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->flag_hil_enabled) { invalid_state = false; /* set system flags according to state */ current_status->flag_system_armed = false; mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); usleep(500000); up_systemreset(); /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ } else { invalid_state = true; mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT"); } break; case SYSTEM_STATE_STANDBY: /* set system flags according to state */ /* standby enforces disarmed */ current_status->flag_system_armed = false; mavlink_log_critical(mavlink_fd, "Switched to STANDBY state"); break; case SYSTEM_STATE_GROUND_READY: /* set system flags according to state */ /* ground ready has motors / actuators armed */ current_status->flag_system_armed = true; mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state"); break; case SYSTEM_STATE_AUTO: /* set system flags according to state */ /* auto is airborne and in auto mode, motors armed */ current_status->flag_system_armed = true; mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode"); break; case SYSTEM_STATE_STABILIZED: /* set system flags according to state */ current_status->flag_system_armed = true; mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode"); break; case SYSTEM_STATE_MANUAL: /* set system flags according to state */ current_status->flag_system_armed = true; mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode"); break; default: invalid_state = true; break; } if (invalid_state == false || old_state != new_state) { current_status->state_machine = new_state; state_machine_publish(status_pub, current_status, mavlink_fd); publish_armed_status(current_status); ret = OK; } if (invalid_state) { mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition"); ret = ERROR; } return ret; }
uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) { uint8_t ret = 1; /* Switch on HIL if in standby and not already in HIL mode */ if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !current_status->flag_hil_enabled) { if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { /* Enable HIL on request */ current_status->flag_hil_enabled = true; ret = OK; state_machine_publish(status_pub, current_status, mavlink_fd); publish_armed_status(current_status); printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && current_status->flag_system_armed) { mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") } else { mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") } } /* switch manual / auto */ if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); } /* vehicle is disarmed, mode requests arming */ if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { /* only arm in standby state */ // XXX REMOVE if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); ret = OK; printf("[cmd] arming due to command request\n"); } } /* vehicle is armed, mode requests disarming */ if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { /* only disarm in ground ready */ if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); ret = OK; printf("[cmd] disarming due to command request\n"); } } /* NEVER actually switch off HIL without reboot */ if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); ret = ERROR; } return ret; }
void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd) { /* announce change */ mavlink_log_info(mavlink_fd, "accel calibration started"); /* set to accel calibration mode */ status->flag_preflight_accel_calibration = true; state_machine_publish(status_pub, status, mavlink_fd); /* measure and calculate offsets & scales */ float accel_offs[3]; float accel_scale[3]; int res = do_accel_calibration_mesurements(mavlink_fd, accel_offs, accel_scale); if (res == OK) { /* measurements complete successfully, set parameters */ if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0])) || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1])) || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2])) || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0])) || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1])) || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) { mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed"); } int fd = open(ACCEL_DEVICE_PATH, 0); struct accel_scale ascale = { accel_offs[0], accel_scale[0], accel_offs[1], accel_scale[1], accel_offs[2], accel_scale[2], }; if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) warn("WARNING: failed to set scale / offsets for accel"); close(fd); /* auto-save to EEPROM */ int save_ret = param_save_default(); if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); } mavlink_log_info(mavlink_fd, "accel calibration done"); tune_confirm(); sleep(2); tune_confirm(); sleep(2); /* third beep by cal end routine */ } else { /* measurements error */ mavlink_log_info(mavlink_fd, "accel calibration aborted"); tune_error(); sleep(2); } /* exit accel calibration mode */ status->flag_preflight_accel_calibration = false; state_machine_publish(status_pub, status, mavlink_fd); }
void do_mag_calibration(int status_pub, struct vehicle_status_s *current_status) { /* set to mag calibration mode */ current_status->preflight_mag_calibration = true; state_machine_publish(status_pub, current_status); int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); struct sensor_combined_s raw; /* 30 seconds */ const uint64_t calibration_interval_us = 10 * 1000000; unsigned int calibration_counter = 0; const int peak_samples = 2000; /* Get rid of 10% */ const int outlier_margin = (peak_samples) / 10; int16_t *mag_maxima[3]; mag_maxima[0] = (int16_t*)malloc(peak_samples * sizeof(uint16_t)); mag_maxima[1] = (int16_t*)malloc(peak_samples * sizeof(uint16_t)); mag_maxima[2] = (int16_t*)malloc(peak_samples * sizeof(uint16_t)); int16_t *mag_minima[3]; mag_minima[0] = (int16_t*)malloc(peak_samples * sizeof(uint16_t)); mag_minima[1] = (int16_t*)malloc(peak_samples * sizeof(uint16_t)); mag_minima[2] = (int16_t*)malloc(peak_samples * sizeof(uint16_t)); /* initialize data table */ for (int i = 0; i < peak_samples; i++) { mag_maxima[0][i] = INT16_MIN; mag_maxima[1][i] = INT16_MIN; mag_maxima[2][i] = INT16_MIN; mag_minima[0][i] = INT16_MAX; mag_minima[1][i] = INT16_MAX; mag_minima[2][i] = INT16_MAX; } uint64_t calibration_start = hrt_absolute_time(); while ((hrt_absolute_time() - calibration_start) < calibration_interval_us) { /* wait blocking for new data */ struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; if (poll(fds, 1, 1000)) { orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); /* get min/max values */ /* iterate through full list */ for (int i = 0; i < peak_samples; i++) { /* x minimum */ if (raw.magnetometer_raw[0] < mag_minima[0][i]) mag_minima[0][i] = raw.magnetometer_raw[0]; /* y minimum */ if (raw.magnetometer_raw[1] < mag_minima[1][i]) mag_minima[1][i] = raw.magnetometer_raw[1]; /* z minimum */ if (raw.magnetometer_raw[2] < mag_minima[2][i]) mag_minima[2][i] = raw.magnetometer_raw[2]; /* x maximum */ if (raw.magnetometer_raw[0] > mag_maxima[0][i]) mag_maxima[0][i] = raw.magnetometer_raw[0]; /* y maximum */ if (raw.magnetometer_raw[1] > mag_maxima[1][i]) mag_maxima[1][i] = raw.magnetometer_raw[1]; /* z maximum */ if (raw.magnetometer_raw[2] > mag_maxima[2][i]) mag_maxima[2][i] = raw.magnetometer_raw[2]; } calibration_counter++; } else { /* any poll failure for 1s is a reason to abort */ mavlink_log_info(mavlink_fd, "[commander] mag calibration aborted, please retry."); break; } }