aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--nuttx-configs/px4fmu-v1/nsh/defconfig4
-rw-r--r--src/modules/commander/accelerometer_calibration.cpp4
-rw-r--r--src/modules/commander/commander.cpp54
-rw-r--r--src/modules/multirotor_pos_control/multirotor_pos_control.c25
-rw-r--r--src/modules/position_estimator_inav/position_estimator_inav_main.c24
5 files changed, 65 insertions, 46 deletions
diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig
index c52d8caf9..a6f914a64 100644
--- a/nuttx-configs/px4fmu-v1/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v1/nsh/defconfig
@@ -564,8 +564,8 @@ CONFIG_CDCACM_EPBULKIN_HSSIZE=512
CONFIG_CDCACM_NWRREQS=4
CONFIG_CDCACM_NRDREQS=4
CONFIG_CDCACM_BULKIN_REQLEN=96
-CONFIG_CDCACM_RXBUFSIZE=256
-CONFIG_CDCACM_TXBUFSIZE=256
+CONFIG_CDCACM_RXBUFSIZE=512
+CONFIG_CDCACM_TXBUFSIZE=512
CONFIG_CDCACM_VENDORID=0x26ac
CONFIG_CDCACM_PRODUCTID=0x0010
CONFIG_CDCACM_VENDORSTR="3D Robotics"
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 82df7c37d..7a7fa6f4e 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -298,7 +298,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
/* set accel error threshold to 5m/s^2 */
float accel_err_thr = 5.0f;
/* still time required in us */
- int64_t still_time = 2000000;
+ hrt_abstime still_time = 2000000;
struct pollfd fds[1];
fds[0].fd = sub_sensor_combined;
fds[0].events = POLLIN;
@@ -342,7 +342,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
t_timeout = t + timeout;
} else {
/* still since t_still */
- if ((int64_t) t - (int64_t) t_still > still_time) {
+ if (t > t_still + still_time) {
/* vehicle is still, exit from the loop to detection of its orientation */
break;
}
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 39d74e37a..d90008633 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1592,43 +1592,41 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v
case MAIN_STATE_AUTO:
if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) {
if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
- /* check if takeoff completed */
- if (local_pos->z < -5.0f && !status.condition_landed) {
- res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
- } else {
+ /* don't switch to other states until takeoff not completed */
+ if (local_pos->z > -5.0f || status.condition_landed) {
res = TRANSITION_NOT_CHANGED;
+ break;
}
+ }
+ /* check again, state can be changed */
+ if (current_status->condition_landed) {
+ /* if landed: transitions only to AUTO_READY are allowed */
+ res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode);
+ // TRANSITION_DENIED is not possible here
+ break;
} else {
- if (current_status->condition_landed) {
- /* if landed: transitions only to AUTO_READY are allowed */
- res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode);
- // TRANSITION_DENIED is not possible here
- break;
+ /* if not landed: */
+ if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) {
+ /* act depending on switches when manual control enabled */
+ if (current_status->return_switch == RETURN_SWITCH_RETURN) {
+ /* RTL */
+ res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode);
- } else {
- /* if not landed: */
- if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) {
- /* act depending on switches when manual control enabled */
- if (current_status->return_switch == RETURN_SWITCH_RETURN) {
- /* RTL */
- res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode);
+ } else {
+ if (current_status->mission_switch == MISSION_SWITCH_MISSION) {
+ /* MISSION */
+ res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
} else {
- if (current_status->mission_switch == MISSION_SWITCH_MISSION) {
- /* MISSION */
- res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
-
- } else {
- /* LOITER */
- res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
- }
+ /* LOITER */
+ res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
}
-
- } else {
- /* always switch to loiter in air when no RC control */
- res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
}
+
+ } else {
+ /* switch to mission in air when no RC control */
+ res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
}
}
} else {
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index e65792c76..385892f04 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -405,30 +405,38 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
}
}
- /* publish local position setpoint */
- orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
+ local_pos_sp.yaw = att_sp.yaw_body;
/* local position setpoint is valid and can be used for loiter after position controlled mode */
local_pos_sp_valid = control_mode.flag_control_position_enabled;
+ reset_auto_pos = true;
+
/* force reprojection of global setpoint after manual mode */
global_pos_sp_reproject = true;
} else {
/* non-manual mode, use global setpoint */
- if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
+ if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) {
+ reset_auto_pos = true;
+ } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
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.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);
}
} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_LOITER) {
if (reset_auto_pos) {
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.z = local_pos.z;
+ local_pos_sp.yaw = att.yaw;
+ local_pos_sp_valid = true;
att_sp.yaw_body = att.yaw;
reset_auto_pos = false;
}
@@ -462,6 +470,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
} else {
local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
}
+ 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);
@@ -472,15 +482,13 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.z = local_pos.z;
+ local_pos_sp.yaw = att.yaw;
+ 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);
}
-
- /* publish local position setpoint as projection of global position setpoint */
- orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
}
- att_sp.yaw_body = global_pos_sp.yaw;
reset_auto_pos = true;
}
@@ -493,6 +501,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
reset_sp_z = true;
}
+ /* publish local position setpoint */
+ orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
+
/* run position & altitude controllers, calculate velocity setpoint */
if (control_mode.flag_control_altitude_enabled) {
global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2];
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 af6a704a2..ef13ade88 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -75,8 +75,11 @@ static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int position_estimator_inav_task; /**< Handle of deamon task / thread */
static bool verbose_mode = false;
-static hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s
-static hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s
+
+static const hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s
+static const hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s
+static const uint32_t updates_counter_len = 1000000;
+static const uint32_t pub_interval = 4000; // limit publish rate to 250 Hz
__EXPORT int position_estimator_inav_main(int argc, char *argv[]);
@@ -172,6 +175,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float alt_avg = 0.0f;
bool landed = true;
hrt_abstime landed_time = 0;
+ bool flag_armed = false;
uint32_t accel_counter = 0;
uint32_t baro_counter = 0;
@@ -275,10 +279,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
uint16_t flow_updates = 0;
hrt_abstime updates_counter_start = hrt_absolute_time();
- uint32_t updates_counter_len = 1000000;
-
hrt_abstime pub_last = hrt_absolute_time();
- uint32_t pub_interval = 4000; // limit publish rate to 250 Hz
/* acceleration in NED frame */
float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G };
@@ -407,7 +408,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} else {
// new ground level
baro_alt0 += sonar_corr;
- warnx("new home: alt = %.3f", baro_alt0);
mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0);
local_pos.ref_alt = baro_alt0;
local_pos.ref_timestamp = hrt_absolute_time();
@@ -429,7 +429,7 @@ 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) {
+ if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout && gps.eph_m < 5.0f) {
/* initialize reference position if needed */
if (!ref_xy_inited) {
if (ref_xy_init_start == 0) {
@@ -486,6 +486,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
t_prev = t;
+ /* reset ground level on arm */
+ if (armed.armed && !flag_armed) {
+ baro_alt0 -= z_est[0];
+ z_est[0] = 0.0f;
+ local_pos.ref_alt = baro_alt0;
+ local_pos.ref_timestamp = hrt_absolute_time();
+ mavlink_log_info(mavlink_fd, "[inav] new home on arm: alt = %.3f", baro_alt0);
+ }
+
/* accel bias correction, now only for Z
* not strictly correct, but stable and works */
accel_bias[2] += (accel_NED[2] + CONSTANTS_ONE_G) * params.w_acc_bias * dt;
@@ -629,6 +638,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
}
+ flag_armed = armed.armed;
}
warnx("exiting.");
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-05 09:46:01 +0000