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

Merge branch 'master' into smooth_pos_hold

This commit is contained in:
Anton Babushkin 2014-08-07 23:08:54 +02:00
parent f7582b4d00 95fc701376
commit a9d3e9854f
12 changed files with 155 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
src/lib/external_lgpl/tecs/tecs.cpp
View File

@ -554,18 +554,30 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
// Calculate pitch demand
_update_pitch();
// // Write internal variables to the log_tuning structure. This
// // structure will be logged in dataflash at 10Hz
// log_tuning.hgt_dem = _hgt_dem_adj;
// log_tuning.hgt = _integ3_state;
// log_tuning.dhgt_dem = _hgt_rate_dem;
// log_tuning.dhgt = _integ2_state;
// log_tuning.spd_dem = _TAS_dem_adj;
// log_tuning.spd = _integ5_state;
// log_tuning.dspd = _vel_dot;
// log_tuning.ithr = _integ6_state;
// log_tuning.iptch = _integ7_state;
// log_tuning.thr = _throttle_dem;
// log_tuning.ptch = _pitch_dem;
// log_tuning.dspd_dem = _TAS_rate_dem;
_tecs_state.timestamp = now;
if (_underspeed) {
_tecs_state.mode = ECL_TECS_MODE_UNDERSPEED;
} else if (_badDescent) {
_tecs_state.mode = ECL_TECS_MODE_BAD_DESCENT;
} else if (_climbOutDem) {
_tecs_state.mode = ECL_TECS_MODE_CLIMBOUT;
} else {
// If no error flag applies, conclude normal
_tecs_state.mode = ECL_TECS_MODE_NORMAL;
}
_tecs_state.hgt_dem = _hgt_dem_adj;
_tecs_state.hgt = _integ3_state;
_tecs_state.dhgt_dem = _hgt_rate_dem;
_tecs_state.dhgt = _integ2_state;
_tecs_state.spd_dem = _TAS_dem_adj;
_tecs_state.spd = _integ5_state;
_tecs_state.dspd = _vel_dot;
_tecs_state.ithr = _integ6_state;
_tecs_state.iptch = _integ7_state;
_tecs_state.thr = _throttle_dem;
_tecs_state.ptch = _pitch_dem;
_tecs_state.dspd_dem = _TAS_rate_dem;
}

47
src/lib/external_lgpl/tecs/tecs.h
View File

@ -28,6 +28,7 @@ class __EXPORT TECS
{
public:
TECS() :
_tecs_state {},
_update_50hz_last_usec(0),
_update_speed_last_usec(0),
_update_pitch_throttle_last_usec(0),
@ -120,24 +121,33 @@ public:
return _spdWeight;
}
// log data on internal state of the controller. Called at 10Hz
// void log_data(DataFlash_Class &dataflash, uint8_t msgid);
enum ECL_TECS_MODE {
ECL_TECS_MODE_NORMAL = 0,
ECL_TECS_MODE_UNDERSPEED,
ECL_TECS_MODE_BAD_DESCENT,
ECL_TECS_MODE_CLIMBOUT
};
// struct PACKED log_TECS_Tuning {
// LOG_PACKET_HEADER;
// float hgt;
// float dhgt;
// float hgt_dem;
// float dhgt_dem;
// float spd_dem;
// float spd;
// float dspd;
// float ithr;
// float iptch;
// float thr;
// float ptch;
// float dspd_dem;
// } log_tuning;
struct tecs_state {
uint64_t timestamp;
float hgt;
float dhgt;
float hgt_dem;
float dhgt_dem;
float spd_dem;
float spd;
float dspd;
float ithr;
float iptch;
float thr;
float ptch;
float dspd_dem;
enum ECL_TECS_MODE mode;
};
void get_tecs_state(struct tecs_state& state) {
state = _tecs_state;
}
void set_time_const(float time_const) {
_timeConst = time_const;
@ -212,6 +222,9 @@ public:
}
private:
struct tecs_state _tecs_state;
// Last time update_50Hz was called
uint64_t _update_50hz_last_usec;

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

@ -393,7 +393,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
// output appropriate error messages if the state cannot transition.
arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd_local);
arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd_local);
if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
@ -1085,7 +1085,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, mavlink_fd)) {
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd)) {
mavlink_log_info(mavlink_fd, "DISARMED by safety switch");
arming_state_changed = true;
}
@ -1288,14 +1288,14 @@ 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, mavlink_fd);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
} else {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
@ -1309,7 +1309,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, mavlink_fd);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
@ -1368,7 +1368,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, mavlink_fd);
arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
@ -1394,7 +1394,7 @@ int commander_thread_main(int argc, char *argv[])
if (status.main_state != MAIN_STATE_MANUAL) {
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
@ -2156,7 +2156,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, mavlink_fd)) {
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, true /* fRunPreArmChecks */, mavlink_fd)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
break;
}
@ -2219,7 +2219,7 @@ void *commander_low_prio_loop(void *arg)
tune_negative(true);
}
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
break;
}

