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

Merge branch 'navigator_rewrite' into navigator_rewrite_estimator

This commit is contained in:
Julian Oes 2014-06-27 14:41:19 +02:00
parent dbd99b6494 cc8f7f4c97
commit b8d07532a7
7 changed files with 59 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -956,7 +956,7 @@ int commander_thread_main(int argc, char *argv[])
if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) {
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) {
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch");
arming_state_changed = true;
}
@ -1155,18 +1155,16 @@ int commander_thread_main(int argc, char *argv[])
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
if (armed.armed) {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
warnx("changed 1");
arming_state_changed = true;
}
} else {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
warnx("changed 2");
arming_state_changed = true;
}
}
@ -1178,7 +1176,7 @@ int commander_thread_main(int argc, char *argv[])
/* If in INIT state, try to proceed to STANDBY state */
if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
/* TODO: check for sensors */
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
@ -1237,7 +1235,7 @@ int commander_thread_main(int argc, char *argv[])
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed);
arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
@ -1262,7 +1260,7 @@ int commander_thread_main(int argc, char *argv[])
print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first.");
} else {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
@ -1937,8 +1935,7 @@ void *commander_low_prio_loop(void *arg)
int calib_ret = ERROR;
/* try to go to INIT/PREFLIGHT arming state */
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, mavlink_fd)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
break;
}
@ -2001,7 +1998,7 @@ void *commander_low_prio_loop(void *arg)
tune_negative(true);
}
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
break;
}

2
src/modules/commander/state_machine_helper.h
View File

@ -59,7 +59,7 @@ typedef enum {
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd = 0);
arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd);
transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);

11
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@ -707,14 +707,21 @@ FixedwingAttitudeControl::task_main()
float throttle_sp = 0.0f;
if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) {
/* read in attitude setpoint from attitude setpoint uorb topic */
roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
throttle_sp = _att_sp.thrust;
/* reset integrals where needed */
if (_att_sp.roll_reset_integral)
if (_att_sp.roll_reset_integral) {
_roll_ctrl.reset_integrator();
}
if (_att_sp.pitch_reset_integral) {
_pitch_ctrl.reset_integrator();
}
if (_att_sp.yaw_reset_integral) {
_yaw_ctrl.reset_integrator();
}
} else {
/*
* Scale down roll and pitch as the setpoints are radians

14
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -872,6 +872,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* current waypoint (the one currently heading for) */
math::Vector<2> curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
/* Initialize attitude controller integrator reset flags to 0 */
_att_sp.roll_reset_integral = false;
_att_sp.pitch_reset_integral = false;
_att_sp.yaw_reset_integral = false;
/* previous waypoint */
math::Vector<2> prev_wp;
@ -1061,15 +1065,21 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) {
/* Perform launch detection */
// warnx("Launch detection running");
if(!launch_detected) { //do not do further checks once a launch was detected
if (launchDetector.launchDetectionEnabled()) {
static hrt_abstime last_sent = 0;
if(hrt_absolute_time() - last_sent > 4e6) {
// warnx("Launch detection running");
mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
last_sent = hrt_absolute_time();
}
/* Tell the attitude controller to stop integrating while we are waiting
* for the launch */
_att_sp.roll_reset_integral = true;
_att_sp.pitch_reset_integral = true;
_att_sp.yaw_reset_integral = true;
/* Detect launch */
launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
if (launchDetector.getLaunchDetected()) {
launch_detected = true;

26
src/modules/mavlink/mavlink_messages.cpp
View File

@ -258,19 +258,21 @@ protected:
struct vehicle_status_s status;
struct position_setpoint_triplet_s pos_sp_triplet;
if (status_sub->update(&status) && pos_sp_triplet_sub->update(&pos_sp_triplet)) {
uint8_t mavlink_state = 0;
uint8_t mavlink_base_mode = 0;
uint32_t mavlink_custom_mode = 0;
get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
/* always send the heartbeat, independent of the update status of the topics */
(void)status_sub->update(&status);
(void)pos_sp_triplet_sub->update(&pos_sp_triplet);
mavlink_msg_heartbeat_send(_channel,
mavlink_system.type,
MAV_AUTOPILOT_PX4,
mavlink_base_mode,
mavlink_custom_mode,
mavlink_state);
}
uint8_t mavlink_state = 0;
uint8_t mavlink_base_mode = 0;
uint32_t mavlink_custom_mode = 0;
get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
mavlink_msg_heartbeat_send(_channel,
mavlink_system.type,
MAV_AUTOPILOT_PX4,
mavlink_base_mode,
mavlink_custom_mode,
mavlink_state);
}
};

30
src/modules/mavlink/mavlink_orb_subscription.cpp
View File

@ -44,8 +44,6 @@
#include <uORB/uORB.h>
#include <stdio.h>
#include <systemlib/err.h>
#include "mavlink_orb_subscription.h"
MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
@ -79,23 +77,21 @@ MavlinkOrbSubscription::update(uint64_t *time, void* data)
time_topic = 0;
}
if (time_topic != *time) {
if (orb_copy(_topic, _fd, data)) {
/* error copying topic data */
memset(data, 0, _topic->o_size);
//warnx("err copy, fd: %d, obj: %s, size: %d", _fd, _topic->o_name, _topic->o_size);
return false;
} else {
/* data copied successfully */
_published = true;
*time = time_topic;
return true;
}
if (orb_copy(_topic, _fd, data)) {
/* error copying topic data */
memset(data, 0, _topic->o_size);
return false;
} else {
return false;
/* data copied successfully */
_published = true;
if (time_topic != *time) {
*time = time_topic;
return true;
} else {
return false;
}
}
}

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

@ -72,6 +72,8 @@ struct vehicle_attitude_setpoint_s {
float thrust; /**< Thrust in Newton the power system should generate */
bool roll_reset_integral; /**< Reset roll integral part (navigation logic change) */
bool pitch_reset_integral; /**< Reset pitch integral part (navigation logic change) */
bool yaw_reset_integral; /**< Reset yaw integral part (navigation logic change) */
};