fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'beta1' into beta

Browse Source
This commit is contained in:
Anton Babushkin 2014-01-29 16:26:10 +01:00
commit da77ae8ffd
21 changed files with 922 additions and 844 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/drivers/frsky_telemetry/frsky_data.c
View File

@ -225,7 +225,7 @@ void frsky_send_frame2(int uart)
float course = 0, lat = 0, lon = 0, speed = 0, alt = 0;
char lat_ns = 0, lon_ew = 0;
int sec = 0;
if (global_pos.valid) {
if (global_pos.global_valid) {
time_t time_gps = global_pos.time_gps_usec / 1000000;
struct tm *tm_gps = gmtime(&time_gps);

27
src/lib/geo/geo.c
View File

@ -72,7 +72,6 @@ __EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are
/* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale
/* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */
const double r_earth = 6371000;
double lat1 = phi_1;
double lon1 = lambda_0;
@ -81,7 +80,7 @@ __EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are
double lon2 = lambda_0 + 0.5 / 180 * M_PI;
double sin_lat_2 = sin(lat2);
double cos_lat_2 = cos(lat2);
double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth;
double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * CONSTANTS_RADIUS_OF_EARTH;
/* 2) calculate distance rho on plane */
double k_bar = 0;
@ -188,8 +187,7 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0d) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad);
double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a));
const double radius_earth = 6371000.0d;
return radius_earth * c;
return CONSTANTS_RADIUS_OF_EARTH * c;
}
__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
@ -210,7 +208,7 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub
return theta;
}
__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy)
__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e)
{
double lat_now_rad = lat_now * M_DEG_TO_RAD;
double lon_now_rad = lon_now * M_DEG_TO_RAD;
@ -221,11 +219,11 @@ __EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double
double d_lon = lon_next_rad - lon_now_rad;
/* conscious mix of double and float trig function to maximize speed and efficiency */
*vy = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad);
*vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon);
*v_n = CONSTANTS_RADIUS_OF_EARTH * (cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
*v_e = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad);
}
__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy)
__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e)
{
double lat_now_rad = lat_now * M_DEG_TO_RAD;
double lon_now_rad = lon_now * M_DEG_TO_RAD;
@ -236,8 +234,17 @@ __EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, d
double d_lon = lon_next_rad - lon_now_rad;
/* conscious mix of double and float trig function to maximize speed and efficiency */
*vy = CONSTANTS_RADIUS_OF_EARTH * d_lon;
*vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad);
*v_n = CONSTANTS_RADIUS_OF_EARTH * d_lat;
*v_e = CONSTANTS_RADIUS_OF_EARTH * d_lon * cos(lat_now_rad);
}
__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res)
{
double lat_now_rad = lat_now * M_DEG_TO_RAD;
double lon_now_rad = lon_now * M_DEG_TO_RAD;
*lat_res = (lat_now_rad + v_n / CONSTANTS_RADIUS_OF_EARTH) * M_RAD_TO_DEG;
*lon_res = (lon_now_rad + v_e / (CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad))) * M_RAD_TO_DEG;
}
// Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>

6
src/lib/geo/geo.h
View File

@ -115,9 +115,11 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
*/
__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy);
__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e);
__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy);
__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e);
__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res);
__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);

2
src/modules/att_pos_estimator_ekf/KalmanNav.cpp
View File

@ -314,7 +314,7 @@ void KalmanNav::updatePublications()
// global position publication
_pos.timestamp = _pubTimeStamp;
_pos.time_gps_usec = _gps.timestamp_position;
_pos.valid = true;
_pos.global_valid = true;
_pos.lat = lat * M_RAD_TO_DEG;
_pos.lon = lon * M_RAD_TO_DEG;
_pos.alt = float(alt);

2
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
View File