10
src/modules/commander/commander_tests/state_machine_helper_test.cpp
View File

@ -286,7 +286,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
armed.ready_to_arm = test->current_state.ready_to_arm;
// Attempt transition
transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, 0 /* no mavlink_fd */);
transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, 0 /* no mavlink_fd */);
// Validate result of transition
ut_assert(test->assertMsg, test->expected_transition_result == result);
@ -335,12 +335,12 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
MTT_ALL_NOT_VALID,
MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
{ "transition: MANUAL to AUTO_MISSION - global position valid",
MTT_GLOBAL_POS_VALID,
{ "transition: MANUAL to AUTO_MISSION - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
{ "transition: AUTO_MISSION to MANUAL - global position valid",
MTT_GLOBAL_POS_VALID,
{ "transition: AUTO_MISSION to MANUAL - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
{ "transition: MANUAL to AUTO_LOITER - global position valid",

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

@ -99,11 +99,12 @@ static const char * const state_names[ARMING_STATE_MAX] = {
};
transition_result_t
arming_state_transition(struct vehicle_status_s *status, /// current vehicle status
const struct safety_s *safety, /// current safety settings
arming_state_t new_arming_state, /// arming state requested
struct actuator_armed_s *armed, /// current armed status
const int mavlink_fd) /// mavlink fd for error reporting, 0 for none
arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status
const struct safety_s *safety, ///< current safety settings
arming_state_t new_arming_state, ///< arming state requested
struct actuator_armed_s *armed, ///< current armed status
bool fRunPreArmChecks, ///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing
const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none
{
// Double check that our static arrays are still valid
ASSERT(ARMING_STATE_INIT == 0);
@ -125,7 +126,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current
int prearm_ret = OK;
/* only perform the check if we have to */
if (new_arming_state == ARMING_STATE_ARMED) {
if (fRunPreArmChecks && new_arming_state == ARMING_STATE_ARMED) {
prearm_ret = prearm_check(status, mavlink_fd);
}

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

@ -57,7 +57,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);
arming_state_t new_arming_state, struct actuator_armed_s *armed, bool fRunPreArmChecks, const int mavlink_fd);
transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);

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

@ -146,6 +146,7 @@ private:
int _range_finder_sub; /**< range finder subscription */
orb_advert_t _attitude_sp_pub; /**< attitude setpoint */
orb_advert_t _tecs_status_pub; /**< TECS status publication */
orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */
struct vehicle_attitude_s _att; /**< vehicle attitude */
@ -414,6 +415,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
/* publications */
_attitude_sp_pub(-1),
_tecs_status_pub(-1),
_nav_capabilities_pub(-1),
/* states */
@ -1384,6 +1386,51 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
climbout_mode, climbout_pitch_min_rad,
throttle_min, throttle_max, throttle_cruise,
pitch_min_rad, pitch_max_rad);
struct TECS::tecs_state s;
_tecs.get_tecs_state(s);
struct tecs_status_s t;
t.timestamp = s.timestamp;
switch (s.mode) {
case TECS::ECL_TECS_MODE_NORMAL:
t.mode = TECS_MODE_NORMAL;
break;
case TECS::ECL_TECS_MODE_UNDERSPEED:
t.mode = TECS_MODE_UNDERSPEED;
break;
case TECS::ECL_TECS_MODE_BAD_DESCENT:
t.mode = TECS_MODE_BAD_DESCENT;
break;
case TECS::ECL_TECS_MODE_CLIMBOUT:
t.mode = TECS_MODE_CLIMBOUT;
break;
}
t.altitudeSp = s.hgt_dem;
t.altitude_filtered = s.hgt;
t.airspeedSp = s.spd_dem;
t.airspeed_filtered = s.spd;
t.flightPathAngleSp = s.dhgt_dem;
t.flightPathAngle = s.dhgt;
t.flightPathAngleFiltered = s.dhgt;
t.airspeedDerivativeSp = s.dspd_dem;
t.airspeedDerivative = s.dspd;
t.totalEnergyRateSp = s.thr;
t.totalEnergyRate = s.ithr;
t.energyDistributionRateSp = s.ptch;
t.energyDistributionRate = s.iptch;
if (_tecs_status_pub > 0) {
orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t);
} else {
_tecs_status_pub = orb_advertise(ORB_ID(tecs_status), &t);
}
}
}

6
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
View File

@ -109,8 +109,7 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
/* Write part of the status message */
_status.altitudeSp = altitudeSp;
_status.altitude = altitude;
_status.altitudeFiltered = altitudeFiltered;
_status.altitude_filtered = altitudeFiltered;
/* use flightpath angle setpoint for total energy control */
@ -146,8 +145,7 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
/* Write part of the status message */
_status.airspeedSp = airspeedSp;
_status.airspeed = airspeed;
_status.airspeedFiltered = airspeedFiltered;
_status.airspeed_filtered = airspeedFiltered;
/* use longitudinal acceleration setpoint for total energy control */

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

@ -1597,14 +1597,12 @@ int sdlog2_thread_main(int argc, char *argv[])
if (copy_if_updated(ORB_ID(tecs_status), subs.tecs_status_sub, &buf.tecs_status)) {
log_msg.msg_type = LOG_TECS_MSG;
log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp;
log_msg.body.log_TECS.altitude = buf.tecs_status.altitude;
log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitudeFiltered;
log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered;
log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp;
log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle;
log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered;
log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp;
log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed;
log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered;
log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeed_filtered;
log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp;
log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative;
log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp;

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

@ -333,13 +333,11 @@ struct log_GS1B_s {
#define LOG_TECS_MSG 30
struct log_TECS_s {
float altitudeSp;
float altitude;
float altitudeFiltered;
float flightPathAngleSp;
float flightPathAngle;
float flightPathAngleFiltered;
float airspeedSp;
float airspeed;
float airspeedFiltered;
float airspeedDerivativeSp;
float airspeedDerivative;
@ -455,7 +453,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(TECS, "fffffffffffffffB", "ASP,A,AF,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"),
/* system-level messages, ID >= 0x80 */

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

@ -51,11 +51,13 @@
*/
typedef enum {
TECS_MODE_NORMAL,
TECS_MODE_NORMAL = 0,
TECS_MODE_UNDERSPEED,
TECS_MODE_TAKEOFF,
TECS_MODE_LAND,
TECS_MODE_LAND_THROTTLELIM
TECS_MODE_LAND_THROTTLELIM,
TECS_MODE_BAD_DESCENT,
TECS_MODE_CLIMBOUT
} tecs_mode;
/**
@ -65,14 +67,12 @@ struct tecs_status_s {
uint64_t timestamp; /**< timestamp, in microseconds since system start */
float altitudeSp;
float altitude;
float altitudeFiltered;
float altitude_filtered;
float flightPathAngleSp;
float flightPathAngle;
float flightPathAngleFiltered;
float airspeedSp;
float airspeed;
float airspeedFiltered;
float airspeed_filtered;
float airspeedDerivativeSp;
float airspeedDerivative;

41
src/modules/uavcan/uavcan_main.cpp
View File

@ -253,7 +253,6 @@ int UavcanNode::run()
// XXX figure out the output count
_output_count = 2;
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
actuator_outputs_s outputs;
@ -273,21 +272,23 @@ int UavcanNode::run()
_node.setStatusOk();
while (!_task_should_exit) {
/*
* This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
* Please note that with such multiplexing it is no longer possible to rely only on
* the value returned from poll() to detect whether actuator control has timed out or not.
* Instead, all ORB events need to be checked individually (see below).
*/
_poll_fds_num = 0;
_poll_fds[_poll_fds_num] = ::pollfd();
_poll_fds[_poll_fds_num].fd = busevent_fd;
_poll_fds[_poll_fds_num].events = POLLIN;
_poll_fds_num += 1;
while (!_task_should_exit) {
// update actuator controls subscriptions if needed
if (_groups_subscribed != _groups_required) {
subscribe();
_groups_subscribed = _groups_required;
/*
* This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
* Please note that with such multiplexing it is no longer possible to rely only on
* the value returned from poll() to detect whether actuator control has timed out or not.
* Instead, all ORB events need to be checked individually (see below).
*/
_poll_fds[_poll_fds_num] = ::pollfd();
_poll_fds[_poll_fds_num].fd = busevent_fd;
_poll_fds[_poll_fds_num].events = POLLIN;
_poll_fds_num += 1;
}
const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
@ -301,7 +302,7 @@ int UavcanNode::run()
} else {
// get controls for required topics
bool controls_updated = false;
unsigned poll_id = 0;
unsigned poll_id = 1;
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
if (_poll_fds[poll_id].revents & POLLIN) {
@ -312,12 +313,7 @@ int UavcanNode::run()
}
}
if (!controls_updated) {
// timeout: no control data, switch to failsafe values
// XXX trigger failsafe
}
//can we mix?
// can we mix?
if (controls_updated && (_mixers != nullptr)) {
// XXX one output group has 8 outputs max,
@ -417,7 +413,8 @@ UavcanNode::subscribe()
// Subscribe/unsubscribe to required actuator control groups
uint32_t sub_groups = _groups_required & ~_groups_subscribed;
uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
_poll_fds_num = 0;
// the first fd used by CAN
_poll_fds_num = 1;
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (sub_groups & (1 << i)) {
warnx("subscribe to actuator_controls_%d", i);
@ -523,8 +520,8 @@ UavcanNode::print_info()
warnx("not running, start first");
}
warnx("groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
warnx("mixer: %s", (_mixers == nullptr) ? "FAIL" : "OK");
warnx("actuators control groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK");
}
/*