diff options
Diffstat (limited to 'src/modules')
29 files changed, 351 insertions, 291 deletions
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 65abcde1e..a70a14fe4 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -308,18 +308,20 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); if (!initialized) { - - gyro_offsets[0] += raw.gyro_rad_s[0]; - gyro_offsets[1] += raw.gyro_rad_s[1]; - gyro_offsets[2] += raw.gyro_rad_s[2]; - offset_count++; - - if (hrt_absolute_time() - start_time > 3000000LL) { - initialized = true; - gyro_offsets[0] /= offset_count; - gyro_offsets[1] /= offset_count; - gyro_offsets[2] /= offset_count; - } + // XXX disabling init for now + initialized = true; + + // gyro_offsets[0] += raw.gyro_rad_s[0]; + // gyro_offsets[1] += raw.gyro_rad_s[1]; + // gyro_offsets[2] += raw.gyro_rad_s[2]; + // offset_count++; + + // if (hrt_absolute_time() - start_time > 3000000LL) { + // initialized = true; + // gyro_offsets[0] /= offset_count; + // gyro_offsets[1] /= offset_count; + // gyro_offsets[2] /= offset_count; + // } } else { diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp index 236052b56..86bda3c75 100755 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp +++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp @@ -730,7 +730,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds // Because proper mount of PX4 will give you a reversed accelerometer readings. NonlinearSO3AHRSupdate(gyro[0],gyro[1],gyro[2],-acc[0],-acc[1],-acc[2],mag[0],mag[1],mag[2],so3_comp_params.Kp,so3_comp_params.Ki, dt); - // Convert q->R. + // Convert q->R, This R converts inertial frame to body frame. Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11 Rot_matrix[1] = 2.0 * (q1*q2 + q0*q3); // 12 Rot_matrix[2] = 2.0 * (q1*q3 - q0*q2); // 13 @@ -794,7 +794,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets)); /* copy rotation matrix */ - memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); + memcpy(&att.R, Rot_matrix, sizeof(float)*9); att.R_valid = true; if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index ed6707f04..cfa7d9e8a 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -241,8 +241,10 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float break; int orient = detect_orientation(mavlink_fd, sensor_combined_sub); - if (orient < 0) + if (orient < 0) { + close(sensor_combined_sub); return ERROR; + } if (data_collected[orient]) { mavlink_log_info(mavlink_fd, "%s done, please rotate to a different axis", orientation_strs[orient]); diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index e414e5f70..248eb4a66 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -85,6 +85,7 @@ int do_airspeed_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); + close(diff_pres_sub); return ERROR; } } @@ -95,6 +96,7 @@ int do_airspeed_calibration(int mavlink_fd) if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, "Setting offs failed!"); + close(diff_pres_sub); return ERROR; } @@ -104,18 +106,17 @@ int do_airspeed_calibration(int mavlink_fd) if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + close(diff_pres_sub); return ERROR; } - //char buf[50]; - //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, "airspeed calibration done"); - + close(diff_pres_sub); return OK; } else { mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + close(diff_pres_sub); return ERROR; } } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a548f943e..333fe30ae 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -50,6 +50,7 @@ #include <unistd.h> #include <fcntl.h> #include <errno.h> +#include <systemlib/err.h> #include <debug.h> #include <sys/prctl.h> #include <sys/stat.h> @@ -153,6 +154,7 @@ static uint64_t last_print_mode_reject_time = 0; /* if connected via USB */ static bool on_usb_power = false; +static float takeoff_alt = 5.0f; /* tasks waiting for low prio thread */ typedef enum { @@ -318,6 +320,8 @@ void print_status() break; } + close(state_sub); + warnx("arming: %s", armed_str); } @@ -492,9 +496,10 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_sys_type = param_find("MAV_TYPE"); param_t _param_system_id = param_find("MAV_SYS_ID"); param_t _param_component_id = param_find("MAV_COMP_ID"); + param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT"); /* welcome user */ - warnx("[commander] starting"); + warnx("starting"); /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -592,9 +597,10 @@ int commander_thread_main(int argc, char *argv[]) pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, 2048); + pthread_attr_setstacksize(&commander_low_prio_attr, 2992); struct sched_param param; + (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); /* low priority */ param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); @@ -689,6 +695,8 @@ int commander_thread_main(int argc, char *argv[]) struct subsystem_info_s info; memset(&info, 0, sizeof(info)); + toggle_status_leds(&status, &armed, true); + /* now initialized */ commander_initialized = true; thread_running = true; @@ -731,8 +739,11 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_component_id, &(status.component_id)); status_changed = true; - /* Re-check RC calibration */ + /* re-check RC calibration */ rc_calibration_ok = (OK == rc_calibration_check()); + + /* navigation parameters */ + param_get(_param_takeoff_alt, &takeoff_alt); } } @@ -890,7 +901,7 @@ int commander_thread_main(int argc, char *argv[]) if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { low_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY"); - status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; + status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; battery_tune_played = false; } @@ -902,7 +913,7 @@ int commander_thread_main(int argc, char *argv[]) if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY"); - status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; + status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; battery_tune_played = false; if (armed.armed) { @@ -1160,12 +1171,12 @@ int commander_thread_main(int argc, char *argv[]) if (tune_arm() == OK) arm_tune_played = true; - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { /* play tune on battery warning */ if (tune_low_bat() == OK) battery_tune_played = true; - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { /* play tune on battery critical */ if (tune_critical_bat() == OK) battery_tune_played = true; @@ -1251,73 +1262,43 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang #endif if (changed) { - - int i; - rgbled_pattern_t pattern; - memset(&pattern, 0, sizeof(pattern)); + /* XXX TODO blink fast when armed and serious error occurs */ if (armed->armed) { - /* armed, solid */ - if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { - pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; - - } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { - pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; - - } else { - pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN; - } - - pattern.duration[0] = 1000; - + rgbled_set_mode(RGBLED_MODE_ON); } else if (armed->ready_to_arm) { - for (i = 0; i < 3; i++) { - if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { - pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; - - } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { - pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; - - } else { - pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN; - } - - pattern.duration[i * 2] = 200; - - pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; - pattern.duration[i * 2 + 1] = 800; - } - - if (status->condition_global_position_valid) { - pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; - pattern.duration[i * 2] = 1000; - pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; - pattern.duration[i * 2 + 1] = 800; - - } else { - for (i = 3; i < 6; i++) { - pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; - pattern.duration[i * 2] = 100; - pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; - pattern.duration[i * 2 + 1] = 100; - } - - pattern.color[6 * 2] = RGBLED_COLOR_OFF; - pattern.duration[6 * 2] = 700; - } - + rgbled_set_mode(RGBLED_MODE_BREATHE); } else { - for (i = 0; i < 3; i++) { - pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; - pattern.duration[i * 2] = 200; - pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; - pattern.duration[i * 2 + 1] = 200; - } - - /* not ready to arm, blink at 10Hz */ + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); } + } - rgbled_set_pattern(&pattern); + if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) { + switch (status->battery_warning) { + case VEHICLE_BATTERY_WARNING_LOW: + rgbled_set_color(RGBLED_COLOR_YELLOW); + break; + case VEHICLE_BATTERY_WARNING_CRITICAL: + rgbled_set_color(RGBLED_COLOR_AMBER); + break; + default: + break; + } + } else { + switch (status->main_state) { + case MAIN_STATE_MANUAL: + rgbled_set_color(RGBLED_COLOR_WHITE); + break; + case MAIN_STATE_SEATBELT: + case MAIN_STATE_EASY: + rgbled_set_color(RGBLED_COLOR_GREEN); + break; + case MAIN_STATE_AUTO: + rgbled_set_color(RGBLED_COLOR_BLUE); + break; + default: + break; + } } /* give system warnings on error LED, XXX maybe add memory usage warning too */ @@ -1480,54 +1461,52 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c if (status->main_state == MAIN_STATE_AUTO) { if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { + // TODO AUTO_LAND handling if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { /* don't switch to other states until takeoff not completed */ - if (local_pos->z > -5.0f || status->condition_landed) { - res = TRANSITION_NOT_CHANGED; + if (local_pos->z > -takeoff_alt || status->condition_landed) { + return TRANSITION_NOT_CHANGED; } } - - if (res != TRANSITION_NOT_CHANGED) { - /* check again, state can be changed */ - if (status->condition_landed) { - /* if landed: transitions only to AUTO_READY are allowed */ - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode); - // TRANSITION_DENIED is not possible here + if (status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF && + status->navigation_state != NAVIGATION_STATE_AUTO_LOITER && + status->navigation_state != NAVIGATION_STATE_AUTO_MISSION && + status->navigation_state != NAVIGATION_STATE_AUTO_RTL) { + /* possibly on ground, switch to TAKEOFF if needed */ + if (local_pos->z > -takeoff_alt || status->condition_landed) { + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); + return res; + } + } + /* switch to AUTO mode */ + if (status->rc_signal_found_once && !status->rc_signal_lost) { + /* act depending on switches when manual control enabled */ + if (status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode); } else { - /* not landed */ - if (status->rc_signal_found_once && !status->rc_signal_lost) { - /* act depending on switches when manual control enabled */ - if (status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode); - - } else { - if (status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); - - } else { - /* LOITER */ - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode); - } - } + if (status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); } else { - /* switch to MISSION in air when no RC control */ - if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER || - status->navigation_state == NAVIGATION_STATE_AUTO_MISSION || - status->navigation_state == NAVIGATION_STATE_AUTO_RTL || - status->navigation_state == NAVIGATION_STATE_AUTO_LAND) { - res = TRANSITION_NOT_CHANGED; - - } else { - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); - } + /* LOITER */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } } - } + } else { + /* switch to MISSION when no RC control and first time in some AUTO mode */ + if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER || + status->navigation_state == NAVIGATION_STATE_AUTO_MISSION || + status->navigation_state == NAVIGATION_STATE_AUTO_RTL || + status->navigation_state == NAVIGATION_STATE_AUTO_LAND) { + res = TRANSITION_NOT_CHANGED; + } else { + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + } + } } else { /* disarmed, always switch to AUTO_READY */ res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode); @@ -1678,13 +1657,13 @@ void *commander_low_prio_loop(void *arg) if (((int)(cmd.param1)) == 1) { answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - usleep(1000000); + usleep(100000); /* reboot */ systemreset(false); } else if (((int)(cmd.param1)) == 3) { answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - usleep(1000000); + usleep(100000); /* reboot to bootloader */ systemreset(true); @@ -1727,6 +1706,7 @@ void *commander_low_prio_loop(void *arg) } else if ((int)(cmd.param4) == 1) { /* RC calibration */ answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + calib_ret = do_rc_calibration(mavlink_fd); } else if ((int)(cmd.param5) == 1) { /* accelerometer calibration */ @@ -1752,22 +1732,36 @@ void *commander_low_prio_loop(void *arg) case VEHICLE_CMD_PREFLIGHT_STORAGE: { if (((int)(cmd.param1)) == 0) { - if (0 == param_load_default()) { + int ret = param_load_default(); + if (ret == OK) { mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); } else { mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); + /* convenience as many parts of NuttX use negative errno */ + if (ret < 0) + ret = -ret; + + if (ret < 1000) + mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret)); answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } } else if (((int)(cmd.param1)) == 1) { - if (0 == param_save_default()) { + int ret = param_save_default(); + if (ret == OK) { mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); } else { mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); + /* convenience as many parts of NuttX use negative errno */ + if (ret < 0) + ret = -ret; + + if (ret < 1000) + mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret)); answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } } @@ -1789,5 +1783,7 @@ void *commander_low_prio_loop(void *arg) } + close(cmd_sub); + return 0; } diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 5df5d8d0c..7feace2b4 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -99,44 +99,44 @@ void buzzer_deinit() void tune_error() { - ioctl(buzzer, TONE_SET_ALARM, 2); + ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE); } void tune_positive() { - ioctl(buzzer, TONE_SET_ALARM, 3); + ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_POSITIVE_TUNE); } void tune_neutral() { - ioctl(buzzer, TONE_SET_ALARM, 4); + ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE); } void tune_negative() { - ioctl(buzzer, TONE_SET_ALARM, 5); + ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE); } int tune_arm() { - return ioctl(buzzer, TONE_SET_ALARM, 12); + return ioctl(buzzer, TONE_SET_ALARM, TONE_ARMING_WARNING_TUNE); } int tune_low_bat() { - return ioctl(buzzer, TONE_SET_ALARM, 13); + return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_SLOW_TUNE); } int tune_critical_bat() { - return ioctl(buzzer, TONE_SET_ALARM, 14); + return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_FAST_TUNE); } void tune_stop() { - ioctl(buzzer, TONE_SET_ALARM, 0); + ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); } static int leds; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 0a1703b2e..40d0386d5 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -45,7 +45,8 @@ #include <nuttx/config.h> #include <systemlib/param/param.h> -PARAM_DEFINE_INT32(SYS_FAILSAFE_LL, 0); /**< Go into low-level failsafe after 0 ms */ +PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 5.0f); +PARAM_DEFINE_FLOAT(NAV_TAKEOFF_GAP, 3.0f); PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 33566d4e5..82a0349c9 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -60,12 +60,12 @@ int do_gyro_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit."); - const int calibration_count = 5000; + const unsigned calibration_count = 5000; int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); struct sensor_combined_s raw; - int calibration_counter = 0; + unsigned calibration_counter = 0; float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; /* set offsets to zero */ @@ -101,6 +101,8 @@ int do_gyro_calibration(int mavlink_fd) gyro_offset[1] += raw.gyro_rad_s[1]; gyro_offset[2] += raw.gyro_rad_s[2]; calibration_counter++; + if (calibration_counter % (calibration_count / 20) == 0) + mavlink_log_info(mavlink_fd, "gyro cal progress <%u> percent", (calibration_counter * 100) / calibration_count); } else { poll_errcount++; @@ -108,6 +110,7 @@ int do_gyro_calibration(int mavlink_fd) if (poll_errcount > 1000) { mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor"); + close(sub_sensor_combined); return ERROR; } } @@ -147,24 +150,23 @@ int do_gyro_calibration(int mavlink_fd) if (save_ret != 0) { warnx("WARNING: auto-save of params to storage failed"); mavlink_log_critical(mavlink_fd, "gyro store failed"); + close(sub_sensor_combined); return ERROR; } - mavlink_log_info(mavlink_fd, "gyro calibration done"); - tune_neutral(); /* third beep by cal end routine */ } else { mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)"); + close(sub_sensor_combined); return ERROR; } /*** --- SCALING --- ***/ - mavlink_log_info(mavlink_fd, "offset calibration finished. Rotate for scale 30x"); - mavlink_log_info(mavlink_fd, "or do not rotate and wait for 5 seconds to skip."); + mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip."); warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip."); unsigned rotations_count = 30; @@ -187,8 +189,8 @@ int do_gyro_calibration(int mavlink_fd) /* abort this loop if not rotated more than 180 degrees within 5 seconds */ if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f) && (hrt_absolute_time() - start_time > 5 * 1e6)) { - mavlink_log_info(mavlink_fd, "gyro scale calibration skipped"); - mavlink_log_info(mavlink_fd, "gyro calibration done"); + mavlink_log_info(mavlink_fd, "scale skipped, gyro calibration done"); + close(sub_sensor_combined); return OK; } @@ -281,9 +283,11 @@ int do_gyro_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, "gyro calibration done"); /* third beep by cal end routine */ + close(sub_sensor_combined); return OK; } else { mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); + close(sub_sensor_combined); return ERROR; } } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index e38616027..b0d4266be 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -142,7 +142,6 @@ int do_mag_calibration(int mavlink_fd) axis_index++; mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c axis.", axislabels[axis_index]); - mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_maxcount * 50) / calibration_counter); tune_neutral(); axis_deadline += calibration_interval / 3; @@ -152,14 +151,6 @@ int do_mag_calibration(int mavlink_fd) break; } - // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time(); - - // if ((axis_left / 1000) == 0 && axis_left > 0) { - // char buf[50]; - // sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]); - // mavlink_log_info(mavlink_fd, buf); - // } - int poll_ret = poll(fds, 1, 1000); if (poll_ret > 0) { @@ -169,30 +160,10 @@ int do_mag_calibration(int mavlink_fd) y[calibration_counter] = mag.y; z[calibration_counter] = mag.z; - /* get min/max values */ - - // if (mag.x < mag_min[0]) { - // mag_min[0] = mag.x; - // } - // else if (mag.x > mag_max[0]) { - // mag_max[0] = mag.x; - // } - - // if (raw.magnetometer_ga[1] < mag_min[1]) { - // mag_min[1] = raw.magnetometer_ga[1]; - // } - // else if (raw.magnetometer_ga[1] > mag_max[1]) { - // mag_max[1] = raw.magnetometer_ga[1]; - // } - - // if (raw.magnetometer_ga[2] < mag_min[2]) { - // mag_min[2] = raw.magnetometer_ga[2]; - // } - // else if (raw.magnetometer_ga[2] > mag_max[2]) { - // mag_max[2] = raw.magnetometer_ga[2]; - // } - calibration_counter++; + if (calibration_counter % (calibration_maxcount / 20) == 0) + mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_counter * 50) / calibration_maxcount); + } else { poll_errcount++; @@ -200,6 +171,10 @@ int do_mag_calibration(int mavlink_fd) if (poll_errcount > 1000) { mavlink_log_info(mavlink_fd, "ERROR: Failed reading mag sensor"); + close(sub_mag); + free(x); + free(y); + free(z); return ERROR; } @@ -211,7 +186,9 @@ int do_mag_calibration(int mavlink_fd) float sphere_z; float sphere_radius; + mavlink_log_info(mavlink_fd, "mag cal progress <70> percent"); sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); + mavlink_log_info(mavlink_fd, "mag cal progress <80> percent"); free(x); free(y); @@ -269,6 +246,7 @@ int do_mag_calibration(int mavlink_fd) if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + close(sub_mag); return ERROR; } @@ -288,11 +266,13 @@ int do_mag_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, "magnetometer calibration completed"); mavlink_log_info(mavlink_fd, "mag cal progress <100> percent"); + close(sub_mag); return OK; /* third beep by cal end routine */ } else { mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); + close(sub_mag); return ERROR; } } diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index fe87a3323..90ede499a 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -40,6 +40,7 @@ #include "commander_helper.h" #include <poll.h> +#include <unistd.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/manual_control_setpoint.h> #include <mavlink/mavlink_log.h> @@ -56,14 +57,16 @@ int do_rc_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, "trim calibration starting"); - /* XXX fix this */ - // if (current_status.rc_signal) { - // mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); - // return; - // } - int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); struct manual_control_setpoint_s sp; + bool changed; + orb_check(sub_man, &changed); + + if (!changed) { + mavlink_log_critical(mavlink_fd, "no manual control, aborting"); + return ERROR; + } + orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); /* set parameters */ @@ -80,9 +83,11 @@ int do_rc_calibration(int mavlink_fd) if (save_ret != 0) { mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); + close(sub_man); return ERROR; } mavlink_log_info(mavlink_fd, "trim calibration done"); + close(sub_man); return OK; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 674f3feda..3cef10185 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -348,20 +348,15 @@ navigation_state_transition(struct vehicle_status_s *status, navigation_state_t break; case NAVIGATION_STATE_AUTO_TAKEOFF: - - /* only transitions from AUTO_READY */ - if (status->navigation_state == NAVIGATION_STATE_AUTO_READY) { - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_climb_rate_enabled = true; - control_mode->flag_control_manual_enabled = false; - control_mode->flag_control_auto_enabled = true; - } - + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; break; case NAVIGATION_STATE_AUTO_LOITER: diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index d7b0fa9c7..5eb7cba9b 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -516,7 +516,7 @@ void mavlink_update_system(void) int mavlink_thread_main(int argc, char *argv[]) { /* initialize mavlink text message buffering */ - mavlink_logbuffer_init(&lb, 5); + mavlink_logbuffer_init(&lb, 10); int ch; char *device_name = "/dev/ttyS1"; @@ -738,6 +738,9 @@ int mavlink_thread_main(int argc, char *argv[]) /* Reset the UART flags to original state */ tcsetattr(uart, TCSANOW, &uart_config_original); + /* destroy log buffer */ + //mavlink_logbuffer_destroy(&lb); + thread_running = false; exit(0); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index af43542da..4674f7a24 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -747,6 +747,7 @@ receive_start(int uart) fcntl(uart, F_SETFL, flags | O_NONBLOCK); struct sched_param param; + (void)pthread_attr_getschedparam(&receiveloop_attr, ¶m); param.sched_priority = SCHED_PRIORITY_MAX - 40; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 8227f76c5..336523072 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -227,7 +227,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) hrt_abstime t_prev = 0; const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; - const float takeoff_alt_default = 10.0f; + float ref_alt = 0.0f; hrt_abstime ref_alt_t = 0; uint64_t local_ref_timestamp = 0; @@ -244,6 +244,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) parameters_init(¶ms_h); parameters_update(¶ms_h, ¶ms); + for (int i = 0; i < 2; i++) { pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); @@ -252,36 +253,35 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f); thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); - int paramcheck_counter = 0; - while (!thread_should_exit) { - /* check parameters at 1 Hz */ - if (++paramcheck_counter >= 50) { - paramcheck_counter = 0; - bool param_updated; - orb_check(param_sub, ¶m_updated); - if (param_updated) { - parameters_update(¶ms_h, ¶ms); + bool param_updated; + orb_check(param_sub, ¶m_updated); - for (int i = 0; i < 2; i++) { - pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); - /* use integral_limit_out = tilt_max / 2 */ - float i_limit; + if (param_updated) { + /* clear updated flag */ + struct parameter_update_s ps; + orb_copy(ORB_ID(parameter_update), param_sub, &ps); + /* update params */ + parameters_update(¶ms_h, ¶ms); - if (params.xy_vel_i == 0.0) { - i_limit = params.tilt_max / params.xy_vel_i / 2.0; + for (int i = 0; i < 2; i++) { + pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); + /* use integral_limit_out = tilt_max / 2 */ + float i_limit; - } else { - i_limit = 1.0f; // not used really - } + if (params.xy_vel_i == 0.0f) { + i_limit = params.tilt_max / params.xy_vel_i / 2.0f; - pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max); + } else { + i_limit = 1.0f; // not used really } - pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); - thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min); + pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max); } + + pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); + thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min); } bool updated; @@ -351,7 +351,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (reset_sp_z) { reset_sp_z = false; local_pos_sp.z = local_pos.z; - mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", -local_pos_sp.z); + mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z); } /* move altitude setpoint with throttle stick */ @@ -377,7 +377,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp.y = local_pos.y; pid_reset_integral(&xy_vel_pids[0]); pid_reset_integral(&xy_vel_pids[1]); - mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); + mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); } /* move position setpoint with roll/pitch stick */ @@ -424,12 +424,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (reset_auto_pos) { local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; - local_pos_sp.z = -takeoff_alt_default; + local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap; local_pos_sp.yaw = att.yaw; local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; reset_auto_pos = false; - mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", local_pos_sp.x, local_pos_sp.y, -local_pos_sp.z); + mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z); } } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { @@ -444,7 +444,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) double lat_home = local_pos.ref_lat * 1e-7; double lon_home = local_pos.ref_lon * 1e-7; map_projection_init(lat_home, lon_home); - mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", lat_home, lon_home); + mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home); } if (global_pos_sp_reproject) { @@ -468,7 +468,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp.yaw = global_pos_sp.yaw; att_sp.yaw_body = global_pos_sp.yaw; - mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); + mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y); } else { if (!local_pos_sp_valid) { @@ -481,7 +481,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp_valid = true; } - mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); + mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); } } @@ -505,7 +505,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* set altitude setpoint to 5m under ground, * don't set it too deep to avoid unexpected landing in case of false "landed" signal */ local_pos_sp.z = 5.0f; - mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", -local_pos_sp.z); + mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z); } reset_sp_z = true; @@ -515,7 +515,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (reset_sp_z) { reset_sp_z = false; local_pos_sp.z = local_pos.z; - mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", -local_pos_sp.z); + mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z); } } @@ -527,7 +527,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp.yaw = att.yaw; local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; - mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", local_pos_sp.x, local_pos_sp.y); + mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); } } } @@ -588,7 +588,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } thrust_pid_set_integral(&z_vel_pid, -i); - mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", i); + mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); } thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 9c1ef2edb..bf9578ea3 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -59,6 +59,8 @@ PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f); int parameters_init(struct multirotor_position_control_param_handles *h) { + h->takeoff_alt = param_find("NAV_TAKEOFF_ALT"); + h->takeoff_gap = param_find("NAV_TAKEOFF_GAP"); h->thr_min = param_find("MPC_THR_MIN"); h->thr_max = param_find("MPC_THR_MAX"); h->z_p = param_find("MPC_Z_P"); @@ -84,6 +86,8 @@ int parameters_init(struct multirotor_position_control_param_handles *h) int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p) { + param_get(h->takeoff_alt, &(p->takeoff_alt)); + param_get(h->takeoff_gap, &(p->takeoff_gap)); param_get(h->thr_min, &(p->thr_min)); param_get(h->thr_max, &(p->thr_max)); param_get(h->z_p, &(p->z_p)); diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h index 3ec85a364..fc658dadb 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -41,6 +41,8 @@ #include <systemlib/param/param.h> struct multirotor_position_control_params { + float takeoff_alt; + float takeoff_gap; float thr_min; float thr_max; float z_p; @@ -63,6 +65,8 @@ struct multirotor_position_control_params { }; struct multirotor_position_control_param_handles { + param_t takeoff_alt; + param_t takeoff_gap; param_t thr_min; param_t thr_max; param_t z_p; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 4a4572b09..932f61088 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -53,7 +53,7 @@ #include <math.h> #include <uORB/uORB.h> #include <uORB/topics/parameter_update.h> -#include <uORB/topics/actuator_controls_effective.h> +#include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/vehicle_attitude.h> @@ -181,7 +181,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) uint32_t baro_counter = 0; /* declare and safely initialize all structs */ - struct actuator_controls_effective_s actuator; + struct actuator_controls_s actuator; memset(&actuator, 0, sizeof(actuator)); struct actuator_armed_s armed; memset(&armed, 0, sizeof(armed)); @@ -200,7 +200,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* subscribe */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); + int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); @@ -337,7 +337,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* actuator */ if (fds[1].revents & POLLIN) { - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_sub, &actuator); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator); } /* armed */ @@ -429,26 +429,31 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds[6].revents & POLLIN) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout && gps.eph_m < 4.0f) { + if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) { /* initialize reference position if needed */ if (!ref_xy_inited) { - if (ref_xy_init_start == 0) { - ref_xy_init_start = t; - - } else if (t > ref_xy_init_start + ref_xy_init_delay) { - ref_xy_inited = true; - /* reference GPS position */ - double lat = gps.lat * 1e-7; - double lon = gps.lon * 1e-7; - - local_pos.ref_lat = gps.lat; - local_pos.ref_lon = gps.lon; - local_pos.ref_timestamp = t; - - /* initialize projection */ - map_projection_init(lat, lon); - warnx("init GPS: lat = %.10f, lon = %.10f", lat, lon); - mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon); + /* require EPH < 10m */ + if (gps.eph_m < 10.0f) { + if (ref_xy_init_start == 0) { + ref_xy_init_start = t; + + } else if (t > ref_xy_init_start + ref_xy_init_delay) { + ref_xy_inited = true; + /* reference GPS position */ + double lat = gps.lat * 1e-7; + double lon = gps.lon * 1e-7; + + local_pos.ref_lat = gps.lat; + local_pos.ref_lon = gps.lon; + local_pos.ref_timestamp = t; + + /* initialize projection */ + map_projection_init(lat, lon); + warnx("init GPS: lat = %.10f, lon = %.10f", lat, lon); + mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon); + } + } else { + ref_xy_init_start = 0; } } @@ -541,7 +546,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) alt_disp = alt_disp * alt_disp; float land_disp2 = params.land_disp * params.land_disp; /* get actual thrust output */ - float thrust = armed.armed ? actuator.control_effective[3] : 0.0f; + float thrust = armed.armed ? actuator.control[3] : 0.0f; if (landed) { if (alt_disp > land_disp2 && thrust > params.land_thr) { diff --git a/src/modules/position_estimator_mc/position_estimator_mc_main.c b/src/modules/position_estimator_mc/position_estimator_mc_main.c index 984bd1329..363961819 100755 --- a/src/modules/position_estimator_mc/position_estimator_mc_main.c +++ b/src/modules/position_estimator_mc/position_estimator_mc_main.c @@ -497,7 +497,12 @@ int position_estimator_mc_thread_main(int argc, char *argv[]) local_pos_est.vz = x_z_aposteriori_k[1]; local_pos_est.timestamp = hrt_absolute_time(); if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))){ - orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est); + if(local_pos_est_pub > 0) + orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est); + else + local_pos_est_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos_est); + //char buf[0xff]; sprintf(buf,"[pos_est_mc] x:%f, y:%f, z:%f",x_x_aposteriori_k[0],x_y_aposteriori_k[0],x_z_aposteriori_k[0]); + //mavlink_log_info(mavlink_fd, buf); } } } /* end of poll return value check */ diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index deed25836..0edd91b24 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -358,8 +358,8 @@ static unsigned mixer_text_length = 0; void mixer_handle_text(const void *buffer, size_t length) { - /* do not allow a mixer change while outputs armed */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + /* do not allow a mixer change while safety off */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { return; } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 2b21bb5a5..e83fb7dd3 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -890,13 +890,14 @@ int sdlog2_thread_main(int argc, char *argv[]) if (fds[ifds++].revents & POLLIN) { // Don't orb_copy, it's already done few lines above log_msg.msg_type = LOG_STAT_MSG; - log_msg.body.log_STAT.main_state = (unsigned char) buf_status.main_state; - log_msg.body.log_STAT.navigation_state = (unsigned char) buf_status.navigation_state; - log_msg.body.log_STAT.arming_state = (unsigned char) buf_status.arming_state; + log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; + log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state; + log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage; log_msg.body.log_STAT.battery_current = buf_status.battery_current; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; - log_msg.body.log_STAT.battery_warning = (unsigned char) buf_status.battery_warning; + log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; + log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; LOGBUFFER_WRITE_AND_COUNT(STAT); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index d99892fe2..4eeb65a87 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -155,6 +155,7 @@ struct log_STAT_s { float battery_current; float battery_remaining; uint8_t battery_warning; + uint8_t landed; }; /* --- RC - RC INPUT CHANNELS --- */ @@ -263,7 +264,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), - LOG_FORMAT(STAT, "BBBfffB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn"), + LOG_FORMAT(STAT, "BBBfffBB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn,Landed"), LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index fd2a6318e..992abf2cc 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -68,7 +68,8 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); -PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0); +PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); +PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0); @@ -157,6 +158,13 @@ PARAM_DEFINE_FLOAT(RC14_MAX, 2000); PARAM_DEFINE_FLOAT(RC14_REV, 1.0f); PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC15_MIN, 1000); +PARAM_DEFINE_FLOAT(RC15_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC15_MAX, 2000); +PARAM_DEFINE_FLOAT(RC15_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f); + + PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */ @@ -164,6 +172,8 @@ PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); #else /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ +/* PX4IOAR: 0.00838095238 */ +/* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f)); #endif diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index e857b1c2f..e98c4d548 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -301,6 +301,7 @@ private: float accel_offset[3]; float accel_scale[3]; float diff_pres_offset_pa; + float diff_pres_analog_enabled; int board_rotation; int external_mag_rotation; @@ -348,6 +349,7 @@ private: param_t mag_offset[3]; param_t mag_scale[3]; param_t diff_pres_offset_pa; + param_t diff_pres_analog_enabled; param_t rc_map_roll; param_t rc_map_pitch; @@ -617,6 +619,7 @@ Sensors::Sensors() : /* Differential pressure offset */ _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); + _parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA"); _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); @@ -657,7 +660,9 @@ int Sensors::parameters_update() { bool rc_valid = true; - + float tmpScaleFactor = 0.0f; + float tmpRevFactor = 0.0f; + /* rc values */ for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) { @@ -667,18 +672,21 @@ Sensors::parameters_update() param_get(_parameter_handles.rev[i], &(_parameters.rev[i])); param_get(_parameter_handles.dz[i], &(_parameters.dz[i])); - _parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); - + tmpScaleFactor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); + tmpRevFactor = tmpScaleFactor * _parameters.rev[i]; + /* handle blowup in the scaling factor calculation */ - if (!isfinite(_parameters.scaling_factor[i]) || - _parameters.scaling_factor[i] * _parameters.rev[i] < 0.000001f || - _parameters.scaling_factor[i] * _parameters.rev[i] > 0.2f) { - + if (!isfinite(tmpScaleFactor) || + (tmpRevFactor < 0.000001f) || + (tmpRevFactor > 0.2f) ) { + warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, tmpScaleFactor, (int)(_parameters.rev[i])); /* scaling factors do not make sense, lock them down */ - _parameters.scaling_factor[i] = 0; + _parameters.scaling_factor[i] = 0.0f; rc_valid = false; } - + else { + _parameters.scaling_factor[i] = tmpScaleFactor; + } } /* handle wrong values */ @@ -784,6 +792,7 @@ Sensors::parameters_update() /* Airspeed offset */ param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa)); + param_get(_parameter_handles.diff_pres_analog_enabled, &(_parameters.diff_pres_analog_enabled)); /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { @@ -1266,9 +1275,10 @@ Sensors::adc_poll(struct sensor_combined_s &raw) /** * The voltage divider pulls the signal down, only act on - * a valid voltage from a connected sensor + * a valid voltage from a connected sensor. Also assume a non- + * zero offset from the sensor if its connected. */ - if (voltage > 0.4f) { + if (voltage > 0.4f && _parameters.diff_pres_analog_enabled) { float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor diff --git a/src/modules/systemlib/mavlink_log.c b/src/modules/systemlib/mavlink_log.c index 27608bdbf..f321f9ceb 100644 --- a/src/modules/systemlib/mavlink_log.c +++ b/src/modules/systemlib/mavlink_log.c @@ -51,7 +51,15 @@ __EXPORT void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) lb->size = size; lb->start = 0; lb->count = 0; - lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage)); + lb->elems = calloc(lb->size, sizeof(struct mavlink_logmessage)); +} + +__EXPORT void mavlink_logbuffer_destroy(struct mavlink_logbuffer *lb) +{ + lb->size = 0; + lb->start = 0; + lb->count = 0; + free(lb->elems); } __EXPORT int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 69a9bdf9b..c69de52b7 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -505,24 +505,31 @@ param_get_default_file(void) int param_save_default(void) { + int result; + /* delete the file in case it exists */ - unlink(param_get_default_file()); + struct stat buffer; + if (stat(param_get_default_file(), &buffer) == 0) { + result = unlink(param_get_default_file()); + if (result != OK) + warnx("unlinking file %s failed.", param_get_default_file()); + } /* create the file */ int fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL); if (fd < 0) { warn("opening '%s' for writing failed", param_get_default_file()); - return -1; + return fd; } - int result = param_export(fd, false); + result = param_export(fd, false); close(fd); if (result != 0) { warn("error exporting parameters to '%s'", param_get_default_file()); unlink(param_get_default_file()); - return -2; + return result; } return 0; diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 4996a8f66..739503ed4 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -51,6 +51,8 @@ #include "pid.h" #include <math.h> +#define SIGMA 0.000001f + __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode, float dt_min) { @@ -168,8 +170,8 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculate the error integral and check for saturation i = pid->integral + (error * dt); - if (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit || - fabsf(i) > pid->intmax) { + if ((pid->limit > SIGMA && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) || + fabsf(i) > pid->intmax) { i = pid->integral; // If saturated then do not update integral value pid->saturated = 1; @@ -186,11 +188,13 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); if (isfinite(output)) { - if (output > pid->limit) { - output = pid->limit; + if (pid->limit > SIGMA) { + if (output > pid->limit) { + output = pid->limit; - } else if (output < -pid->limit) { - output = -pid->limit; + } else if (output < -pid->limit) { + output = -pid->limit; + } } pid->last_output = output; diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index 9d47d8000..60d6473b8 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -66,7 +66,7 @@ int rc_calibration_check(void) { // count++; // } - + int channel_fail_count = 0; for (int i = 0; i < RC_CHANNELS_MAX; i++) { /* should the channel be enabled? */ @@ -112,13 +112,13 @@ int rc_calibration_check(void) { } if (param_trim < param_min) { count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1); + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN (%d/%d)", i+1, (int)param_trim, (int)param_min); /* give system time to flush error message in case there are more */ usleep(100000); } if (param_trim > param_max) { count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1); + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX (%d/%d)", i+1, (int)param_trim, (int)param_max); /* give system time to flush error message in case there are more */ usleep(100000); } @@ -142,7 +142,12 @@ int rc_calibration_check(void) { /* sanity checks pass, enable channel */ if (count) { mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); + warnx(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); usleep(100000); } + + channel_fail_count += count; } + + return channel_fail_count; } diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index a7cda34a8..686fd765c 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -64,6 +64,12 @@ struct vehicle_attitude_setpoint_s float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ bool R_valid; /**< Set to true if rotation matrix is valid */ + //! For quaternion-based attitude control + float q_d[4]; /** Desired quaternion for quaternion control */ + bool q_d_valid; /**< Set to true if quaternion vector is valid */ + float q_e[4]; /** Attitude error in quaternion */ + bool q_e_valid; /**< Set to true if quaternion error vector is valid */ + float thrust; /**< Thrust in Newton the power system should generate */ }; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 43218eac4..1c184d3a7 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -153,9 +153,9 @@ enum VEHICLE_TYPE { }; enum VEHICLE_BATTERY_WARNING { - VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */ - VEHICLE_BATTERY_WARNING_WARNING, /**< warning of low voltage 1. stage */ - VEHICLE_BATTERY_WARNING_ALERT /**< alerting of low voltage 2. stage */ + VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */ + VEHICLE_BATTERY_WARNING_LOW, /**< warning of low voltage */ + VEHICLE_BATTERY_WARNING_CRITICAL /**< alerting of critical voltage */ }; /** |