aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/modules/att_pos_estimator_ekf/KalmanNav.cpp2
-rw-r--r--src/modules/commander/commander.cpp14
-rw-r--r--src/modules/commander/state_machine_helper.cpp1
-rw-r--r--src/modules/mavlink/orb_listener.c2
-rw-r--r--src/modules/mavlink/waypoints.c40
-rw-r--r--src/modules/multirotor_pos_control/multirotor_pos_control.c101
-rw-r--r--src/modules/position_estimator_inav/position_estimator_inav_main.c3
-rw-r--r--src/modules/uORB/topics/vehicle_control_mode.h2
-rw-r--r--src/modules/uORB/topics/vehicle_global_position.h3
-rw-r--r--src/modules/uORB/topics/vehicle_local_position.h2
10 files changed, 101 insertions, 69 deletions
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
index 191d20f30..33879892e 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
@@ -325,7 +325,7 @@ void KalmanNav::updatePublications()
_pos.vx = vN;
_pos.vy = vE;
_pos.vz = vD;
- _pos.hdg = psi;
+ _pos.yaw = psi;
// attitude publication
_att.timestamp = _pubTimeStamp;
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 74cd5e36a..39d74e37a 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -210,7 +210,7 @@ void print_reject_arm(const char *msg);
void print_status();
-transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode);
+transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
/**
* Loop that runs at a lower rate and priority for calibration and parameter tasks.
@@ -1214,7 +1214,7 @@ int commander_thread_main(int argc, char *argv[])
}
/* evaluate the navigation state machine */
- transition_result_t res = check_navigation_state_machine(&status, &control_mode);
+ transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position);
if (res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
@@ -1572,7 +1572,7 @@ print_reject_arm(const char *msg)
}
transition_result_t
-check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode)
+check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos)
{
transition_result_t res = TRANSITION_DENIED;
@@ -1592,8 +1592,12 @@ 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) {
- /* don't act while taking off */
- res = TRANSITION_NOT_CHANGED;
+ /* 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 {
+ res = TRANSITION_NOT_CHANGED;
+ }
} else {
if (current_status->condition_landed) {
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 80ee3db23..fe7e8cc53 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -413,6 +413,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
if (ret == TRANSITION_CHANGED) {
current_status->navigation_state = new_navigation_state;
+ control_mode->auto_state = current_status->navigation_state;
navigation_state_changed = true;
}
}
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index dcdc03281..53d86ec00 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -341,7 +341,7 @@ l_global_position(const struct listener *l)
int16_t vz = (int16_t)(global_pos.vz * 100.0f);
/* heading in degrees * 10, from 0 to 36.000) */
- uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f);
+ uint16_t hdg = (global_pos.yaw / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f);
mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
timestamp / 1000,
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index eea928a17..16a7c2d35 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -294,16 +294,13 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq)
*/
float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt)
{
- //TODO: implement for z once altidude contoller is implemented
-
static uint16_t counter;
-// if(counter % 10 == 0) printf(" x = %.10f, y = %.10f\n", x, y);
if (seq < wpm->size) {
- mavlink_mission_item_t *cur = &(wpm->waypoints[seq]);
+ mavlink_mission_item_t *wp = &(wpm->waypoints[seq]);
- double current_x_rad = cur->x / 180.0 * M_PI;
- double current_y_rad = cur->y / 180.0 * M_PI;
+ double current_x_rad = wp->x / 180.0 * M_PI;
+ double current_y_rad = wp->y / 180.0 * M_PI;
double x_rad = lat / 180.0 * M_PI;
double y_rad = lon / 180.0 * M_PI;
@@ -315,7 +312,10 @@ float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float
const double radius_earth = 6371000.0;
- return radius_earth * c;
+ float dxy = radius_earth * c;
+ float dz = alt - wp->z;
+
+ return sqrtf(dxy * dxy + dz * dz);
} else {
return -1.0f;
@@ -383,21 +383,19 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
// XXX TODO
}
- if (dist >= 0.f && dist <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw
-
+ if (dist >= 0.f && dist <= orbit) {
wpm->pos_reached = true;
-
}
-
-// else
-// {
-// if(counter % 100 == 0)
-// printf("Setpoint not reached yet: %0.4f, orbit: %.4f, coordinate frame: %d\n",dist, orbit, coordinate_frame);
-// }
+ // check if required yaw reached
+ float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI);
+ float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw);
+ if (fabsf(yaw_err) < 0.05f) {
+ wpm->yaw_reached = true;
+ }
}
//check if the current waypoint was reached
- if (wpm->pos_reached /*wpm->yaw_reached &&*/ && !wpm->idle) {
+ if (wpm->pos_reached && /*wpm->yaw_reached &&*/ !wpm->idle) {
if (wpm->current_active_wp_id < wpm->size) {
mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]);
@@ -412,11 +410,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
bool time_elapsed = false;
- if (cur_wp->command == (int)MAV_CMD_NAV_LOITER_TIME) {
- if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
- time_elapsed = true;
- }
- } else if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
+ if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
time_elapsed = true;
} else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) {
time_elapsed = true;
@@ -493,7 +487,7 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio
// mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
// }
- check_waypoints_reached(now, global_position , local_position);
+ check_waypoints_reached(now, global_position, local_position);
return OK;
}
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index a6ebeeacb..e65792c76 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -54,6 +54,7 @@
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/manual_control_setpoint.h>
@@ -221,11 +222,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
bool reset_int_xy = true;
bool was_armed = false;
bool reset_integral = true;
+ bool reset_auto_pos = true;
hrt_abstime t_prev = 0;
- /* integrate in NED frame to estimate wind but not attitude offset */
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;
@@ -414,50 +416,76 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
} else {
/* non-manual mode, use global setpoint */
- /* init local projection using local position ref */
- if (local_pos.ref_timestamp != local_ref_timestamp) {
- global_pos_sp_reproject = true;
- local_ref_timestamp = local_pos.ref_timestamp;
- 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);
- }
-
- if (global_pos_sp_reproject) {
- /* update global setpoint projection */
- global_pos_sp_reproject = false;
+ 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;
+ att_sp.yaw_body = att.yaw;
+ reset_auto_pos = false;
+ }
+ } 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;
+ att_sp.yaw_body = att.yaw;
+ reset_auto_pos = false;
+ }
+ } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) {
+ // TODO
+ reset_auto_pos = true;
+ } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) {
+ /* init local projection using local position ref */
+ if (local_pos.ref_timestamp != local_ref_timestamp) {
+ global_pos_sp_reproject = true;
+ local_ref_timestamp = local_pos.ref_timestamp;
+ 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);
+ }
- if (global_pos_sp_valid) {
- /* global position setpoint valid, use it */
- double sp_lat = global_pos_sp.lat * 1e-7;
- double sp_lon = global_pos_sp.lon * 1e-7;
- /* project global setpoint to local setpoint */
- map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y));
+ if (global_pos_sp_reproject) {
+ /* update global setpoint projection */
+ global_pos_sp_reproject = false;
+ if (global_pos_sp_valid) {
+ /* global position setpoint valid, use it */
+ double sp_lat = global_pos_sp.lat * 1e-7;
+ double sp_lon = global_pos_sp.lon * 1e-7;
+ /* project global setpoint to local setpoint */
+ map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y));
+
+ if (global_pos_sp.altitude_is_relative) {
+ local_pos_sp.z = -global_pos_sp.altitude;
+
+ } else {
+ local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
+ }
- if (global_pos_sp.altitude_is_relative) {
- local_pos_sp.z = -global_pos_sp.altitude;
+ mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y);
} else {
- local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
- }
-
- mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y);
+ if (!local_pos_sp_valid) {
+ /* local position setpoint is invalid,
+ * use current position as setpoint for loiter */
+ local_pos_sp.x = local_pos.x;
+ local_pos_sp.y = local_pos.y;
+ local_pos_sp.z = local_pos.z;
+ }
- } else {
- if (!local_pos_sp_valid) {
- /* local position setpoint is invalid,
- * use current position as setpoint for loiter */
- local_pos_sp.x = local_pos.x;
- local_pos_sp.y = local_pos.y;
- local_pos_sp.z = local_pos.z;
+ 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", 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;
+ }
- /* 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);
+ if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) {
+ global_pos_sp_reproject = true;
}
/* reset setpoints after non-manual modes */
@@ -575,6 +603,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
reset_int_z = true;
reset_int_xy = true;
global_pos_sp_reproject = true;
+ reset_auto_pos = true;
}
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
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 1e20f9de9..af6a704a2 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -591,6 +591,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
local_pos.z = z_est[0];
local_pos.vz = z_est[1];
local_pos.landed = landed;
+ local_pos.yaw = att.yaw;
orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
@@ -609,7 +610,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (local_pos.v_xy_valid) {
global_pos.vx = local_pos.vx;
global_pos.vy = local_pos.vy;
- global_pos.hdg = atan2f(local_pos.vy, local_pos.vx); // TODO is it correct?
}
if (local_pos.z_valid) {
@@ -623,6 +623,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (local_pos.v_z_valid) {
global_pos.vz = local_pos.vz;
}
+ global_pos.yaw = local_pos.yaw;
global_pos.timestamp = t;
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
index 4f4db5dbc..e15ddde26 100644
--- a/src/modules/uORB/topics/vehicle_control_mode.h
+++ b/src/modules/uORB/topics/vehicle_control_mode.h
@@ -82,6 +82,8 @@ struct vehicle_control_mode_s
bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */
bool failsave_highlevel; /**< Set to true if high-level failsafe mode is enabled */
+
+ uint8_t auto_state; // TEMP navigation state for AUTO modes
};
/**
diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h
index 822c323cf..0fc0ed5c9 100644
--- a/src/modules/uORB/topics/vehicle_global_position.h
+++ b/src/modules/uORB/topics/vehicle_global_position.h
@@ -72,8 +72,7 @@ struct vehicle_global_position_s
float vx; /**< Ground X velocity, m/s in NED */
float vy; /**< Ground Y velocity, m/s in NED */
float vz; /**< Ground Z velocity, m/s in NED */
- float hdg; /**< Compass heading in radians -PI..+PI. */
-
+ float yaw; /**< Compass heading in radians -PI..+PI. */
};
/**
diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h
index 26e8f335b..31a0e632b 100644
--- a/src/modules/uORB/topics/vehicle_local_position.h
+++ b/src/modules/uORB/topics/vehicle_local_position.h
@@ -67,6 +67,8 @@ struct vehicle_local_position_s
float vx; /**< Ground X Speed (Latitude), m/s in NED */
float vy; /**< Ground Y Speed (Longitude), m/s in NED */
float vz; /**< Ground Z Speed (Altitude), m/s in NED */
+ /* Heading */
+ float yaw;
/* Reference position in GPS / WGS84 frame */
bool global_xy; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */
bool global_z; /**< true if z is valid and has valid global reference (ref_alt) */
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-02 22:05:41 +0000