dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander, multirotor_att_control, position_estimator_inav: position valid flag fixed, other fixes and cleaunup

This commit is contained in:
Anton Babushkin 2013-08-17 12:37:41 +02:00
parent c543f89ec1
commit 2be5240b9f
3 changed files with 74 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -117,12 +117,9 @@ extern struct system_load_s system_load;
#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define GPS_FIX_TYPE_2D 2
#define GPS_FIX_TYPE_3D 3
#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000
#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */
#define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */
#define RC_TIMEOUT 100000
#define DIFFPRESS_TIMEOUT 2000000
#define PRINT_INTERVAL 5000000
#define PRINT_MODE_REJECT_INTERVAL 2000000
@ -197,6 +194,8 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode
*/
int commander_thread_main(int argc, char *argv[]);
void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position);
void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status);
@ -354,19 +353,21 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
case VEHICLE_CMD_NAV_TAKEOFF: {
transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
if (nav_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command");
}
transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
if (nav_res != TRANSITION_DENIED) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
if (nav_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command");
}
} else {
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
if (nav_res != TRANSITION_DENIED) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
break;
}
break;
}
case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
if (is_safe(status, safety, armed)) {
@ -713,8 +714,6 @@ int commander_thread_main(int argc, char *argv[])
start_time = hrt_absolute_time();
while (!thread_should_exit) {
hrt_abstime t = hrt_absolute_time();
/* update parameters */
orb_check(param_changed_sub, &updated);
@ -773,16 +772,9 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
last_diff_pres_time = diff_pres.timestamp;
}
/* Check for valid airspeed/differential pressure measurements */
if (t - last_diff_pres_time < 2000000 && t > 2000000) {
status.condition_airspeed_valid = true;
} else {
status.condition_airspeed_valid = false;
}
check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed);
orb_check(cmd_sub, &updated);
@ -809,19 +801,8 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
}
/* set the condition to valid if there has recently been a global position estimate */
if (t - global_position.timestamp < POSITION_TIMEOUT && t > POSITION_TIMEOUT && global_position.valid) {
if (!status.condition_global_position_valid) {
status.condition_global_position_valid = true;
status_changed = true;
}
} else {
if (status.condition_global_position_valid) {
status.condition_global_position_valid = false;
status_changed = true;
}
}
/* update condition_global_position_valid */
check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.valid, &(status.condition_global_position_valid), &status_changed);
/* update local position estimate */
orb_check(local_position_sub, &updated);
@ -831,19 +812,8 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
}
/* set the condition to valid if there has recently been a local position estimate */
if (t - local_position.timestamp < POSITION_TIMEOUT && t > POSITION_TIMEOUT && local_position.valid) {
if (!status.condition_local_position_valid) {
status.condition_local_position_valid = true;
status_changed = true;
}
} else {
if (status.condition_local_position_valid) {
status.condition_local_position_valid = false;
status_changed = true;
}
}
/* update condition_local_position_valid */
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.valid, &(status.condition_local_position_valid), &status_changed);
/* update battery status */
orb_check(battery_sub, &updated);
@ -854,7 +824,7 @@ int commander_thread_main(int argc, char *argv[])
warnx("bat v: %2.2f", battery.voltage_v);
/* only consider battery voltage if system has been running 2s and battery voltage is not 0 */
if ((t - start_time) > 2000000 && battery.voltage_v > 0.001f) {
if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 0.001f) {
status.battery_voltage = battery.voltage_v;
status.condition_battery_voltage_valid = true;
status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage);
@ -981,10 +951,9 @@ int commander_thread_main(int argc, char *argv[])
* position to the current position.
*/
if (!home_position_set && gps_position.fix_type == GPS_FIX_TYPE_3D &&
if (!home_position_set && gps_position.fix_type >= 3 &&
(hdop_m < hdop_threshold_m) && (vdop_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk
(t - gps_position.timestamp_position < 2000000)
&& !armed.armed) {
(hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) {
/* copy position data to uORB home message, store it locally as well */
// TODO use global position estimate
home.lat = gps_position.lat;
@ -1019,7 +988,7 @@ int commander_thread_main(int argc, char *argv[])
/* ignore RC signals if in offboard control mode */
if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) {
/* start RC input check */
if ((t - sp_man.timestamp) < 100000) {
if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
/* handle the case where RC signal was regained */
if (!status.rc_signal_found_once) {
status.rc_signal_found_once = true;
@ -1123,7 +1092,7 @@ int commander_thread_main(int argc, char *argv[])
} else {
/* print error message for first RC glitch and then every 5s */
if (!status.rc_signal_cutting_off || (t - last_print_control_time) > PRINT_INTERVAL) {
if (!status.rc_signal_cutting_off || (hrt_absolute_time() - last_print_control_time) > PRINT_INTERVAL) {
// TODO remove debug code
if (!status.rc_signal_cutting_off) {
warnx("Reason: not rc_signal_cutting_off\n");
@ -1136,11 +1105,11 @@ int commander_thread_main(int argc, char *argv[])
status.rc_signal_cutting_off = true;
mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO RC CONTROL");
last_print_control_time = t;
last_print_control_time = hrt_absolute_time();
}
/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
status.rc_signal_lost_interval = t - sp_man.timestamp;
status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp;
/* if the RC signal is gone for a full second, consider it lost */
if (status.rc_signal_lost_interval > 1000000) {
@ -1157,7 +1126,7 @@ int commander_thread_main(int argc, char *argv[])
// TODO check this
/* state machine update for offboard control */
if (!status.rc_signal_found_once && sp_offboard.timestamp != 0) {
if ((t - sp_offboard.timestamp) < 5000000) { // TODO 5s is too long ?
if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { // TODO 5s is too long ?
// /* decide about attitude control flag, enable in att/pos/vel */
// bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE ||
@ -1213,23 +1182,23 @@ int commander_thread_main(int argc, char *argv[])
} else {
/* print error message for first offboard signal glitch and then every 5s */
if (!status.offboard_control_signal_weak || ((t - last_print_control_time) > PRINT_INTERVAL)) {
if (!status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_control_time) > PRINT_INTERVAL)) {
status.offboard_control_signal_weak = true;
mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO OFFBOARD CONTROL");
last_print_control_time = t;
last_print_control_time = hrt_absolute_time();
}
/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
status.offboard_control_signal_lost_interval = t - sp_offboard.timestamp;
status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp;
/* if the signal is gone for 0.1 seconds, consider it lost */
if (status.offboard_control_signal_lost_interval > 100000) {
status.offboard_control_signal_lost = true;
status.failsave_lowlevel_start_time = t;
status.failsave_lowlevel_start_time = hrt_absolute_time();
tune_positive();
/* kill motors after timeout */
if (t - status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) {
if (hrt_absolute_time() > status.failsave_lowlevel_start_time + failsafe_lowlevel_timeout_ms * 1000) {
status.failsave_lowlevel = true;
status_changed = true;
}
@ -1256,9 +1225,11 @@ int commander_thread_main(int argc, char *argv[])
status_changed = true;
}
hrt_abstime t1 = hrt_absolute_time();
/* publish arming state */
if (arming_state_changed) {
armed.timestamp = t;
armed.timestamp = t1;
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
}
@ -1266,14 +1237,14 @@ int commander_thread_main(int argc, char *argv[])
if (navigation_state_changed) {
/* publish new navigation state */
control_mode.counter++;
control_mode.timestamp = t;
control_mode.timestamp = t1;
orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
}
/* publish vehicle status at least with 1 Hz */
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
status.counter++;
status.timestamp = t;
status.timestamp = t1;
orb_publish(ORB_ID(vehicle_status), status_pub, &status);
status_changed = false;
}
@ -1340,6 +1311,18 @@ int commander_thread_main(int argc, char *argv[])
return 0;
}
void
check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed)
{
hrt_abstime t = hrt_absolute_time();
bool valid_new = (t < timestamp + timeout && t > timeout && valid_in);
if (*valid_out != valid_new) {
*valid_out = valid_new;
*changed = true;
}
}
void
toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position)
{
@ -1367,7 +1350,7 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gp
/* position estimated, solid */
led_on(LED_BLUE);
} else if (leds_counter == 0) {
} else if (leds_counter == 6) {
/* waiting for position estimate, short blink at 1.25Hz */
led_on(LED_BLUE);
@ -1548,6 +1531,7 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v
if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
/* don't act while taking off */
res = TRANSITION_NOT_CHANGED;
} else {
if (current_status->condition_landed) {
/* if landed: transitions only to AUTO_READY are allowed */

20
src/modules/multirotor_att_control/multirotor_att_control_main.c
View File

@ -367,14 +367,20 @@ mc_thread_main(int argc, char *argv[])
}
/* apply controller */
float gyro[3];
gyro[0] = att.rollspeed;
gyro[1] = att.pitchspeed;
gyro[2] = att.yawspeed;
multirotor_control_rates(&rates_sp, gyro, &actuators);
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
float rates[3];
rates[0] = att.rollspeed;
rates[1] = att.pitchspeed;
rates[2] = att.yawspeed;
multirotor_control_rates(&rates_sp, rates, &actuators);
} else {
/* rates controller disabled, set actuators to zero for safety */
actuators.control[0] = 0.0f;
actuators.control[1] = 0.0f;
actuators.control[2] = 0.0f;
actuators.control[3] = 0.0f;
}
actuators.timestamp = hrt_absolute_time();
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
/* update state */
flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled;

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

@ -75,6 +75,7 @@ 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
__EXPORT int position_estimator_inav_main(int argc, char *argv[]);
@ -490,7 +491,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc);
/* dont't try to estimate position when no any position source available */
bool can_estimate_pos = params.use_gps && gps.fix_type >= 3;
bool can_estimate_pos = params.use_gps && gps.fix_type >= 3 && t - gps.timestamp_position < gps_timeout;
if (can_estimate_pos) {
/* inertial filter prediction for position */
@ -501,14 +502,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc);
inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc);
if (params.use_gps && gps.fix_type >= 3) {
inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p);
inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p);
inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p);
inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p);
if (gps.vel_ned_valid) {
inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v);
inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v);
}
if (gps.vel_ned_valid && t - gps.timestamp_velocity < gps_timeout) {
inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v);
inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v);
}
}