@ -410,7 +410,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
vel(2) = gps.vel_d_m_s;
}
} else if (ekf_params.acc_comp == 2 && global_pos.valid && hrt_absolute_time() < global_pos.timestamp + 500000) {
} else if (ekf_params.acc_comp == 2 && global_pos.global_valid && hrt_absolute_time() < global_pos.timestamp + 500000) {
vel_valid = true;
if (global_pos_updated) {
vel_t = global_pos.timestamp;

140
src/modules/commander/commander.cpp
View File

@ -203,6 +203,8 @@ void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicl
transition_result_t set_main_state_rc(struct vehicle_status_s *status);
void set_control_mode();
void print_reject_mode(const char *msg);
void print_reject_arm(const char *msg);
@ -555,10 +557,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
static struct vehicle_status_s status;
/* armed topic */
static struct vehicle_control_mode_s control_mode;
static struct actuator_armed_s armed;
static struct safety_s safety;
int commander_thread_main(int argc, char *argv[])
@ -613,16 +613,9 @@ int commander_thread_main(int argc, char *argv[])
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
/* Main state machine */
/* make sure we are in preflight state */
/* vehicle status topic */
memset(&status, 0, sizeof(status));
status.condition_landed = true; // initialize to safe value
/* armed topic */
orb_advert_t armed_pub;
/* Initialize armed with all false */
memset(&armed, 0, sizeof(armed));
status.main_state = MAIN_STATE_MANUAL;
status.set_nav_state = NAV_STATE_NONE;
status.set_nav_state_timestamp = 0;
@ -645,14 +638,20 @@ int commander_thread_main(int argc, char *argv[])
// XXX for now just set sensors as initialized
status.condition_system_sensors_initialized = true;
/* advertise to ORB */
status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
/* publish current state machine */
/* publish initial state */
status.counter++;
status.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_status), status_pub, &status);
/* publish initial state */
status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
/* armed topic */
orb_advert_t armed_pub;
/* Initialize armed with all false */
memset(&armed, 0, sizeof(armed));
/* vehicle control mode topic */
memset(&control_mode, 0, sizeof(control_mode));
orb_advert_t control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
@ -872,7 +871,7 @@ int commander_thread_main(int argc, char *argv[])
}
/* update condition_global_position_valid */
check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.valid, &(status.condition_global_position_valid), &status_changed);
check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.global_valid, &(status.condition_global_position_valid), &status_changed);
/* update local position estimate */
orb_check(local_position_sub, &updated);
@ -1031,7 +1030,7 @@ int commander_thread_main(int argc, char *argv[])
if (!status.condition_home_position_valid && gps_position.fix_type >= 3 &&
(gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) &&
(hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed
&& global_position.valid) {
&& global_position.global_valid) {
/* copy position data to uORB home message, store it locally as well */
home.lat = global_position.lat;
@ -1244,8 +1243,13 @@ int commander_thread_main(int argc, char *argv[])
/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
set_control_mode();
control_mode.timestamp = t1;
orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
status.timestamp = t1;
orb_publish(ORB_ID(vehicle_status), status_pub, &status);
armed.timestamp = t1;
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
}
@ -1472,7 +1476,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta
transition_result_t
set_main_state_rc(struct vehicle_status_s *status)
{
/* evaluate the main state machine */
/* set main state according to RC switches */
transition_result_t res = TRANSITION_DENIED;
switch (status->mode_switch) {
@ -1530,6 +1534,102 @@ set_main_state_rc(struct vehicle_status_s *status)
return res;
}
void
set_control_mode()
{
/* set vehicle_control_mode according to main state and failsafe state */
control_mode.flag_armed = armed.armed;
control_mode.flag_external_manual_override_ok = !status.is_rotary_wing;
control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON;
control_mode.flag_control_termination_enabled = false;
/* set this flag when navigator should act */
bool navigator_enabled = false;
switch (status.failsafe_state) {
case FAILSAFE_STATE_NORMAL:
switch (status.main_state) {
case MAIN_STATE_MANUAL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = status.is_rotary_wing;
control_mode.flag_control_attitude_enabled = status.is_rotary_wing;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
break;
case MAIN_STATE_SEATBELT:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
break;
case MAIN_STATE_EASY:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = true;
control_mode.flag_control_velocity_enabled = true;
break;
case MAIN_STATE_AUTO:
navigator_enabled = true;
default:
break;
}
break;
case FAILSAFE_STATE_RTL:
navigator_enabled = true;
break;
case FAILSAFE_STATE_LAND:
navigator_enabled = true;
break;
case FAILSAFE_STATE_TERMINATION:
/* disable all controllers on termination */
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = false;
control_mode.flag_control_attitude_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_termination_enabled = true;
break;
default:
break;
}
/* navigator has control, set control mode flags according to nav state*/
if (navigator_enabled) {
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_position_enabled = true;
control_mode.flag_control_velocity_enabled = true;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
}
}
void
print_reject_mode(const char *msg)
{

4
src/modules/commander/state_machine_helper.cpp
View File

@ -384,6 +384,8 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f
case FAILSAFE_STATE_RTL:
/* global position and home position required for RTL */
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->set_nav_state = NAV_STATE_RTL;
status->set_nav_state_timestamp = hrt_absolute_time();
ret = TRANSITION_CHANGED;
}
@ -392,6 +394,8 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f
case FAILSAFE_STATE_LAND:
/* at least relative altitude estimate required for landing */
if (status->condition_local_altitude_valid || status->condition_global_position_valid) {
status->set_nav_state = NAV_STATE_LAND;
status->set_nav_state_timestamp = hrt_absolute_time();
ret = TRANSITION_CHANGED;
}

43
src/modules/mavlink/mavlink.c
View File

@ -199,8 +199,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
}
/* arming state */
if (v_status.arming_state == ARMING_STATE_ARMED
|| v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
if (armed.armed) {
*mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
}
@ -208,28 +207,36 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
*mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
union px4_custom_mode custom_mode;
custom_mode.data = 0;
if (v_status.main_state == MAIN_STATE_MANUAL) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
} else if (v_status.main_state == MAIN_STATE_SEATBELT) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
} else if (v_status.main_state == MAIN_STATE_EASY) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
} else if (v_status.main_state == MAIN_STATE_AUTO) {
if (pos_sp_triplet.nav_state == NAV_STATE_NONE) {
/* use main state when navigator is not active */
if (v_status.main_state == MAIN_STATE_MANUAL) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
} else if (v_status.main_state == MAIN_STATE_SEATBELT) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
} else if (v_status.main_state == MAIN_STATE_EASY) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
} else if (v_status.main_state == MAIN_STATE_AUTO) {
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
}
} else {
/* use navigation state when navigator is active */
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
if (control_mode.nav_state == NAV_STATE_NONE) { // failsafe, shouldn't happen
if (pos_sp_triplet.nav_state == NAV_STATE_READY) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
} else if (control_mode.nav_state == NAV_STATE_READY) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
} else if (control_mode.nav_state == NAV_STATE_LOITER) {
} else if (pos_sp_triplet.nav_state == NAV_STATE_LOITER) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
} else if (control_mode.nav_state == NAV_STATE_MISSION) {
} else if (pos_sp_triplet.nav_state == NAV_STATE_MISSION) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
} else if (control_mode.nav_state == NAV_STATE_RTL) {
} else if (pos_sp_triplet.nav_state == NAV_STATE_RTL) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
} else if (pos_sp_triplet.nav_state == NAV_STATE_LAND) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
}
}
*mavlink_custom_mode = custom_mode.data;

2
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -634,7 +634,7 @@ handle_message(mavlink_message_t *msg)
orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
// global position packet
hil_global_pos.timestamp = timestamp;
hil_global_pos.valid = true;
hil_global_pos.global_valid = true;
hil_global_pos.lat = hil_state.lat;
hil_global_pos.lon = hil_state.lon;
hil_global_pos.alt = hil_state.alt / 1000.0f;

31
src/modules/mavlink/orb_listener.c
View File

@ -70,7 +70,7 @@ struct vehicle_local_position_s local_pos;
struct home_position_s home;
struct navigation_capabilities_s nav_cap;
struct vehicle_status_s v_status;
struct vehicle_control_mode_s control_mode;
struct position_setpoint_triplet_s pos_sp_triplet;
struct rc_channels_s rc;
struct rc_input_values rc_raw;
struct actuator_armed_s armed;
@ -127,7 +127,6 @@ static void l_vehicle_rates_setpoint(const struct listener *l);
static void l_home(const struct listener *l);
static void l_airspeed(const struct listener *l);
static void l_nav_cap(const struct listener *l);
static void l_control_mode(const struct listener *l);
static const struct listener listeners[] = {
{l_sensor_combined, &mavlink_subs.sensor_sub, 0},
@ -154,7 +153,6 @@ static const struct listener listeners[] = {
{l_home, &mavlink_subs.home_sub, 0},
{l_airspeed, &mavlink_subs.airspeed_sub, 0},
{l_nav_cap, &mavlink_subs.navigation_capabilities_sub, 0},
{l_control_mode, &mavlink_subs.control_mode_sub, 0},
};
static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
@ -315,6 +313,7 @@ l_vehicle_status(const struct listener *l)
/* immediately communicate state changes back to user */
orb_copy(ORB_ID(vehicle_status), status_sub, &v_status);
orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
orb_copy(ORB_ID(position_setpoint_triplet), mavlink_subs.position_setpoint_triplet_sub, &pos_sp_triplet);
/* enable or disable HIL */
if (v_status.hil_state == HIL_STATE_ON)
@ -682,26 +681,6 @@ l_nav_cap(const struct listener *l)
}
void
l_control_mode(const struct listener *l)
{
orb_copy(ORB_ID(vehicle_control_mode), mavlink_subs.control_mode_sub, &control_mode);
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_base_mode = 0;
uint32_t mavlink_custom_mode = 0;
get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
/* send heartbeat */
mavlink_msg_heartbeat_send(chan,
mavlink_system.type,
MAV_AUTOPILOT_PX4,
mavlink_base_mode,
mavlink_custom_mode,
mavlink_state);
}
static void *
uorb_receive_thread(void *arg)
{
@ -777,9 +756,9 @@ uorb_receive_start(void)
status_sub = orb_subscribe(ORB_ID(vehicle_status));
orb_set_interval(status_sub, 300); /* max 3.33 Hz updates */
/* --- CONTROL MODE --- */
mavlink_subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
orb_set_interval(mavlink_subs.control_mode_sub, 300); /* max 3.33 Hz updates */
/* --- POSITION SETPOINT TRIPLET --- */
mavlink_subs.position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
orb_set_interval(mavlink_subs.position_setpoint_triplet_sub, 0); /* not polled, don't limit */
/* --- RC CHANNELS VALUE --- */
rc_sub = orb_subscribe(ORB_ID(rc_channels));

6
src/modules/mavlink/orb_topics.h
View File

@ -94,7 +94,7 @@ struct mavlink_subscriptions {
int home_sub;
int airspeed_sub;
int navigation_capabilities_sub;
int control_mode_sub;
int position_setpoint_triplet_sub;
};
extern struct mavlink_subscriptions mavlink_subs;
@ -111,8 +111,8 @@ extern struct navigation_capabilities_s nav_cap;
/** Vehicle status */
extern struct vehicle_status_s v_status;
/** Vehicle control mode */
extern struct vehicle_control_mode_s control_mode;
/** Position setpoint triplet */
extern struct position_setpoint_triplet_s pos_sp_triplet;
/** RC channels */
extern struct rc_channels_s rc;

816
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

File diff suppressed because it is too large Load Diff

563
src/modules/navigator/navigator_main.cpp
View File

@ -84,6 +84,7 @@
#include <sys/types.h>
#include <sys/stat.h>
#include "navigator_state.h"
#include "navigator_mission.h"
#include "mission_feasibility_checker.h"
#include "geofence.h"
@ -151,10 +152,10 @@ private:
int _offboard_mission_sub; /**< notification of offboard mission updates */
int _onboard_mission_sub; /**< notification of onboard mission updates */
int _capabilities_sub; /**< notification of vehicle capabilities updates */
int _control_mode_sub; /**< vehicle control mode subscription */
orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */
orb_advert_t _mission_result_pub; /**< publish mission result topic */
orb_advert_t _control_mode_pub; /**< publish vehicle control mode topic */
struct vehicle_status_s _vstatus; /**< vehicle status */
struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */
@ -177,6 +178,7 @@ private:
class Mission _mission;
bool _global_pos_valid; /**< track changes of global_position.global_valid flag */
bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */
bool _waypoint_position_reached;
bool _waypoint_yaw_reached;
@ -188,6 +190,8 @@ private:
uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */
bool _pos_sp_triplet_updated;
char *nav_states_str[NAV_STATE_MAX];
struct {
@ -233,8 +237,7 @@ private:
RTL_STATE_NONE = 0,
RTL_STATE_CLIMB,
RTL_STATE_RETURN,
RTL_STATE_DESCEND,
RTL_STATE_LAND
RTL_STATE_DESCEND
};
enum RTLState _rtl_state;
@ -274,6 +277,10 @@ private:
*/
void vehicle_status_update();
/**
* Retrieve vehicle control mode
*/
void vehicle_control_mode_update();
/**
* Shim for calling task_main from task_create.
@ -296,6 +303,7 @@ private:
void start_mission();
void start_rtl();
void start_land();
void start_land_home();
/**
* Guards offboard mission
@ -341,11 +349,6 @@ private:
* Publish a new mission item triplet for position controller
*/
void publish_position_setpoint_triplet();
/**
* Publish vehicle_control_mode topic for controllers
*/
void publish_control_mode();
};
namespace navigator
@ -373,6 +376,7 @@ Navigator::Navigator() :
_global_pos_sub(-1),
_home_pos_sub(-1),
_vstatus_sub(-1),
_control_mode_sub(-1),
_params_sub(-1),
_offboard_mission_sub(-1),
_onboard_mission_sub(-1),
@ -381,7 +385,6 @@ Navigator::Navigator() :
/* publications */
_pos_sp_triplet_pub(-1),
_mission_result_pub(-1),
_control_mode_pub(-1),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
@ -391,6 +394,7 @@ Navigator::Navigator() :
_fence_valid(false),
_inside_fence(true),
_mission(),
_global_pos_valid(false),
_reset_loiter_pos(true),
_waypoint_position_reached(false),
_waypoint_yaw_reached(false),
@ -399,6 +403,7 @@ Navigator::Navigator() :
_mission_item_valid(false),
_need_takeoff(true),
_do_takeoff(false),
_pos_sp_triplet_updated(false),
_geofence_violation_warning_sent(false)
{
_parameter_handles.min_altitude = param_find("NAV_MIN_ALT");
@ -541,9 +546,19 @@ Navigator::onboard_mission_update()
void
Navigator::vehicle_status_update()
{
/* try to load initial states */
if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) {
_vstatus.arming_state = ARMING_STATE_STANDBY; /* in case the commander is not be running */
/* in case the commander is not be running */
_vstatus.arming_state = ARMING_STATE_STANDBY;
}
}
void
Navigator::vehicle_control_mode_update()
{
if (orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode) != OK) {
/* in case the commander is not be running */
_control_mode.flag_control_auto_enabled = false;
_control_mode.flag_armed = false;
}
}
@ -589,11 +604,13 @@ Navigator::task_main()
_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_home_pos_sub = orb_subscribe(ORB_ID(home_position));
/* copy all topics first time */
vehicle_status_update();
vehicle_control_mode_update();
parameters_update();
global_position_update();
home_position_update();
@ -605,12 +622,11 @@ Navigator::task_main()
orb_set_interval(_global_pos_sub, 20);
unsigned prevState = NAV_STATE_NONE;
bool pub_control_mode = true;
hrt_abstime mavlink_open_time = 0;
const hrt_abstime mavlink_open_interval = 500000;
/* wakeup source(s) */
struct pollfd fds[7];
struct pollfd fds[8];
/* Setup of loop */
fds[0].fd = _params_sub;
@ -627,6 +643,8 @@ Navigator::task_main()
fds[5].events = POLLIN;
fds[6].fd = _vstatus_sub;
fds[6].events = POLLIN;
fds[7].fd = _control_mode_sub;
fds[7].events = POLLIN;
while (!_task_should_exit) {
@ -652,127 +670,116 @@ Navigator::task_main()
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
}
/* vehicle control mode updated */
if (fds[7].revents & POLLIN) {
vehicle_control_mode_update();
}
/* vehicle status updated */
if (fds[6].revents & POLLIN) {
vehicle_status_update();
pub_control_mode = true;
/* evaluate state machine from commander and set the navigator mode accordingly */
if (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR) {
if (_vstatus.failsafe_state == FAILSAFE_STATE_NORMAL) {
if (_vstatus.main_state == MAIN_STATE_AUTO) {
bool stick_mode = false;
if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
bool stick_mode = false;
if (!_vstatus.rc_signal_lost) {
/* RC signal available, use control switches to set mode */
/* RETURN switch, overrides MISSION switch */
if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) {
dispatch(EVENT_RTL_REQUESTED);
}
stick_mode = true;
} else {
/* MISSION switch */
if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
dispatch(EVENT_LOITER_REQUESTED);
stick_mode = true;
} else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
/* switch to mission only if available */
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
stick_mode = true;
}
if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) {
/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
dispatch(EVENT_LOITER_REQUESTED);
stick_mode = true;
}
}
if (!_vstatus.rc_signal_lost) {
/* RC signal available, use control switches to set mode */
/* RETURN switch, overrides MISSION switch */
if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
/* switch to RTL if not already landed after RTL and home position set */
if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) &&
_vstatus.condition_home_position_valid) {
dispatch(EVENT_RTL_REQUESTED);
}
if (!stick_mode) {
if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
/* commander requested new navigation mode, try to set it */
_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
stick_mode = true;
switch (_vstatus.set_nav_state) {
case NAV_STATE_NONE:
/* nothing to do */
break;
} else {
/* MISSION switch */
if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
dispatch(EVENT_LOITER_REQUESTED);
stick_mode = true;
case NAV_STATE_LOITER:
dispatch(EVENT_LOITER_REQUESTED);
break;
case NAV_STATE_MISSION:
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
break;
case NAV_STATE_RTL:
if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) {
dispatch(EVENT_RTL_REQUESTED);
}
break;
default:
warnx("ERROR: Requested navigation state not supported");
break;
}
} else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
/* switch to mission only if available */
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
/* on first switch to AUTO try mission by default, if none is available fallback to loiter */
if (myState == NAV_STATE_NONE) {
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
}
dispatch(EVENT_LOITER_REQUESTED);
}
stick_mode = true;
}
if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) {
/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
dispatch(EVENT_LOITER_REQUESTED);
stick_mode = true;
}
}
}
if (!stick_mode) {
if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
/* commander requested new navigation mode, try to set it */
_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
switch (_vstatus.set_nav_state) {
case NAV_STATE_NONE:
/* nothing to do */
break;
case NAV_STATE_LOITER:
dispatch(EVENT_LOITER_REQUESTED);
break;
case NAV_STATE_MISSION:
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
break;
case NAV_STATE_RTL:
if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) &&
_vstatus.condition_home_position_valid) {
dispatch(EVENT_RTL_REQUESTED);
}
break;
case NAV_STATE_LAND:
if (myState != NAV_STATE_READY) {
dispatch(EVENT_LAND_REQUESTED);
}
break;
default:
warnx("ERROR: Requested navigation state not supported");
break;
}
} else {
/* not in AUTO mode */
dispatch(EVENT_NONE_REQUESTED);
/* on first switch to AUTO try mission by default, if none is available fallback to loiter */
if (myState == NAV_STATE_NONE) {
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
}
}
} else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) {
/* RTL on failsafe */
if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) {
dispatch(EVENT_RTL_REQUESTED);
}
} else if (_vstatus.failsafe_state == FAILSAFE_STATE_LAND) {
/* LAND on failsafe */
if (myState != NAV_STATE_READY) {
dispatch(EVENT_LAND_REQUESTED);
}
} else {
/* shouldn't act */
dispatch(EVENT_NONE_REQUESTED);
}
} else {
/* not armed */
/* navigator shouldn't act */
dispatch(EVENT_NONE_REQUESTED);
}
}
@ -813,10 +820,22 @@ Navigator::task_main()
if (fds[1].revents & POLLIN) {
global_position_update();
/* only check if waypoint has been reached in MISSION and RTL modes */
if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) {
if (check_mission_item_reached()) {
on_mission_item_reached();
/* publish position setpoint triplet on each position update if navigator active */
if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
_pos_sp_triplet_updated = true;
if (myState == NAV_STATE_LAND && _global_pos.global_valid && !_global_pos_valid) {
/* got global position when landing, update setpoint */
start_land();
}
_global_pos_valid = _global_pos.global_valid;
/* check if waypoint has been reached in MISSION, RTL and LAND modes */
if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) {
if (check_mission_item_reached()) {
on_mission_item_reached();
}
}
}
@ -836,16 +855,16 @@ Navigator::task_main()
}
}
/* publish position setpoint triplet if updated */
if (_pos_sp_triplet_updated) {
_pos_sp_triplet_updated = false;
publish_position_setpoint_triplet();
}
/* notify user about state changes */
if (myState != prevState) {
mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]);
prevState = myState;
pub_control_mode = true;
}
/* publish control mode if updated */
if (pub_control_mode) {
publish_control_mode();
}
perf_end(_loop_perf);
@ -881,9 +900,9 @@ Navigator::start()
void
Navigator::status()
{
warnx("Global position is %svalid", _global_pos.valid ? "" : "in");
warnx("Global position is %svalid", _global_pos.global_valid ? "" : "in");
if (_global_pos.valid) {
if (_global_pos.global_valid) {
warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat);
warnx("Altitude %5.5f meters, altitude above home %5.5f meters",
(double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt));
@ -977,7 +996,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL},
/* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND},
/* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land_home), NAV_STATE_LAND},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL},
/* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state
},
@ -1006,26 +1025,29 @@ Navigator::start_none()
_do_takeoff = false;
_rtl_state = RTL_STATE_NONE;
publish_position_setpoint_triplet();
_pos_sp_triplet_updated = true;
}
void
Navigator::start_ready()
{
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = false;
_pos_sp_triplet.current.valid = true;
_pos_sp_triplet.next.valid = false;
_pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE;
_mission_item_valid = false;
_reset_loiter_pos = true;
_do_takeoff = false;
if (_rtl_state != RTL_STATE_LAND) {
/* allow RTL if landed not at home */
if (_rtl_state != RTL_STATE_DESCEND) {
/* reset RTL state if landed not at home */
_rtl_state = RTL_STATE_NONE;
}
publish_position_setpoint_triplet();
_pos_sp_triplet_updated = true;
}
void
@ -1054,11 +1076,6 @@ Navigator::start_loiter()
}
_pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL;
if (_rtl_state == RTL_STATE_LAND) {
/* if RTL landing was interrupted, avoid landing from MIN_ALT on next RTL */
_rtl_state = RTL_STATE_DESCEND;
}
}
_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
@ -1068,7 +1085,7 @@ Navigator::start_loiter()
_pos_sp_triplet.next.valid = false;
_mission_item_valid = false;
publish_position_setpoint_triplet();
_pos_sp_triplet_updated = true;
}
void
@ -1181,7 +1198,7 @@ Navigator::set_mission_item()
}
}
publish_position_setpoint_triplet();
_pos_sp_triplet_updated = true;
}
void
@ -1189,20 +1206,22 @@ Navigator::start_rtl()
{
_do_takeoff = false;
/* decide if we need climb */
if (_rtl_state == RTL_STATE_NONE) {
if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) {
_rtl_state = RTL_STATE_CLIMB;
} else {
_rtl_state = RTL_STATE_RETURN;
if (_reset_loiter_pos) {
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _global_pos.alt;
}
}
}
/* if switching directly to return state, reset altitude setpoint */
if (_rtl_state == RTL_STATE_RETURN) {
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _global_pos.alt;
}
_reset_loiter_pos = true;
set_rtl_item();
}
@ -1210,20 +1229,62 @@ Navigator::start_rtl()
void
Navigator::start_land()
{
/* this state can be requested by commander even if no global position available,
* in his case controller must perform landing without position control */
_do_takeoff = false;
_reset_loiter_pos = true;
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.next.valid = false;
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
_pos_sp_triplet.current.valid = true;
_pos_sp_triplet.current.type = SETPOINT_TYPE_LAND;
_pos_sp_triplet.current.lat = _global_pos.lat;
_pos_sp_triplet.current.lon = _global_pos.lon;
_pos_sp_triplet.current.alt = _global_pos.alt;
_pos_sp_triplet.current.loiter_direction = 1;
_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
_pos_sp_triplet.current.yaw = NAN;
_mission_item_valid = true;
_mission_item.lat = _global_pos.lat;
_mission_item.lon = _global_pos.lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _global_pos.alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LAND;
_mission_item.acceptance_radius = _parameters.acceptance_radius;
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
_pos_sp_triplet.next.valid = false;
}
void
Navigator::start_land_home()
{
/* land to home position, should be called when hovering above home, from RTL state */
_do_takeoff = false;
_reset_loiter_pos = true;
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
_mission_item_valid = true;
_mission_item.lat = _home_pos.lat;
_mission_item.lon = _home_pos.lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _home_pos.alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LAND;
_mission_item.acceptance_radius = _parameters.acceptance_radius;
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
_pos_sp_triplet.next.valid = false;
}
void
@ -1285,7 +1346,7 @@ Navigator::set_rtl_item()
_pos_sp_triplet.next.valid = false;
mavlink_log_info(_mavlink_fd, "[navigator] RTL: return");
mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt);
break;
}
@ -1316,33 +1377,6 @@ Navigator::set_rtl_item()
break;
}
case RTL_STATE_LAND: {
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
_mission_item_valid = true;
_mission_item.lat = _home_pos.lat;
_mission_item.lon = _home_pos.lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _home_pos.alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LAND;
_mission_item.acceptance_radius = _parameters.acceptance_radius;
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
_pos_sp_triplet.next.valid = false;
mavlink_log_info(_mavlink_fd, "[navigator] RTL: land");
break;
}
default: {
mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state);
start_loiter();
@ -1350,7 +1384,7 @@ Navigator::set_rtl_item()
}
}
publish_position_setpoint_triplet();
_pos_sp_triplet_updated = true;
}
void
@ -1527,27 +1561,40 @@ Navigator::on_mission_item_reached()
}
}
} else {
/* RTL finished */
if (_rtl_state == RTL_STATE_LAND) {
/* landed at home position */
mavlink_log_info(_mavlink_fd, "[navigator] RTL completed, landed");
dispatch(EVENT_READY_REQUESTED);
} else if (myState == NAV_STATE_RTL) {
/* RTL completed */
if (_rtl_state == RTL_STATE_DESCEND) {
/* hovering above home position, land if needed or loiter */
mavlink_log_info(_mavlink_fd, "[navigator] RTL completed");
if (_mission_item.autocontinue) {
dispatch(EVENT_LAND_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
} else {
/* next RTL step */
_rtl_state = (RTLState)(_rtl_state + 1);
set_rtl_item();
}
} else if (myState == NAV_STATE_LAND) {
/* landing completed */
mavlink_log_info(_mavlink_fd, "[navigator] landing completed");
dispatch(EVENT_READY_REQUESTED);
}
}
void
Navigator::publish_position_setpoint_triplet()
{
/* lazily publish the mission triplet only once available */
/* update navigation state */
_pos_sp_triplet.nav_state = static_cast<nav_state_t>(myState);
/* lazily publish the position setpoint triplet only once available */
if (_pos_sp_triplet_pub > 0) {
/* publish the mission triplet */
/* publish the position setpoint triplet */
orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet);
} else {
@ -1556,140 +1603,6 @@ Navigator::publish_position_setpoint_triplet()
}
}
void
Navigator::publish_control_mode()
{
/* update vehicle_control_mode topic*/
_control_mode.main_state = _vstatus.main_state;
_control_mode.nav_state = static_cast<nav_state_t>(myState);
_control_mode.flag_armed = _vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR;
_control_mode.flag_external_manual_override_ok = !_vstatus.is_rotary_wing;
_control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON;
_control_mode.flag_control_offboard_enabled = false;
_control_mode.flag_control_termination_enabled = false;
/* set this flag when navigator has control */
bool navigator_enabled = false;
switch (_vstatus.failsafe_state) {
case FAILSAFE_STATE_NORMAL:
switch (_vstatus.main_state) {
case MAIN_STATE_MANUAL:
_control_mode.flag_control_manual_enabled = true;
_control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing;
_control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing;
_control_mode.flag_control_altitude_enabled = false;
_control_mode.flag_control_climb_rate_enabled = false;
_control_mode.flag_control_position_enabled = false;
_control_mode.flag_control_velocity_enabled = false;
break;
case MAIN_STATE_SEATBELT:
_control_mode.flag_control_manual_enabled = true;
_control_mode.flag_control_rates_enabled = true;
_control_mode.flag_control_attitude_enabled = true;
_control_mode.flag_control_altitude_enabled = true;
_control_mode.flag_control_climb_rate_enabled = true;
_control_mode.flag_control_position_enabled = false;
_control_mode.flag_control_velocity_enabled = false;
break;
case MAIN_STATE_EASY:
_control_mode.flag_control_manual_enabled = true;
_control_mode.flag_control_rates_enabled = true;
_control_mode.flag_control_attitude_enabled = true;
_control_mode.flag_control_altitude_enabled = true;
_control_mode.flag_control_climb_rate_enabled = true;
_control_mode.flag_control_position_enabled = true;
_control_mode.flag_control_velocity_enabled = true;
break;
case MAIN_STATE_AUTO:
navigator_enabled = true;
break;
default:
break;
}
break;
case FAILSAFE_STATE_RTL:
navigator_enabled = true;
break;
case FAILSAFE_STATE_LAND:
navigator_enabled = true;
break;
case FAILSAFE_STATE_TERMINATION:
navigator_enabled = true;
/* disable all controllers on termination */
_control_mode.flag_control_manual_enabled = false;
_control_mode.flag_control_rates_enabled = false;
_control_mode.flag_control_attitude_enabled = false;
_control_mode.flag_control_position_enabled = false;
_control_mode.flag_control_velocity_enabled = false;
_control_mode.flag_control_altitude_enabled = false;
_control_mode.flag_control_climb_rate_enabled = false;
_control_mode.flag_control_termination_enabled = true;
break;
default:
break;
}
/* navigator has control, set control mode flags according to nav state*/
if (navigator_enabled) {
_control_mode.flag_control_manual_enabled = false;
switch (myState) {
case NAV_STATE_READY:
/* disable all controllers, armed but idle */
_control_mode.flag_control_rates_enabled = false;
_control_mode.flag_control_attitude_enabled = false;
_control_mode.flag_control_position_enabled = false;
_control_mode.flag_control_velocity_enabled = false;
_control_mode.flag_control_altitude_enabled = false;
_control_mode.flag_control_climb_rate_enabled = false;
break;
case NAV_STATE_LAND:
/* land with or without position control */
_control_mode.flag_control_manual_enabled = false;
_control_mode.flag_control_rates_enabled = true;
_control_mode.flag_control_attitude_enabled = true;
_control_mode.flag_control_position_enabled = _vstatus.condition_global_position_valid;
_control_mode.flag_control_velocity_enabled = _vstatus.condition_global_position_valid;
_control_mode.flag_control_altitude_enabled = true;
_control_mode.flag_control_climb_rate_enabled = true;
break;
default:
_control_mode.flag_control_rates_enabled = true;
_control_mode.flag_control_attitude_enabled = true;
_control_mode.flag_control_position_enabled = true;
_control_mode.flag_control_velocity_enabled = true;
_control_mode.flag_control_altitude_enabled = true;
_control_mode.flag_control_climb_rate_enabled = true;
break;
}
}
_control_mode.timestamp = hrt_absolute_time();
/* lazily publish the mission triplet only once available */
if (_control_mode_pub > 0) {
/* publish the mission triplet */
orb_publish(ORB_ID(vehicle_control_mode), _control_mode_pub, &_control_mode);
} else {
/* advertise and publish */
_control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &_control_mode);
}
}
void Navigator::add_fence_point(int argc, char *argv[])
{
_geofence.addPoint(argc, argv);

21
src/modules/navigator/navigator_state.h Normal file
View File

@ -0,0 +1,21 @@
/*
* navigator_state.h
*
* Created on: 27.01.2014
* Author: ton
*/
#ifndef NAVIGATOR_STATE_H_
#define NAVIGATOR_STATE_H_
typedef enum {
NAV_STATE_NONE = 0,
NAV_STATE_READY,
NAV_STATE_LOITER,
NAV_STATE_MISSION,
NAV_STATE_RTL,
NAV_STATE_LAND,
NAV_STATE_MAX
} nav_state_t;
#endif /* NAVIGATOR_STATE_H_ */

23
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -202,8 +202,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
bool landed = true;
hrt_abstime landed_time = 0;
bool flag_armed = false;
uint32_t accel_counter = 0;
uint32_t baro_counter = 0;
@ -329,6 +327,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset);
local_pos.z_valid = true;
local_pos.v_z_valid = true;
global_pos.baro_valid = true;
}
}
}
@ -379,17 +378,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
/* reset ground level on arm */
if (armed.armed && !flag_armed) {
flag_armed = armed.armed;
baro_offset -= z_est[0];
corr_baro = 0.0f;
local_pos.ref_alt -= z_est[0];
local_pos.ref_timestamp = t;
z_est[0] = 0.0f;
alt_avg = 0.0f;
}
}
/* sensor combined */
@ -637,6 +625,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
dt = fmaxf(fminf(0.02, dt), 0.005);
t_prev = t;
/* use GPS if it's valid and reference position initialized */
@ -679,7 +668,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (use_gps_z) {
float offs_corr = corr_gps[2][0] * w_z_gps_p * dt;
baro_offset += offs_corr;
baro_counter += offs_corr;
corr_baro += offs_corr;
}
/* accelerometer bias correction */
@ -835,7 +824,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
/* publish global position */
global_pos.valid = local_pos.xy_global;
global_pos.global_valid = local_pos.xy_global;
if (local_pos.xy_global) {
double est_lat, est_lon;
@ -855,6 +844,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
global_pos.alt = local_pos.ref_alt - local_pos.z;
}
if (local_pos.z_valid) {
global_pos.baro_alt = baro_offset - local_pos.z;
}
if (local_pos.v_z_valid) {
global_pos.vel_d = local_pos.vz;
}

