diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2012-12-30 00:04:54 +0100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-12-30 00:04:54 +0100 |
| commit | 0298714db53c44a28ab19d5ba01d70de28dd39b3 (patch) | |
| tree | 1733a67f898a6c98c7bc52574ed814afa97fcf26 /apps | |
| parent | 4976a3a47d9ed368734135df04ad67634c39c65d (diff) | |
| parent | b240e31c1c25c07ffe046a3433d43fa8b862c136 (diff) | |
| download | px4-firmware-0298714db53c44a28ab19d5ba01d70de28dd39b3.tar.gz px4-firmware-0298714db53c44a28ab19d5ba01d70de28dd39b3.tar.bz2 px4-firmware-0298714db53c44a28ab19d5ba01d70de28dd39b3.zip | |
Merge branch 'fixedwing_outdoor' of github.com:PX4/Firmware into fixedwing_outdoor
Diffstat (limited to 'apps')
32 files changed, 1317 insertions, 564 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 52412e20a..be50289c3 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -95,6 +95,9 @@ PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ //PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */ +PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); +PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); +PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); #include <systemlib/cpuload.h> extern struct system_load_s system_load; @@ -144,7 +147,6 @@ int commander_thread_main(int argc, char *argv[]); static int buzzer_init(void); static void buzzer_deinit(void); -static void tune_confirm(void); static int led_init(void); static void led_deinit(void); static int led_toggle(int led); @@ -152,6 +154,7 @@ static int led_on(int led); static int led_off(int led); static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status); static void do_mag_calibration(int status_pub, struct vehicle_status_s *status); +static void do_rc_calibration(int status_pub, struct vehicle_status_s *status); static void do_accel_calibration(int status_pub, struct vehicle_status_s *status); static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); @@ -179,7 +182,7 @@ static int buzzer_init() buzzer = open("/dev/tone_alarm", O_WRONLY); if (buzzer < 0) { - fprintf(stderr, "[commander] Buzzer: open fail\n"); + fprintf(stderr, "[cmd] Buzzer: open fail\n"); return ERROR; } @@ -197,12 +200,12 @@ static int led_init() leds = open(LED_DEVICE_PATH, 0); if (leds < 0) { - fprintf(stderr, "[commander] LED: open fail\n"); + fprintf(stderr, "[cmd] LED: open fail\n"); return ERROR; } if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) { - fprintf(stderr, "[commander] LED: ioctl fail\n"); + fprintf(stderr, "[cmd] LED: ioctl fail\n"); return ERROR; } @@ -268,6 +271,34 @@ void tune_confirm(void) { ioctl(buzzer, TONE_SET_ALARM, 3); } +void tune_error(void) { + ioctl(buzzer, TONE_SET_ALARM, 4); +} + +void do_rc_calibration(int status_pub, struct vehicle_status_s *status) +{ + int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); + struct manual_control_setpoint_s sp; + orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); + + /* set parameters */ + + float p = sp.roll; + param_set(param_find("TRIM_ROLL"), &p); + p = sp.pitch; + param_set(param_find("TRIM_PITCH"), &p); + p = sp.yaw; + param_set(param_find("TRIM_YAW"), &p); + + /* store to permanent storage */ + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + if(save_ret != 0) { + mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); + } + mavlink_log_info(mavlink_fd, "[cmd] trim calibration done"); +} + void do_mag_calibration(int status_pub, struct vehicle_status_s *status) { @@ -310,7 +341,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) }; if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { warn("WARNING: failed to set scale / offsets for mag"); - mavlink_log_info(mavlink_fd, "[commander] failed to set scale / offsets for mag"); + mavlink_log_info(mavlink_fd, "[cmd] failed to set scale / offsets for mag"); } /* calibrate range */ @@ -358,7 +389,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) axis_index++; char buf[50]; - sprintf(buf, "[commander] Please rotate around %c", axislabels[axis_index]); + sprintf(buf, "[cmd] Please rotate around %c", axislabels[axis_index]); mavlink_log_info(mavlink_fd, buf); tune_confirm(); @@ -373,7 +404,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) // if ((axis_left / 1000) == 0 && axis_left > 0) { // char buf[50]; - // sprintf(buf, "[commander] %d seconds left for axis %c", axis_left, axislabels[axis_index]); + // sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]); // mavlink_log_info(mavlink_fd, buf); // } @@ -410,7 +441,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) calibration_counter++; } else { /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "[commander] mag cal canceled"); + mavlink_log_info(mavlink_fd, "[cmd] mag cal canceled"); break; } } @@ -446,27 +477,27 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) /* announce and set new offset */ if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { - fprintf(stderr, "[commander] Setting X mag offset failed!\n"); + fprintf(stderr, "[cmd] Setting X mag offset failed!\n"); } if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { - fprintf(stderr, "[commander] Setting Y mag offset failed!\n"); + fprintf(stderr, "[cmd] Setting Y mag offset failed!\n"); } if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { - fprintf(stderr, "[commander] Setting Z mag offset failed!\n"); + fprintf(stderr, "[cmd] Setting Z mag offset failed!\n"); } if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { - fprintf(stderr, "[commander] Setting X mag scale failed!\n"); + fprintf(stderr, "[cmd] Setting X mag scale failed!\n"); } if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { - fprintf(stderr, "[commander] Setting Y mag scale failed!\n"); + fprintf(stderr, "[cmd] Setting Y mag scale failed!\n"); } if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { - fprintf(stderr, "[commander] Setting Z mag scale failed!\n"); + fprintf(stderr, "[cmd] Setting Z mag scale failed!\n"); } /* auto-save to EEPROM */ @@ -489,7 +520,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) (double)mscale.y_scale, (double)mscale.z_scale); mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "[commander] mag calibration done"); + mavlink_log_info(mavlink_fd, "[cmd] mag calibration done"); tune_confirm(); sleep(2); @@ -498,7 +529,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) /* third beep by cal end routine */ } else { - mavlink_log_info(mavlink_fd, "[commander] mag calibration FAILED (NaN)"); + mavlink_log_info(mavlink_fd, "[cmd] mag calibration FAILED (NaN)"); } /* disable calibration mode */ @@ -549,7 +580,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) calibration_counter++; } else { /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "[commander] gyro calibration aborted, retry"); + mavlink_log_info(mavlink_fd, "[cmd] gyro calibration aborted, retry"); return; } } @@ -565,15 +596,15 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting X gyro offset failed!"); + mavlink_log_critical(mavlink_fd, "[cmd] Setting X gyro offset failed!"); } if (param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting Y gyro offset failed!"); + mavlink_log_critical(mavlink_fd, "[cmd] Setting Y gyro offset failed!"); } if (param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting Z gyro offset failed!"); + mavlink_log_critical(mavlink_fd, "[cmd] Setting Z gyro offset failed!"); } /* set offsets to actual value */ @@ -599,7 +630,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) // char buf[50]; // sprintf(buf, "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, buf); - mavlink_log_info(mavlink_fd, "[commander] gyro calibration done"); + mavlink_log_info(mavlink_fd, "[cmd] gyro calibration done"); tune_confirm(); sleep(2); @@ -607,7 +638,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) sleep(2); /* third beep by cal end routine */ } else { - mavlink_log_info(mavlink_fd, "[commander] gyro calibration FAILED (NaN)"); + mavlink_log_info(mavlink_fd, "[cmd] gyro calibration FAILED (NaN)"); } close(sub_sensor_combined); @@ -617,7 +648,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) { /* announce change */ - mavlink_log_info(mavlink_fd, "[commander] keep it level and still"); + mavlink_log_info(mavlink_fd, "[cmd] keep it level and still"); /* set to accel calibration mode */ status->flag_preflight_accel_calibration = true; state_machine_publish(status_pub, status, mavlink_fd); @@ -655,7 +686,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) calibration_counter++; } else { /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "[commander] acceleration calibration aborted"); + mavlink_log_info(mavlink_fd, "[cmd] acceleration calibration aborted"); return; } } @@ -674,27 +705,27 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) float scale = 9.80665f / total_len; if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offset[0]))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting X accel offset failed!"); + mavlink_log_critical(mavlink_fd, "[cmd] Setting X accel offset failed!"); } if (param_set(param_find("SENS_ACC_YOFF"), &(accel_offset[1]))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting Y accel offset failed!"); + mavlink_log_critical(mavlink_fd, "[cmd] Setting Y accel offset failed!"); } if (param_set(param_find("SENS_ACC_ZOFF"), &(accel_offset[2]))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting Z accel offset failed!"); + mavlink_log_critical(mavlink_fd, "[cmd] Setting Z accel offset failed!"); } if (param_set(param_find("SENS_ACC_XSCALE"), &(scale))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting X accel offset failed!"); + mavlink_log_critical(mavlink_fd, "[cmd] Setting X accel offset failed!"); } if (param_set(param_find("SENS_ACC_YSCALE"), &(scale))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting Y accel offset failed!"); + mavlink_log_critical(mavlink_fd, "[cmd] Setting Y accel offset failed!"); } if (param_set(param_find("SENS_ACC_ZSCALE"), &(scale))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting Z accel offset failed!"); + mavlink_log_critical(mavlink_fd, "[cmd] Setting Z accel offset failed!"); } fd = open(ACCEL_DEVICE_PATH, 0); @@ -717,9 +748,9 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) } //char buf[50]; - //sprintf(buf, "[commander] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); + //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); //mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "[commander] accel calibration done"); + mavlink_log_info(mavlink_fd, "[cmd] accel calibration done"); tune_confirm(); sleep(2); @@ -727,7 +758,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) sleep(2); /* third beep by cal end routine */ } else { - mavlink_log_info(mavlink_fd, "[commander] accel calibration FAILED (NaN)"); + mavlink_log_info(mavlink_fd, "[cmd] accel calibration FAILED (NaN)"); } /* exit accel calibration mode */ @@ -853,15 +884,15 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta 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] CMD starting gyro calibration"); + mavlink_log_info(mavlink_fd, "[cmd] starting gyro calibration"); tune_confirm(); do_gyro_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "[commander] CMD finished gyro calibration"); + mavlink_log_info(mavlink_fd, "[cmd] finished gyro calibration"); tune_confirm(); 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"); + mavlink_log_critical(mavlink_fd, "[cmd] REJECTING gyro calibration"); result = MAV_RESULT_DENIED; } handled = true; @@ -873,15 +904,50 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta 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] CMD starting mag calibration"); + mavlink_log_info(mavlink_fd, "[cmd] starting mag calibration"); tune_confirm(); do_mag_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "[commander] CMD finished mag calibration"); + mavlink_log_info(mavlink_fd, "[cmd] finished mag calibration"); tune_confirm(); do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); result = MAV_RESULT_ACCEPTED; } else { - mavlink_log_critical(mavlink_fd, "[commander] CMD REJECTING mag calibration"); + mavlink_log_critical(mavlink_fd, "[cmd] REJECTING mag calibration"); + result = MAV_RESULT_DENIED; + } + handled = true; + } + + /* zero-altitude pressure calibration */ + if ((int)(cmd->param3) == 1) { + /* transition to calibration state */ + 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, "[cmd] zero altitude cal. not implemented"); + tune_confirm(); + } else { + mavlink_log_critical(mavlink_fd, "[cmd] REJECTING altitude calibration"); + result = MAV_RESULT_DENIED; + } + handled = true; + } + + /* trim calibration */ + if ((int)(cmd->param4) == 1) { + /* transition to calibration state */ + 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, "[cmd] starting trim calibration"); + tune_confirm(); + do_rc_calibration(status_pub, ¤t_status); + mavlink_log_info(mavlink_fd, "[cmd] finished trim calibration"); + tune_confirm(); + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + result = MAV_RESULT_ACCEPTED; + } else { + mavlink_log_critical(mavlink_fd, "[cmd] REJECTING trim calibration"); result = MAV_RESULT_DENIED; } handled = true; @@ -893,15 +959,15 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta 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] CMD starting accel calibration"); + mavlink_log_info(mavlink_fd, "[cmd] CMD starting accel calibration"); tune_confirm(); do_accel_calibration(status_pub, ¤t_status); tune_confirm(); - mavlink_log_info(mavlink_fd, "[commander] CMD finished accel calibration"); + mavlink_log_info(mavlink_fd, "[cmd] CMD finished accel calibration"); do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); result = MAV_RESULT_ACCEPTED; } else { - mavlink_log_critical(mavlink_fd, "[commander] REJECTING accel calibration"); + mavlink_log_critical(mavlink_fd, "[cmd] REJECTING accel calibration"); result = MAV_RESULT_DENIED; } handled = true; @@ -909,16 +975,21 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* none found */ if (!handled) { - //fprintf(stderr, "[commander] refusing unsupported calibration request\n"); - mavlink_log_critical(mavlink_fd, "[commander] CMD refusing unsup. calib. request"); + //fprintf(stderr, "[cmd] refusing unsupported calibration request\n"); + mavlink_log_critical(mavlink_fd, "[cmd] CMD refusing unsup. calib. request"); result = MAV_RESULT_UNSUPPORTED; } } break; case MAV_CMD_PREFLIGHT_STORAGE: { - if (current_status.flag_system_armed) { - mavlink_log_info(mavlink_fd, "[cmd] REJECTING param command while armed"); + if (current_status.flag_system_armed && + ((current_status.system_type == MAV_TYPE_QUADROTOR) || + (current_status.system_type == MAV_TYPE_HEXAROTOR) || + (current_status.system_type == MAV_TYPE_OCTOROTOR))) { + /* do not perform expensive memory tasks on multirotors in flight */ + // XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed + mavlink_log_info(mavlink_fd, "[cmd] REJECTING save cmd while multicopter armed"); } else { // XXX move this to LOW PRIO THREAD of commander app @@ -1172,7 +1243,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_sys_type = param_find("MAV_TYPE"); /* welcome user */ - printf("[commander] I am in command now!\n"); + printf("[cmd] I am in command now!\n"); /* pthreads for command and subsystem info handling */ // pthread_t command_handling_thread; @@ -1180,17 +1251,17 @@ int commander_thread_main(int argc, char *argv[]) /* initialize */ if (led_init() != 0) { - fprintf(stderr, "[commander] ERROR: Failed to initialize leds\n"); + fprintf(stderr, "[cmd] ERROR: Failed to initialize leds\n"); } if (buzzer_init() != 0) { - fprintf(stderr, "[commander] ERROR: Failed to initialize buzzer\n"); + fprintf(stderr, "[cmd] ERROR: Failed to initialize buzzer\n"); } mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (mavlink_fd < 0) { - fprintf(stderr, "[commander] ERROR: Failed to open MAVLink log stream, start mavlink app first.\n"); + fprintf(stderr, "[cmd] ERROR: Failed to open MAVLink log stream, start mavlink app first.\n"); } /* make sure we are in preflight state */ @@ -1205,6 +1276,8 @@ int commander_thread_main(int argc, char *argv[]) current_status.offboard_control_signal_lost = true; /* allow manual override initially */ current_status.flag_external_manual_override_ok = true; + /* flag position info as bad, do not allow auto mode */ + current_status.flag_vector_flight_mode_ok = false; /* advertise to ORB */ stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); @@ -1212,11 +1285,11 @@ int commander_thread_main(int argc, char *argv[]) state_machine_publish(stat_pub, ¤t_status, mavlink_fd); if (stat_pub < 0) { - printf("[commander] ERROR: orb_advertise for topic vehicle_status failed.\n"); + printf("[cmd] ERROR: orb_advertise for topic vehicle_status failed.\n"); exit(ERROR); } - mavlink_log_info(mavlink_fd, "[commander] system is running"); + mavlink_log_info(mavlink_fd, "[cmd] system is running"); /* create pthreads */ pthread_attr_t subsystem_info_attr; @@ -1268,6 +1341,8 @@ int commander_thread_main(int argc, char *argv[]) int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); struct sensor_combined_s sensors; memset(&sensors, 0, sizeof(sensors)); + sensors.battery_voltage_v = 0.0f; + sensors.battery_voltage_valid = false; /* Subscribe to command topic */ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -1291,6 +1366,8 @@ int commander_thread_main(int argc, char *argv[]) uint64_t failsave_ll_start_time = 0; bool state_changed = true; + bool param_init_forced = true; + while (!thread_should_exit) { @@ -1305,7 +1382,13 @@ int commander_thread_main(int argc, char *argv[]) if (new_data) { orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); } - orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + + orb_check(sensor_sub, &new_data); + if (new_data) { + orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + } else { + sensors.battery_voltage_valid = false; + } orb_check(cmd_sub, &new_data); if (new_data) { @@ -1315,17 +1398,18 @@ int commander_thread_main(int argc, char *argv[]) /* handle it */ handle_command(stat_pub, ¤t_status, &cmd); } - /* update parameters */ orb_check(param_changed_sub, &new_data); - if (new_data) { + if (new_data || param_init_forced) { + param_init_forced = false; /* parameters changed */ orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); /* update parameters */ if (!current_status.flag_system_armed) { - current_status.system_type = param_get(_param_sys_type, &(current_status.system_type)); - + if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { + warnx("failed setting new system type"); + } /* disable manual override for all systems that rely on electronic stabilization */ if (current_status.system_type == MAV_TYPE_QUADROTOR || current_status.system_type == MAV_TYPE_HEXAROTOR || @@ -1334,7 +1418,6 @@ int commander_thread_main(int argc, char *argv[]) } else { current_status.flag_external_manual_override_ok = true; } - printf("param changed, val: %d, override: %s\n", current_status.system_type, (current_status.flag_external_manual_override_ok) ? "ON" : "OFF"); } else { printf("ARMED, rejecting sys type change\n"); } @@ -1432,14 +1515,18 @@ int commander_thread_main(int argc, char *argv[]) /* Check battery voltage */ /* write to sys_status */ - current_status.voltage_battery = battery_voltage; + if (battery_voltage_valid) { + current_status.voltage_battery = battery_voltage; + } else { + current_status.voltage_battery = 0.0f; + } /* if battery voltage is getting lower, warn using buzzer, etc. */ if (battery_voltage_valid && (bat_remain < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { low_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[commander] WARNING! LOW BATTERY!"); + mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!"); } low_voltage_counter++; @@ -1449,7 +1536,7 @@ int commander_thread_main(int argc, char *argv[]) else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY! CRITICAL BATTERY!"); + mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); state_machine_emergency(stat_pub, ¤t_status, mavlink_fd); } @@ -1462,21 +1549,45 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ + + /* + * Check for valid position information. + * + * If the system has a valid position source from an onboard + * position estimator, it is safe to operate it autonomously. + * The flag_vector_flight_mode_ok flag indicates that a minimum + * set of position measurements is available. + */ + + /* store current state to reason later about a state change */ bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; bool global_pos_valid = current_status.flag_global_position_valid; bool local_pos_valid = current_status.flag_local_position_valid; /* check for global or local position updates, set a timeout of 2s */ if (hrt_absolute_time() - last_global_position_time < 2000000) { - current_status.flag_vector_flight_mode_ok = true; current_status.flag_global_position_valid = true; // XXX check for controller status and home position as well + } else { + current_status.flag_global_position_valid = false; } if (hrt_absolute_time() - last_local_position_time < 2000000) { - current_status.flag_vector_flight_mode_ok = true; current_status.flag_local_position_valid = true; // XXX check for controller status and home position as well + } else { + current_status.flag_local_position_valid = false; + } + + /* + * Consolidate global position and local position valid flags + * for vector flight mode. + */ + if (current_status.flag_local_position_valid || + current_status.flag_global_position_valid) { + current_status.flag_vector_flight_mode_ok = true; + } else { + current_status.flag_vector_flight_mode_ok = false; } /* consolidate state change, flag as changed if required */ @@ -1486,6 +1597,26 @@ int commander_thread_main(int argc, char *argv[]) state_changed = true; } + /* + * Mark the position of the first position lock as return to launch (RTL) + * position. The MAV will return here on command or emergency. + * + * Conditions: + * + * 1) The system aquired position lock just now + * 2) The system has not aquired position lock before + * 3) The system is not armed (on the ground) + */ + if (!current_status.flag_valid_launch_position && + !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && + !current_status.flag_system_armed) { + /* first time a valid position, store it and emit it */ + + // XXX implement storage and publication of RTL position + current_status.flag_valid_launch_position = true; + current_status.flag_auto_flight_mode_ok = true; + state_changed = true; + } /* Check if last transition deserved an audio event */ @@ -1553,8 +1684,97 @@ int commander_thread_main(int argc, char *argv[]) if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { - /* check if left stick is in lower left position --> switch to standby state */ - if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && sp_man.throttle < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values + /* + * Check if manual control modes have to be switched + */ + if (!isfinite(sp_man.manual_mode_switch)) { + printf("man mode sw not finite\n"); + + /* this switch is not properly mapped, set default */ + if ((current_status.system_type == MAV_TYPE_QUADROTOR) || + (current_status.system_type == MAV_TYPE_HEXAROTOR) || + (current_status.system_type == MAV_TYPE_OCTOROTOR)) { + + /* assuming a rotary wing, fall back 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, fall back 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; + } + + } else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom stick position, set direct mode for vehicles supporting it */ + if ((current_status.system_type == MAV_TYPE_QUADROTOR) || + (current_status.system_type == MAV_TYPE_HEXAROTOR) || + (current_status.system_type == MAV_TYPE_OCTOROTOR)) { + + /* assuming a rotary wing, fall back 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 as requested */ + current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; + current_status.flag_control_attitude_enabled = false; + current_status.flag_control_rates_enabled = false; + } + + } else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) { + + /* top stick position, set SAS for all vehicle types */ + 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 { + /* center stick position, set rate control */ + current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES; + current_status.flag_control_attitude_enabled = false; + current_status.flag_control_rates_enabled = true; + } + + printf("man ctrl mode: %d\n", (int)current_status.manual_control_mode); + + /* + * Check if manual stability control modes have to be switched + */ + if (!isfinite(sp_man.manual_sas_switch)) { + + /* this switch is not properly mapped, set default */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + + } else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom stick position, set altitude hold */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE; + + } else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) { + + /* top stick position */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE; + + } else { + /* center stick position, set default */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + } + + /* + * Check if left stick is in lower left position --> switch to standby state. + * Do this only for multirotors, not for fixed wing aircraft. + */ + if (((current_status.system_type == MAV_TYPE_QUADROTOR) || + (current_status.system_type == MAV_TYPE_HEXAROTOR) || + (current_status.system_type == MAV_TYPE_OCTOROTOR) + ) && + ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) && + (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); stick_on_counter = 0; @@ -1566,7 +1786,7 @@ int commander_thread_main(int argc, char *argv[]) } /* check if left stick is in lower right position --> arm */ - if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values + if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); stick_on_counter = 0; @@ -1576,30 +1796,31 @@ int commander_thread_main(int argc, char *argv[]) stick_off_counter = 0; } } - //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 (sp_man.aux1_cam_pan_flaps > STICK_ON_OFF_LIMIT) { + /* check manual override switch - switch to manual or auto mode */ + if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) { + /* enable manual override */ update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); } else { - if (sp_man.override_mode_switch > STICK_ON_OFF_LIMIT) { - //update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); - // XXX hack + /* check auto mode switch for correct mode */ + if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) { + /* enable stabilized mode */ update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); - } else if (sp_man.override_mode_switch < -STICK_ON_OFF_LIMIT) { + } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) { update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); } else { - update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); + update_state_machine_mode_hold(stat_pub, ¤t_status, mavlink_fd); } } /* handle the case where RC signal was regained */ if (!current_status.rc_signal_found_once) { current_status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "[commander] DETECTED RC SIGNAL FIRST TIME."); + mavlink_log_critical(mavlink_fd, "[cmd] DETECTED RC SIGNAL FIRST TIME."); } else { - if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[commander] RECOVERY - RC SIGNAL GAINED!"); + if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); } current_status.rc_signal_cutting_off = false; @@ -1612,7 +1833,7 @@ int commander_thread_main(int argc, char *argv[]) if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { /* only complain if the offboard control is NOT active */ current_status.rc_signal_cutting_off = true; - mavlink_log_critical(mavlink_fd, "[commander] CRITICAL - NO REMOTE SIGNAL!"); + mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL - NO REMOTE SIGNAL!"); last_print_time = hrt_absolute_time(); } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ @@ -1671,10 +1892,10 @@ int commander_thread_main(int argc, char *argv[]) state_changed = true; tune_confirm(); - mavlink_log_critical(mavlink_fd, "[commander] DETECTED OFFBOARD CONTROL SIGNAL FIRST"); + mavlink_log_critical(mavlink_fd, "[cmd] DETECTED OFFBOARD CONTROL SIGNAL FIRST"); } else { if (current_status.offboard_control_signal_lost) { - mavlink_log_critical(mavlink_fd, "[commander] OK:RECOVERY OFFBOARD CONTROL"); + mavlink_log_critical(mavlink_fd, "[cmd] OK:RECOVERY OFFBOARD CONTROL"); state_changed = true; tune_confirm(); } @@ -1698,7 +1919,7 @@ int commander_thread_main(int argc, char *argv[]) /* print error message for first RC glitch and then every 5 s / 5000 ms) */ if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { current_status.offboard_control_signal_weak = true; - mavlink_log_critical(mavlink_fd, "[commander] CRIT:NO OFFBOARD CONTROL!"); + mavlink_log_critical(mavlink_fd, "[cmd] CRIT:NO OFFBOARD CONTROL!"); last_print_time = hrt_absolute_time(); } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ @@ -1761,7 +1982,7 @@ int commander_thread_main(int argc, char *argv[]) close(sensor_sub); close(cmd_sub); - printf("[commander] exiting..\n"); + printf("[cmd] exiting..\n"); fflush(stdout); thread_running = false; diff --git a/apps/commander/commander.h b/apps/commander/commander.h index bea67bead..04b4e72ab 100644 --- a/apps/commander/commander.h +++ b/apps/commander/commander.h @@ -52,4 +52,7 @@ #define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f #define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f +void tune_confirm(void); +void tune_error(void); + #endif /* COMMANDER_H_ */ diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index aa916dafa..a8cef8c01 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -93,8 +93,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* set system flags according to state */ current_status->flag_system_armed = true; - fprintf(stderr, "[commander] EMERGENCY LANDING!\n"); - mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY LANDING!"); + fprintf(stderr, "[cmd] EMERGENCY LANDING!\n"); + mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY LANDING!"); break; case SYSTEM_STATE_EMCY_CUTOFF: @@ -103,8 +103,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* set system flags according to state */ current_status->flag_system_armed = false; - fprintf(stderr, "[commander] EMERGENCY MOTOR CUTOFF!\n"); - mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY MOTOR CUTOFF!"); + fprintf(stderr, "[cmd] EMERGENCY MOTOR CUTOFF!\n"); + mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY MOTOR CUTOFF!"); break; case SYSTEM_STATE_GROUND_ERROR: @@ -114,8 +114,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* prevent actuators from arming */ current_status->flag_system_armed = false; - fprintf(stderr, "[commander] GROUND ERROR, locking down propulsion system\n"); - mavlink_log_critical(mavlink_fd, "[commander] GROUND ERROR, locking down propulsion system"); + fprintf(stderr, "[cmd] GROUND ERROR, locking down propulsion system\n"); + mavlink_log_critical(mavlink_fd, "[cmd] GROUND ERROR, locking down propulsion system"); break; case SYSTEM_STATE_PREFLIGHT: @@ -123,10 +123,10 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { /* set system flags according to state */ current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "[commander] Switched to PREFLIGHT state"); + mavlink_log_critical(mavlink_fd, "[cmd] Switched to PREFLIGHT state"); } else { invalid_state = true; - mavlink_log_critical(mavlink_fd, "[commander] REFUSED to switch to PREFLIGHT state"); + mavlink_log_critical(mavlink_fd, "[cmd] REFUSED to switch to PREFLIGHT state"); } break; @@ -136,13 +136,13 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con invalid_state = false; /* set system flags according to state */ current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "[commander] REBOOTING SYSTEM"); + mavlink_log_critical(mavlink_fd, "[cmd] REBOOTING SYSTEM"); usleep(500000); up_systemreset(); /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ } else { invalid_state = true; - mavlink_log_critical(mavlink_fd, "[commander] REFUSED to REBOOT"); + mavlink_log_critical(mavlink_fd, "[cmd] REFUSED to REBOOT"); } break; @@ -152,7 +152,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* standby enforces disarmed */ current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "[commander] Switched to STANDBY state"); + mavlink_log_critical(mavlink_fd, "[cmd] Switched to STANDBY state"); break; case SYSTEM_STATE_GROUND_READY: @@ -162,7 +162,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* ground ready has motors / actuators armed */ current_status->flag_system_armed = true; - mavlink_log_critical(mavlink_fd, "[commander] Switched to GROUND READY state"); + mavlink_log_critical(mavlink_fd, "[cmd] Switched to GROUND READY state"); break; case SYSTEM_STATE_AUTO: @@ -172,7 +172,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* auto is airborne and in auto mode, motors armed */ current_status->flag_system_armed = true; - mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / AUTO mode"); + mavlink_log_critical(mavlink_fd, "[cmd] Switched to FLYING / AUTO mode"); break; case SYSTEM_STATE_STABILIZED: @@ -180,7 +180,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* set system flags according to state */ current_status->flag_system_armed = true; - mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / STABILIZED mode"); + mavlink_log_critical(mavlink_fd, "[cmd] Switched to FLYING / STABILIZED mode"); break; case SYSTEM_STATE_MANUAL: @@ -188,7 +188,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* set system flags according to state */ current_status->flag_system_armed = true; - mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / MANUAL mode"); + mavlink_log_critical(mavlink_fd, "[cmd] Switched to FLYING / MANUAL mode"); break; default: @@ -203,7 +203,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con ret = OK; } if (invalid_state) { - mavlink_log_critical(mavlink_fd, "[commander] REJECTING invalid state transition"); + mavlink_log_critical(mavlink_fd, "[cmd] REJECTING invalid state transition"); ret = ERROR; } return ret; @@ -232,7 +232,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - printf("[commander] new state: %s\n", system_state_txt[current_status->state_machine]); + printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]); } void publish_armed_status(const struct vehicle_status_s *current_status) { @@ -250,7 +250,7 @@ void publish_armed_status(const struct vehicle_status_s *current_status) { */ void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { - fprintf(stderr, "[commander] EMERGENCY HANDLER\n"); + fprintf(stderr, "[cmd] 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) { @@ -262,7 +262,7 @@ void state_machine_emergency_always_critical(int status_pub, struct vehicle_stat //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); + fprintf(stderr, "[cmd] Unknown system state: #%d\n", current_status->state_machine); } } @@ -272,7 +272,7 @@ void state_machine_emergency(int status_pub, struct vehicle_status_s *current_st 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); + //global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); } } @@ -497,7 +497,7 @@ void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { if (current_status->state_machine == SYSTEM_STATE_STANDBY) { - printf("[commander] arming\n"); + printf("[cmd] arming\n"); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); } } @@ -505,11 +505,11 @@ 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, 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"); + printf("[cmd] going standby\n"); 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"); + printf("[cmd] MISSION ABORT!\n"); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); } } @@ -518,6 +518,7 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c { int old_mode = current_status->flight_mode; current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; + current_status->flag_control_manual_enabled = true; /* enable attitude control per default */ current_status->flag_control_attitude_enabled = true; @@ -525,58 +526,92 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c 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"); + 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_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->flag_control_manual_enabled = true; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); + if (!current_status->flag_vector_flight_mode_ok) { + mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. STAB MODE"); + tune_error(); + return; + } + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { + printf("[cmd] stabilized mode\n"); + int old_mode = current_status->flight_mode; + current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB; + 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_STABILIZED); + if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); + + } +} +void update_state_machine_mode_hold(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. HOLD MODE"); + return; + } + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[commander] stabilized mode\n"); + printf("[cmd] stabilized mode\n"); + int old_mode = current_status->flight_mode; + current_status->flight_mode = VEHICLE_FLIGHT_MODE_HOLD; + 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_STABILIZED); + if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); } } 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; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, 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("[commander] auto mode\n"); + 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); } } 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; - /* 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) + /* Switch on HIL if in standby and not already in HIL mode */ + if ((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"); - } else if (current_status->state_machine != SYSTEM_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "[commander] REJECTING HIL, not in standby.") + 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, "[cmd] REJECTING HIL, disarm first!") + } else { + + mavlink_log_critical(mavlink_fd, "[cmd] REJECTING HIL, not in standby.") + } } /* switch manual / auto */ @@ -595,7 +630,7 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_ 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"); + printf("[cmd] arming due to command request\n"); } } @@ -605,13 +640,14 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_ 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"); + 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)) { - fprintf(stderr, "[commander] DENYING request to switch of HIL. Please power cycle (safety reasons)\n"); + fprintf(stderr, "[cmd] DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); + mavlink_log_critical(mavlink_fd, "[cmd] Power-cycle to exit HIL"); ret = ERROR; } @@ -636,7 +672,8 @@ 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); + mavlink_log_critical(mavlink_fd, "[cmd] Rebooting.."); + usleep(200000); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT); ret = 0; } diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index c4d1b78a5..815645089 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -128,6 +128,15 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c 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 hold + * + * @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_hold(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 diff --git a/apps/drivers/boards/px4fmu/px4fmu_adc.c b/apps/drivers/boards/px4fmu/px4fmu_adc.c index 987894163..b55af486d 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_adc.c +++ b/apps/drivers/boards/px4fmu/px4fmu_adc.c @@ -67,10 +67,10 @@ /* Identifying number of each ADC channel: Variable Resistor. */ #ifdef CONFIG_STM32_ADC3 -static const uint8_t g_chanlist[ADC3_NCHANNELS] = {10, 11};// , 12, 13}; ADC12 and 13 are used by MPU on v1.5 boards +static const uint8_t g_chanlist[ADC3_NCHANNELS] = {10, 11, 12, 13}; /* Configurations of pins used byte each ADC channels */ -static const uint32_t g_pinlist[ADC3_NCHANNELS] = {GPIO_ADC3_IN10, GPIO_ADC3_IN11}; // ADC12 and 13 are used by MPU on v1.5 boards, GPIO_ADC3_IN12, GPIO_ADC3_IN13}; +static const uint32_t g_pinlist[ADC3_NCHANNELS] = {GPIO_ADC3_IN10, GPIO_ADC3_IN11, GPIO_ADC3_IN12, GPIO_ADC3_IN13}; #endif /************************************************************************************ diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c index 57ffb77d3..bc047aa4f 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_init.c +++ b/apps/drivers/boards/px4fmu/px4fmu_init.c @@ -150,9 +150,7 @@ __EXPORT int nsh_archinitialize(void) int result; /* configure the high-resolution time/callout interface */ -#ifdef CONFIG_HRT_TIMER hrt_init(); -#endif /* configure CPU load estimation */ #ifdef CONFIG_SCHED_INSTRUMENTATION @@ -160,27 +158,21 @@ __EXPORT int nsh_archinitialize(void) #endif /* set up the serial DMA polling */ -#ifdef SERIAL_HAVE_DMA - { - static struct hrt_call serial_dma_call; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); - } -#endif - - message("\r\n"); + static struct hrt_call serial_dma_call; + struct timespec ts; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)stm32_serial_dma_poll, + NULL); // initial LED state drv_led_start(); @@ -209,8 +201,7 @@ __EXPORT int nsh_archinitialize(void) message("[boot] Successfully initialized SPI port 1\r\n"); -#if defined(CONFIG_STM32_SPI3) - /* Get the SPI port */ + /* Get the SPI port for the microsd slot */ message("[boot] Initializing SPI port 3\n"); spi3 = up_spiinitialize(3); @@ -233,7 +224,6 @@ __EXPORT int nsh_archinitialize(void) } message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n"); -#endif /* SPI3 */ #ifdef CONFIG_ADC int adc_state = adc_devinit(); diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h index 97033f2cd..b2fee65ac 100644 --- a/apps/drivers/drv_pwm_output.h +++ b/apps/drivers/drv_pwm_output.h @@ -95,7 +95,7 @@ ORB_DECLARE(output_pwm); * Note that ioctls and ObjDev updates should not be mixed, as the * behaviour of the system in this case is not defined. */ -#define _PWM_SERVO_BASE 0x2700 +#define _PWM_SERVO_BASE 0x2a00 /** arm all servo outputs handle by this driver */ #define PWM_SERVO_ARM _IOC(_PWM_SERVO_BASE, 0) diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp index 67b16aa42..3c6355d6e 100644 --- a/apps/drivers/hil/hil.cpp +++ b/apps/drivers/hil/hil.cpp @@ -68,11 +68,11 @@ #include <drivers/device/device.h> #include <drivers/drv_pwm_output.h> #include <drivers/drv_gpio.h> -// #include <drivers/boards/HIL/HIL_internal.h> +#include <drivers/drv_hrt.h> +#include <drivers/drv_mixer.h> #include <systemlib/systemlib.h> #include <systemlib/mixer/mixer.h> -#include <drivers/drv_mixer.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_outputs.h> @@ -382,7 +382,6 @@ HIL::task_main() /* this would be bad... */ if (ret < 0) { log("poll error %d", errno); - usleep(1000000); continue; } @@ -396,16 +395,27 @@ HIL::task_main() if (_mixers != nullptr) { /* do mixing */ - _mixers->mix(&outputs.output[0], num_outputs); + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.timestamp = hrt_absolute_time(); /* iterate actuators */ for (unsigned i = 0; i < num_outputs; i++) { - /* scale for PWM output 900 - 2100us */ - outputs.output[i] = 1500 + (600 * outputs.output[i]); - - /* output to the servo */ - // up_pwm_servo_set(i, outputs.output[i]); + /* last resort: catch NaN, INF and out-of-band errors */ + if (i < (unsigned)outputs.noutputs && + isfinite(outputs.output[i]) && + outputs.output[i] >= -1.0f && + outputs.output[i] <= 1.0f) { + /* scale for PWM output 900 - 2100us */ + outputs.output[i] = 1500 + (600 * outputs.output[i]); + } else { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = 900; + } } /* and publish for anyone that cares to see */ @@ -419,9 +429,6 @@ HIL::task_main() /* get new value */ orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); - - /* update PWM servo armed status */ - // up_pwm_servo_arm(aa.armed); } } diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index ed79440cc..55b7cfa85 100644 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ b/apps/drivers/mpu6000/mpu6000.cpp @@ -446,8 +446,12 @@ MPU6000::init() // write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ); usleep(1000); - /* do CDev init for the gyro device node */ - ret = _gyro->init(); + /* do CDev init for the gyro device node, keep it optional */ + int gyro_ret = _gyro->init(); + + if (gyro_ret != OK) { + _gyro_topic = -1; + } return ret; } @@ -938,7 +942,9 @@ MPU6000::measure() /* and publish for subscribers */ orb_publish(ORB_ID(sensor_accel), _accel_topic, &_accel_report); - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &_gyro_report); + if (_gyro_topic != -1) { + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &_gyro_report); + } /* stop measuring */ perf_end(_sample_perf); diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index dac0b5e84..a672bd2fb 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -58,8 +58,10 @@ #include <drivers/drv_pwm_output.h> #include <drivers/drv_gpio.h> #include <drivers/boards/px4fmu/px4fmu_internal.h> +#include <drivers/drv_hrt.h> #include <systemlib/systemlib.h> +#include <systemlib/err.h> #include <systemlib/mixer/mixer.h> #include <drivers/drv_mixer.h> @@ -112,9 +114,9 @@ private: void task_main() __attribute__((noreturn)); static int control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input); + uint8_t control_group, + uint8_t control_index, + float &input); int pwm_ioctl(file *filp, int cmd, unsigned long arg); @@ -180,6 +182,7 @@ PX4FMU::~PX4FMU() _task_should_exit = true; unsigned i = 10; + do { /* wait 50ms - it should wake every 100ms or so worst-case */ usleep(50000); @@ -215,6 +218,7 @@ PX4FMU::init() /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); + if (ret == OK) { log("default PWM output device"); _primary_pwm_device = true; @@ -248,7 +252,7 @@ PX4FMU::task_main_trampoline(int argc, char *argv[]) int PX4FMU::set_mode(Mode mode) { - /* + /* * Configure for PWM output. * * Note that regardless of the configured mode, the task is always @@ -282,6 +286,7 @@ PX4FMU::set_mode(Mode mode) default: return -EINVAL; } + _mode = mode; return OK; } @@ -341,6 +346,7 @@ PX4FMU::task_main() /* handle update rate changes */ if (_current_update_rate != _update_rate) { int update_rate_in_ms = int(1000 / _update_rate); + /* reject faster than 500 Hz updates */ if (update_rate_in_ms < 2) { update_rate_in_ms = 2; @@ -351,6 +357,7 @@ PX4FMU::task_main() update_rate_in_ms = 20; _update_rate = 50; } + orb_set_interval(_t_actuators, update_rate_in_ms); up_pwm_servo_set_rate(_update_rate); _current_update_rate = _update_rate; @@ -376,7 +383,8 @@ PX4FMU::task_main() if (_mixers != nullptr) { /* do mixing */ - _mixers->mix(&outputs.output[0], num_outputs); + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.timestamp = hrt_absolute_time(); // XXX output actual limited values memcpy(&controls_effective, &_controls, sizeof(controls_effective)); @@ -386,8 +394,21 @@ PX4FMU::task_main() /* iterate actuators */ for (unsigned i = 0; i < num_outputs; i++) { - /* scale for PWM output 900 - 2100us */ - outputs.output[i] = 1500 + (600 * outputs.output[i]); + /* last resort: catch NaN, INF and out-of-band errors */ + if (i < outputs.noutputs && + isfinite(outputs.output[i]) && + outputs.output[i] >= -1.0f && + outputs.output[i] <= 1.0f) { + /* scale for PWM output 900 - 2100us */ + outputs.output[i] = 1500 + (600 * outputs.output[i]); + } else { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = 900; + } /* output to the servo */ up_pwm_servo_set(i, outputs.output[i]); @@ -428,9 +449,9 @@ PX4FMU::task_main() int PX4FMU::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) + uint8_t control_group, + uint8_t control_index, + float &input) { const actuator_controls_s *controls = (actuator_controls_s *)handle; @@ -448,15 +469,17 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) /* try it as a GPIO ioctl first */ ret = gpio_ioctl(filp, cmd, arg); + if (ret != -ENOTTY) return ret; /* if we are in valid PWM mode, try it as a PWM ioctl as well */ - switch(_mode) { + switch (_mode) { case MODE_2PWM: case MODE_4PWM: ret = pwm_ioctl(filp, cmd, arg); break; + default: debug("not in a PWM mode"); break; @@ -574,8 +597,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) } _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + if (_mixers == nullptr) { ret = -ENOMEM; + } else { debug("loading mixers from %s", path); @@ -606,7 +631,7 @@ void PX4FMU::gpio_reset(void) { /* - * Setup default GPIO config - all pins as GPIOs, GPIO driver chip + * Setup default GPIO config - all pins as GPIOs, GPIO driver chip * to input mode. */ for (unsigned i = 0; i < _ngpio; i++) @@ -633,17 +658,20 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function) /* configure selected GPIOs as required */ for (unsigned i = 0; i < _ngpio; i++) { - if (gpios & (1<<i)) { + if (gpios & (1 << i)) { switch (function) { case GPIO_SET_INPUT: stm32_configgpio(_gpio_tab[i].input); break; + case GPIO_SET_OUTPUT: stm32_configgpio(_gpio_tab[i].output); break; + case GPIO_SET_ALT_1: if (_gpio_tab[i].alt != 0) stm32_configgpio(_gpio_tab[i].alt); + break; } } @@ -660,7 +688,7 @@ PX4FMU::gpio_write(uint32_t gpios, int function) int value = (function == GPIO_SET) ? 1 : 0; for (unsigned i = 0; i < _ngpio; i++) - if (gpios & (1<<i)) + if (gpios & (1 << i)) stm32_gpiowrite(_gpio_tab[i].output, value); } @@ -684,7 +712,7 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg) lock(); switch (cmd) { - + case GPIO_RESET: gpio_reset(); break; @@ -786,6 +814,7 @@ fmu_new_mode(PortMode new_mode, int update_rate) /* (re)set the PWM output mode */ g_fmu->set_mode(servo_mode); + if ((servo_mode != PX4FMU::MODE_NONE) && (update_rate != 0)) g_fmu->set_pwm_rate(update_rate); @@ -824,13 +853,18 @@ test(void) fd = open(PWM_OUTPUT_DEVICE_PATH, 0); - if (fd < 0) { - puts("open fail"); - exit(1); - } + if (fd < 0) + errx(1, "open fail"); + + if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed"); + + if (ioctl(fd, PWM_SERVO_SET(0), 1000) < 0) err(1, "servo 1 set failed"); - ioctl(fd, PWM_SERVO_ARM, 0); - ioctl(fd, PWM_SERVO_SET(0), 1000); + if (ioctl(fd, PWM_SERVO_SET(1), 1200) < 0) err(1, "servo 2 set failed"); + + if (ioctl(fd, PWM_SERVO_SET(2), 1400) < 0) err(1, "servo 3 set failed"); + + if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed"); close(fd); @@ -840,10 +874,8 @@ test(void) void fake(int argc, char *argv[]) { - if (argc < 5) { - puts("fmu fake <roll> <pitch> <yaw> <thrust> (values -100 .. 100)"); - exit(1); - } + if (argc < 5) + errx(1, "fmu fake <roll> <pitch> <yaw> <thrust> (values -100 .. 100)"); actuator_controls_s ac; @@ -857,10 +889,18 @@ fake(int argc, char *argv[]) orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac); - if (handle < 0) { - puts("advertise failed"); - exit(1); - } + if (handle < 0) + errx(1, "advertise failed"); + + actuator_armed_s aa; + + aa.armed = true; + aa.lockdown = false; + + handle = orb_advertise(ORB_ID(actuator_armed), &aa); + + if (handle < 0) + errx(1, "advertise failed 2"); exit(0); } @@ -914,15 +954,17 @@ fmu_main(int argc, char *argv[]) if (new_mode == PORT_FULL_PWM || new_mode == PORT_PWM_AND_GPIO) { if (argc > i + 1) { pwm_update_rate_in_hz = atoi(argv[i + 1]); + } else { - fprintf(stderr, "missing argument for pwm update rate (-u)\n"); + errx(1, "missing argument for pwm update rate (-u)"); return 1; } + } else { - fprintf(stderr, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio\n"); + errx(1, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio"); } } - } + } /* was a new mode set? */ if (new_mode != PORT_MODE_UNSET) { @@ -939,5 +981,5 @@ fmu_main(int argc, char *argv[]) fprintf(stderr, "FMU: unrecognised command, try:\n"); fprintf(stderr, " mode_gpio, mode_serial, mode_pwm [-u pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n"); - return -EINVAL; + exit(1); } diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 1fb65413a..99f573d1d 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -62,6 +62,7 @@ #include <drivers/device/device.h> #include <drivers/drv_rc_input.h> #include <drivers/drv_pwm_output.h> +#include <drivers/drv_gpio.h> #include <drivers/drv_hrt.h> #include <drivers/drv_mixer.h> @@ -70,6 +71,7 @@ #include <systemlib/hx_stream.h> #include <systemlib/err.h> #include <systemlib/systemlib.h> +#include <systemlib/scheduling_priorities.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls_effective.h> @@ -131,6 +133,8 @@ private: bool _primary_pwm_device; ///< true if we are the default PWM output + uint32_t _relays; ///< state of the PX4IO relays, one bit per relay + volatile bool _switch_armed; ///< PX4IO switch armed state // XXX how should this work? @@ -206,6 +210,7 @@ PX4IO::PX4IO() : _t_outputs(-1), _mixers(nullptr), _primary_pwm_device(false), + _relays(0), _switch_armed(false), _send_needed(false), _config_needed(false) @@ -255,7 +260,7 @@ PX4IO::init() } /* start the IO interface task */ - _task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr); + _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr); if (_task < 0) { debug("task start failed: %d", errno); return -errno; @@ -402,19 +407,22 @@ PX4IO::task_main() /* mix */ if (_mixers != nullptr) { - /* XXX is this the right count? */ - _mixers->mix(&_outputs.output[0], _max_actuators); + _outputs.timestamp = hrt_absolute_time(); + _outputs.noutputs = _mixers->mix(&_outputs.output[0], _max_actuators); // XXX output actual limited values memcpy(&_controls_effective, &_controls, sizeof(_controls_effective)); orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &_controls_effective); - /* convert to PWM values */ for (unsigned i = 0; i < _max_actuators; i++) { /* last resort: catch NaN, INF and out-of-band errors */ - if (isfinite(_outputs.output[i]) && _outputs.output[i] >= -1.0f && _outputs.output[i] <= 1.0f) { + if (i < _outputs.noutputs && + isfinite(_outputs.output[i]) && + _outputs.output[i] >= -1.0f && + _outputs.output[i] <= 1.0f) { + /* scale for PWM output 900 - 2100us */ _outputs.output[i] = 1500 + (600 * _outputs.output[i]); } else { /* @@ -571,9 +579,13 @@ PX4IO::io_send() cmd.servo_command[i] = _outputs.output[i]; /* publish as we send */ + _outputs.timestamp = hrt_absolute_time(); orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &_outputs); - // XXX relays + + /* update relays */ + for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) + cmd.relay_state[i] = (_relays & (1<< i)) ? true : false; /* armed and not locked down -> arming ok */ cmd.arm_ok = (_armed.armed && !_armed.lockdown); @@ -641,6 +653,30 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) *(servo_position_t *)arg = _outputs.output[cmd - PWM_SERVO_GET(0)]; break; + case GPIO_RESET: + _relays = 0; + _send_needed = true; + break; + + case GPIO_SET: + case GPIO_CLEAR: + /* make sure only valid bits are being set */ + if ((arg & ((1UL << PX4IO_RELAY_CHANNELS) - 1)) != arg) { + ret = EINVAL; + break; + } + if (cmd == GPIO_SET) { + _relays |= arg; + } else { + _relays &= ~arg; + } + _send_needed = true; + break; + + case GPIO_GET: + *(uint32_t *)arg = _relays; + break; + case MIXERIOCGETOUTPUTCOUNT: *(unsigned *)arg = _max_actuators; break; diff --git a/apps/drivers/stm32/drv_pwm_servo.c b/apps/drivers/stm32/drv_pwm_servo.c index 50aa34d81..954b458f5 100644 --- a/apps/drivers/stm32/drv_pwm_servo.c +++ b/apps/drivers/stm32/drv_pwm_servo.c @@ -60,13 +60,13 @@ #include "drv_pwm_servo.h" -#include "chip.h" -#include "up_internal.h" -#include "up_arch.h" +#include <chip.h> +#include <up_internal.h> +#include <up_arch.h> -#include "stm32_internal.h" -#include "stm32_gpio.h" -#include "stm32_tim.h" +#include <stm32_internal.h> +#include <stm32_gpio.h> +#include <stm32_tim.h> /* default rate (in Hz) of PWM updates */ @@ -143,27 +143,27 @@ pwm_channel_init(unsigned channel) /* configure the channel */ switch (pwm_channels[channel].timer_channel) { case 1: - rCCMR1(timer) |= (6 << 4); + rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE; rCCR1(timer) = pwm_channels[channel].default_value; - rCCER(timer) |= (1 << 0); + rCCER(timer) |= GTIM_CCER_CC1E; break; case 2: - rCCMR1(timer) |= (6 << 12); + rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC2M_SHIFT) | GTIM_CCMR1_OC2PE; rCCR2(timer) = pwm_channels[channel].default_value; - rCCER(timer) |= (1 << 4); + rCCER(timer) |= GTIM_CCER_CC2E; break; case 3: - rCCMR2(timer) |= (6 << 4); + rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC3M_SHIFT) | GTIM_CCMR2_OC3PE; rCCR3(timer) = pwm_channels[channel].default_value; - rCCER(timer) |= (1 << 8); + rCCER(timer) |= GTIM_CCER_CC3E; break; case 4: - rCCMR2(timer) |= (6 << 12); + rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC4M_SHIFT) | GTIM_CCMR2_OC4PE; rCCR4(timer) = pwm_channels[channel].default_value; - rCCER(timer) |= (1 << 12); + rCCER(timer) |= GTIM_CCER_CC4E; break; } } @@ -288,17 +288,20 @@ up_pwm_servo_set_rate(unsigned rate) void up_pwm_servo_arm(bool armed) { - /* - * XXX this is inelgant and in particular will either jam outputs at whatever level - * they happen to be at at the time the timers stop or generate runts. - * The right thing is almost certainly to kill auto-reload on the timers so that - * they just stop at the end of their count for disable, and to reset/restart them - * for enable. - */ - /* iterate timers and arm/disarm appropriately */ for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) { - if (pwm_timers[i].base != 0) - rCR1(i) = armed ? GTIM_CR1_CEN : 0; + if (pwm_timers[i].base != 0) { + if (armed) { + /* force an update to preload all registers */ + rEGR(i) = GTIM_EGR_UG; + + /* arm requires the timer be enabled */ + rCR1(i) |= GTIM_CR1_CEN | GTIM_CR1_ARPE; + + } else { + /* on disarm, just stop auto-reload so we don't generate runts */ + rCR1(i) &= ~GTIM_CR1_ARPE; + } + } } } diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c index 3e1fc4952..cd479d40d 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_main.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c @@ -195,16 +195,20 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) /* pass through throttle */ actuators.control[3] = att_sp.thrust; + /* set flaps to zero */ + actuators.control[4] = 0.0f; + } else if (vstatus.state_machine == SYSTEM_STATE_STABILIZED) { /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if(vstatus.rc_signal_lost) { + if (vstatus.rc_signal_lost) { // XXX define failsafe throttle param //param_get(failsafe_throttle_handle, &failsafe_throttle); att_sp.roll_body = 0.3f; att_sp.pitch_body = 0.0f; - att_sp.thrust = 0.5f; + att_sp.yaw_body = 0; + att_sp.thrust = 0.4f; // XXX disable yaw control, loiter @@ -214,9 +218,16 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) att_sp.pitch_body = manual_sp.pitch; att_sp.yaw_body = 0; att_sp.thrust = manual_sp.throttle; - att_sp.timestamp = hrt_absolute_time(); } + att_sp.timestamp = hrt_absolute_time(); + + // XXX: Stop copying setpoint / reference from bus, instead keep position + // and mix RC inputs in. + // XXX: For now just stabilize attitude, not anything else + // proper implementation should do stabilization in position controller + // and just check for stabilized or auto state + /* attitude control */ fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); @@ -226,13 +237,63 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) /* pass through throttle */ actuators.control[3] = att_sp.thrust; + /* set flaps to zero */ + actuators.control[4] = 0.0f; + } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { - /* directly pass through values */ - actuators.control[0] = manual_sp.roll; - /* positive pitch means negative actuator -> pull up */ - actuators.control[1] = manual_sp.pitch; - actuators.control[2] = manual_sp.yaw; - actuators.control[3] = manual_sp.throttle; + if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { + + /* if the RC signal is lost, try to stay level and go slowly back down to ground */ + if (vstatus.rc_signal_lost) { + + // XXX define failsafe throttle param + //param_get(failsafe_throttle_handle, &failsafe_throttle); + att_sp.roll_body = 0.3f; + att_sp.pitch_body = 0.0f; + att_sp.thrust = 0.4f; + att_sp.yaw_body = 0; + + // XXX disable yaw control, loiter + + } else { + + att_sp.roll_body = manual_sp.roll; + att_sp.pitch_body = manual_sp.pitch; + att_sp.yaw_body = 0; + att_sp.thrust = manual_sp.throttle; + } + + att_sp.timestamp = hrt_absolute_time(); + + /* attitude control */ + fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); + + /* angular rate control */ + fixedwing_att_control_rates(&rates_sp, gyro, &actuators); + + /* pass through throttle */ + actuators.control[3] = att_sp.thrust; + + /* pass through flaps */ + if (isfinite(manual_sp.flaps)) { + actuators.control[4] = manual_sp.flaps; + } else { + actuators.control[4] = 0.0f; + } + + } else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + /* directly pass through values */ + actuators.control[0] = manual_sp.roll; + /* positive pitch means negative actuator -> pull up */ + actuators.control[1] = manual_sp.pitch; + actuators.control[2] = manual_sp.yaw; + actuators.control[3] = manual_sp.throttle; + if (isfinite(manual_sp.flaps)) { + actuators.control[4] = manual_sp.flaps; + } else { + actuators.control[4] = 0.0f; + } + } } /* publish rates */ diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 575b42b37..ccc9d6d7d 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -189,10 +189,34 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) *mavlink_mode = 0; /* set mode flags independent of system state */ + + /* HIL */ if (v_status.flag_hil_enabled) { *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; } + /* manual input */ + if (v_status.flag_control_manual_enabled) { + *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + } + + /* attitude or rate control */ + if (v_status.flag_control_attitude_enabled || + v_status.flag_control_rates_enabled) { + *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; + } + + /* vector control */ + if (v_status.flag_control_velocity_enabled || + v_status.flag_control_position_enabled) { + *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; + } + + /* autonomous mode */ + if (v_status.state_machine == SYSTEM_STATE_AUTO) { + *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED; + } + /* set arming state */ if (armed.armed) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; @@ -221,20 +245,14 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) case SYSTEM_STATE_MANUAL: *mavlink_state = MAV_STATE_ACTIVE; - *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; break; case SYSTEM_STATE_STABILIZED: *mavlink_state = MAV_STATE_ACTIVE; - *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; - *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; break; case SYSTEM_STATE_AUTO: *mavlink_state = MAV_STATE_ACTIVE; - *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; - *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; - *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED; break; case SYSTEM_STATE_MISSION_ABORT: @@ -469,14 +487,15 @@ mavlink_message_t* mavlink_get_channel_buffer(uint8_t channel) void mavlink_update_system(void) { static bool initialized = false; - param_t param_system_id; - param_t param_component_id; - param_t param_system_type; + static param_t param_system_id; + static param_t param_component_id; + static param_t param_system_type; if (!initialized) { param_system_id = param_find("MAV_SYS_ID"); param_component_id = param_find("MAV_COMP_ID"); param_system_type = param_find("MAV_TYPE"); + initialized = true; } /* update system and component id */ diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index b0aa401fc..40e52a500 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -455,7 +455,7 @@ l_actuator_outputs(struct listener *l) act_outputs.output[7]); /* only send in HIL mode */ - if (mavlink_hil_enabled) { + if (mavlink_hil_enabled && armed.armed) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; @@ -506,20 +506,19 @@ l_actuator_outputs(struct listener *l) mavlink_mode, 0); } else { - float rudder, throttle; - /* SCALING: PWM min: 900, PWM max: 2100, center: 1500 */ + /* + * Catch the case where no rudder is in use and throttle is not + * on output four + */ + float rudder, throttle; - // XXX very ugly, needs rework - if (isfinite(act_outputs.output[3]) - && act_outputs.output[3] > 800 && act_outputs.output[3] < 2200) { - /* throttle is fourth output */ - rudder = (act_outputs.output[2] - 1500.0f) / 600.0f; - throttle = (((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f); + if (act_outputs.noutputs < 4) { + rudder = 0.0f; + throttle = (act_outputs.output[2] - 900.0f) / 1200.0f; } else { - /* only three outputs, put throttle on position 4 / index 3 */ - rudder = 0; - throttle = (((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f); + rudder = (act_outputs.output[2] - 1500.0f) / 600.0f; + throttle = (act_outputs.output[3] - 900.0f) / 1200.0f; } mavlink_msg_hil_controls_send(chan, diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index ce1e52c1b..f184859a3 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -232,19 +232,9 @@ mc_thread_main(int argc, char *argv[]) /* STEP 2: publish the result to the vehicle actuators */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } - /* decide wether we want rate or position input */ - } - else if (state.flag_control_manual_enabled) { - - /* manual inputs, from RC control or joystick */ - if (state.flag_control_rates_enabled && !state.flag_control_attitude_enabled) { - rates_sp.roll = manual.roll; + - rates_sp.pitch = manual.pitch; - rates_sp.yaw = manual.yaw; - rates_sp.thrust = manual.throttle; - rates_sp.timestamp = hrt_absolute_time(); - } + } else if (state.flag_control_manual_enabled) { if (state.flag_control_attitude_enabled) { @@ -258,7 +248,7 @@ mc_thread_main(int argc, char *argv[]) static bool rc_loss_first_time = true; /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if(state.rc_signal_lost) { + if (state.rc_signal_lost) { /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ param_get(failsafe_throttle_handle, &failsafe_throttle); att_sp.roll_body = 0.0f; @@ -285,41 +275,66 @@ mc_thread_main(int argc, char *argv[]) att_sp.yaw_body = att.yaw; } - /* only move setpoint if manual input is != 0 */ + /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ + if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || + state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { - if(manual.mode == MANUAL_CONTROL_MODE_ATT_YAW_POS) { - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { + if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { rates_sp.yaw = manual.yaw; control_yaw_position = false; - first_time_after_yaw_speed_control = true; } else { - if (first_time_after_yaw_speed_control) { - att_sp.yaw_body = att.yaw; - first_time_after_yaw_speed_control = false; + /* + * This mode SHOULD be the default mode, which is: + * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS + * + * However, we fall back to this setting for all other (nonsense) + * settings as well. + */ + + /* only move setpoint if manual input is != 0 */ + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { + rates_sp.yaw = manual.yaw; + control_yaw_position = false; + first_time_after_yaw_speed_control = true; + } else { + if (first_time_after_yaw_speed_control) { + att_sp.yaw_body = att.yaw; + first_time_after_yaw_speed_control = false; + } + control_yaw_position = true; } - control_yaw_position = true; } - } else if (manual.mode == MANUAL_CONTROL_MODE_ATT_YAW_RATE) { - rates_sp.yaw = manual.yaw; - control_yaw_position = false; } att_sp.thrust = manual.throttle; att_sp.timestamp = hrt_absolute_time(); } - } - /* STEP 2: publish the result to the vehicle actuators */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - - if (motor_test_mode) { - printf("testmode"); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.yaw_body = 0.0f; - att_sp.thrust = 0.1f; - att_sp.timestamp = hrt_absolute_time(); - /* STEP 2: publish the result to the vehicle actuators */ + + /* STEP 2: publish the controller output */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + + if (motor_test_mode) { + printf("testmode"); + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.yaw_body = 0.0f; + att_sp.thrust = 0.1f; + att_sp.timestamp = hrt_absolute_time(); + /* STEP 2: publish the result to the vehicle actuators */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } + + } else { + /* manual rate inputs, from RC control or joystick */ + if (state.flag_control_rates_enabled && + state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { + rates_sp.roll = manual.roll; + + rates_sp.pitch = manual.pitch; + rates_sp.yaw = manual.yaw; + rates_sp.thrust = manual.throttle; + rates_sp.timestamp = hrt_absolute_time(); + } } } diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index e94d7c55d..b98736cee 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -197,7 +197,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s } /* load new parameters with lower rate */ - if (motor_skip_counter % 1000 == 0) { + if (motor_skip_counter % 500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); @@ -206,6 +206,13 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); } + /* reset integral if on ground */ + if(att_sp->thrust < 0.1f) { + pid_reset_integral(&pitch_controller); + pid_reset_integral(&roll_controller); + } + + /* calculate current control outputs */ /* control pitch (forward) output */ diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 0fcf952ab..d41de54a2 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -52,6 +52,7 @@ #include <nuttx/clock.h> #include <drivers/drv_hrt.h> +#include <drivers/drv_pwm_output.h> #include <systemlib/hx_stream.h> #include <systemlib/perf_counter.h> @@ -183,7 +184,7 @@ comms_handle_command(const void *buffer, size_t length) system_state.vector_flight_mode_ok = cmd->vector_flight_mode_ok; system_state.manual_override_ok = cmd->manual_override_ok; system_state.mixer_fmu_available = true; - system_state.fmu_data_received = true; + system_state.fmu_data_received_time = hrt_absolute_time(); /* set PWM update rate if changed (after limiting) */ uint16_t new_servo_rate = cmd->servo_rate; @@ -201,9 +202,32 @@ comms_handle_command(const void *buffer, size_t length) system_state.servo_rate = new_servo_rate; } - /* XXX do relay changes here */ + /* + * update servo values immediately. + * the updates are done in addition also + * in the mainloop, since this function will only + * update with a connected FMU. + */ + mixer_tick(); + + /* handle relay state changes here */ for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) { - system_state.relays[i] = cmd->relay_state[i]; + if (system_state.relays[i] != cmd->relay_state[i]) { + switch (i) { + case 0: + POWER_ACC1(cmd->relay_state[i]); + break; + case 1: + POWER_ACC2(cmd->relay_state[i]); + break; + case 2: + POWER_RELAY1(cmd->relay_state[i]); + break; + case 3: + POWER_RELAY2(cmd->relay_state[i]); + break; + } + } } irqrestore(flags); diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 9db9a0fa5..48370d9d0 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -61,8 +61,8 @@ #include "px4io.h" #define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */ -#define RC_CHANNEL_HIGH_THRESH 1600 -#define RC_CHANNEL_LOW_THRESH 1400 +#define RC_CHANNEL_HIGH_THRESH 1700 +#define RC_CHANNEL_LOW_THRESH 1300 static void ppm_input(void); @@ -92,10 +92,22 @@ controls_main(void) */ bool locked = false; + /* + * Store RC channel count to detect switch to RC loss sooner + * than just by timeout + */ + unsigned rc_channels = system_state.rc_channels; + + /* + * Track if any input got an update in this round + */ + bool rc_updated; + if (fds[0].revents & POLLIN) locked |= dsm_input(); if (fds[1].revents & POLLIN) - locked |= sbus_input(); + locked |= sbus_input(fds[1].fd, PX4IO_INPUT_CHANNELS, &system_state.rc_channel_data, + &system_state.rc_channels, &system_state.rc_channels_timestamp, &rc_updated); /* * If we don't have lock from one of the serial receivers, @@ -127,13 +139,20 @@ controls_main(void) /* set the number of channels to zero - no inputs */ system_state.rc_channels = 0; - system_state.rc_lost = true; - - /* trigger an immediate report to the FMU */ - system_state.fmu_report_due = true; + rc_updated = true; } - /* do PWM output updates */ + /* + * If there was a RC update OR the RC signal status (lost / present) has + * just changed, request an update immediately. + */ + system_state.fmu_report_due |= rc_updated; + + /* + * PWM output updates are performed in addition on each comm update. + * the updates here are required to ensure operation if FMU is not started + * or stopped responding. + */ mixer_tick(); } } diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index 5a644906b..9cdb98e23 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -48,15 +48,17 @@ #include <unistd.h> #include <fcntl.h> +#include <debug.h> + #include <drivers/drv_pwm_output.h> +#include <drivers/drv_hrt.h> #include "px4io.h" /* - * Count of periodic calls in which we have no FMU input. + * Maximum interval in us before FMU signal is considered lost */ -static unsigned fmu_input_drops; -#define FMU_INPUT_DROP_LIMIT 20 +#define FMU_INPUT_DROP_LIMIT_US 200000 /* * Update a mixer based on the current control signals. @@ -82,37 +84,45 @@ mixer_tick(void) int i; bool should_arm; + /* check that we are receiving fresh data from the FMU */ + if ((hrt_absolute_time() - system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { + /* too many frames without FMU input, time to go to failsafe */ + system_state.mixer_manual_override = true; + system_state.mixer_fmu_available = false; + lib_lowprintf("\nRX timeout\n"); + } + /* * Decide which set of inputs we're using. */ - if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) { - /* we have recent control data from the FMU */ - control_count = PX4IO_OUTPUT_CHANNELS; - control_values = &system_state.fmu_channel_data[0]; - - /* check that we are receiving fresh data from the FMU */ - if (!system_state.fmu_data_received) { - fmu_input_drops++; - - /* too many frames without FMU input, time to go to failsafe */ - if (fmu_input_drops >= FMU_INPUT_DROP_LIMIT) { - system_state.mixer_manual_override = true; - system_state.mixer_fmu_available = false; - } + /* this is for planes, where manual override makes sense */ + if(system_state.manual_override_ok) { + /* if everything is ok */ + if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) { + /* we have recent control data from the FMU */ + control_count = PX4IO_OUTPUT_CHANNELS; + control_values = &system_state.fmu_channel_data[0]; + /* when override is on or the fmu is not available */ + } else if (system_state.rc_channels > 0) { + control_count = system_state.rc_channels; + control_values = &system_state.rc_channel_data[0]; } else { - fmu_input_drops = 0; - system_state.fmu_data_received = false; + /* we have no control input (no FMU, no RC) */ + + // XXX builtin failsafe would activate here + control_count = 0; } - } else if (system_state.rc_channels > 0 && system_state.manual_override_ok) { - /* we have control data from an R/C input */ - control_count = system_state.rc_channels; - control_values = &system_state.rc_channel_data[0]; + /* this is for multicopters, etc. where manual override does not make sense */ } else { - /* we have no control input */ - - // XXX builtin failsafe would activate here - control_count = 0; + /* if the fmu is available whe are good */ + if(system_state.mixer_fmu_available) { + control_count = PX4IO_OUTPUT_CHANNELS; + control_values = &system_state.fmu_channel_data[0]; + /* we better shut everything off */ + } else { + control_count = 0; + } } /* diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 84f5ba029..e9a2ff053 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -106,9 +106,9 @@ struct sys_state_s bool fmu_report_due; /** - * If true, new control data from the FMU has been received. + * Last FMU receive time, in microseconds since system boot */ - bool fmu_data_received; + uint64_t fmu_data_received_time; /** * Current serial interface mode, per the serial_rx_mode parameter @@ -162,8 +162,8 @@ extern volatile int timers[TIMER_NUM_TIMERS]; #define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) #define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) -#define POWER_ACC1(_s) stm32_gpiowrite(GPIO_SERVO_ACC1_EN, (_s)) -#define POWER_ACC2(_s) stm32_gpiowrite(GPIO_SERVO_ACC2_EN, (_s)) +#define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s)) +#define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s)) #define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) #define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s)) @@ -193,7 +193,8 @@ extern void controls_main(void); extern int dsm_init(const char *device); extern bool dsm_input(void); extern int sbus_init(const char *device); -extern bool sbus_input(void); +extern bool sbus_input(int fd, unsigned max_channels, uint16_t *channel_data, unsigned *channel_count, + uint64_t *receive_time, bool *updated); /* * Assertion codes diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index 9ce4589b7..3314ef513 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -64,15 +64,21 @@ static unsigned counter = 0; * Define the various LED flash sequences for each system state. */ #define LED_PATTERN_SAFE 0xffff /**< always on */ -#define LED_PATTERN_VECTOR_FLIGHT_MODE_OK 0x3000 /**< always on with short break */ +#define LED_PATTERN_VECTOR_FLIGHT_MODE_OK 0xFFFE /**< always on with short break */ #define LED_PATTERN_FMU_ARMED 0x4444 /**< slow blinking */ #define LED_PATTERN_IO_ARMED 0x5555 /**< fast blinking */ #define LED_PATTERN_IO_FMU_ARMED 0x5050 /**< long off then double blink */ static unsigned blink_counter = 0; +/* + * IMPORTANT: The arming state machine critically + * depends on using the same threshold + * for arming and disarming. Since disarming + * is quite deadly for the system, a similar + * length can be justified. + */ #define ARM_COUNTER_THRESHOLD 10 -#define DISARM_COUNTER_THRESHOLD 4 static bool safety_button_pressed; @@ -102,8 +108,16 @@ safety_check_button(void *arg) */ safety_button_pressed = BUTTON_SAFETY; - /* Keep pressed for a while to arm */ + /* + * Keep pressed for a while to arm. + * + * Note that the counting sequence has to be same length + * for arming / disarming in order to end up as proper + * state machine, keep ARM_COUNTER_THRESHOLD the same + * length in all cases of the if/else struct below. + */ if (safety_button_pressed && !system_state.armed) { + if (counter < ARM_COUNTER_THRESHOLD) { counter++; } else if (counter == ARM_COUNTER_THRESHOLD) { @@ -114,9 +128,10 @@ safety_check_button(void *arg) } /* Disarm quickly */ } else if (safety_button_pressed && system_state.armed) { - if (counter < DISARM_COUNTER_THRESHOLD) { + + if (counter < ARM_COUNTER_THRESHOLD) { counter++; - } else if (counter == DISARM_COUNTER_THRESHOLD) { + } else if (counter == ARM_COUNTER_THRESHOLD) { /* change to disarmed state and notify the FMU */ system_state.armed = false; counter++; diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index a8f628a84..0de0593e7 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -49,14 +49,11 @@ #define DEBUG #include "px4io.h" -#include "protocol.h" #include "debug.h" #define SBUS_FRAME_SIZE 25 #define SBUS_INPUT_CHANNELS 16 -static int sbus_fd = -1; - static hrt_abstime last_rx_time; static hrt_abstime last_frame_time; @@ -66,11 +63,14 @@ static unsigned partial_frame_count; unsigned sbus_frame_drops; -static void sbus_decode(hrt_abstime frame_time); +static int sbus_decode(hrt_abstime frame_time, unsigned max_channels, + uint16_t *channel_data, unsigned *channel_count, uint64_t *receive_time); int sbus_init(const char *device) { + static int sbus_fd = -1; + if (sbus_fd < 0) sbus_fd = open(device, O_RDONLY); @@ -96,7 +96,8 @@ sbus_init(const char *device) } bool -sbus_input(void) +sbus_input(int fd, unsigned max_channels, uint16_t *channel_data, unsigned *channel_count, + uint64_t *receive_time, bool *updated) { ssize_t ret; hrt_abstime now; @@ -128,7 +129,7 @@ sbus_input(void) * Fetch bytes, but no more than we would need to complete * the current frame. */ - ret = read(sbus_fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count); + ret = read(fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count); /* if the read failed for any reason, just give up here */ if (ret < 1) @@ -148,9 +149,9 @@ sbus_input(void) /* * Great, it looks like we might have a frame. Go ahead and - * decode it. + * decode it, report if the receiver got something. */ - sbus_decode(now); + *updated = (sbus_decode(now, max_channels, channel_data, channel_count, receive_time) == OK); partial_frame_count = 0; out: @@ -196,25 +197,32 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { /* 15 */ { {20, 5, 0x07, 0},{21, 0, 0xff, 3},{ 0, 0, 0x00, 0} } }; -static void -sbus_decode(hrt_abstime frame_time) +static int +sbus_decode(hrt_abstime frame_time, unsigned max_channels, uint16_t *channel_data, unsigned *channel_count, uint64_t *receive_time) { /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { sbus_frame_drops++; - return; + return 1; } - /* if the failsafe bit is set, we consider the frame invalid */ - if (frame[23] & (1 << 4)) { - return; + /* if the failsafe or connection lost bit is set, we consider the frame invalid */ + if ((frame[23] & (1 << 2)) && /* signal lost */ + (frame[23] & (1 << 3))) { /* failsafe */ + + /* actively announce signal loss */ + *channel_count = 0; + + return 1; } + /* decode failsafe and RC status */ + /* we have received something we think is a frame */ last_frame_time = frame_time; - unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ? - SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS; + unsigned chancount = (max_channels > SBUS_INPUT_CHANNELS) ? + SBUS_INPUT_CHANNELS : max_channels; /* use the decoder matrix to extract channel data */ for (unsigned channel = 0; channel < chancount; channel++) { @@ -236,17 +244,19 @@ sbus_decode(hrt_abstime frame_time) system_state.rc_channel_data[channel] = (value / 2) + 998; } - if (PX4IO_INPUT_CHANNELS >= 18) { - chancount = 18; - /* XXX decode the two switch channels */ + /* decode switch channels if data fields are wide enough */ + if (chancount > 17) { + /* channel 17 (index 16) */ + system_state.rc_channel_data[16] = (frame[23] & (1 << 0)) * 1000 + 998; + /* channel 18 (index 17) */ + system_state.rc_channel_data[17] = (frame[23] & (1 << 1)) * 1000 + 998; } /* note the number of channels decoded */ - system_state.rc_channels = chancount; + *channel_count = chancount; /* and note that we have received data from the R/C controller */ - system_state.rc_channels_timestamp = frame_time; + *receive_time = frame_time; - /* trigger an immediate report to the FMU */ - system_state.fmu_report_due = true; + return 0; } diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index 37ba0ec3e..1546fb56d 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -69,60 +69,100 @@ PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); PARAM_DEFINE_FLOAT(RC1_REV, 1.0f); PARAM_DEFINE_FLOAT(RC1_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC1_EXP, 0.0f); +// PARAM_DEFINE_FLOAT(RC1_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC2_MIN, 1000); PARAM_DEFINE_FLOAT(RC2_TRIM, 1500); PARAM_DEFINE_FLOAT(RC2_MAX, 2000); PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); PARAM_DEFINE_FLOAT(RC2_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC2_EXP, 0.0f); +// PARAM_DEFINE_FLOAT(RC2_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC3_MIN, 1000); PARAM_DEFINE_FLOAT(RC3_TRIM, 1500); PARAM_DEFINE_FLOAT(RC3_MAX, 2000); PARAM_DEFINE_FLOAT(RC3_REV, 1.0f); PARAM_DEFINE_FLOAT(RC3_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC3_EXP, 0.0f); +// PARAM_DEFINE_FLOAT(RC3_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC4_MIN, 1000); PARAM_DEFINE_FLOAT(RC4_TRIM, 1500); PARAM_DEFINE_FLOAT(RC4_MAX, 2000); PARAM_DEFINE_FLOAT(RC4_REV, 1.0f); PARAM_DEFINE_FLOAT(RC4_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC4_EXP, 0.0f); +// PARAM_DEFINE_FLOAT(RC4_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC5_MIN, 1000); PARAM_DEFINE_FLOAT(RC5_TRIM, 1500); PARAM_DEFINE_FLOAT(RC5_MAX, 2000); PARAM_DEFINE_FLOAT(RC5_REV, 1.0f); PARAM_DEFINE_FLOAT(RC5_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC5_EXP, 0.0f); +// PARAM_DEFINE_FLOAT(RC5_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC6_MIN, 1000); PARAM_DEFINE_FLOAT(RC6_TRIM, 1500); PARAM_DEFINE_FLOAT(RC6_MAX, 2000); PARAM_DEFINE_FLOAT(RC6_REV, 1.0f); PARAM_DEFINE_FLOAT(RC6_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC6_EXP, 0.0f); +// PARAM_DEFINE_FLOAT(RC6_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC7_MIN, 1000); PARAM_DEFINE_FLOAT(RC7_TRIM, 1500); PARAM_DEFINE_FLOAT(RC7_MAX, 2000); PARAM_DEFINE_FLOAT(RC7_REV, 1.0f); PARAM_DEFINE_FLOAT(RC7_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC7_EXP, 0.0f); +// PARAM_DEFINE_FLOAT(RC7_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC8_MIN, 1000); PARAM_DEFINE_FLOAT(RC8_TRIM, 1500); PARAM_DEFINE_FLOAT(RC8_MAX, 2000); PARAM_DEFINE_FLOAT(RC8_REV, 1.0f); PARAM_DEFINE_FLOAT(RC8_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC8_EXP, 0.0f); - -PARAM_DEFINE_INT32(RC_TYPE, 1); // 1 = FUTABA - -PARAM_DEFINE_INT32(RC_DEMIX, 0); /**< 0 = off, 1 = auto, 2 = delta */ +// PARAM_DEFINE_FLOAT(RC8_EXP, 0.0f); + +PARAM_DEFINE_FLOAT(RC9_MIN, 1000); +PARAM_DEFINE_FLOAT(RC9_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC9_MAX, 2000); +PARAM_DEFINE_FLOAT(RC9_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC9_DZ, 0.0f); +// PARAM_DEFINE_FLOAT(RC9_EXP, 0.0f); + +PARAM_DEFINE_FLOAT(RC10_MIN, 1000); +PARAM_DEFINE_FLOAT(RC10_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC10_MAX, 2000); +PARAM_DEFINE_FLOAT(RC10_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC10_DZ, 0.0f); +// PARAM_DEFINE_FLOAT(RC10_EXP, 0.0f); + +PARAM_DEFINE_FLOAT(RC11_MIN, 1000); +PARAM_DEFINE_FLOAT(RC11_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC11_MAX, 2000); +PARAM_DEFINE_FLOAT(RC11_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC11_DZ, 0.0f); +// PARAM_DEFINE_FLOAT(RC11_EXP, 0.0f); + +PARAM_DEFINE_FLOAT(RC12_MIN, 1000); +PARAM_DEFINE_FLOAT(RC12_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC12_MAX, 2000); +PARAM_DEFINE_FLOAT(RC12_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC12_DZ, 0.0f); +// PARAM_DEFINE_FLOAT(RC12_EXP, 0.0f); + +PARAM_DEFINE_FLOAT(RC13_MIN, 1000); +PARAM_DEFINE_FLOAT(RC13_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC13_MAX, 2000); +PARAM_DEFINE_FLOAT(RC13_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC13_DZ, 0.0f); +// PARAM_DEFINE_FLOAT(RC13_EXP, 0.0f); + +PARAM_DEFINE_FLOAT(RC14_MIN, 1000); +PARAM_DEFINE_FLOAT(RC14_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC14_MAX, 2000); +PARAM_DEFINE_FLOAT(RC14_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); +// PARAM_DEFINE_FLOAT(RC12_EXP, 0.0f); + +PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */ /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f)); @@ -131,12 +171,23 @@ PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); PARAM_DEFINE_INT32(RC_MAP_YAW, 4); -PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); -PARAM_DEFINE_INT32(RC_MAP_AUX1, 6); -PARAM_DEFINE_INT32(RC_MAP_AUX2, 7); -PARAM_DEFINE_INT32(RC_MAP_AUX3, 8); + +PARAM_DEFINE_INT32(RC_MAP_OVER_SW, 5); +PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 6); + +PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_SAS_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_RTL_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); + +PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); + +PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera yaw / azimuth */ +PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera pitch / tilt */ +PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera trigger */ +PARAM_DEFINE_INT32(RC_MAP_AUX4, 0); /**< default function: camera roll */ +PARAM_DEFINE_INT32(RC_MAP_AUX5, 0); /**< default function: payload drop */ PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.4f); PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.4f); PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 1.0f); - diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index b63bfb195..c331ec3e3 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -95,11 +95,7 @@ #define PPM_INPUT_TIMEOUT_INTERVAL 50000 /**< 50 ms timeout / 20 Hz */ -enum RC_DEMIX { - RC_DEMIX_NONE = 0, - RC_DEMIX_AUTO = 1, - RC_DEMIX_DELTA = 2 -}; +#define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg) /** * Sensor app start / stop handling function @@ -129,7 +125,7 @@ public: int start(); private: - static const unsigned _rc_max_chan_count = 8; /**< maximum number of r/c channels we handle */ + static const unsigned _rc_max_chan_count = RC_CHANNELS_MAX; /**< maximum number of r/c channels we handle */ #if CONFIG_HRT_PPM hrt_abstime _ppm_last_valid; /**< last time we got a valid ppm signal */ @@ -173,7 +169,7 @@ private: float max[_rc_max_chan_count]; float rev[_rc_max_chan_count]; float dz[_rc_max_chan_count]; - float ex[_rc_max_chan_count]; + // float ex[_rc_max_chan_count]; float scaling_factor[_rc_max_chan_count]; float gyro_offset[3]; @@ -184,21 +180,31 @@ private: int rc_type; - int rc_demix; /**< enabled de-mixing of RC mixers */ - int rc_map_roll; int rc_map_pitch; int rc_map_yaw; int rc_map_throttle; - int rc_map_mode_sw; + + int rc_map_manual_override_sw; + int rc_map_auto_mode_sw; + + int rc_map_manual_mode_sw; + int rc_map_sas_mode_sw; + int rc_map_rtl_sw; + int rc_map_offboard_ctrl_mode_sw; + + int rc_map_flaps; int rc_map_aux1; int rc_map_aux2; int rc_map_aux3; + int rc_map_aux4; + int rc_map_aux5; float rc_scale_roll; float rc_scale_pitch; float rc_scale_yaw; + float rc_scale_flaps; float battery_voltage_scaling; } _parameters; /**< local copies of interesting parameters */ @@ -209,7 +215,7 @@ private: param_t max[_rc_max_chan_count]; param_t rev[_rc_max_chan_count]; param_t dz[_rc_max_chan_count]; - param_t ex[_rc_max_chan_count]; + // param_t ex[_rc_max_chan_count]; param_t rc_type; param_t rc_demix; @@ -224,15 +230,27 @@ private: param_t rc_map_pitch; param_t rc_map_yaw; param_t rc_map_throttle; - param_t rc_map_mode_sw; + + param_t rc_map_manual_override_sw; + param_t rc_map_auto_mode_sw; + + param_t rc_map_manual_mode_sw; + param_t rc_map_sas_mode_sw; + param_t rc_map_rtl_sw; + param_t rc_map_offboard_ctrl_mode_sw; + + param_t rc_map_flaps; param_t rc_map_aux1; param_t rc_map_aux2; param_t rc_map_aux3; + param_t rc_map_aux4; + param_t rc_map_aux5; param_t rc_scale_roll; param_t rc_scale_pitch; param_t rc_scale_yaw; + param_t rc_scale_flaps; param_t battery_voltage_scaling; } _parameter_handles; /**< handles for interesting parameters */ @@ -395,27 +413,43 @@ Sensors::Sensors() : sprintf(nbuf, "RC%d_DZ", i + 1); _parameter_handles.dz[i] = param_find(nbuf); - /* channel exponential gain */ - sprintf(nbuf, "RC%d_EXP", i + 1); - _parameter_handles.ex[i] = param_find(nbuf); + // /* channel exponential gain */ + // sprintf(nbuf, "RC%d_EXP", i + 1); + // _parameter_handles.ex[i] = param_find(nbuf); } _parameter_handles.rc_type = param_find("RC_TYPE"); - _parameter_handles.rc_demix = param_find("RC_DEMIX"); + // _parameter_handles.rc_demix = param_find("RC_DEMIX"); + /* mandatory input switched, mapped to channels 1-4 per default */ _parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL"); _parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH"); _parameter_handles.rc_map_yaw = param_find("RC_MAP_YAW"); _parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); - _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW"); + + /* mandatory mode switches, mapped to channel 5 and 6 per default */ + _parameter_handles.rc_map_manual_override_sw = param_find("RC_MAP_OVER_SW"); + _parameter_handles.rc_map_auto_mode_sw = param_find("RC_MAP_MODE_SW"); + + _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); + + /* optional mode switches, not mapped per default */ + _parameter_handles.rc_map_manual_mode_sw = param_find("RC_MAP_MAN_SW"); + _parameter_handles.rc_map_sas_mode_sw = param_find("RC_MAP_SAS_SW"); + _parameter_handles.rc_map_rtl_sw = param_find("RC_MAP_RTL_SW"); + _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); + _parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); _parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); _parameter_handles.rc_map_aux3 = param_find("RC_MAP_AUX3"); + _parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4"); + _parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5"); _parameter_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); _parameter_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); _parameter_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); + _parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS"); /* gyro offsets */ _parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF"); @@ -472,10 +506,10 @@ Sensors::~Sensors() int Sensors::parameters_update() { - const unsigned int nchans = 8; + bool rc_valid = true; /* rc values */ - for (unsigned int i = 0; i < nchans; i++) { + for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) { if (param_get(_parameter_handles.min[i], &(_parameters.min[i])) != OK) { warnx("Failed getting min for chan %d", i); @@ -492,32 +526,33 @@ Sensors::parameters_update() if (param_get(_parameter_handles.dz[i], &(_parameters.dz[i])) != OK) { warnx("Failed getting dead zone for chan %d", i); } - if (param_get(_parameter_handles.ex[i], &(_parameters.ex[i])) != OK) { - warnx("Failed getting exponential gain for chan %d", i); - } + // if (param_get(_parameter_handles.ex[i], &(_parameters.ex[i])) != OK) { + // warnx("Failed getting exponential gain for chan %d", i); + // } _parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); /* handle blowup in the scaling factor calculation */ - if (isnan(_parameters.scaling_factor[i]) || isinf(_parameters.scaling_factor[i])) { + if (!isfinite(_parameters.scaling_factor[i]) || + _parameters.scaling_factor[i] * _parameters.rev[i] < 0.000001f || + _parameters.scaling_factor[i] * _parameters.rev[i] > 0.2f) { + + /* scaling factors do not make sense, lock them down */ _parameters.scaling_factor[i] = 0; + rc_valid = false; } - /* handle wrong values */ - // XXX TODO - } + /* handle wrong values */ + if (!rc_valid) + warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); + /* remote control type */ if (param_get(_parameter_handles.rc_type, &(_parameters.rc_type)) != OK) { warnx("Failed getting remote control type"); } - /* de-mixing */ - if (param_get(_parameter_handles.rc_demix, &(_parameters.rc_demix)) != OK) { - warnx("Failed getting demix setting"); - } - /* channel mapping */ if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) { warnx("Failed getting roll chan index"); @@ -531,9 +566,30 @@ Sensors::parameters_update() if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) { warnx("Failed getting throttle chan index"); } - if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { - warnx("Failed getting mode sw chan index"); + if (param_get(_parameter_handles.rc_map_manual_override_sw, &(_parameters.rc_map_manual_override_sw)) != OK) { + warnx("Failed getting override sw chan index"); + } + if (param_get(_parameter_handles.rc_map_auto_mode_sw, &(_parameters.rc_map_auto_mode_sw)) != OK) { + warnx("Failed getting auto mode sw chan index"); + } + + if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { + warnx("Failed getting flaps chan index"); + } + + if (param_get(_parameter_handles.rc_map_manual_mode_sw, &(_parameters.rc_map_manual_mode_sw)) != OK) { + warnx("Failed getting manual mode sw chan index"); + } + if (param_get(_parameter_handles.rc_map_rtl_sw, &(_parameters.rc_map_rtl_sw)) != OK) { + warnx("Failed getting rtl sw chan index"); + } + if (param_get(_parameter_handles.rc_map_sas_mode_sw, &(_parameters.rc_map_sas_mode_sw)) != OK) { + warnx("Failed getting sas mode sw chan index"); } + if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { + warnx("Failed getting offboard control mode sw chan index"); + } + if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) { warnx("Failed getting mode aux 1 index"); } @@ -543,6 +599,12 @@ Sensors::parameters_update() if (param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)) != OK) { warnx("Failed getting mode aux 3 index"); } + if (param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)) != OK) { + warnx("Failed getting mode aux 4 index"); + } + if (param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)) != OK) { + warnx("Failed getting mode aux 5 index"); + } if (param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)) != OK) { warnx("Failed getting rc scaling for roll"); @@ -553,16 +615,31 @@ Sensors::parameters_update() if (param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)) != OK) { warnx("Failed getting rc scaling for yaw"); } + if (param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps)) != OK) { + warnx("Failed getting rc scaling for flaps"); + } /* update RC function mappings */ _rc.function[THROTTLE] = _parameters.rc_map_throttle - 1; _rc.function[ROLL] = _parameters.rc_map_roll - 1; _rc.function[PITCH] = _parameters.rc_map_pitch - 1; _rc.function[YAW] = _parameters.rc_map_yaw - 1; - _rc.function[OVERRIDE] = _parameters.rc_map_mode_sw - 1; - _rc.function[FUNC_0] = _parameters.rc_map_aux1 - 1; - _rc.function[FUNC_1] = _parameters.rc_map_aux2 - 1; - _rc.function[FUNC_2] = _parameters.rc_map_aux3 - 1; + + _rc.function[OVERRIDE] = _parameters.rc_map_manual_override_sw - 1; + _rc.function[AUTO_MODE] = _parameters.rc_map_auto_mode_sw - 1; + + _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; + + _rc.function[MANUAL_MODE] = _parameters.rc_map_manual_mode_sw - 1; + _rc.function[RTL] = _parameters.rc_map_rtl_sw - 1; + _rc.function[SAS_MODE] = _parameters.rc_map_sas_mode_sw - 1; + _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1; + + _rc.function[AUX_1] = _parameters.rc_map_aux1 - 1; + _rc.function[AUX_2] = _parameters.rc_map_aux2 - 1; + _rc.function[AUX_3] = _parameters.rc_map_aux3 - 1; + _rc.function[AUX_4] = _parameters.rc_map_aux4 - 1; + _rc.function[AUX_5] = _parameters.rc_map_aux5 - 1; /* gyro offsets */ param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0])); @@ -948,8 +1025,23 @@ Sensors::ppm_poll() struct manual_control_setpoint_s manual_control; - /* get a copy first, to prevent altering values */ - orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &manual_control); + /* initialize to default values */ + manual_control.roll = NAN; + manual_control.pitch = NAN; + manual_control.yaw = NAN; + manual_control.throttle = NAN; + + manual_control.manual_mode_switch = NAN; + manual_control.manual_sas_switch = NAN; + manual_control.return_to_launch_switch = NAN; + manual_control.auto_offboard_input_switch = NAN; + + manual_control.flaps = NAN; + manual_control.aux1 = NAN; + manual_control.aux2 = NAN; + manual_control.aux3 = NAN; + manual_control.aux4 = NAN; + manual_control.aux5 = NAN; /* require at least four channels to consider the signal valid */ if (rc_input.channel_count < 4) @@ -996,79 +1088,100 @@ Sensors::ppm_poll() manual_control.timestamp = rc_input.timestamp; - /* check if input needs to be de-mixed */ - if (_parameters.rc_demix == (int)RC_DEMIX_DELTA) { - // XXX hardcoded for testing purposes, replace with inverted delta mixer from ROMFS - - /* roll input - mixed roll and pitch channels */ - manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled - _rc.chan[_rc.function[PITCH]].scaled; - /* pitch input - mixed roll and pitch channels */ - manual_control.pitch = -0.5f * (_rc.chan[_rc.function[ROLL]].scaled + _rc.chan[_rc.function[PITCH]].scaled); - /* yaw input - stick right is positive and positive rotation */ - manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled; - /* throttle input */ - manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; - - /* direct pass-through of manual input */ - } else { - /* roll input - rolling right is stick-wise and rotation-wise positive */ - manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled; - /* - * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, - * so reverse sign. - */ - manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled; - /* yaw input - stick right is positive and positive rotation */ - manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled; - /* throttle input */ - manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; - } + /* roll input - rolling right is stick-wise and rotation-wise positive */ + manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled); + /* + * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, + * so reverse sign. + */ + manual_control.pitch = limit_minus_one_to_one(-1.0f * _rc.chan[_rc.function[PITCH]].scaled); + /* yaw input - stick right is positive and positive rotation */ + manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled); + /* throttle input */ + manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; + if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; + if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; - /* limit outputs */ - if (manual_control.roll < -1.0f) manual_control.roll = -1.0f; - if (manual_control.roll > 1.0f) manual_control.roll = 1.0f; + /* scale output */ if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) { manual_control.roll *= _parameters.rc_scale_roll; } - - if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f; - if (manual_control.pitch > 1.0f) manual_control.pitch = 1.0f; if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) { manual_control.pitch *= _parameters.rc_scale_pitch; } - if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f; - if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f; if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) { manual_control.yaw *= _parameters.rc_scale_yaw; } - - if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; - if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; + + /* override switch input */ + manual_control.manual_override_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OVERRIDE]].scaled); /* mode switch input */ - manual_control.override_mode_switch = _rc.chan[_rc.function[OVERRIDE]].scaled; - if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f; - if (manual_control.override_mode_switch > 1.0f) manual_control.override_mode_switch = 1.0f; - - /* aux functions */ - manual_control.aux1_cam_pan_flaps = _rc.chan[_rc.function[FUNC_0]].scaled; - if (manual_control.aux1_cam_pan_flaps < -1.0f) manual_control.aux1_cam_pan_flaps = -1.0f; - if (manual_control.aux1_cam_pan_flaps > 1.0f) manual_control.aux1_cam_pan_flaps = 1.0f; - - /* aux inputs */ - manual_control.aux2_cam_tilt = _rc.chan[_rc.function[FUNC_1]].scaled; - if (manual_control.aux2_cam_tilt < -1.0f) manual_control.aux2_cam_tilt = -1.0f; - if (manual_control.aux2_cam_tilt > 1.0f) manual_control.aux2_cam_tilt = 1.0f; - - /* aux inputs */ - manual_control.aux3_cam_zoom = _rc.chan[_rc.function[FUNC_2]].scaled; - if (manual_control.aux3_cam_zoom < -1.0f) manual_control.aux3_cam_zoom = -1.0f; - if (manual_control.aux3_cam_zoom > 1.0f) manual_control.aux3_cam_zoom = 1.0f; - - orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); - orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); + manual_control.auto_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[AUTO_MODE]].scaled); + + /* flaps */ + if (_rc.function[FLAPS] >= 0) { + + manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled); + + if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) { + manual_control.flaps *= _parameters.rc_scale_flaps; + } + } + + if (_rc.function[MANUAL_MODE] >= 0) { + manual_control.manual_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MANUAL_MODE]].scaled); + } + + if (_rc.function[SAS_MODE] >= 0) { + manual_control.manual_sas_switch = limit_minus_one_to_one(_rc.chan[_rc.function[SAS_MODE]].scaled); + } + + if (_rc.function[RTL] >= 0) { + manual_control.return_to_launch_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RTL]].scaled); + } + + if (_rc.function[OFFBOARD_MODE] >= 0) { + manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); + } + + /* aux functions, only assign if valid mapping is present */ + if (_rc.function[AUX_1] >= 0) { + manual_control.aux1 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_1]].scaled); + } + + if (_rc.function[AUX_2] >= 0) { + manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].scaled); + } + + if (_rc.function[AUX_3] >= 0) { + manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].scaled); + } + + if (_rc.function[AUX_4] >= 0) { + manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled); + } + + if (_rc.function[AUX_5] >= 0) { + manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled); + } + + /* check if ready for publishing */ + if (_rc_pub > 0) { + orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); + } else { + /* advertise the rc topic */ + _rc_pub = orb_advertise(ORB_ID(rc_channels), &_rc); + } + + /* check if ready for publishing */ + if (_manual_control_pub > 0) { + orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); + } else { + _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); + } } } @@ -1117,7 +1230,7 @@ Sensors::task_main() memset(&raw, 0, sizeof(raw)); raw.timestamp = hrt_absolute_time(); raw.battery_voltage_v = BAT_VOL_INITIAL; - raw.adc_voltage_v[0] = 0.9f; + raw.adc_voltage_v[0] = 0.0f; raw.adc_voltage_v[1] = 0.0f; raw.adc_voltage_v[2] = 0.0f; raw.battery_voltage_counter = 0; @@ -1134,27 +1247,6 @@ Sensors::task_main() /* advertise the sensor_combined topic and make the initial publication */ _sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); - /* advertise the manual_control topic */ - struct manual_control_setpoint_s manual_control; - manual_control.mode = MANUAL_CONTROL_MODE_ATT_YAW_POS; - manual_control.roll = 0.0f; - manual_control.pitch = 0.0f; - manual_control.yaw = 0.0f; - manual_control.throttle = 0.0f; - manual_control.aux1_cam_pan_flaps = 0.0f; - manual_control.aux2_cam_tilt = 0.0f; - manual_control.aux3_cam_zoom = 0.0f; - manual_control.aux4_cam_roll = 0.0f; - - _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); - - /* advertise the rc topic */ - { - struct rc_channels_s rc; - memset(&rc, 0, sizeof(rc)); - _rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); - } - /* wakeup source(s) */ struct pollfd fds[1]; diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c index 0358caa25..49315cdc9 100644 --- a/apps/systemlib/pid/pid.c +++ b/apps/systemlib/pid/pid.c @@ -183,3 +183,9 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo return pid->last_output; } + + +__EXPORT void pid_reset_integral(PID_t *pid) +{ + pid->integral = 0; +} diff --git a/apps/systemlib/pid/pid.h b/apps/systemlib/pid/pid.h index b51afef9e..64d668867 100644 --- a/apps/systemlib/pid/pid.h +++ b/apps/systemlib/pid/pid.h @@ -72,6 +72,7 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float //void pid_set(PID_t *pid, float sp); __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); +__EXPORT void pid_reset_integral(PID_t *pid); #endif /* PID_H_ */ diff --git a/apps/systemlib/scheduling_priorities.h b/apps/systemlib/scheduling_priorities.h new file mode 100644 index 000000000..be1dbfcd8 --- /dev/null +++ b/apps/systemlib/scheduling_priorities.h @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include <nuttx/sched.h> + +/* SCHED_PRIORITY_MAX */ +#define SCHED_PRIORITY_FAST_DRIVER SCHED_PRIORITY_MAX +#define SCHED_PRIORITY_WATCHDOG (SCHED_PRIORITY_MAX - 5) +#define SCHED_PRIORITY_ACTUATOR_OUTPUTS (SCHED_PRIORITY_MAX - 15) +#define SCHED_PRIORITY_ATTITUDE_CONTROL (SCHED_PRIORITY_MAX - 25) +#define SCHED_PRIORITY_SLOW_DRIVER (SCHED_PRIORITY_MAX - 35) +#define SCHED_PRIORITY_POSITION_CONTROL (SCHED_PRIORITY_MAX - 40) +/* SCHED_PRIORITY_DEFAULT */ +#define SCHED_PRIORITY_LOGGING (SCHED_PRIORITY_DEFAULT - 10) +#define SCHED_PRIORITY_PARAMS (SCHED_PRIORITY_DEFAULT - 15) +/* SCHED_PRIORITY_IDLE */ diff --git a/apps/uORB/topics/actuator_outputs.h b/apps/uORB/topics/actuator_outputs.h index accd560af..bbe429073 100644 --- a/apps/uORB/topics/actuator_outputs.h +++ b/apps/uORB/topics/actuator_outputs.h @@ -53,8 +53,9 @@ #define NUM_ACTUATOR_OUTPUT_GROUPS 4 /**< for sanity checking */ struct actuator_outputs_s { - uint64_t timestamp; - float output[NUM_ACTUATOR_OUTPUTS]; + uint64_t timestamp; /**< output timestamp in us since system boot */ + float output[NUM_ACTUATOR_OUTPUTS]; /**< output data, in natural output units */ + int noutputs; /**< valid outputs */ }; /* actuator output sets; this list can be expanded as more drivers emerge */ diff --git a/apps/uORB/topics/manual_control_setpoint.h b/apps/uORB/topics/manual_control_setpoint.h index 1368cb716..261a8a4ad 100644 --- a/apps/uORB/topics/manual_control_setpoint.h +++ b/apps/uORB/topics/manual_control_setpoint.h @@ -48,29 +48,33 @@ * @{ */ -enum MANUAL_CONTROL_MODE -{ - MANUAL_CONTROL_MODE_DIRECT = 0, - MANUAL_CONTROL_MODE_ATT_YAW_RATE = 1, - MANUAL_CONTROL_MODE_ATT_YAW_POS = 2, - MANUAL_CONTROL_MODE_MULTIROTOR_SIMPLE = 3 /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ -}; - struct manual_control_setpoint_s { uint64_t timestamp; - enum MANUAL_CONTROL_MODE mode; /**< The current control inputs mode */ - float roll; /**< ailerons roll / roll rate input */ - float pitch; /**< elevator / pitch / pitch rate */ - float yaw; /**< rudder / yaw rate / yaw */ - float throttle; /**< throttle / collective thrust / altitude */ + float roll; /**< ailerons roll / roll rate input */ + float pitch; /**< elevator / pitch / pitch rate */ + float yaw; /**< rudder / yaw rate / yaw */ + float throttle; /**< throttle / collective thrust / altitude */ + + float manual_override_switch; /**< manual override mode (mandatory) */ + float auto_mode_switch; /**< auto mode switch (mandatory) */ + + /** + * Any of the channels below may not be available and be set to NaN + * to indicate that it does not contain valid data. + */ + float manual_mode_switch; /**< manual mode (man, sas, alt) switch (optional) */ + float manual_sas_switch; /**< sas mode (rates / attitude) switch (optional) */ + float return_to_launch_switch; /**< return to launch switch (0 = disabled, 1 = enabled) */ + float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */ - float override_mode_switch; + float flaps; /**< flap position */ - float aux1_cam_pan_flaps; - float aux2_cam_tilt; - float aux3_cam_zoom; - float aux4_cam_roll; + float aux1; /**< default function: camera yaw / azimuth */ + float aux2; /**< default function: camera pitch / tilt */ + float aux3; /**< default function: camera trigger */ + float aux4; /**< default function: camera roll */ + float aux5; /**< default function: payload drop */ }; /**< manual control inputs */ diff --git a/apps/uORB/topics/rc_channels.h b/apps/uORB/topics/rc_channels.h index fef6ef2b3..9dd54df91 100644 --- a/apps/uORB/topics/rc_channels.h +++ b/apps/uORB/topics/rc_channels.h @@ -50,6 +50,13 @@ * @{ */ +/** + * The number of RC channel inputs supported. + * Current (Q1/2013) radios support up to 18 channels, + * leaving at a sane value of 14. + */ +#define RC_CHANNELS_MAX 14 + /** * This defines the mapping of the RC functions. * The value assigned to the specific function corresponds to the entry of @@ -62,14 +69,18 @@ enum RC_CHANNELS_FUNCTION PITCH = 2, YAW = 3, OVERRIDE = 4, - FUNC_0 = 5, - FUNC_1 = 6, - FUNC_2 = 7, - FUNC_3 = 8, - FUNC_4 = 9, - FUNC_5 = 10, - FUNC_6 = 11, - RC_CHANNELS_FUNCTION_MAX = 12 + AUTO_MODE = 5, + MANUAL_MODE = 6, + SAS_MODE = 7, + RTL = 8, + OFFBOARD_MODE = 9, + FLAPS = 10, + AUX_1 = 11, + AUX_2 = 12, + AUX_3 = 13, + AUX_4 = 14, + AUX_5 = 15, + RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; struct rc_channels_s { @@ -78,14 +89,13 @@ struct rc_channels_s { uint64_t timestamp_last_valid; /**< timestamp of last valid RC signal. */ struct { float scaled; /**< Scaled to -1..1 (throttle: 0..1) */ - } chan[RC_CHANNELS_FUNCTION_MAX]; - uint8_t chan_count; /**< maximum number of valid channels */ + } chan[RC_CHANNELS_MAX]; + uint8_t chan_count; /**< number of valid channels */ /*String array to store the names of the functions*/ char function_name[RC_CHANNELS_FUNCTION_MAX][20]; - uint8_t function[RC_CHANNELS_FUNCTION_MAX]; + int8_t function[RC_CHANNELS_FUNCTION_MAX]; uint8_t rssi; /**< Overall receive signal strength */ - bool is_valid; /**< Inputs are valid, no timeout */ }; /**< radio control channels. */ /** diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 738ca644f..230521f53 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -87,16 +87,23 @@ enum VEHICLE_MODE_FLAG { }; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ enum VEHICLE_FLIGHT_MODE { - VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, same as VEHICLE_FLIGHT_MODE_ATTITUDE for multirotors */ - VEHICLE_FLIGHT_MODE_ATTITUDE, /**< attitude or rate stabilization, as defined by VEHICLE_ATTITUDE_MODE */ - VEHICLE_FLIGHT_MODE_STABILIZED, /**< attitude or rate stabilization plus velocity or position stabilization */ - VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */ + VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */ + VEHICLE_FLIGHT_MODE_STAB, /**< attitude or rate stabilization plus velocity or position stabilization */ + VEHICLE_FLIGHT_MODE_HOLD, /**< hold current position (hover or loiter around position when switched) */ + VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */ }; -enum VEHICLE_ATTITUDE_MODE { - VEHICLE_ATTITUDE_MODE_DIRECT, /**< no attitude control, direct stick input mixing (only fixed wing) */ - VEHICLE_ATTITUDE_MODE_RATES, /**< body rates control mode */ - VEHICLE_ATTITUDE_MODE_ATTITUDE /**< tait-bryan attitude control mode */ +enum VEHICLE_MANUAL_CONTROL_MODE { + VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0, /**< no attitude control, direct stick input mixing (only fixed wing) */ + VEHICLE_MANUAL_CONTROL_MODE_RATES, /**< body rates control mode */ + VEHICLE_MANUAL_CONTROL_MODE_SAS /**< stability augmented system (SAS) mode */ +}; + +enum VEHICLE_MANUAL_SAS_MODE { + VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0, /**< roll, pitch and yaw absolute */ + VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE, /**< roll and pitch absolute, yaw rate */ + VEHICLE_MANUAL_SAS_MODE_SIMPLE, /**< simple mode (includes altitude hold) */ + VEHICLE_MANUAL_SAS_MODE_ALTITUDE /**< altitude hold */ }; /** @@ -115,12 +122,10 @@ struct vehicle_status_s commander_state_machine_t state_machine; /**< current flight state, main state machine */ enum VEHICLE_FLIGHT_MODE flight_mode; /**< current flight mode, as defined by mode switch */ - enum VEHICLE_ATTITUDE_MODE attitute_mode; /**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */ + enum VEHICLE_MANUAL_CONTROL_MODE manual_control_mode; /**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */ + enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; /**< current stabilization mode */ int32_t system_type; /**< system type, inspired by MAVLinks MAV_TYPE enum */ - // uint8_t mode; - - /* system flags - these represent the state predicates */ bool flag_system_armed; /**< true is motors / actuators are armed */ @@ -165,11 +170,12 @@ struct vehicle_status_s uint16_t errors_count3; uint16_t errors_count4; -// bool remote_manual; /**< set to true by the commander when the manual-switch on the remote is set to manual */ - bool flag_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */ + bool flag_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */ bool flag_local_position_valid; - bool flag_vector_flight_mode_ok; /**< position estimation, battery voltage and other critical subsystems are good for autonomous flight */ - bool flag_external_manual_override_ok; + bool flag_vector_flight_mode_ok; /**< position estimation, battery voltage and other critical subsystems are good for autonomous flight */ + bool flag_auto_flight_mode_ok; /**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */ + bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ + bool flag_valid_launch_position; /**< indicates a valid launch position */ }; /** |