fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 12:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'navigator_new' into navigator_new_vector, WIP

Browse Source
This commit is contained in:
Anton Babushkin
2013-12-28 10:04:13 +04:00
parent 153114aec8 72d9c80ce9
commit 7b7539fbbd
77 changed files with 5914 additions and 1412 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+35 -14
View File
@@ -509,6 +509,21 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
break;
/* Flight termination */
case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON, control_mode);
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
/* reject parachute depoyment not armed */
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
}
break;
default:
break;
}
@@ -1033,23 +1048,17 @@ int commander_thread_main(int argc, char *argv[])
if (!home_position_set && gps_position.fix_type >= 3 &&
(gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk
(hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) {
(hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed
&& global_position.valid) {
/* copy position data to uORB home message, store it locally as well */
// TODO use global position estimate
home.lat = gps_position.lat;
home.lon = gps_position.lon;
home.alt = gps_position.alt;
home.eph_m = gps_position.eph_m;
home.epv_m = gps_position.epv_m;
home.s_variance_m_s = gps_position.s_variance_m_s;
home.p_variance_m = gps_position.p_variance_m;
home.lat = (double)global_position.lat / 1e7d;
home.lon = (double)global_position.lon / 1e7d;
home.altitude = (float)global_position.alt;
double home_lat_d = home.lat * 1e-7;
double home_lon_d = home.lon * 1e-7;
warnx("home: lat = %.7f, lon = %.7f", home_lat_d, home_lon_d);
mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f", home_lat_d, home_lon_d);
warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.altitude);
mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.altitude);
/* announce new home position */
if (home_pub > 0) {
@@ -1174,6 +1183,16 @@ int commander_thread_main(int argc, char *argv[])
}
}
/* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */
if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON, &control_mode);
if (flighttermination_res == TRANSITION_CHANGED) {
tune_positive();
}
} else {
flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF, &control_mode);
}
/* handle commands last, as the system needs to be updated to handle them */
orb_check(cmd_sub, &updated);
@@ -1199,6 +1218,7 @@ int commander_thread_main(int argc, char *argv[])
bool arming_state_changed = check_arming_state_changed();
bool main_state_changed = check_main_state_changed();
bool navigation_state_changed = check_navigation_state_changed();
bool flighttermination_state_changed = check_flighttermination_state_changed();
hrt_abstime t1 = hrt_absolute_time();
@@ -1725,7 +1745,8 @@ void *commander_low_prio_loop(void *arg)
/* ignore commands the high-prio loop handles */
if (cmd.command == VEHICLE_CMD_DO_SET_MODE ||
cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM ||
cmd.command == VEHICLE_CMD_NAV_TAKEOFF)
cmd.command == VEHICLE_CMD_NAV_TAKEOFF ||
cmd.command == VEHICLE_CMD_DO_SET_SERVO)
continue;
/* only handle low-priority commands here */
src/modules/commander/state_machine_helper.cpp
+54 -2
View File
@@ -65,6 +65,7 @@ static const int ERROR = -1;
static bool arming_state_changed = true;
static bool main_state_changed = true;
static bool navigation_state_changed = true;
static bool flighttermination_state_changed = true;
transition_result_t
arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety,
@@ -239,8 +240,9 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
case MAIN_STATE_SEATBELT:
/* need at minimum altitude estimate */
if (current_state->condition_local_altitude_valid ||
current_state->condition_global_position_valid) {
if (!current_state->is_rotary_wing ||
(current_state->condition_local_altitude_valid ||
current_state->condition_global_position_valid)) {
ret = TRANSITION_CHANGED;
}
@@ -450,6 +452,18 @@ check_navigation_state_changed()
}
}
bool
check_flighttermination_state_changed()
{
if (flighttermination_state_changed) {
flighttermination_state_changed = false;
return true;
} else {
return false;
}
}
void
set_navigation_state_changed()
{
@@ -522,6 +536,44 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
}
/**
* Transition from one flightermination state to another
*/
transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state, struct vehicle_control_mode_s *control_mode)
{
transition_result_t ret = TRANSITION_DENIED;
/* only check transition if the new state is actually different from the current one */
if (new_flighttermination_state == status->flighttermination_state) {
ret = TRANSITION_NOT_CHANGED;
} else {
switch (new_flighttermination_state) {
case FLIGHTTERMINATION_STATE_ON:
ret = TRANSITION_CHANGED;
status->flighttermination_state = FLIGHTTERMINATION_STATE_ON;
warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON");
break;
case FLIGHTTERMINATION_STATE_OFF:
ret = TRANSITION_CHANGED;
status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF;
break;
default:
break;
}
if (ret == TRANSITION_CHANGED) {
flighttermination_state_changed = true;
control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON;
}
}
return ret;
}
// /*
// * Wrapper functions (to be used in the commander), all functions assume lock on current_status
src/modules/commander/state_machine_helper.h
+4
View File
@@ -70,8 +70,12 @@ bool check_main_state_changed();
transition_result_t navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode);
transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state, struct vehicle_control_mode_s *control_mode);
bool check_navigation_state_changed();
bool check_flighttermination_state_changed();
void set_navigation_state_changed();
int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd);