Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 15:40:31 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

Merged seatbelt_multirotor_new

Browse Source
This commit is contained in:
Lorenz Meier
2013-08-23 08:33:42 +02:00
parent 5f1004117f 330908225e
commit 29b926db1b
7 changed files with 78 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+2 -2
View File
@@ -799,9 +799,9 @@ int commander_thread_main(int argc, char *argv[])
status.condition_landed = local_position.landed;
status_changed = true;
if (status.condition_landed) {
mavlink_log_info(mavlink_fd, "[cmd] LANDED");
mavlink_log_critical(mavlink_fd, "[cmd] LANDED");
} else {
mavlink_log_info(mavlink_fd, "[cmd] IN AIR");
mavlink_log_critical(mavlink_fd, "[cmd] IN AIR");
}
}
}
src/modules/mavlink/orb_listener.c
+4 -4
View File
@@ -666,12 +666,12 @@ l_airspeed(const struct listener *l)
orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed);
float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f;
float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1);
float alt = global_pos.alt;
float climb = global_pos.vz;
float alt = global_pos.relative_alt;
float climb = -global_pos.vz;
mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed,
((att.yaw + M_PI_F) / M_PI_F) * 180.0f, throttle, alt, climb);
mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb);
}
static void *
src/modules/multirotor_att_control/multirotor_att_control_main.c
+58 -42
View File
@@ -81,6 +81,8 @@ __EXPORT int multirotor_att_control_main(int argc, char *argv[]);
static bool thread_should_exit;
static int mc_task;
static bool motor_test_mode = false;
static const float min_takeoff_throttle = 0.3f;
static const float yaw_deadzone = 0.01f;
static int
mc_thread_main(int argc, char *argv[])
@@ -144,13 +146,17 @@ mc_thread_main(int argc, char *argv[])
/* welcome user */
warnx("starting");
/* store last control mode to detect mode switches */
bool flag_control_manual_enabled = false;
bool flag_control_attitude_enabled = false;
bool control_yaw_position = true;
bool reset_yaw_sp = true;
bool failsafe_first_time = true;
/* prepare the handle for the failsafe throttle */
param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR");
float failsafe_throttle = 0.0f;
param_get(failsafe_throttle_handle, &failsafe_throttle);
while (!thread_should_exit) {
@@ -172,7 +178,7 @@ mc_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(parameter_update), param_sub, &update);
/* update parameters */
// XXX no params here yet
param_get(failsafe_throttle_handle, &failsafe_throttle);
}
/* only run controller if attitude changed */
@@ -204,6 +210,9 @@ mc_thread_main(int argc, char *argv[])
/* get a local copy of the current sensor values */
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
/* set flag to safe value */
control_yaw_position = true;
/* define which input is the dominating control input */
if (control_mode.flag_control_offboard_enabled) {
/* offboard inputs */
@@ -221,41 +230,41 @@ mc_thread_main(int argc, char *argv[])
att_sp.yaw_body = offboard_sp.p3;
att_sp.thrust = offboard_sp.p4;
att_sp.timestamp = hrt_absolute_time();
/* STEP 2: publish the result to the vehicle actuators */
/* publish the result to the vehicle actuators */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
} else if (control_mode.flag_control_manual_enabled) {
/* direct manual input */
/* manual input */
if (control_mode.flag_control_attitude_enabled) {
static bool rc_loss_first_time = true;
/* control attitude, update attitude setpoint depending on mode */
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
if (control_mode.failsave_highlevel) {
failsafe_first_time = false;
if (!control_mode.flag_control_velocity_enabled) {
/* Don't reset attitude setpoint in position control mode, it's handled by position controller. */
/* don't reset attitude setpoint in position control mode, it's handled by position controller. */
att_sp.roll_body = 0.0f;
att_sp.pitch_body = 0.0f;
}
if (!control_mode.flag_control_climb_rate_enabled) {
/* Don't touch throttle in modes with altitude hold, it's handled by position controller.
*
* Only go to failsafe throttle if last known throttle was
* high enough to create some lift to make hovering state likely.
*
* This is to prevent that someone landing, but not disarming his
* multicopter (throttle = 0) does not make it jump up in the air
* if shutting down his remote.
*/
if (isfinite(manual.throttle) && manual.throttle > 0.2f) { // TODO use landed status instead of throttle
/* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */
param_get(failsafe_throttle_handle, &failsafe_throttle);
att_sp.thrust = failsafe_throttle;
if (!control_mode.flag_control_climb_rate_enabled) {
/* don't touch throttle in modes with altitude hold, it's handled by position controller.
*
* Only go to failsafe throttle if last known throttle was
* high enough to create some lift to make hovering state likely.
*
* This is to prevent that someone landing, but not disarming his
* multicopter (throttle = 0) does not make it jump up in the air
* if shutting down his remote.
*/
if (isfinite(manual.throttle) && manual.throttle > min_takeoff_throttle) { // TODO use landed status instead of throttle
/* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */
att_sp.thrust = failsafe_throttle;
} else {
att_sp.thrust = 0.0f;
}
} else {
att_sp.thrust = 0.0f;
}
}
@@ -263,46 +272,50 @@ mc_thread_main(int argc, char *argv[])
* since if the pilot regains RC control, he will be lost regarding
* the current orientation.
*/
if (rc_loss_first_time)
att_sp.yaw_body = att.yaw;
rc_loss_first_time = false;
if (failsafe_first_time) {
reset_yaw_sp = true;
}
} else {
rc_loss_first_time = true;
failsafe_first_time = true;
/* control yaw in all manual / assisted modes */
/* set yaw if arming or switching to attitude stabilized mode */
if (!control_mode.flag_armed) {
if (!flag_control_manual_enabled || !flag_control_attitude_enabled || !control_mode.flag_armed) {
reset_yaw_sp = true;
}
/* only move setpoint if manual input is != 0 */
if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.1f) { // TODO use landed status instead of throttle
rates_sp.yaw = manual.yaw;
// TODO review yaw restpoint reset
if ((manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) && manual.throttle > min_takeoff_throttle) {
/* control yaw rate */
control_yaw_position = false;
reset_yaw_sp = true;
rates_sp.yaw = manual.yaw;
reset_yaw_sp = true; // has no effect on control, just for beautiful log
} else {
if (reset_yaw_sp) {
att_sp.yaw_body = att.yaw;
reset_yaw_sp = false;
}
control_yaw_position = true;
}
if (!control_mode.flag_control_velocity_enabled) {
/* don't update attitude setpoint in position control mode */
/* update attitude setpoint if not in position control mode */
att_sp.roll_body = manual.roll;
att_sp.pitch_body = manual.pitch;
if (!control_mode.flag_control_climb_rate_enabled) {
/* don't set throttle in altitude hold modes */
/* pass throttle directly if not in altitude control mode */
att_sp.thrust = manual.throttle;
}
}
att_sp.timestamp = hrt_absolute_time();
}
/* reset yaw setpint to current position if needed */
if (reset_yaw_sp) {
att_sp.yaw_body = att.yaw;
reset_yaw_sp = false;
}
if (motor_test_mode) {
@@ -311,10 +324,11 @@ mc_thread_main(int argc, char *argv[])
att_sp.pitch_body = 0.0f;
att_sp.yaw_body = 0.0f;
att_sp.thrust = 0.1f;
att_sp.timestamp = hrt_absolute_time();
}
/* STEP 2: publish the controller output */
att_sp.timestamp = hrt_absolute_time();
/* publish the controller output */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
} else {
@@ -357,6 +371,7 @@ mc_thread_main(int argc, char *argv[])
rates[1] = att.pitchspeed;
rates[2] = att.yawspeed;
multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral);
} else {
/* rates controller disabled, set actuators to zero for safety */
actuators.control[0] = 0.0f;
@@ -364,6 +379,7 @@ mc_thread_main(int argc, char *argv[])
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);
src/modules/position_estimator_inav/position_estimator_inav_main.c
+1 -1
View File
@@ -609,7 +609,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (local_pos.v_xy_valid) {
global_pos.vx = local_pos.vx;
global_pos.vy = local_pos.vy;
global_pos.hdg = atan2f(local_pos.vy, local_pos.vx);
global_pos.hdg = atan2f(local_pos.vy, local_pos.vx); // TODO is it correct?
}
if (local_pos.z_valid) {
src/modules/sdlog2/sdlog2.c
+2
View File
@@ -1182,6 +1182,8 @@ int sdlog2_thread_main(int argc, char *argv[])
pthread_mutex_destroy(&logbuffer_mutex);
pthread_cond_destroy(&logbuffer_cond);
free(lb.data);
warnx("exiting.");
thread_running = false;
src/modules/sensors/sensor_params.c
+2 -2
View File
@@ -174,8 +174,8 @@ PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5);
PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 6);
PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 6);
PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0);
//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
src/modules/uORB/topics/vehicle_global_position.h
+9 -9
View File
@@ -62,17 +62,17 @@
struct vehicle_global_position_s
{
uint64_t timestamp; /**< time of this estimate, in microseconds since system start */
uint64_t time_gps_usec; /**< GPS timestamp in microseconds */
uint64_t time_gps_usec; /**< GPS timestamp in microseconds */
bool valid; /**< true if position satisfies validity criteria of estimator */
int32_t lat; /**< Latitude in 1E7 degrees LOGME */
int32_t lon; /**< Longitude in 1E7 degrees LOGME */
float alt; /**< Altitude in meters LOGME */
float relative_alt; /**< Altitude above home position in meters, LOGME */
float vx; /**< Ground X Speed (Latitude), m/s in ENU LOGME */
float vy; /**< Ground Y Speed (Longitude), m/s in ENU LOGME */
float vz; /**< Ground Z Speed (Altitude), m/s in ENU LOGME */
float hdg; /**< Compass heading in radians -PI..+PI. */
int32_t lat; /**< Latitude in 1E7 degrees */
int32_t lon; /**< Longitude in 1E7 degrees */
float alt; /**< Altitude in meters */
float relative_alt; /**< Altitude above home position in meters, */
float vx; /**< Ground X velocity, m/s in NED */
float vy; /**< Ground Y velocity, m/s in NED */
float vz; /**< Ground Z velocity, m/s in NED */
float hdg; /**< Compass heading in radians -PI..+PI. */
};