yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

Commander: Remove duplicate code for setting home position

Browse Source
This commit is contained in:
Johan Jansen
2015-01-16 16:43:11 +01:00
parent 818ccf7a04
commit a581ae8ed6
1 changed files with 64 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -240,6 +240,12 @@ transition_result_t check_navigation_state_machine(struct vehicle_status_s *stat
transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy);
/**
* @brief This function initializes the home position of the vehicle. This happens first time we get a good GPS fix and each
* time the vehicle is armed with a good GPS fix.
**/
static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home, const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition);
/**
* Loop that runs at a lower rate and priority for calibration and parameter tasks.
*/
@@ -716,6 +722,52 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
return true;
}
/**
* @brief This function initializes the home position of the vehicle. This happens first time we get a good GPS fix and each
* time the vehicle is armed with a good GPS fix.
**/
static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home, const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition)
{
//Need global position fix to be able to set home
if(!status.condition_global_position_valid) {
return;
}
//Ensure that the GPS accuracy is good enough for intializing home
if(globalPosition.eph > eph_threshold || globalPosition.epv > epv_threshold) {
return;
}
//Set home position
home.timestamp = hrt_absolute_time();
home.lat = globalPosition.lat;
home.lon = globalPosition.lon;
home.alt = globalPosition.alt;
home.x = localPosition.x;
home.y = localPosition.y;
home.z = localPosition.z;
warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
/* announce new home position */
if (homePub > 0) {
orb_publish(ORB_ID(home_position), homePub, &home);
} else {
homePub = orb_advertise(ORB_ID(home_position), &home);
}
//Play tune first time we initialize HOME
if(!status.condition_home_position_valid) {
tune_positive(true);
}
/* mark home position as set */
status.condition_home_position_valid = true;
}
int commander_thread_main(int argc, char *argv[])
{
/* not yet initialized */
@@ -904,7 +956,6 @@ int commander_thread_main(int argc, char *argv[])
bool critical_battery_voltage_actions_done = false;
hrt_abstime last_idle_time = 0;
hrt_abstime start_time = 0;
bool status_changed = true;
bool param_init_forced = true;
@@ -1035,7 +1086,7 @@ int commander_thread_main(int argc, char *argv[])
commander_initialized = true;
thread_running = true;
start_time = hrt_absolute_time();
const hrt_abstime commander_boot_timestamp = hrt_absolute_time();
transition_result_t arming_ret;
@@ -1325,34 +1376,6 @@ int commander_thread_main(int argc, char *argv[])
check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid),
&status_changed);
/* update home position */
if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
(global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) {
home.lat = global_position.lat;
home.lon = global_position.lon;
home.alt = global_position.alt;
home.x = local_position.x;
home.y = local_position.y;
home.z = local_position.z;
warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
/* announce new home position */
if (home_pub > 0) {
orb_publish(ORB_ID(home_position), home_pub, &home);
} else {
home_pub = orb_advertise(ORB_ID(home_position), &home);
}
/* mark home position as set */
status.condition_home_position_valid = true;
tune_positive(true);
}
/* update condition_local_position_valid and condition_local_altitude_valid */
/* hysteresis for EPH */
bool local_eph_good;
@@ -1401,7 +1424,7 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
/* only consider battery voltage if system has been running 2s and battery voltage is valid */
if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
if (hrt_absolute_time() > commander_boot_timestamp + 2000000 && battery.voltage_filtered_v > 0.0f) {
status.battery_voltage = battery.voltage_filtered_v;
status.battery_current = battery.current_a;
status.condition_battery_voltage_valid = true;
@@ -1859,44 +1882,23 @@ int commander_thread_main(int argc, char *argv[])
}
}
//Get current timestamp
const hrt_abstime now = hrt_absolute_time();
//First time home position update
if(!status.condition_home_position_valid) {
commander_set_home_position(home_pub, home, local_position, global_position);
}
/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
else if(arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + 2000000) {
commander_set_home_position(home_pub, home, local_position, global_position);
}
/* print new state */
if (arming_state_changed) {
status_changed = true;
mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]);
/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid &&
(global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) {
// TODO remove code duplication (setting home is also done somewhere else in this file)
home.timestamp = now;
home.lat = global_position.lat;
home.lon = global_position.lon;
home.alt = global_position.alt;
home.x = local_position.x;
home.y = local_position.y;
home.z = local_position.z;
warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
/* announce new home position */
if (home_pub > 0) {
orb_publish(ORB_ID(home_position), home_pub, &home);
} else {
home_pub = orb_advertise(ORB_ID(home_position), &home);
}
/* mark home position as set */
status.condition_home_position_valid = true;
}
arming_state_changed = false;
}