23
src/modules/sdlog2/sdlog2.c
View File

@ -62,7 +62,6 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
@ -740,7 +739,6 @@ int sdlog2_thread_main(int argc, char *argv[])
/* warning! using union here to save memory, elements should be used separately! */
union {
struct vehicle_command_s cmd;
struct vehicle_control_mode_s control_mode;
struct sensor_combined_s sensor;
struct vehicle_attitude_s att;
struct vehicle_attitude_setpoint_s att_sp;
@ -767,7 +765,6 @@ int sdlog2_thread_main(int argc, char *argv[])
struct {
int cmd_sub;
int status_sub;
int control_mode_sub;
int sensor_sub;
int att_sub;
int att_sp_sub;
@ -847,12 +844,6 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- VEHICLE CONTROL MODE --- */
subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
fds[fdsc_count].fd = subs.control_mode_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- SENSORS COMBINED --- */
subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
fds[fdsc_count].fd = subs.sensor_sub;
@ -1002,7 +993,7 @@ int sdlog2_thread_main(int argc, char *argv[])
/* decide use usleep() or blocking poll() */
bool use_sleep = sleep_delay > 0 && logging_enabled;
/* poll all topics if logging enabled or only management (first 2) if not */
/* poll all topics if logging enabled or only management (first 3) if not */
int poll_ret = poll(fds, logging_enabled ? fdsc_count : 3, use_sleep ? 0 : poll_timeout);
/* handle the poll result */
@ -1064,11 +1055,8 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- VEHICLE STATUS --- */
if (fds[ifds++].revents & POLLIN) {
/* don't orb_copy, it's already done few lines above */
/* copy VEHICLE CONTROL MODE control mode here to construct STAT message */
orb_copy(ORB_ID(vehicle_control_mode), subs.control_mode_sub, &buf.control_mode);
log_msg.msg_type = LOG_STAT_MSG;
log_msg.body.log_STAT.main_state = (uint8_t) buf.control_mode.main_state;
log_msg.body.log_STAT.navigation_state = (uint8_t) buf.control_mode.nav_state;
log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state;
log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning;
@ -1078,7 +1066,7 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- GPS POSITION --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
/* don't orb_copy, it's already done few lines above */
log_msg.msg_type = LOG_GPS_MSG;
log_msg.body.log_GPS.gps_time = buf.gps_pos.time_gps_usec;
log_msg.body.log_GPS.fix_type = buf.gps_pos.fix_type;
@ -1094,8 +1082,6 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(GPS);
}
ifds++; // skip CONTROL MODE, already handled
/* --- SENSOR COMBINED --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor);
@ -1258,6 +1244,8 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n;
log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e;
log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d;
log_msg.body.log_GPOS.baro_alt = buf.global_pos.baro_alt;
log_msg.body.log_GPOS.flags = (buf.global_pos.baro_valid ? 1 : 0) | (buf.global_pos.global_valid ? 2 : 0);
LOGBUFFER_WRITE_AND_COUNT(GPOS);
}
@ -1265,6 +1253,7 @@ int sdlog2_thread_main(int argc, char *argv[])
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet);
log_msg.msg_type = LOG_GPSP_MSG;
log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state;
log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d);
log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d);
log_msg.body.log_GPSP.alt = buf.triplet.current.alt;

10
src/modules/sdlog2/sdlog2_messages.h
View File

@ -149,7 +149,6 @@ struct log_ATTC_s {
#define LOG_STAT_MSG 10
struct log_STAT_s {
uint8_t main_state;
uint8_t navigation_state;
uint8_t arming_state;
float battery_remaining;
uint8_t battery_warning;
@ -205,11 +204,14 @@ struct log_GPOS_s {
float vel_n;
float vel_e;
float vel_d;
float baro_alt;
uint8_t flags;
};
/* --- GPSP - GLOBAL POSITION SETPOINT --- */
#define LOG_GPSP_MSG 17
struct log_GPSP_s {
uint8_t nav_state;
int32_t lat;
int32_t lon;
float alt;
@ -297,14 +299,14 @@ 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, "BBBfBB", "MainState,NavState,ArmState,BatRem,BatWarn,Landed"),
LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"),
LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
LOG_FORMAT(GPSP, "LLffBfbf", "Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"),
LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),

12
src/modules/uORB/topics/position_setpoint_triplet.h
View File

@ -45,6 +45,7 @@
#include <stdint.h>
#include <stdbool.h>
#include "../uORB.h"
#include <navigator/navigator_state.h>
/**
* @addtogroup topics
@ -53,10 +54,11 @@
enum SETPOINT_TYPE
{
SETPOINT_TYPE_NORMAL = 0,
SETPOINT_TYPE_LOITER,
SETPOINT_TYPE_TAKEOFF,
SETPOINT_TYPE_LAND,
SETPOINT_TYPE_NORMAL = 0, /**< normal setpoint */
SETPOINT_TYPE_LOITER, /**< loiter setpoint */
SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */
SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, vehicle must descend until landing */
SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */
};
struct position_setpoint_s
@ -82,6 +84,8 @@ struct position_setpoint_triplet_s
struct position_setpoint_s previous;
struct position_setpoint_s current;
struct position_setpoint_s next;
nav_state_t nav_state; /**< navigation state */
};
/**

19
src/modules/uORB/topics/vehicle_control_mode.h
View File

@ -61,23 +61,10 @@
* Encodes the complete system state and is set by the commander app.
*/
typedef enum {
NAV_STATE_NONE = 0,
NAV_STATE_READY,
NAV_STATE_LOITER,
NAV_STATE_MISSION,
NAV_STATE_RTL,
NAV_STATE_LAND,
NAV_STATE_MAX
} nav_state_t;
struct vehicle_control_mode_s
{
uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
main_state_t main_state;
nav_state_t nav_state;
bool flag_armed;
bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */
@ -86,14 +73,14 @@ struct vehicle_control_mode_s
bool flag_system_hil_enabled;
bool flag_control_manual_enabled; /**< true if manual input is mixed in */
bool flag_control_offboard_enabled; /**< true if offboard control input is on */
bool flag_control_auto_enabled; /**< true if onboard autopilot should act */
bool flag_control_rates_enabled; /**< true if rates are stabilized */
bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */
bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */
bool flag_control_position_enabled; /**< true if position is controlled */
bool flag_control_altitude_enabled; /**< true if altitude is controlled */
bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */
bool flag_control_termination_enabled; /**< true if flighttermination is enabled */
bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */
bool flag_control_termination_enabled; /**< true if flighttermination is enabled */
};
/**

12
src/modules/uORB/topics/vehicle_global_position.h
View File

@ -61,17 +61,21 @@
*/
struct vehicle_global_position_s
{
uint64_t timestamp; /**< time of this estimate, in microseconds since system start */
uint64_t time_gps_usec; /**< GPS timestamp in microseconds */
bool valid; /**< true if position satisfies validity criteria of estimator */
uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */
bool global_valid; /**< true if position satisfies validity criteria of estimator */
bool baro_valid; /**< true if baro_alt is valid (vel_d is also valid in this case) */
uint64_t time_gps_usec; /**< GPS timestamp in microseconds */
double lat; /**< Latitude in degrees */
double lon; /**< Longitude in degrees */
float alt; /**< Altitude in meters */
float alt; /**< Altitude AMSL in meters */
float vel_n; /**< Ground north velocity, m/s */
float vel_e; /**< Ground east velocity, m/s */
float vel_d; /**< Ground downside velocity, m/s */
float yaw; /**< Yaw in radians -PI..+PI. */
float baro_alt; /**< Barometric altitude (not raw baro but fused with accelerometer) */
};
/**

2
src/modules/uORB/topics/vehicle_status.h
View File

@ -54,6 +54,8 @@
#include <stdbool.h>
#include "../uORB.h"
#include <navigator/navigator_state.h>
/**
* @addtogroup topics @{
*/