fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 22:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:PX4/Firmware into fixedwing_l1

Browse Source
This commit is contained in:
Lorenz Meier
2013-09-05 22:18:00 +02:00
parent 7fa2b9c91a 373a74adb9
commit d74bea57ac
87 changed files with 3144 additions and 1074 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+12 -10
View File
@@ -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) {
// 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++;
// 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;
}
// 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 {
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
+2 -2
View File
@@ -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)) {
src/modules/commander/accelerometer_calibration.cpp
+3 -1
View File
@@ -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]);
src/modules/commander/airspeed_calibration.cpp
+5 -4
View File
@@ -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;
}
}
src/modules/commander/commander.cpp
+105 -109
View File
@@ -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, &param);
/* low priority */
param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
(void)pthread_attr_setschedparam(&commander_low_prio_attr, &param);
@@ -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;
}
src/modules/commander/commander_helper.cpp
+8 -8
View File
@@ -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;
src/modules/commander/commander_params.c
+2 -1
View File
@@ -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);
src/modules/commander/gyro_calibration.cpp
+12 -8
View File
@@ -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;
}
}
src/modules/commander/mag_calibration.cpp
+12 -32
View File
@@ -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;
}
}
src/modules/commander/rc_calibration.cpp
+11 -6
View File
@@ -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;
}
src/modules/commander/state_machine_helper.cpp
+9 -14
View File
@@ -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:
src/modules/mavlink/mavlink.c
+4 -1
View File
@@ -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);
src/modules/mavlink/mavlink_receiver.cpp
+1
View File
@@ -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, &param);
param.sched_priority = SCHED_PRIORITY_MAX - 40;
(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
src/modules/multirotor_pos_control/multirotor_pos_control.c
+33 -33
View File
@@ -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(&params_h);
parameters_update(&params_h, &params);
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, &param_updated);
if (param_updated) {
parameters_update(&params_h, &params);
bool param_updated;
orb_check(param_sub, &param_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(&params_h, &params);
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]);
src/modules/multirotor_pos_control/multirotor_pos_control_params.c
+4
View File
@@ -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));
src/modules/multirotor_pos_control/multirotor_pos_control_params.h
+4
View File
@@ -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;
src/modules/position_estimator_inav/position_estimator_inav_main.c
+25 -20
View File
@@ -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;
/* 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;
} 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;
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);
/* 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) {
src/modules/position_estimator_mc/position_estimator_mc_main.c
+6 -1
View File
@@ -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 */
src/modules/px4iofirmware/mixer.cpp
+2 -2
View File
@@ -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;
}
src/modules/sdlog2/sdlog2.c
+5 -4
View File
@@ -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);
}
src/modules/sdlog2/sdlog2_messages.h
+2 -1
View File
@@ -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"),
src/modules/sensors/sensor_params.c
+11 -1
View File
@@ -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
src/modules/sensors/sensors.cpp
+21 -11
View File
@@ -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
src/modules/systemlib/mavlink_log.c
+9 -1
View File
@@ -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)
src/modules/systemlib/param/param.c
+11 -4
View File
@@ -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;
src/modules/systemlib/pid/pid.c
+10 -6
View File
@@ -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;
src/modules/systemlib/rc_check.c
+8 -3
View File
@@ -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;
}
src/modules/uORB/topics/vehicle_attitude_setpoint.h
+6
View File
@@ -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 */
};
src/modules/uORB/topics/vehicle_status.h
+3 -3
View File
@@ -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 */
};
/**