fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 12:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged beta into mavlink rework branch

Browse Source
This commit is contained in:
Lorenz Meier
2014-01-28 15:13:14 +01:00
parent ac77fe9c27
commit 9c355d280e
220 changed files with 10717 additions and 9729 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+57 -67
View File
@@ -53,21 +53,6 @@ static const int8_t ret_error = -1; // error occurred
KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
// ekf matrices
F(9, 9),
G(9, 6),
P(9, 9),
P0(9, 9),
V(6, 6),
// attitude measurement ekf matrices
HAtt(4, 9),
RAtt(4, 4),
// position measurement ekf matrices
HPos(6, 9),
RPos(6, 6),
// attitude representations
C_nb(),
q(),
// subscriptions
_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz
@@ -112,8 +97,17 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
{
using namespace math;
F.zero();
G.zero();
V.zero();
HAtt.zero();
RAtt.zero();
HPos.zero();
RPos.zero();
// initial state covariance matrix
P0 = Matrix::identity(9) * 0.01f;
P0.identity();
P0 *= 0.01f;
P = P0;
// initial state
@@ -138,7 +132,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
_sensors.magnetometer_ga[2]);
// initialize dcm
C_nb = Dcm(q);
C_nb = q.to_dcm();
// HPos is constant
HPos(0, 3) = 1.0f;
@@ -228,8 +222,8 @@ void KalmanNav::update()
if (correctAtt() == ret_ok) _attitudeInitCounter++;
if (_attitudeInitCounter > 100) {
warnx("initialized EKF attitude\n");
warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f\n",
warnx("initialized EKF attitude");
warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f",
double(phi), double(theta), double(psi));
_attitudeInitialized = true;
}
@@ -259,8 +253,8 @@ void KalmanNav::update()
// lat/lon and not have init
map_projection_init(lat0, lon0);
_positionInitialized = true;
warnx("initialized EKF state with GPS\n");
warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
warnx("initialized EKF state with GPS");
warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f",
double(vN), double(vE), double(vD),
lat, lon, double(alt));
}
@@ -321,13 +315,12 @@ void KalmanNav::updatePublications()
_pos.timestamp = _pubTimeStamp;
_pos.time_gps_usec = _gps.timestamp_position;
_pos.valid = true;
_pos.lat = getLatDegE7();
_pos.lon = getLonDegE7();
_pos.lat = lat * M_RAD_TO_DEG;
_pos.lon = lon * M_RAD_TO_DEG;
_pos.alt = float(alt);
_pos.relative_alt = float(alt); // TODO, make relative
_pos.vx = vN;
_pos.vy = vE;
_pos.vz = vD;
_pos.vel_n = vN;
_pos.vel_e = vE;
_pos.vel_d = vD;
_pos.yaw = psi;
// local position publication
@@ -404,28 +397,28 @@ int KalmanNav::predictState(float dt)
// attitude prediction
if (_attitudeInitialized) {
Vector3 w(_sensors.gyro_rad_s);
Vector<3> w(_sensors.gyro_rad_s);
// attitude
q = q + q.derivative(w) * dt;
// renormalize quaternion if needed
if (fabsf(q.norm() - 1.0f) > 1e-4f) {
q = q.unit();
if (fabsf(q.length() - 1.0f) > 1e-4f) {
q.normalize();
}
// C_nb update
C_nb = Dcm(q);
C_nb = q.to_dcm();
// euler update
EulerAngles euler(C_nb);
phi = euler.getPhi();
theta = euler.getTheta();
psi = euler.getPsi();
Vector<3> euler = C_nb.to_euler();
phi = euler.data[0];
theta = euler.data[1];
psi = euler.data[2];
// specific acceleration in nav frame
Vector3 accelB(_sensors.accelerometer_m_s2);
Vector3 accelN = C_nb * accelB;
Vector<3> accelB(_sensors.accelerometer_m_s2);
Vector<3> accelN = C_nb * accelB;
fN = accelN(0);
fE = accelN(1);
fD = accelN(2);
@@ -549,10 +542,10 @@ int KalmanNav::predictStateCovariance(float dt)
G(5, 4) = C_nb(2, 1);
G(5, 5) = C_nb(2, 2);
// continuous predictioon equations
// for discrte time EKF
// continuous prediction equations
// for discrete time EKF
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * dt;
P = P + (F * P + P * F.transposed() + G * V * G.transposed()) * dt;
return ret_ok;
}
@@ -577,13 +570,14 @@ int KalmanNav::correctAtt()
// compensate roll and pitch, but not yaw
// XXX take the vectors out of the C_nb matrix to avoid singularities
math::Dcm C_rp(math::EulerAngles(phi, theta, 0.0f));//C_nb.transpose();
math::Matrix<3,3> C_rp;
C_rp.from_euler(phi, theta, 0.0f);//C_nb.transposed();
// mag measurement
Vector3 magBody(_sensors.magnetometer_ga);
Vector<3> magBody(_sensors.magnetometer_ga);
// transform to earth frame
Vector3 magNav = C_rp * magBody;
Vector<3> magNav = C_rp * magBody;
// calculate error between estimate and measurement
// apply declination correction for true heading as well.
@@ -592,12 +586,12 @@ int KalmanNav::correctAtt()
if (yMag < -M_PI_F) yMag += 2*M_PI_F;
// accel measurement
Vector3 zAccel(_sensors.accelerometer_m_s2);
float accelMag = zAccel.norm();
zAccel = zAccel.unit();
Vector<3> zAccel(_sensors.accelerometer_m_s2);
float accelMag = zAccel.length();
zAccel.normalize();
// ignore accel correction when accel mag not close to g
Matrix RAttAdjust = RAtt;
Matrix<4,4> RAttAdjust = RAtt;
bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f;
@@ -611,14 +605,10 @@ int KalmanNav::correctAtt()
}
// accel predicted measurement
Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit();
Vector<3> zAccelHat = (C_nb.transposed() * Vector<3>(0, 0, -_g.get())).normalized();
// calculate residual
Vector y(4);
y(0) = yMag;
y(1) = zAccel(0) - zAccelHat(0);
y(2) = zAccel(1) - zAccelHat(1);
y(3) = zAccel(2) - zAccelHat(2);
Vector<4> y(yMag, zAccel(0) - zAccelHat(0), zAccel(1) - zAccelHat(1), zAccel(2) - zAccelHat(2));
// HMag
HAtt(0, 2) = 1;
@@ -632,17 +622,17 @@ int KalmanNav::correctAtt()
// compute correction
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance
Matrix K = P * HAtt.transpose() * S.inverse();
Vector xCorrect = K * y;
Matrix<4, 4> S = HAtt * P * HAtt.transposed() + RAttAdjust; // residual covariance
Matrix<9, 4> K = P * HAtt.transposed() * S.inversed();
Vector<9> xCorrect = K * y;
// check correciton is sane
for (size_t i = 0; i < xCorrect.getRows(); i++) {
for (size_t i = 0; i < xCorrect.get_size(); i++) {
float val = xCorrect(i);
if (isnan(val) || isinf(val)) {
// abort correction and return
warnx("numerical failure in att correction\n");
warnx("numerical failure in att correction");
// reset P matrix to P0
P = P0;
return ret_error;
@@ -669,7 +659,7 @@ int KalmanNav::correctAtt()
P = P - K * HAtt * P;
// fault detection
float beta = y.dot(S.inverse() * y);
float beta = y * (S.inversed() * y);
if (beta > _faultAtt.get()) {
warnx("fault in attitude: beta = %8.4f", (double)beta);
@@ -678,7 +668,7 @@ int KalmanNav::correctAtt()
// update quaternions from euler
// angle correction
q = Quaternion(EulerAngles(phi, theta, psi));
q.from_euler(phi, theta, psi);
return ret_ok;
}
@@ -688,7 +678,7 @@ int KalmanNav::correctPos()
using namespace math;
// residual
Vector y(6);
Vector<6> y;
y(0) = _gps.vel_n_m_s - vN;
y(1) = _gps.vel_e_m_s - vE;
y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG;
@@ -698,17 +688,17 @@ int KalmanNav::correctPos()
// compute correction
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance
Matrix K = P * HPos.transpose() * S.inverse();
Vector xCorrect = K * y;
Matrix<6,6> S = HPos * P * HPos.transposed() + RPos; // residual covariance
Matrix<9,6> K = P * HPos.transposed() * S.inversed();
Vector<9> xCorrect = K * y;
// check correction is sane
for (size_t i = 0; i < xCorrect.getRows(); i++) {
for (size_t i = 0; i < xCorrect.get_size(); i++) {
float val = xCorrect(i);
if (!isfinite(val)) {
// abort correction and return
warnx("numerical failure in gps correction\n");
warnx("numerical failure in gps correction");
// fallback to GPS
vN = _gps.vel_n_m_s;
vE = _gps.vel_e_m_s;
@@ -735,7 +725,7 @@ int KalmanNav::correctPos()
P = P - K * HPos * P;
// fault detetcion
float beta = y.dot(S.inverse() * y);
float beta = y * (S.inversed() * y);
static int counter = 0;
if (beta > _faultPos.get() && (counter % 10 == 0)) {
src/modules/att_pos_estimator_ekf/KalmanNav.hpp
+10 -10
View File
@@ -125,17 +125,17 @@ public:
virtual void updateParams();
protected:
// kalman filter
math::Matrix F; /**< Jacobian(f,x), where dx/dt = f(x,u) */
math::Matrix G; /**< noise shaping matrix for gyro/accel */
math::Matrix P; /**< state covariance matrix */
math::Matrix P0; /**< initial state covariance matrix */
math::Matrix V; /**< gyro/ accel noise matrix */
math::Matrix HAtt; /**< attitude measurement matrix */
math::Matrix RAtt; /**< attitude measurement noise matrix */
math::Matrix HPos; /**< position measurement jacobian matrix */
math::Matrix RPos; /**< position measurement noise matrix */
math::Matrix<9,9> F; /**< Jacobian(f,x), where dx/dt = f(x,u) */
math::Matrix<9,6> G; /**< noise shaping matrix for gyro/accel */
math::Matrix<9,9> P; /**< state covariance matrix */
math::Matrix<9,9> P0; /**< initial state covariance matrix */
math::Matrix<6,6> V; /**< gyro/ accel noise matrix */
math::Matrix<4,9> HAtt; /**< attitude measurement matrix */
math::Matrix<4,4> RAtt; /**< attitude measurement noise matrix */
math::Matrix<6,9> HPos; /**< position measurement jacobian matrix */
math::Matrix<6,6> RPos; /**< position measurement noise matrix */
// attitude
math::Dcm C_nb; /**< direction cosine matrix from body to nav frame */
math::Matrix<3,3> C_nb; /**< direction cosine matrix from body to nav frame */
math::Quaternion q; /**< quaternion from body to nav frame */
// subscriptions
control::UOrbSubscription<sensor_combined_s> _sensors; /**< sensors sub. */
src/modules/att_pos_estimator_ekf/kalman_main.cpp
+1 -1
View File
@@ -107,7 +107,7 @@ int att_pos_estimator_ekf_main(int argc, char *argv[])
daemon_task = task_spawn_cmd("att_pos_estimator_ekf",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 30,
4096,
8192,
kalman_demo_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+105 -10
View File
@@ -58,9 +58,13 @@
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>
#include <lib/mathlib/mathlib.h>
#include <systemlib/systemlib.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
@@ -214,6 +218,10 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
struct sensor_combined_s raw;
memset(&raw, 0, sizeof(raw));
struct vehicle_gps_position_s gps;
memset(&gps, 0, sizeof(gps));
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_control_mode_s control_mode;
@@ -221,12 +229,32 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
uint64_t last_data = 0;
uint64_t last_measurement = 0;
uint64_t last_vel_t = 0;
/* current velocity */
math::Vector<3> vel;
vel.zero();
/* previous velocity */
math::Vector<3> vel_prev;
vel_prev.zero();
/* actual acceleration (by GPS velocity) in body frame */
math::Vector<3> acc;
acc.zero();
/* rotation matrix */
math::Matrix<3, 3> R;
R.identity();
/* subscribe to raw data */
int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
/* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */
orb_set_interval(sub_raw, 3);
/* subscribe to GPS */
int sub_gps = orb_subscribe(ORB_ID(vehicle_gps_position));
/* subscribe to GPS */
int sub_global_pos = orb_subscribe(ORB_ID(vehicle_global_position));
/* subscribe to param changes */
int sub_params = orb_subscribe(ORB_ID(parameter_update));
@@ -265,6 +293,10 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
unsigned offset_count = 0;
/* rotation matrix for magnetic declination */
math::Matrix<3, 3> R_decl;
R_decl.identity();
/* register the perf counter */
perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf");
@@ -299,6 +331,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* update parameters */
parameters_update(&ekf_param_handles, &ekf_params);
/* update mag declination rotation matrix */
R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl);
}
/* only run filter if sensor values changed */
@@ -307,6 +342,18 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* get latest measurements */
orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);
bool gps_updated;
orb_check(sub_gps, &gps_updated);
if (gps_updated) {
orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps);
}
bool global_pos_updated;
orb_check(sub_global_pos, &global_pos_updated);
if (global_pos_updated) {
orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos);
}
if (!initialized) {
// XXX disabling init for now
initialized = true;
@@ -352,9 +399,50 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
sensor_last_timestamp[1] = raw.timestamp;
}
z_k[3] = raw.accelerometer_m_s2[0];
z_k[4] = raw.accelerometer_m_s2[1];
z_k[5] = raw.accelerometer_m_s2[2];
hrt_abstime vel_t = 0;
bool vel_valid = false;
if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph_m < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) {
vel_valid = true;
if (gps_updated) {
vel_t = gps.timestamp_velocity;
vel(0) = gps.vel_n_m_s;
vel(1) = gps.vel_e_m_s;
vel(2) = gps.vel_d_m_s;
}
} else if (ekf_params.acc_comp == 2 && global_pos.valid && hrt_absolute_time() < global_pos.timestamp + 500000) {
vel_valid = true;
if (global_pos_updated) {
vel_t = global_pos.timestamp;
vel(0) = global_pos.vel_n;
vel(1) = global_pos.vel_e;
vel(2) = global_pos.vel_d;
}
}
if (vel_valid) {
/* velocity is valid */
if (vel_t != 0) {
/* velocity updated */
if (last_vel_t != 0 && vel_t != last_vel_t) {
float vel_dt = (vel_t - last_vel_t) / 1000000.0f;
/* calculate acceleration in body frame */
acc = R.transposed() * ((vel - vel_prev) / vel_dt);
}
last_vel_t = vel_t;
vel_prev = vel;
}
} else {
/* velocity is valid, reset acceleration */
acc.zero();
vel_prev.zero();
last_vel_t = 0;
}
z_k[3] = raw.accelerometer_m_s2[0] - acc(0);
z_k[4] = raw.accelerometer_m_s2[1] - acc(1);
z_k[5] = raw.accelerometer_m_s2[2] - acc(2);
/* update magnetometer measurements */
if (sensor_last_count[2] != raw.magnetometer_counter) {
@@ -425,7 +513,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
continue;
}
if (last_data > 0 && raw.timestamp - last_data > 12000)
if (last_data > 0 && raw.timestamp - last_data > 30000)
printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);
last_data = raw.timestamp;
@@ -433,10 +521,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* send out */
att.timestamp = raw.timestamp;
// XXX Apply the same transformation to the rotation matrix
att.roll = euler[0] - ekf_params.roll_off;
att.pitch = euler[1] - ekf_params.pitch_off;
att.yaw = euler[2] - ekf_params.yaw_off;
att.roll = euler[0];
att.pitch = euler[1];
att.yaw = euler[2] + ekf_params.mag_decl;
att.rollspeed = x_aposteriori[0];
att.pitchspeed = x_aposteriori[1];
@@ -445,12 +532,20 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
att.pitchacc = x_aposteriori[4];
att.yawacc = x_aposteriori[5];
//att.yawspeed =z_k[2] ;
att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0);
att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1);
att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2);
/* copy offsets */
memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
/* magnetic declination */
math::Matrix<3, 3> R_body = (&Rot_matrix[0]);
R = R_decl * R_body;
/* copy rotation matrix */
memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix));
memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R));
att.R_valid = true;
if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+13
View File
@@ -65,6 +65,11 @@ PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f);
PARAM_DEFINE_FLOAT(ATT_PITCH_OFF3, 0.0f);
PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f);
/* magnetic declination, in degrees */
PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
PARAM_DEFINE_INT32(ATT_ACC_COMP, 0);
int parameters_init(struct attitude_estimator_ekf_param_handles *h)
{
/* PID parameters */
@@ -83,6 +88,10 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
h->pitch_off = param_find("ATT_PITCH_OFF3");
h->yaw_off = param_find("ATT_YAW_OFF3");
h->mag_decl = param_find("ATT_MAG_DECL");
h->acc_comp = param_find("ATT_ACC_COMP");
return OK;
}
@@ -103,5 +112,9 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
param_get(h->pitch_off, &(p->pitch_off));
param_get(h->yaw_off, &(p->yaw_off));
param_get(h->mag_decl, &(p->mag_decl));
param_get(h->acc_comp, &(p->acc_comp));
return OK;
}
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
+4
View File
@@ -47,12 +47,16 @@ struct attitude_estimator_ekf_params {
float roll_off;
float pitch_off;
float yaw_off;
float mag_decl;
int acc_comp;
};
struct attitude_estimator_ekf_param_handles {
param_t r0, r1, r2, r3;
param_t q0, q1, q2, q3, q4;
param_t roll_off, pitch_off, yaw_off;
param_t mag_decl;
param_t acc_comp;
};
/**
src/modules/commander/accelerometer_calibration.cpp
+6 -8
View File
@@ -194,15 +194,13 @@ int do_accel_calibration(int mavlink_fd)
int32_t board_rotation_int;
param_get(board_rotation_h, &(board_rotation_int));
enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
math::Matrix board_rotation(3, 3);
math::Matrix<3,3> board_rotation;
get_rot_matrix(board_rotation_id, &board_rotation);
math::Matrix board_rotation_t = board_rotation.transpose();
math::Vector3 accel_offs_vec;
accel_offs_vec.set(&accel_offs[0]);
math::Vector3 accel_offs_rotated = board_rotation_t * accel_offs_vec;
math::Matrix accel_T_mat(3, 3);
accel_T_mat.set(&accel_T[0][0]);
math::Matrix accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
math::Matrix<3,3> board_rotation_t = board_rotation.transposed();
math::Vector<3> accel_offs_vec(&accel_offs[0]);
math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec;
math::Matrix<3,3> accel_T_mat(&accel_T[0][0]);
math::Matrix<3,3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
accel_scale.x_offset = accel_offs_rotated(0);
accel_scale.x_scale = accel_T_rotated(0, 0);
src/modules/commander/commander.cpp
+229 -186
View File
@@ -106,14 +106,9 @@ static const int ERROR = -1;
extern struct system_load_s system_load;
#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
/* Decouple update interval and hysteris counters, all depends on intervals */
#define COMMANDER_MONITORING_INTERVAL 50000
#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f))
#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define MAVLINK_OPEN_INTERVAL 50000
@@ -204,9 +199,9 @@ void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actua
void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status);
void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status);
transition_result_t check_main_state_machine(struct vehicle_status_s *current_status);
transition_result_t set_main_state_rc(struct vehicle_status_s *status);
void print_reject_mode(const char *msg);
@@ -376,6 +371,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state");
}
}
if (hil_ret == OK)
ret = true;
@@ -410,6 +406,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
arming_res = TRANSITION_NOT_CHANGED;
}
}
if (arming_res == TRANSITION_CHANGED)
ret = true;
@@ -452,6 +449,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
}
}
if (main_res == TRANSITION_CHANGED)
ret = true;
@@ -491,8 +489,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
break;
case VEHICLE_CMD_OVERRIDE_GOTO: {
// TODO listen vehicle_command topic directly from navigator (?)
// TODO listen vehicle_command topic directly from navigator (?)
unsigned int mav_goto = cmd->param1;
if (mav_goto == 0) { // MAV_GOTO_DO_HOLD
status->set_nav_state = NAV_STATE_LOITER;
status->set_nav_state_timestamp = hrt_absolute_time();
@@ -513,11 +512,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
break;
/* Flight termination */
/* Flight termination */
case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON);
transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
result = VEHICLE_CMD_RESULT_ACCEPTED;
ret = true;
@@ -566,7 +565,6 @@ int commander_thread_main(int argc, char *argv[])
{
/* not yet initialized */
commander_initialized = false;
bool home_position_set = false;
bool battery_tune_played = false;
bool arm_tune_played = false;
@@ -580,6 +578,27 @@ int commander_thread_main(int argc, char *argv[])
/* welcome user */
warnx("starting");
char *main_states_str[MAIN_STATE_MAX];
main_states_str[0] = "MANUAL";
main_states_str[1] = "SEATBELT";
main_states_str[2] = "EASY";
main_states_str[3] = "AUTO";
char *arming_states_str[ARMING_STATE_MAX];
arming_states_str[0] = "INIT";
arming_states_str[1] = "STANDBY";
arming_states_str[2] = "ARMED";
arming_states_str[3] = "ARMED_ERROR";
arming_states_str[4] = "STANDBY_ERROR";
arming_states_str[5] = "REBOOT";
arming_states_str[6] = "IN_AIR_RESTORE";
char *failsafe_states_str[FAILSAFE_STATE_MAX];
failsafe_states_str[0] = "NORMAL";
failsafe_states_str[1] = "RTL";
failsafe_states_str[2] = "LAND";
failsafe_states_str[3] = "TERMINATION";
/* pthread for slow low prio thread */
pthread_t commander_low_prio_thread;
@@ -609,6 +628,7 @@ int commander_thread_main(int argc, char *argv[])
status.set_nav_state_timestamp = 0;
status.arming_state = ARMING_STATE_INIT;
status.hil_state = HIL_STATE_OFF;
status.failsafe_state = FAILSAFE_STATE_NORMAL;
/* neither manual nor offboard control commands have been received */
status.offboard_control_signal_found_once = false;
@@ -666,8 +686,6 @@ int commander_thread_main(int argc, char *argv[])
/* Start monitoring loop */
unsigned counter = 0;
unsigned low_voltage_counter = 0;
unsigned critical_voltage_counter = 0;
unsigned stick_off_counter = 0;
unsigned stick_on_counter = 0;
@@ -745,7 +763,6 @@ int commander_thread_main(int argc, char *argv[])
int battery_sub = orb_subscribe(ORB_ID(battery_status));
struct battery_status_s battery;
memset(&battery, 0, sizeof(battery));
battery.voltage_v = 0.0f;
/* Subscribe to subsystem info topic */
int subsys_sub = orb_subscribe(ORB_ID(subsystem_info));
@@ -889,13 +906,12 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
// warnx("bat v: %2.2f", battery.voltage_v);
/* only consider battery voltage if system has been running 2s and battery voltage is higher than 4V */
if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 4.0f) {
status.battery_voltage = battery.voltage_v;
/* 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) {
status.battery_voltage = battery.voltage_filtered_v;
status.battery_current = battery.current_a;
status.condition_battery_voltage_valid = true;
status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage);
status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah);
}
}
@@ -948,46 +964,29 @@ int commander_thread_main(int argc, char *argv[])
//on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
}
// XXX remove later
//warnx("bat remaining: %2.2f", status.battery_remaining);
/* if battery voltage is getting lower, warn using buzzer, etc. */
if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) {
//TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
low_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
battery_tune_played = false;
}
low_voltage_counter++;
low_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
battery_tune_played = false;
} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
/* critical battery voltage, this is rather an emergency, change state machine */
if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
critical_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
battery_tune_played = false;
critical_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
battery_tune_played = false;
if (armed.armed) {
arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
if (armed.armed) {
arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
} else {
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
}
status_changed = true;
} else {
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
}
critical_voltage_counter++;
} else {
low_voltage_counter = 0;
critical_voltage_counter = 0;
status_changed = true;
}
/* End battery voltage check */
@@ -1029,19 +1028,18 @@ int commander_thread_main(int argc, char *argv[])
* position to the current position.
*/
if (!home_position_set && gps_position.fix_type >= 3 &&
(gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk
if (!status.condition_home_position_valid && gps_position.fix_type >= 3 &&
(gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) &&
(hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed
&& global_position.valid) {
/* copy position data to uORB home message, store it locally as well */
home.lat = global_position.lat;
home.lon = global_position.lon;
home.alt = global_position.alt;
home.lat = (double)global_position.lat / 1e7d;
home.lon = (double)global_position.lon / 1e7d;
home.altitude = (float)global_position.alt;
warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.altitude);
mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.altitude);
warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt);
mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt);
/* announce new home position */
if (home_pub > 0) {
@@ -1052,129 +1050,163 @@ int commander_thread_main(int argc, char *argv[])
}
/* mark home position as set */
home_position_set = true;
status.condition_home_position_valid = true;
tune_positive();
}
}
/* ignore RC signals if in offboard control mode */
if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) {
/* start RC input check */
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;
mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time");
/* start RC input check */
if (sp_man.timestamp != 0 && 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;
mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time");
status_changed = true;
} else {
if (status.rc_signal_lost) {
mavlink_log_critical(mavlink_fd, "#audio: RC signal regained");
status_changed = true;
} else {
if (status.rc_signal_lost) {
mavlink_log_critical(mavlink_fd, "#audio: RC signal regained");
status_changed = true;
}
}
}
status.rc_signal_lost = false;
status.rc_signal_lost = false;
transition_result_t res; // store all transitions results here
transition_result_t res; // store all transitions results here
/* arm/disarm by RC */
res = TRANSITION_NOT_CHANGED;
/* arm/disarm by RC */
res = TRANSITION_NOT_CHANGED;
/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm
* do it only for rotary wings */
if (status.is_rotary_wing &&
(status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
(status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm
* do it only for rotary wings */
if (status.is_rotary_wing &&
(status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
(status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
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);
res = arming_state_transition(&status, &safety, new_arming_state, &armed);
stick_off_counter = 0;
} else {
stick_off_counter++;
}
} else {
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);
res = arming_state_transition(&status, &safety, new_arming_state, &armed);
stick_off_counter = 0;
}
/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
if (status.arming_state == ARMING_STATE_STANDBY &&
sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
if (safety.safety_switch_available && !safety.safety_off) {
print_reject_arm("NOT ARMING: Press safety switch first.");
} else if (status.main_state != MAIN_STATE_MANUAL) {
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else {
res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
}
stick_on_counter = 0;
} else {
stick_on_counter++;
}
} else {
stick_on_counter = 0;
}
if (res == TRANSITION_CHANGED) {
if (status.arming_state == ARMING_STATE_ARMED) {
mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
} else {
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
}
} else if (res == TRANSITION_DENIED) {
warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
}
/* fill current_status according to mode switches */
check_mode_switches(&sp_man, &status);
/* evaluate the main state machine */
res = check_main_state_machine(&status);
if (res == TRANSITION_CHANGED) {
//mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state);
tune_positive();
} else if (res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
stick_off_counter++;
}
} else {
if (!status.rc_signal_lost) {
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
status.rc_signal_lost = true;
status_changed = true;
stick_off_counter = 0;
}
/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
if (status.arming_state == ARMING_STATE_STANDBY &&
sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
if (safety.safety_switch_available && !safety.safety_off) {
print_reject_arm("NOT ARMING: Press safety switch first.");
} else if (status.main_state != MAIN_STATE_MANUAL) {
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else {
res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
}
stick_on_counter = 0;
} else {
stick_on_counter++;
}
} else {
stick_on_counter = 0;
}
if (res == TRANSITION_CHANGED) {
if (status.arming_state == ARMING_STATE_ARMED) {
mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
} else {
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
}
} else if (res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied");
}
if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
/* recover from failsafe */
transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
}
/* fill status according to mode switches */
check_mode_switches(&sp_man, &status);
/* evaluate the main state machine according to mode switches */
res = set_main_state_rc(&status);
if (res == TRANSITION_CHANGED) {
tune_positive();
} else if (res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied");
}
} else {
if (!status.rc_signal_lost) {
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
status.rc_signal_lost = true;
status_changed = true;
}
if (armed.armed) {
if (status.main_state == MAIN_STATE_AUTO) {
/* check if AUTO mode still allowed */
transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO);
if (res == TRANSITION_DENIED) {
/* AUTO mode denied, don't try RTL, switch to failsafe state LAND */
res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
if (res == TRANSITION_DENIED) {
/* LAND not allowed, set TERMINATION state */
transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
}
}
} else {
/* failsafe for manual modes */
transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
if (res == TRANSITION_DENIED) {
/* RTL not allowed (no global position estimate), try LAND */
res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
if (res == TRANSITION_DENIED) {
/* LAND not allowed, set TERMINATION state */
res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
}
}
}
} else {
if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
/* reset failsafe when disarmed */
transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
}
}
}
/* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */
if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON);
if (flighttermination_res == TRANSITION_CHANGED) {
// TODO remove this hack
/* flight termination in manual mode if assisted switch is on easy position */
if (!status.is_rotary_wing && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
tune_positive();
}
} else {
flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF);
}
/* handle commands last, as the system needs to be updated to handle them */
orb_check(cmd_sub, &updated);
@@ -1190,13 +1222,24 @@ int commander_thread_main(int argc, char *argv[])
/* check which state machines for changes, clear "changed" flag */
bool arming_state_changed = check_arming_state_changed();
bool main_state_changed = check_main_state_changed();
bool flighttermination_state_changed = check_flighttermination_state_changed();
bool failsafe_state_changed = check_failsafe_state_changed();
hrt_abstime t1 = hrt_absolute_time();
if (arming_state_changed || main_state_changed) {
mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d", status.arming_state, status.main_state);
/* 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]);
}
if (main_state_changed) {
status_changed = true;
mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]);
}
if (failsafe_state_changed) {
status_changed = true;
mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]);
}
/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
@@ -1375,72 +1418,72 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
}
void
check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status)
check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status)
{
/* main mode switch */
if (!isfinite(sp_man->mode_switch)) {
warnx("mode sw not finite");
current_status->mode_switch = MODE_SWITCH_MANUAL;
status->mode_switch = MODE_SWITCH_MANUAL;
} else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) {
current_status->mode_switch = MODE_SWITCH_AUTO;
status->mode_switch = MODE_SWITCH_AUTO;
} else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) {
current_status->mode_switch = MODE_SWITCH_MANUAL;
status->mode_switch = MODE_SWITCH_MANUAL;
} else {
current_status->mode_switch = MODE_SWITCH_ASSISTED;
status->mode_switch = MODE_SWITCH_ASSISTED;
}
/* return switch */
if (!isfinite(sp_man->return_switch)) {
current_status->return_switch = RETURN_SWITCH_NONE;
status->return_switch = RETURN_SWITCH_NONE;
} else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) {
current_status->return_switch = RETURN_SWITCH_RETURN;
status->return_switch = RETURN_SWITCH_RETURN;
} else {
current_status->return_switch = RETURN_SWITCH_NORMAL;
status->return_switch = RETURN_SWITCH_NORMAL;
}
/* assisted switch */
if (!isfinite(sp_man->assisted_switch)) {
current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
} else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) {
current_status->assisted_switch = ASSISTED_SWITCH_EASY;
status->assisted_switch = ASSISTED_SWITCH_EASY;
} else {
current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
}
/* mission switch */
if (!isfinite(sp_man->mission_switch)) {
current_status->mission_switch = MISSION_SWITCH_NONE;
status->mission_switch = MISSION_SWITCH_NONE;
} else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) {
current_status->mission_switch = MISSION_SWITCH_LOITER;
status->mission_switch = MISSION_SWITCH_LOITER;
} else {
current_status->mission_switch = MISSION_SWITCH_MISSION;
status->mission_switch = MISSION_SWITCH_MISSION;
}
}
transition_result_t
check_main_state_machine(struct vehicle_status_s *current_status)
set_main_state_rc(struct vehicle_status_s *status)
{
/* evaluate the main state machine */
transition_result_t res = TRANSITION_DENIED;
switch (current_status->mode_switch) {
switch (status->mode_switch) {
case MODE_SWITCH_MANUAL:
res = main_state_transition(current_status, MAIN_STATE_MANUAL);
res = main_state_transition(status, MAIN_STATE_MANUAL);
// TRANSITION_DENIED is not possible here
break;
case MODE_SWITCH_ASSISTED:
if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) {
res = main_state_transition(current_status, MAIN_STATE_EASY);
if (status->assisted_switch == ASSISTED_SWITCH_EASY) {
res = main_state_transition(status, MAIN_STATE_EASY);
if (res != TRANSITION_DENIED)
break; // changed successfully or already in this state
@@ -1449,34 +1492,34 @@ check_main_state_machine(struct vehicle_status_s *current_status)
print_reject_mode("EASY");
}
res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
res = main_state_transition(status, MAIN_STATE_SEATBELT);
if (res != TRANSITION_DENIED)
break; // changed successfully or already in this mode
if (current_status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages
if (status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages
print_reject_mode("SEATBELT");
// else fallback to MANUAL
res = main_state_transition(current_status, MAIN_STATE_MANUAL);
res = main_state_transition(status, MAIN_STATE_MANUAL);
// TRANSITION_DENIED is not possible here
break;
case MODE_SWITCH_AUTO:
res = main_state_transition(current_status, MAIN_STATE_AUTO);
res = main_state_transition(status, MAIN_STATE_AUTO);
if (res != TRANSITION_DENIED)
break; // changed successfully or already in this state
// else fallback to SEATBELT (EASY likely will not work too)
print_reject_mode("AUTO");
res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
res = main_state_transition(status, MAIN_STATE_SEATBELT);
if (res != TRANSITION_DENIED)
break; // changed successfully or already in this state
// else fallback to MANUAL
res = main_state_transition(current_status, MAIN_STATE_MANUAL);
res = main_state_transition(status, MAIN_STATE_MANUAL);
// TRANSITION_DENIED is not possible here
break;
src/modules/commander/commander_helper.cpp
+28 -16
View File
@@ -44,6 +44,7 @@
#include <stdint.h>
#include <stdbool.h>
#include <fcntl.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
@@ -251,36 +252,47 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern)
ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
}
float battery_remaining_estimate_voltage(float voltage)
float battery_remaining_estimate_voltage(float voltage, float discharged)
{
float ret = 0;
static param_t bat_volt_empty;
static param_t bat_volt_full;
static param_t bat_n_cells;
static param_t bat_v_empty_h;
static param_t bat_v_full_h;
static param_t bat_n_cells_h;
static param_t bat_capacity_h;
static float bat_v_empty = 3.2f;
static float bat_v_full = 4.0f;
static int bat_n_cells = 3;
static float bat_capacity = -1.0f;
static bool initialized = false;
static unsigned int counter = 0;
static float ncells = 3;
// XXX change cells to int (and param to INT32)
if (!initialized) {
bat_volt_empty = param_find("BAT_V_EMPTY");
bat_volt_full = param_find("BAT_V_FULL");
bat_n_cells = param_find("BAT_N_CELLS");
bat_v_empty_h = param_find("BAT_V_EMPTY");
bat_v_full_h = param_find("BAT_V_FULL");
bat_n_cells_h = param_find("BAT_N_CELLS");
bat_capacity_h = param_find("BAT_CAPACITY");
initialized = true;
}
static float chemistry_voltage_empty = 3.2f;
static float chemistry_voltage_full = 4.05f;
if (counter % 100 == 0) {
param_get(bat_volt_empty, &chemistry_voltage_empty);
param_get(bat_volt_full, &chemistry_voltage_full);
param_get(bat_n_cells, &ncells);
param_get(bat_v_empty_h, &bat_v_empty);
param_get(bat_v_full_h, &bat_v_full);
param_get(bat_n_cells_h, &bat_n_cells);
param_get(bat_capacity_h, &bat_capacity);
}
counter++;
ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty));
/* remaining charge estimate based on voltage */
float remaining_voltage = (voltage - bat_n_cells * bat_v_empty) / (bat_n_cells * (bat_v_full - bat_v_empty));
if (bat_capacity > 0.0f) {
/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */
ret = fminf(remaining_voltage, 1.0f - discharged / bat_capacity);
} else {
/* else use voltage */
ret = remaining_voltage;
}
/* limit to sane values */
ret = (ret < 0.0f) ? 0.0f : ret;
src/modules/commander/commander_helper.h
+4 -3
View File
@@ -75,12 +75,13 @@ void rgbled_set_mode(rgbled_mode_t mode);
void rgbled_set_pattern(rgbled_pattern_t *pattern);
/**
* Provides a coarse estimate of remaining battery power.
* Estimate remaining battery charge.
*
* The estimate is very basic and based on decharging voltage curves.
* Use integral of current if battery capacity known (BAT_CAPACITY parameter set),
* else use simple estimate based on voltage.
*
* @return the estimated remaining capacity in 0..1
*/
float battery_remaining_estimate_voltage(float voltage);
float battery_remaining_estimate_voltage(float voltage, float discharged);
#endif /* COMMANDER_HELPER_H_ */
src/modules/commander/commander_params.c
+4 -3
View File
@@ -48,6 +48,7 @@
PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f);
PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f);
PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3);
PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f);
PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
src/modules/commander/state_machine_helper.cpp
+95 -73
View File
@@ -63,11 +63,11 @@ static const int ERROR = -1;
static bool arming_state_changed = true;
static bool main_state_changed = true;
static bool flighttermination_state_changed = true;
static bool failsafe_state_changed = true;
transition_result_t
arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety,
arming_state_t new_arming_state, struct actuator_armed_s *armed)
arming_state_t new_arming_state, struct actuator_armed_s *armed)
{
/*
* Perform an atomic state update
@@ -85,6 +85,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
/* enforce lockdown in HIL */
if (status->hil_state == HIL_STATE_ON) {
armed->lockdown = true;
} else {
armed->lockdown = false;
}
@@ -219,55 +220,54 @@ check_arming_state_changed()
}
transition_result_t
main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state)
main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state)
{
transition_result_t ret = TRANSITION_DENIED;
/* only check transition if the new state is actually different from the current one */
if (new_main_state == current_state->main_state) {
ret = TRANSITION_NOT_CHANGED;
/* transition may be denied even if requested the same state because conditions may be changed */
switch (new_main_state) {
case MAIN_STATE_MANUAL:
ret = TRANSITION_CHANGED;
break;
} else {
case MAIN_STATE_SEATBELT:
switch (new_main_state) {
case MAIN_STATE_MANUAL:
/* need at minimum altitude estimate */
if (!status->is_rotary_wing ||
(status->condition_local_altitude_valid ||
status->condition_global_position_valid)) {
ret = TRANSITION_CHANGED;
break;
case MAIN_STATE_SEATBELT:
/* need at minimum altitude estimate */
if (!current_state->is_rotary_wing ||
(current_state->condition_local_altitude_valid ||
current_state->condition_global_position_valid)) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_EASY:
/* need at minimum local position estimate */
if (current_state->condition_local_position_valid ||
current_state->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_AUTO:
/* need global position estimate */
if (current_state->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
}
if (ret == TRANSITION_CHANGED) {
current_state->main_state = new_main_state;
break;
case MAIN_STATE_EASY:
/* need at minimum local position estimate */
if (status->condition_local_position_valid ||
status->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_AUTO:
/* need global position estimate */
if (status->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
}
if (ret == TRANSITION_CHANGED) {
if (status->main_state != new_main_state) {
status->main_state = new_main_state;
main_state_changed = true;
} else {
ret = TRANSITION_NOT_CHANGED;
}
}
@@ -287,10 +287,10 @@ check_main_state_changed()
}
bool
check_flighttermination_state_changed()
check_failsafe_state_changed()
{
if (flighttermination_state_changed) {
flighttermination_state_changed = false;
if (failsafe_state_changed) {
failsafe_state_changed = false;
return true;
} else {
@@ -361,41 +361,63 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
/**
* Transition from one flightermination state to another
* Transition from one failsafe state to another
*/
transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state)
transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state)
{
transition_result_t ret = TRANSITION_DENIED;
/* only check transition if the new state is actually different from the current one */
if (new_flighttermination_state == status->flighttermination_state) {
/* transition may be denied even if requested the same state because conditions may be changed */
if (status->failsafe_state == FAILSAFE_STATE_TERMINATION) {
/* transitions from TERMINATION to other states not allowed */
if (new_failsafe_state == FAILSAFE_STATE_TERMINATION) {
ret = TRANSITION_NOT_CHANGED;
} else {
switch (new_flighttermination_state) {
case FLIGHTTERMINATION_STATE_ON:
ret = TRANSITION_CHANGED;
status->flighttermination_state = FLIGHTTERMINATION_STATE_ON;
warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON");
break;
case FLIGHTTERMINATION_STATE_OFF:
ret = TRANSITION_CHANGED;
status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF;
break;
default:
break;
}
if (ret == TRANSITION_CHANGED) {
flighttermination_state_changed = true;
// TODO
//control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON;
}
}
return ret;
} else {
switch (new_failsafe_state) {
case FAILSAFE_STATE_NORMAL:
/* always allowed (except from TERMINATION state) */
ret = TRANSITION_CHANGED;
break;
case FAILSAFE_STATE_RTL:
/* global position and home position required for RTL */
if (status->condition_global_position_valid && status->condition_home_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case FAILSAFE_STATE_LAND:
/* at least relative altitude estimate required for landing */
if (status->condition_local_altitude_valid || status->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case FAILSAFE_STATE_TERMINATION:
/* always allowed */
ret = TRANSITION_CHANGED;
break;
default:
break;
}
if (ret == TRANSITION_CHANGED) {
if (status->failsafe_state != new_failsafe_state) {
status->failsafe_state = new_failsafe_state;
failsafe_state_changed = true;
} else {
ret = TRANSITION_NOT_CHANGED;
}
}
}
return ret;
}
src/modules/commander/state_machine_helper.h
+2 -2
View File
@@ -67,11 +67,11 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state
bool check_main_state_changed();
transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state);
transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state);
bool check_navigation_state_changed();
bool check_flighttermination_state_changed();
bool check_failsafe_state_changed();
void set_navigation_state_changed();
src/modules/controllib/uorb/blocks.cpp
+3 -3
View File
@@ -54,8 +54,8 @@ BlockWaypointGuidance::~BlockWaypointGuidance() {};
void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
vehicle_attitude_s &att,
mission_item_s &missionCmd,
mission_item_s &lastMissionCmd)
position_setpoint_s &missionCmd,
position_setpoint_s &lastMissionCmd)
{
// heading to waypoint
@@ -86,7 +86,7 @@ BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const c
_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
_missionCmd(&getSubscriptions(), ORB_ID(mission_item_triplet), 20),
_missionCmd(&getSubscriptions(), ORB_ID(position_setpoint_triplet), 20),
_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
src/modules/controllib/uorb/blocks.hpp
+4 -4
View File
@@ -43,7 +43,7 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/mission_item_triplet.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
@@ -82,8 +82,8 @@ public:
virtual ~BlockWaypointGuidance();
void update(vehicle_global_position_s &pos,
vehicle_attitude_s &att,
mission_item_s &missionCmd,
mission_item_s &lastMissionCmd);
position_setpoint_s &missionCmd,
position_setpoint_s &lastMissionCmd);
float getPsiCmd() { return _psiCmd; }
};
@@ -98,7 +98,7 @@ protected:
UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
UOrbSubscription<vehicle_global_position_s> _pos;
UOrbSubscription<mission_item_triplet_s> _missionCmd;
UOrbSubscription<position_setpoint_triplet_s> _missionCmd;
UOrbSubscription<manual_control_setpoint_s> _manual;
UOrbSubscription<vehicle_status_s> _status;
UOrbSubscription<parameter_update_s> _param_update;
src/modules/fixedwing_backside/fixedwing.cpp
+4 -4
View File
@@ -174,9 +174,9 @@ void BlockMultiModeBacksideAutopilot::update()
// of control we will limit the velocity feedback between
// the min/max velocity
float v = _vLimit.update(sqrtf(
_pos.vx * _pos.vx +
_pos.vel_n * _pos.vel_n +
_pos.vy * _pos.vy +
_pos.vz * _pos.vz));
_pos.vel_d * _pos.vel_d));
// limit velocity command between min/max velocity
float vCmd = _vLimit.update(_vCmd.get());
@@ -236,9 +236,9 @@ void BlockMultiModeBacksideAutopilot::update()
// for the purpose of control we will limit the velocity feedback between
// the min/max velocity
float v = _vLimit.update(sqrtf(
_pos.vx * _pos.vx +
_pos.vel_n * _pos.vel_n +
_pos.vy * _pos.vy +
_pos.vz * _pos.vz));
_pos.vel_d * _pos.vel_d));
// pitch channel -> rate of climb
// TODO, might want to put a gain on this, otherwise commanding
src/modules/fixedwing_backside/fixedwing.hpp
+1 -1
View File
@@ -264,7 +264,7 @@ private:
BlockParamFloat _crMax;
struct pollfd _attPoll;
mission_item_triplet_s _lastMissionCmd;
position_setpoint_triplet_s _lastMissionCmd;
enum {CH_AIL, CH_ELV, CH_RDR, CH_THR};
uint64_t _timeStamp;
public:
src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
+1 -1
View File
@@ -299,7 +299,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint)
global_sp_updated_set_once = true;
psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
psi_track = get_bearing_to_next_waypoint(global_pos.lat, global_pos.lon,
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
printf("next wp direction: %0.4f\n", (double)psi_track);
src/modules/fw_att_control/fw_att_control_main.cpp
+5 -5
View File
@@ -619,7 +619,7 @@ FixedwingAttitudeControl::task_main()
}
/* Simple handling of failsafe: deploy parachute if failsafe is on */
if (_vcontrol_mode.flag_control_flighttermination_enabled) {
if (_vcontrol_mode.flag_control_termination_enabled) {
_actuators_airframe.control[1] = 1.0f;
// warnx("_actuators_airframe.control[1] = 1.0f;");
} else {
@@ -637,7 +637,7 @@ FixedwingAttitudeControl::task_main()
/* if airspeed is smaller than min, the sensor is not giving good readings */
if (!_airspeed_valid ||
(_airspeed.indicated_airspeed_m_s < 0.1f * _parameters.airspeed_min) ||
(_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) ||
!isfinite(_airspeed.indicated_airspeed_m_s)) {
airspeed = _parameters.airspeed_trim;
@@ -704,9 +704,9 @@ FixedwingAttitudeControl::task_main()
float speed_body_v = 0.0f;
float speed_body_w = 0.0f;
if(_att.R_valid) {
speed_body_u = _att.R[0][0] * _global_pos.vx + _att.R[1][0] * _global_pos.vy + _att.R[2][0] * _global_pos.vz;
speed_body_v = _att.R[0][1] * _global_pos.vx + _att.R[1][1] * _global_pos.vy + _att.R[2][1] * _global_pos.vz;
speed_body_w = _att.R[0][2] * _global_pos.vx + _att.R[1][2] * _global_pos.vy + _att.R[2][2] * _global_pos.vz;
speed_body_u = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d;
speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d;
speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d;
} else {
warnx("Did not get a valid R\n");
}
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+70 -74
View File
@@ -68,7 +68,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/mission_item_triplet.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
@@ -126,7 +126,7 @@ private:
int _control_task; /**< task handle for sensor task */
int _global_pos_sub;
int _mission_item_triplet_sub;
int _pos_sp_triplet_sub;
int _att_sub; /**< vehicle attitude subscription */
int _attitude_sub; /**< raw rc channels data subscription */
int _airspeed_sub; /**< airspeed subscription */
@@ -145,7 +145,7 @@ private:
struct airspeed_s _airspeed; /**< airspeed */
struct vehicle_control_mode_s _control_mode; /**< vehicle status */
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */
struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */
struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */
perf_counter_t _loop_perf; /**< loop performance counter */
@@ -175,7 +175,6 @@ private:
/* takeoff/launch states */
bool launch_detected;
bool usePreTakeoffThrust;
bool launch_detection_message_sent;
/* Landingslope object */
Landingslope landingslope;
@@ -193,7 +192,7 @@ private:
uint64_t _airspeed_last_valid; ///< last time airspeed was valid. Used to detect sensor failures
float _groundspeed_undershoot; ///< ground speed error to min. speed in m/s
bool _global_pos_valid; ///< global position is valid
math::Dcm _R_nb; ///< current attitude
math::Matrix<3, 3> _R_nb; ///< current attitude
ECL_L1_Pos_Controller _l1_control;
TECS _tecs;
@@ -332,11 +331,11 @@ private:
/**
* Control position.
*/
bool control_position(const math::Vector2f &global_pos, const math::Vector2f &ground_speed,
const struct mission_item_triplet_s &_mission_item_triplet);
bool control_position(const math::Vector<2> &global_pos, const math::Vector<2> &ground_speed,
const struct position_setpoint_triplet_s &_pos_sp_triplet);
float calculate_target_airspeed(float airspeed_demand);
void calculate_gndspeed_undershoot(const math::Vector2f &current_position, const math::Vector2f &ground_speed, const struct mission_item_triplet_s &mission_item_triplet);
void calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet);
/**
* Shim for calling task_main from task_create.
@@ -368,7 +367,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
/* subscriptions */
_global_pos_sub(-1),
_mission_item_triplet_sub(-1),
_pos_sp_triplet_sub(-1),
_att_sub(-1),
_airspeed_sub(-1),
_control_mode_sub(-1),
@@ -397,8 +396,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_mavlink_fd(-1),
launchDetector(),
launch_detected(false),
usePreTakeoffThrust(false),
launch_detection_message_sent(false)
usePreTakeoffThrust(false)
{
/* safely initialize structs */
vehicle_attitude_s _att = {0};
@@ -408,7 +406,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
airspeed_s _airspeed = {0};
vehicle_control_mode_s _control_mode = {0};
vehicle_global_position_s _global_pos = {0};
mission_item_triplet_s _mission_item_triplet = {0};
position_setpoint_triplet_s _pos_sp_triplet = {0};
sensor_combined_s _sensor_combined = {0};
@@ -655,11 +653,11 @@ void
FixedwingPositionControl::vehicle_setpoint_poll()
{
/* check if there is a new setpoint */
bool mission_item_triplet_updated;
orb_check(_mission_item_triplet_sub, &mission_item_triplet_updated);
bool pos_sp_triplet_updated;
orb_check(_pos_sp_triplet_sub, &pos_sp_triplet_updated);
if (mission_item_triplet_updated) {
orb_copy(ORB_ID(mission_item_triplet), _mission_item_triplet_sub, &_mission_item_triplet);
if (pos_sp_triplet_updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
_setpoint_valid = true;
}
}
@@ -702,25 +700,25 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand)
}
void
FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector2f &current_position, const math::Vector2f &ground_speed, const struct mission_item_triplet_s &mission_item_triplet)
FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet)
{
if (_global_pos_valid) {
/* rotate ground speed vector with current attitude */
math::Vector2f yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
yaw_vector.normalize();
float ground_speed_body = yaw_vector * ground_speed;
/* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/
float distance = 0.0f;
float delta_altitude = 0.0f;
if (mission_item_triplet.previous_valid) {
distance = get_distance_to_next_waypoint(mission_item_triplet.previous.lat, mission_item_triplet.previous.lon, mission_item_triplet.current.lat, mission_item_triplet.current.lon);
delta_altitude = mission_item_triplet.current.altitude - mission_item_triplet.previous.altitude;
if (pos_sp_triplet.previous.valid) {
distance = get_distance_to_next_waypoint(pos_sp_triplet.previous.lat, pos_sp_triplet.previous.lon, pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
delta_altitude = pos_sp_triplet.current.alt - pos_sp_triplet.previous.alt;
} else {
distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), mission_item_triplet.current.lat, mission_item_triplet.current.lon);
delta_altitude = mission_item_triplet.current.altitude - _global_pos.alt;
distance = get_distance_to_next_waypoint(current_position(0), current_position(1), pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
delta_altitude = pos_sp_triplet.current.alt - _global_pos.alt;
}
float ground_speed_desired = _parameters.airspeed_min * cosf(atan2f(delta_altitude, distance));
@@ -752,12 +750,12 @@ void FixedwingPositionControl::navigation_capabilities_publish()
}
bool
FixedwingPositionControl::control_position(const math::Vector2f &current_position, const math::Vector2f &ground_speed,
const struct mission_item_triplet_s &mission_item_triplet)
FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed,
const struct position_setpoint_triplet_s &pos_sp_triplet)
{
bool setpoint = true;
calculate_gndspeed_undershoot(current_position, ground_speed, mission_item_triplet);
calculate_gndspeed_undershoot(current_position, ground_speed, pos_sp_triplet);
float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
@@ -765,11 +763,11 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
float baro_altitude = _global_pos.alt;
/* filter speed and altitude for controller */
math::Vector3 accel_body(_sensor_combined.accelerometer_m_s2[0], _sensor_combined.accelerometer_m_s2[1], _sensor_combined.accelerometer_m_s2[2]);
math::Vector3 accel_earth = _R_nb * accel_body;
math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
math::Vector<3> accel_earth = _R_nb * accel_body;
_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
float altitude_error = _mission_item_triplet.current.altitude - _global_pos.alt;
float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
/* no throttle limit as default */
float throttle_max = 1.0f;
@@ -787,70 +785,68 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
_tecs.set_speed_weight(_parameters.speed_weight);
/* current waypoint (the one currently heading for) */
math::Vector2f next_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon);
math::Vector<2> next_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
/* current waypoint (the one currently heading for) */
math::Vector2f curr_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon);
math::Vector<2> curr_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
/* previous waypoint */
math::Vector2f prev_wp;
math::Vector<2> prev_wp;
if (mission_item_triplet.previous_valid) {
prev_wp.setX(mission_item_triplet.previous.lat);
prev_wp.setY(mission_item_triplet.previous.lon);
if (pos_sp_triplet.previous.valid) {
prev_wp(0) = pos_sp_triplet.previous.lat;
prev_wp(1) = pos_sp_triplet.previous.lon;
} else {
/*
* No valid previous waypoint, go for the current wp.
* This is automatically handled by the L1 library.
*/
prev_wp.setX(mission_item_triplet.current.lat);
prev_wp.setY(mission_item_triplet.current.lon);
prev_wp(0) = pos_sp_triplet.current.lat;
prev_wp(1) = pos_sp_triplet.current.lon;
}
if (mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT || mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
if (pos_sp_triplet.current.type == SETPOINT_TYPE_NORMAL) {
/* waypoint is a plain navigation waypoint */
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, math::radians(_parameters.pitch_limit_min),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
} else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) {
/* waypoint is a loiter waypoint */
_l1_control.navigate_loiter(curr_wp, current_position, mission_item_triplet.current.loiter_radius,
mission_item_triplet.current.loiter_direction, ground_speed);
_l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius,
pos_sp_triplet.current.loiter_direction, ground_speed);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, math::radians(_parameters.pitch_limit_min),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
} else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) {
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
/* Horizontal landing control */
/* switch to heading hold for the last meters, continue heading hold after */
float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), curr_wp.getX(), curr_wp.getY());
float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) {
/* heading hold, along the line connecting this and the last waypoint */
if (!land_noreturn_horizontal) {//set target_bearing in first occurrence
if (mission_item_triplet.previous_valid) {
target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY());
if (pos_sp_triplet.previous.valid) {
target_bearing = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
} else {
target_bearing = _att.yaw;
}
@@ -881,23 +877,23 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
// /* do not go down too early */
// if (wp_distance > 50.0f) {
// altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt;
// altitude_error = (_global_triplet.current.alt + 25.0f) - _global_pos.alt;
// }
/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
// XXX this could make a great param
float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(mission_item_triplet.current.param1)
float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(pos_sp_triplet.current.param1)
float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
float airspeed_land = 1.3f * _parameters.airspeed_min;
float airspeed_approach = 1.3f * _parameters.airspeed_min;
float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length;
float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement);
float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement);
float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length;
float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement);
float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement);
if ( (_global_pos.alt < _mission_item_triplet.current.altitude + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out
if ( (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out
/* land with minimal speed */
@@ -916,12 +912,12 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
}
float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _mission_item_triplet.current.altitude);
float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _pos_sp_triplet.current.alt);
/* avoid climbout */
if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground)
{
flare_curve_alt = mission_item_triplet.current.altitude;
flare_curve_alt = pos_sp_triplet.current.alt;
land_stayonground = true;
}
@@ -979,16 +975,17 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
}
} else if (mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
} 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()) {
// warnx("Launch detection enabled");
if(!launch_detection_message_sent) {
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");
launch_detection_message_sent = true;
last_sent = hrt_absolute_time();
}
launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
if (launchDetector.getLaunchDetected()) {
@@ -1012,9 +1009,9 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
if (altitude_error > 15.0f) {
/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(1.3f * _parameters.airspeed_min),
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min),
_airspeed.indicated_airspeed_m_s, eas2tas,
true, math::max(math::radians(mission_item_triplet.current.pitch_min), math::radians(10.0f)),
true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
@@ -1023,7 +1020,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
} else {
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, math::radians(_parameters.pitch_limit_min),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
@@ -1037,15 +1034,15 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
// warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(),
// (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing());
// warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp.getX(), (double)prev_wp.getY(),
// (double)next_wp.getX(), (double)next_wp.getY(), (mission_item_triplet.previous_valid) ? "valid" : "invalid");
// warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp(0), (double)prev_wp(1),
// (double)next_wp(0), (double)next_wp(1), (pos_sp_triplet.previous_valid) ? "valid" : "invalid");
// XXX at this point we always want no loiter hold if a
// mission is active
_loiter_hold = false;
/* reset land state */
if (mission_item_triplet.current.nav_cmd != NAV_CMD_LAND) {
if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) {
land_noreturn_horizontal = false;
land_noreturn_vertical = false;
land_stayonground = false;
@@ -1054,10 +1051,9 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
}
/* reset takeoff/launch state */
if (mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) {
if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) {
launch_detected = false;
usePreTakeoffThrust = false;
launch_detection_message_sent = false;
}
if (was_circle_mode && !_l1_control.circle_mode()) {
@@ -1178,7 +1174,7 @@ FixedwingPositionControl::task_main()
* do subscriptions
*/
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_mission_item_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet));
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
@@ -1266,14 +1262,14 @@ FixedwingPositionControl::task_main()
vehicle_airspeed_poll();
// vehicle_baro_poll();
math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy);
math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e);
math::Vector<2> current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
/*
* Attempt to control position, on success (= sensors present and not in manual mode),
* publish setpoint.
*/
if (control_position(current_position, ground_speed, _mission_item_triplet)) {
if (control_position(current_position, ground_speed, _pos_sp_triplet)) {
_att_sp.timestamp = hrt_absolute_time();
/* lazily publish the setpoint only once available */
@@ -1287,7 +1283,7 @@ FixedwingPositionControl::task_main()
}
/* XXX check if radius makes sense here */
float turn_distance = _l1_control.switch_distance(_mission_item_triplet.current.acceptance_radius);
float turn_distance = _l1_control.switch_distance(100.0f);
/* lazily publish navigation capabilities */
if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) {
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+60 -7
View File
@@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -49,21 +48,75 @@
*
*/
/**
* L1 period
*
* This is the L1 distance and defines the tracking
* point ahead of the aircraft its following.
* A value of 25 meters works for most aircraft. Shorten
* slowly during tuning until response is sharp without oscillation.
*
* @min 1.0
* @max 100.0
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 25.0f);
/**
* L1 damping
*
* Damping factor for L1 control.
*
* @min 0.6
* @max 0.9
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f);
/**
* Default Loiter Radius
*
* This radius is used when no other loiter radius is set.
*
* @min 10.0
* @max 100.0
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_LOITER_R, 50.0f);
/**
* Cruise throttle
*
* This is the throttle setting required to achieve the desired cruise speed. Most airframes have a value of 0.5-0.7.
*
* @min 0.0
* @max 1.0
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.7f);
/**
* Negative pitch limit
*
* The minimum negative pitch the controller will output.
*
* @unit degrees
* @min -60.0
* @max 0.0
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f);
/**
* Positive pitch limit
*
* The maximum positive pitch the controller will output.
*
* @unit degrees
* @min 0.0
* @max 60.0
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f);
src/modules/mavlink/mavlink_orb_listener.cpp
+15 -23
View File
@@ -59,7 +59,6 @@
#include "mavlink_orb_listener.h"
#include "mavlink_main.h"
//#include "util.h"
void *uorb_receive_thread(void *arg);
@@ -200,10 +199,10 @@ MavlinkOrbListener::l_vehicle_attitude(const struct listener *l)
hrt_abstime t = hrt_absolute_time();
if (t >= last_sent_vfr + 100000) {
last_sent_vfr = t;
float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
float groundspeed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e);
uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f;
mavlink_msg_vfr_hud_send(_mavlink->get_mavlink_chan(), airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz);
mavlink_msg_vfr_hud_send(_mavlink->get_mavlink_chan(), airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vel_d);
}
/* send quaternion values if it exists */
@@ -333,13 +332,13 @@ MavlinkOrbListener::l_global_position(const struct listener *l)
mavlink_msg_global_position_int_send(_mavlink->get_mavlink_chan(),
global_pos.timestamp / 1000,
global_pos.lat,
global_pos.lon,
global_pos.lat * 1e7,
global_pos.lon * 1e7,
global_pos.alt * 1000.0f,
global_pos.relative_alt * 1000.0f,
global_pos.vx * 100.0f,
global_pos.vy * 100.0f,
global_pos.vz * 100.0f,
(global_pos.alt - home.alt) * 1000.0f,
global_pos.vel_n * 100.0f,
global_pos.vel_e * 100.0f,
global_pos.vel_d * 100.0f,
_wrap_2pi(global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f);
}
@@ -363,23 +362,18 @@ MavlinkOrbListener::l_local_position(const struct listener *l)
void
MavlinkOrbListener::l_global_position_setpoint(const struct listener *l)
{
struct mission_item_triplet_s triplet;
orb_copy(ORB_ID(mission_item_triplet), mavlink_subs.triplet_sub, &triplet);
struct position_setpoint_triplet_s triplet;
orb_copy(ORB_ID(position_setpoint_triplet), mavlink_subs.triplet_sub, &triplet);
uint8_t coordinate_frame = MAV_FRAME_GLOBAL;
if (!triplet.current_valid)
if (!triplet.current.valid)
return;
if (triplet.current.altitude_is_relative)
coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
if (gcs_link)
mavlink_msg_global_position_setpoint_int_send(_mavlink->get_mavlink_chan(),
coordinate_frame,
MAV_FRAME_GLOBAL,
(int32_t)(triplet.current.lat * 1e7d),
(int32_t)(triplet.current.lon * 1e7d),
(int32_t)(triplet.current.altitude * 1e3f),
(int32_t)(triplet.current.alt * 1e3f),
(int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f));
}
@@ -615,11 +609,9 @@ MavlinkOrbListener::l_optical_flow(const struct listener *l)
void
MavlinkOrbListener::l_home(const struct listener *l)
{
struct home_position_s home;
orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home);
mavlink_msg_gps_global_origin_send(_mavlink->get_mavlink_chan(), (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.altitude)*1e3f);
mavlink_msg_gps_global_origin_send(_mavlink->get_mavlink_chan(), (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f);
}
void
@@ -757,7 +749,7 @@ MavlinkOrbListener::uorb_receive_start(void)
orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */
/* --- GLOBAL SETPOINT VALUE --- */
mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet));
mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
orb_set_interval(mavlink_subs.triplet_sub, 2000); /* 0.5 Hz updates */
/* --- LOCAL SETPOINT VALUE --- */
src/modules/mavlink/mavlink_receiver.cpp
+10 -8
View File
@@ -555,6 +555,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
hil_gps.alt = gps.alt;
hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m
hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m
hil_gps.timestamp_variance = gps.time_usec;
hil_gps.s_variance_m_s = 5.0f;
hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m;
hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
@@ -566,6 +567,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
if (heading_rad > M_PI_F)
heading_rad -= 2.0f * M_PI_F;
hil_gps.timestamp_velocity = gps.time_usec;
hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m
hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m
hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m
@@ -613,9 +615,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
hil_global_pos.lat = hil_state.lat;
hil_global_pos.lon = hil_state.lon;
hil_global_pos.alt = hil_state.alt / 1000.0f;
hil_global_pos.vx = hil_state.vx / 100.0f;
hil_global_pos.vy = hil_state.vy / 100.0f;
hil_global_pos.vz = hil_state.vz / 100.0f;
hil_global_pos.vel_n = hil_state.vx / 100.0f;
hil_global_pos.vel_e = hil_state.vy / 100.0f;
hil_global_pos.vel_d = hil_state.vz / 100.0f;
} else {
pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
@@ -659,8 +661,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
/* Calculate Rotation Matrix */
math::Quaternion q(hil_state.attitude_quaternion);
math::Dcm C_nb(q);
math::EulerAngles euler(C_nb);
math::Matrix<3,3> C_nb = q.to_dcm();
math::Vector<3> euler = C_nb.to_euler();
/* set rotation matrix */
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
@@ -675,9 +677,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
hil_attitude.q[3] = q(3);
hil_attitude.q_valid = true;
hil_attitude.roll = euler.getPhi();
hil_attitude.pitch = euler.getTheta();
hil_attitude.yaw = euler.getPsi();
hil_attitude.roll = euler(0);
hil_attitude.pitch = euler(1);
hil_attitude.yaw = euler(2);
hil_attitude.rollspeed = hil_state.rollspeed;
hil_attitude.pitchspeed = hil_state.pitchspeed;
hil_attitude.yawspeed = hil_state.yawspeed;
src/modules/mavlink/mavlink_receiver.h
+1 -2
View File
@@ -53,9 +53,8 @@
#include <uORB/topics/offboard_control_setpoint.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/mission_item_triplet.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/mission_item_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
src/modules/mavlink_onboard/orb_topics.h
+1 -1
View File
@@ -50,7 +50,7 @@
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/mission_item_triplet.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/optical_flow.h>
src/modules/mc_att_control/mc_att_control_main.cpp
+779
View File
@@ -0,0 +1,779 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Author: @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mc_att_control_main.c
* Multicopter attitude controller.
*
* The controller has two loops: P loop for angular error and PD loop for angular rate error.
* Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch.
* For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw,
* so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis.
* These two approaches fused seamlessly with weight depending on angular error.
* When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity.
* Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging.
* If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/pid/pid.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <mathlib/mathlib.h>
#include <lib/geo/geo.h>
/**
* Multicopter attitude control app start / stop handling function
*
* @ingroup apps
*/
extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]);
#define MIN_TAKEOFF_THROTTLE 0.3f
#define YAW_DEADZONE 0.01f
#define RATES_I_LIMIT 0.5f
class MulticopterAttitudeControl
{
public:
/**
* Constructor
*/
MulticopterAttitudeControl();
/**
* Destructor, also kills the sensors task.
*/
~MulticopterAttitudeControl();
/**
* Start the sensors task.
*
* @return OK on success.
*/
int start();
private:
bool _task_should_exit; /**< if true, sensor task should exit */
int _control_task; /**< task handle for sensor task */
int _att_sub; /**< vehicle attitude subscription */
int _att_sp_sub; /**< vehicle attitude setpoint */
int _control_mode_sub; /**< vehicle control mode subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_sub; /**< notification of manual control updates */
int _arming_sub; /**< arming status of outputs */
orb_advert_t _att_sp_pub; /**< attitude setpoint publication */
orb_advert_t _rates_sp_pub; /**< rate setpoint publication */
orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */
struct actuator_controls_s _actuators; /**< actuator control inputs */
struct actuator_armed_s _arming; /**< actuator arming status */
struct vehicle_rates_setpoint_s _rates_sp; /**< vehicle rates setpoint */
perf_counter_t _loop_perf; /**< loop performance counter */
math::Vector<3> _rates_prev; /**< angular rates on previous step */
struct {
param_t att_p;
param_t yaw_p;
param_t att_rate_p;
param_t att_rate_i;
param_t att_rate_d;
param_t yaw_rate_p;
param_t yaw_rate_i;
param_t yaw_rate_d;
} _params_handles; /**< handles for interesting parameters */
struct {
math::Vector<3> p; /**< P gain for angular error */
math::Vector<3> rate_p; /**< P gain for angular rate error */
math::Vector<3> rate_i; /**< I gain for angular rate error */
math::Vector<3> rate_d; /**< D gain for angular rate error */
} _params;
/**
* Update our local parameter cache.
*/
int parameters_update();
/**
* Update control outputs
*/
void control_update();
/**
* Check for changes in vehicle control mode.
*/
void vehicle_control_mode_poll();
/**
* Check for changes in manual inputs.
*/
void vehicle_manual_poll();
/**
* Check for set triplet updates.
*/
void vehicle_setpoint_poll();
/**
* Check for arming status updates.
*/
void arming_status_poll();
/**
* Shim for calling task_main from task_create.
*/
static void task_main_trampoline(int argc, char *argv[]);
/**
* Main sensor collection task.
*/
void task_main() __attribute__((noreturn));
};
namespace att_control
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
MulticopterAttitudeControl *g_control;
}
MulticopterAttitudeControl::MulticopterAttitudeControl() :
_task_should_exit(false),
_control_task(-1),
/* subscriptions */
_att_sub(-1),
_att_sp_sub(-1),
_control_mode_sub(-1),
_params_sub(-1),
_manual_sub(-1),
_arming_sub(-1),
/* publications */
_att_sp_pub(-1),
_rates_sp_pub(-1),
_actuators_0_pub(-1),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fw att control"))
{
memset(&_att, 0, sizeof(_att));
memset(&_att_sp, 0, sizeof(_att_sp));
memset(&_manual, 0, sizeof(_manual));
memset(&_control_mode, 0, sizeof(_control_mode));
memset(&_arming, 0, sizeof(_arming));
_params.p.zero();
_params.rate_p.zero();
_params.rate_i.zero();
_params.rate_d.zero();
_rates_prev.zero();
_params_handles.att_p = param_find("MC_ATT_P");
_params_handles.yaw_p = param_find("MC_YAW_P");
_params_handles.att_rate_p = param_find("MC_ATTRATE_P");
_params_handles.att_rate_i = param_find("MC_ATTRATE_I");
_params_handles.att_rate_d = param_find("MC_ATTRATE_D");
_params_handles.yaw_rate_p = param_find("MC_YAWRATE_P");
_params_handles.yaw_rate_i = param_find("MC_YAWRATE_I");
_params_handles.yaw_rate_d = param_find("MC_YAWRATE_D");
/* fetch initial parameter values */
parameters_update();
}
MulticopterAttitudeControl::~MulticopterAttitudeControl()
{
if (_control_task != -1) {
/* task wakes up every 100ms or so at the longest */
_task_should_exit = true;
/* wait for a second for the task to quit at our request */
unsigned i = 0;
do {
/* wait 20ms */
usleep(20000);
/* if we have given up, kill it */
if (++i > 50) {
task_delete(_control_task);
break;
}
} while (_control_task != -1);
}
att_control::g_control = nullptr;
}
int
MulticopterAttitudeControl::parameters_update()
{
float v;
param_get(_params_handles.att_p, &v);
_params.p(0) = v;
_params.p(1) = v;
param_get(_params_handles.yaw_p, &v);
_params.p(2) = v;
param_get(_params_handles.att_rate_p, &v);
_params.rate_p(0) = v;
_params.rate_p(1) = v;
param_get(_params_handles.yaw_rate_p, &v);
_params.rate_p(2) = v;
param_get(_params_handles.att_rate_i, &v);
_params.rate_i(0) = v;
_params.rate_i(1) = v;
param_get(_params_handles.yaw_rate_i, &v);
_params.rate_i(2) = v;
param_get(_params_handles.att_rate_d, &v);
_params.rate_d(0) = v;
_params.rate_d(1) = v;
param_get(_params_handles.yaw_rate_d, &v);
_params.rate_d(2) = v;
return OK;
}
void
MulticopterAttitudeControl::vehicle_control_mode_poll()
{
bool control_mode_updated;
/* Check HIL state if vehicle status has changed */
orb_check(_control_mode_sub, &control_mode_updated);
if (control_mode_updated) {
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
}
}
void
MulticopterAttitudeControl::vehicle_manual_poll()
{
bool manual_updated;
/* get pilots inputs */
orb_check(_manual_sub, &manual_updated);
if (manual_updated) {
orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual);
}
}
void
MulticopterAttitudeControl::vehicle_setpoint_poll()
{
/* check if there is a new setpoint */
bool att_sp_updated;
orb_check(_att_sp_sub, &att_sp_updated);
if (att_sp_updated) {
orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
}
}
void
MulticopterAttitudeControl::arming_status_poll()
{
/* check if there is a new setpoint */
bool arming_updated;
orb_check(_arming_sub, &arming_updated);
if (arming_updated) {
orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming);
}
}
void
MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[])
{
att_control::g_control->task_main();
}
void
MulticopterAttitudeControl::task_main()
{
/* inform about start */
warnx("started");
fflush(stdout);
/*
* do subscriptions
*/
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
/* rate limit attitude updates to 100Hz */
orb_set_interval(_att_sub, 10);
parameters_update();
/* initialize values of critical structs until first regular update */
_arming.armed = false;
/* get an initial update for all sensor and status data */
vehicle_setpoint_poll();
vehicle_control_mode_poll();
vehicle_manual_poll();
arming_status_poll();
/* setpoint rotation matrix */
math::Matrix<3, 3> R_sp;
R_sp.identity();
/* rotation matrix for current state */
math::Matrix<3, 3> R;
R.identity();
/* current angular rates */
math::Vector<3> rates;
rates.zero();
/* angular rates integral error */
math::Vector<3> rates_int;
rates_int.zero();
/* identity matrix */
math::Matrix<3, 3> I;
I.identity();
math::Quaternion q;
bool reset_yaw_sp = true;
/* wakeup source(s) */
struct pollfd fds[2];
/* Setup of loop */
fds[0].fd = _params_sub;
fds[0].events = POLLIN;
fds[1].fd = _att_sub;
fds[1].events = POLLIN;
while (!_task_should_exit) {
/* wait for up to 500ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit, etc. */
if (pret == 0)
continue;
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("poll error %d, %d", pret, errno);
continue;
}
perf_begin(_loop_perf);
/* only update parameters if they changed */
if (fds[0].revents & POLLIN) {
/* copy the topic to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
parameters_update();
}
/* only run controller if attitude changed */
if (fds[1].revents & POLLIN) {
static uint64_t last_run = 0;
float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
/* guard against too large dt's */
if (dt > 0.02f)
dt = 0.02f;
/* copy attitude topic */
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
vehicle_setpoint_poll();
vehicle_control_mode_poll();
arming_status_poll();
vehicle_manual_poll();
float yaw_sp_move_rate = 0.0f;
bool publish_att_sp = false;
/* define which input is the dominating control input */
if (_control_mode.flag_control_manual_enabled) {
/* manual input */
if (!_control_mode.flag_control_climb_rate_enabled) {
/* pass throttle directly if not in altitude control mode */
_att_sp.thrust = _manual.throttle;
}
if (!_arming.armed) {
/* reset yaw setpoint when disarmed */
reset_yaw_sp = true;
}
if (_control_mode.flag_control_attitude_enabled) {
/* control attitude, update attitude setpoint depending on mode */
if (_att_sp.thrust < 0.1f) {
// TODO
//if (_status.condition_landed) {
/* reset yaw setpoint if on ground */
// reset_yaw_sp = true;
//}
} else {
if (_manual.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual.yaw) {
/* move yaw setpoint */
yaw_sp_move_rate = _manual.yaw;
_att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt);
_att_sp.R_valid = false;
publish_att_sp = true;
}
}
/* reset yaw setpint to current position if needed */
if (reset_yaw_sp) {
reset_yaw_sp = false;
_att_sp.yaw_body = _att.yaw;
_att_sp.R_valid = false;
publish_att_sp = true;
}
if (!_control_mode.flag_control_velocity_enabled) {
/* update attitude setpoint if not in position control mode */
_att_sp.roll_body = _manual.roll;
_att_sp.pitch_body = _manual.pitch;
_att_sp.R_valid = false;
publish_att_sp = true;
}
} else {
/* manual rate inputs (ACRO) */
// TODO
/* reset yaw setpoint after ACRO */
reset_yaw_sp = true;
}
} else {
/* reset yaw setpoint after non-manual control */
reset_yaw_sp = true;
}
if (_att_sp.R_valid) {
/* rotation matrix in _att_sp is valid, use it */
R_sp.set(&_att_sp.R_body[0][0]);
} else {
/* rotation matrix in _att_sp is not valid, use euler angles instead */
R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body);
/* copy rotation matrix back to setpoint struct */
memcpy(&_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
}
if (publish_att_sp) {
/* publish the attitude setpoint */
_att_sp.timestamp = hrt_absolute_time();
if (_att_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
} else {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
}
}
/* rotation matrix for current state */
R.set(_att.R);
/* current body angular rates */
rates(0) = _att.rollspeed;
rates(1) = _att.pitchspeed;
rates(2) = _att.yawspeed;
/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));
math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
/* axis and sin(angle) of desired rotation */
math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);
/* calculate angle error */
float e_R_z_sin = e_R.length();
float e_R_z_cos = R_z * R_sp_z;
/* calculate weight for yaw control */
float yaw_w = R_sp(2, 2) * R_sp(2, 2);
/* calculate rotation matrix after roll/pitch only rotation */
math::Matrix<3, 3> R_rp;
if (e_R_z_sin > 0.0f) {
/* get axis-angle representation */
float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
math::Vector<3> e_R_z_axis = e_R / e_R_z_sin;
e_R = e_R_z_axis * e_R_z_angle;
/* cross product matrix for e_R_axis */
math::Matrix<3, 3> e_R_cp;
e_R_cp.zero();
e_R_cp(0, 1) = -e_R_z_axis(2);
e_R_cp(0, 2) = e_R_z_axis(1);
e_R_cp(1, 0) = e_R_z_axis(2);
e_R_cp(1, 2) = -e_R_z_axis(0);
e_R_cp(2, 0) = -e_R_z_axis(1);
e_R_cp(2, 1) = e_R_z_axis(0);
/* rotation matrix for roll/pitch only rotation */
R_rp = R * (I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
} else {
/* zero roll/pitch rotation */
R_rp = R;
}
/* R_rp and R_sp has the same Z axis, calculate yaw error */
math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
if (e_R_z_cos < 0.0f) {
/* for large thrust vector rotations use another rotation method:
* calculate angle and axis for R -> R_sp rotation directly */
q.from_dcm(R.transposed() * R_sp);
math::Vector<3> e_R_d = q.imag();
e_R_d.normalize();
e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));
/* use fusion of Z axis based rotation and direct rotation */
float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
}
/* angular rates setpoint*/
math::Vector<3> rates_sp = _params.p.emult(e_R);
/* feed forward yaw setpoint rate */
rates_sp(2) += yaw_sp_move_rate * yaw_w;
/* reset integral if disarmed */
// TODO add LANDED flag here
if (!_arming.armed) {
rates_int.zero();
}
/* rate controller */
math::Vector<3> rates_err = rates_sp - rates;
math::Vector<3> control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / fmaxf(dt, 0.003f) + rates_int;
_rates_prev = rates;
/* update integral */
for (int i = 0; i < 3; i++) {
float rate_i = rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
if (isfinite(rate_i)) {
if (rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && control(i) > -RATES_I_LIMIT && control(i) < RATES_I_LIMIT) {
rates_int(i) = rate_i;
}
}
}
/* publish the attitude rates setpoint */
_rates_sp.roll = rates_sp(0);
_rates_sp.pitch = rates_sp(1);
_rates_sp.yaw = rates_sp(2);
_rates_sp.thrust = _att_sp.thrust;
_rates_sp.timestamp = hrt_absolute_time();
if (_rates_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &_rates_sp);
} else {
_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp);
}
/* publish the attitude controls */
if (_control_mode.flag_control_rates_enabled) {
_actuators.control[0] = (isfinite(control(0))) ? control(0) : 0.0f;
_actuators.control[1] = (isfinite(control(1))) ? control(1) : 0.0f;
_actuators.control[2] = (isfinite(control(2))) ? control(2) : 0.0f;
_actuators.control[3] = (isfinite(_rates_sp.thrust)) ? _rates_sp.thrust : 0.0f;
_actuators.timestamp = hrt_absolute_time();
} else {
/* controller disabled, publish zero attitude controls */
_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();
}
if (_actuators_0_pub > 0) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
} else {
/* advertise and publish */
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
}
}
perf_end(_loop_perf);
}
warnx("exit");
_control_task = -1;
_exit(0);
}
int
MulticopterAttitudeControl::start()
{
ASSERT(_control_task == -1);
/* start the task */
_control_task = task_spawn_cmd("mc_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2048,
(main_t)&MulticopterAttitudeControl::task_main_trampoline,
nullptr);
if (_control_task < 0) {
warn("task start failed");
return -errno;
}
return OK;
}
int mc_att_control_main(int argc, char *argv[])
{
if (argc < 1)
errx(1, "usage: mc_att_control {start|stop|status}");
if (!strcmp(argv[1], "start")) {
if (att_control::g_control != nullptr)
errx(1, "already running");
att_control::g_control = new MulticopterAttitudeControl;
if (att_control::g_control == nullptr)
errx(1, "alloc failed");
if (OK != att_control::g_control->start()) {
delete att_control::g_control;
att_control::g_control = nullptr;
err(1, "start failed");
}
exit(0);
}
if (!strcmp(argv[1], "stop")) {
if (att_control::g_control == nullptr)
errx(1, "not running");
delete att_control::g_control;
att_control::g_control = nullptr;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (att_control::g_control) {
errx(0, "running");
} else {
errx(1, "not running");
}
}
warnx("unrecognized command");
return 1;
}
src/modules/mc_att_control/mc_att_control_params.c
+53
View File
@@ -0,0 +1,53 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Author: @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mc_att_control_params.c
* Parameters for multicopter attitude controller.
*/
#include <systemlib/param/param.h>
PARAM_DEFINE_FLOAT(MC_ATT_P, 6.0f);
PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f);
PARAM_DEFINE_FLOAT(MC_YAW_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.1f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
src/modules/multirotor_pos_control/module.mk → src/modules/mc_att_control/module.mk
+5 -6
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
# Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -32,11 +32,10 @@
############################################################################
#
# Build multirotor position control
# Multirotor attitude controller (vector based, no Euler singularities)
#
MODULE_COMMAND = multirotor_pos_control
MODULE_COMMAND = mc_att_control
SRCS = multirotor_pos_control.c \
multirotor_pos_control_params.c \
thrust_pid.c
SRCS = mc_att_control_main.cpp \
mc_att_control_params.c
src/modules/mc_pos_control/mc_pos_control_main.cpp
+1031
View File
File diff suppressed because it is too large Load Diff
src/modules/multirotor_pos_control/thrust_pid.h → src/modules/mc_pos_control/mc_pos_control_params.c
+22 -40
View File
@@ -1,7 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <anton.babushkin@me.com>
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -33,44 +33,26 @@
****************************************************************************/
/**
* @file thrust_pid.h
*
* Definition of thrust control PID interface.
*
* @author Anton Babushkin <anton.babushkin@me.com>
* @file mc_pos_control_params.c
* Multicopter position controller parameters.
*/
#ifndef THRUST_PID_H_
#define THRUST_PID_H_
#include <systemlib/param/param.h>
#include <stdint.h>
__BEGIN_DECLS
/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error */
#define THRUST_PID_MODE_DERIVATIV_CALC 0
/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored */
#define THRUST_PID_MODE_DERIVATIV_CALC_NO_SP 1
typedef struct {
float kp;
float ki;
float kd;
float sp;
float integral;
float error_previous;
float last_output;
float limit_min;
float limit_max;
float dt_min;
uint8_t mode;
} thrust_pid_t;
__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min);
__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max);
__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22);
__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i);
__END_DECLS
#endif /* THRUST_PID_H_ */
PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.0f);
PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f);
PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.02f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f);
PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f);
PARAM_DEFINE_FLOAT(MPC_XY_P, 1.0f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.02f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f);
PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f);
PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 1.0f);
PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f);
PARAM_DEFINE_FLOAT(MPC_LAND_TILT, 0.3f);
src/modules/multirotor_att_control/module.mk → src/modules/mc_pos_control/module.mk
Executable → Regular
+5 -6
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
# Copyright (c) 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -32,11 +32,10 @@
############################################################################
#
# Build multirotor attitude controller
# Build multicopter position controller
#
MODULE_COMMAND = multirotor_att_control
MODULE_COMMAND = mc_pos_control
SRCS = multirotor_att_control_main.c \
multirotor_attitude_control.c \
multirotor_rate_control.c
SRCS = mc_pos_control_main.cpp \
mc_pos_control_params.c
src/modules/multirotor_att_control/multirotor_att_control_main.c
-465
View File
@@ -1,465 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file multirotor_att_control_main.c
*
* Implementation of multirotor attitude control main loop.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <debug.h>
#include <getopt.h>
#include <time.h>
#include <math.h>
#include <poll.h>
#include <sys/prctl.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <drivers/drv_gyro.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/offboard_control_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include "multirotor_attitude_control.h"
#include "multirotor_rate_control.h"
__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[])
{
/* declare and safely initialize all structs */
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_attitude_setpoint_s att_sp;
memset(&att_sp, 0, sizeof(att_sp));
struct offboard_control_setpoint_s offboard_sp;
memset(&offboard_sp, 0, sizeof(offboard_sp));
struct vehicle_control_mode_s control_mode;
memset(&control_mode, 0, sizeof(control_mode));
struct manual_control_setpoint_s manual;
memset(&manual, 0, sizeof(manual));
struct sensor_combined_s sensor;
memset(&sensor, 0, sizeof(sensor));
struct vehicle_rates_setpoint_s rates_sp;
memset(&rates_sp, 0, sizeof(rates_sp));
struct vehicle_status_s status;
memset(&status, 0, sizeof(status));
struct actuator_controls_s actuators;
memset(&actuators, 0, sizeof(actuators));
/* subscribe */
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
int vehicle_attitude_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int offboard_control_setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
int vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
int vehicle_rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
/* publish actuator controls */
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
actuators.control[i] = 0.0f;
}
orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
/* register the perf counter */
perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control_runtime");
perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "multirotor_att_control_interval");
perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err");
warnx("starting");
/* store last control mode to detect mode switches */
bool control_yaw_position = true;
bool reset_yaw_sp = true;
struct pollfd fds[1] = {
{ .fd = vehicle_attitude_sub, .events = POLLIN },
};
while (!thread_should_exit) {
/* wait for a sensor update, check for exit condition every 500 ms */
int ret = poll(fds, 1, 500);
if (ret < 0) {
/* poll error, count it in perf */
perf_count(mc_err_perf);
} else if (ret > 0) {
/* only run controller if attitude changed */
perf_begin(mc_loop_perf);
/* attitude */
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
bool updated;
/* parameters */
orb_check(parameter_update_sub, &updated);
if (updated) {
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
/* update parameters */
}
/* control mode */
orb_check(vehicle_control_mode_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub, &control_mode);
}
/* manual control setpoint */
orb_check(manual_control_setpoint_sub, &updated);
if (updated) {
orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual);
}
/* attitude setpoint */
orb_check(vehicle_attitude_setpoint_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp);
}
/* offboard control setpoint */
orb_check(offboard_control_setpoint_sub, &updated);
if (updated) {
orb_copy(ORB_ID(offboard_control_setpoint), offboard_control_setpoint_sub, &offboard_sp);
}
/* vehicle status */
orb_check(vehicle_status_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &status);
}
/* sensors */
orb_check(sensor_combined_sub, &updated);
if (updated) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
}
/* set flag to safe value */
control_yaw_position = true;
/* reset yaw setpoint if not armed */
if (!control_mode.flag_armed) {
reset_yaw_sp = true;
}
/* define which input is the dominating control input */
if (control_mode.flag_control_offboard_enabled) {
/* offboard inputs */
if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
rates_sp.roll = offboard_sp.p1;
rates_sp.pitch = offboard_sp.p2;
rates_sp.yaw = offboard_sp.p3;
rates_sp.thrust = offboard_sp.p4;
rates_sp.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
att_sp.roll_body = offboard_sp.p1;
att_sp.pitch_body = offboard_sp.p2;
att_sp.yaw_body = offboard_sp.p3;
att_sp.thrust = offboard_sp.p4;
att_sp.timestamp = hrt_absolute_time();
/* publish the result to the vehicle actuators */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
/* reset yaw setpoint after offboard control */
reset_yaw_sp = true;
} else if (control_mode.flag_control_manual_enabled) {
/* manual input */
if (control_mode.flag_control_attitude_enabled) {
/* control attitude, update attitude setpoint depending on mode */
if (att_sp.thrust < 0.1f) {
/* no thrust, don't try to control yaw */
rates_sp.yaw = 0.0f;
control_yaw_position = false;
if (status.condition_landed) {
/* reset yaw setpoint if on ground */
reset_yaw_sp = true;
}
} else {
/* only move yaw setpoint if manual input is != 0 */
if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) {
/* control yaw rate */
control_yaw_position = false;
rates_sp.yaw = manual.yaw;
reset_yaw_sp = true; // has no effect on control, just for beautiful log
} else {
control_yaw_position = true;
}
}
if (!control_mode.flag_control_velocity_enabled) {
/* 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) {
/* pass throttle directly if not in altitude control mode */
att_sp.thrust = manual.throttle;
}
}
/* 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) {
printf("testmode");
att_sp.roll_body = 0.0f;
att_sp.pitch_body = 0.0f;
att_sp.yaw_body = 0.0f;
att_sp.thrust = 0.1f;
}
att_sp.timestamp = hrt_absolute_time();
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
} else {
/* manual rate inputs (ACRO), from RC control or joystick */
if (control_mode.flag_control_rates_enabled) {
rates_sp.roll = manual.roll;
rates_sp.pitch = manual.pitch;
rates_sp.yaw = manual.yaw;
rates_sp.thrust = manual.throttle;
rates_sp.timestamp = hrt_absolute_time();
}
/* reset yaw setpoint after ACRO */
reset_yaw_sp = true;
}
} else {
if (!control_mode.flag_control_attitude_enabled) {
/* no control, try to stay on place */
if (!control_mode.flag_control_velocity_enabled) {
/* no velocity control, reset attitude setpoint */
att_sp.roll_body = 0.0f;
att_sp.pitch_body = 0.0f;
att_sp.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
}
/* reset yaw setpoint after non-manual control */
reset_yaw_sp = true;
}
/* check if we should we reset integrals */
bool reset_integral = !control_mode.flag_armed || att_sp.thrust < 0.1f; // TODO use landed status instead of throttle
/* run attitude controller if needed */
if (control_mode.flag_control_attitude_enabled) {
multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral);
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
}
/* measure in what intervals the controller runs */
perf_count(mc_interval_perf);
/* run rates controller if needed */
if (control_mode.flag_control_rates_enabled) {
/* get current rate setpoint */
bool rates_sp_updated = false;
orb_check(vehicle_rates_setpoint_sub, &rates_sp_updated);
if (rates_sp_updated) {
orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rates_setpoint_sub, &rates_sp);
}
/* apply controller */
float rates[3];
rates[0] = att.rollspeed;
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;
actuators.control[1] = 0.0f;
actuators.control[2] = 0.0f;
actuators.control[3] = 0.0f;
}
/* fill in manual control values */
actuators.control[4] = manual.flaps;
actuators.control[5] = manual.aux1;
actuators.control[6] = manual.aux2;
actuators.control[7] = manual.aux3;
actuators.timestamp = hrt_absolute_time();
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
perf_end(mc_loop_perf);
}
}
warnx("stopping, disarming motors");
/* kill all outputs */
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
actuators.control[i] = 0.0f;
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
close(vehicle_attitude_sub);
close(vehicle_control_mode_sub);
close(manual_control_setpoint_sub);
close(actuator_pub);
close(att_sp_pub);
perf_print_counter(mc_loop_perf);
perf_free(mc_loop_perf);
fflush(stdout);
exit(0);
}
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: multirotor_att_control [-m <mode>] [-t] {start|status|stop}\n");
fprintf(stderr, " <mode> is 'rates' or 'attitude'\n");
fprintf(stderr, " -t enables motor test mode with 10%% thrust\n");
exit(1);
}
int multirotor_att_control_main(int argc, char *argv[])
{
int ch;
unsigned int optioncount = 0;
while ((ch = getopt(argc, argv, "tm:")) != EOF) {
switch (ch) {
case 't':
motor_test_mode = true;
optioncount += 1;
break;
case ':':
usage("missing parameter");
break;
default:
fprintf(stderr, "option: -%c\n", ch);
usage("unrecognized option");
break;
}
}
argc -= optioncount;
//argv += optioncount;
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1 + optioncount], "start")) {
thread_should_exit = false;
mc_task = task_spawn_cmd("multirotor_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 15,
2048,
mc_thread_main,
NULL);
exit(0);
}
if (!strcmp(argv[1 + optioncount], "stop")) {
thread_should_exit = true;
exit(0);
}
usage("unrecognized command");
exit(1);
}
src/modules/multirotor_att_control/multirotor_attitude_control.c
-254
View File
@@ -1,254 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Laurens Mackay <mackayl@student.ethz.ch>
* Tobias Naegeli <naegelit@student.ethz.ch>
* Martin Rutschmann <rutmarti@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file multirotor_attitude_control.c
*
* Implementation of attitude controller for multirotors.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Laurens Mackay <mackayl@student.ethz.ch>
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include "multirotor_attitude_control.h"
#include <stdio.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <stdbool.h>
#include <float.h>
#include <math.h>
#include <systemlib/pid/pid.h>
#include <systemlib/param/param.h>
#include <drivers/drv_hrt.h>
PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 2.0f);
PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f);
PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f);
//PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f);
//PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f);
PARAM_DEFINE_FLOAT(MC_ATT_P, 6.8f);
PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_ATT_D, 0.0f);
//PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f);
//PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f);
//PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f);
//PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f);
struct mc_att_control_params {
float yaw_p;
float yaw_i;
float yaw_d;
//float yaw_awu;
//float yaw_lim;
float att_p;
float att_i;
float att_d;
//float att_awu;
//float att_lim;
//float att_xoff;
//float att_yoff;
};
struct mc_att_control_param_handles {
param_t yaw_p;
param_t yaw_i;
param_t yaw_d;
//param_t yaw_awu;
//param_t yaw_lim;
param_t att_p;
param_t att_i;
param_t att_d;
//param_t att_awu;
//param_t att_lim;
//param_t att_xoff;
//param_t att_yoff;
};
/**
* Initialize all parameter handles and values
*
*/
static int parameters_init(struct mc_att_control_param_handles *h);
/**
* Update all parameters
*
*/
static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p);
static int parameters_init(struct mc_att_control_param_handles *h)
{
/* PID parameters */
h->yaw_p = param_find("MC_YAWPOS_P");
h->yaw_i = param_find("MC_YAWPOS_I");
h->yaw_d = param_find("MC_YAWPOS_D");
//h->yaw_awu = param_find("MC_YAWPOS_AWU");
//h->yaw_lim = param_find("MC_YAWPOS_LIM");
h->att_p = param_find("MC_ATT_P");
h->att_i = param_find("MC_ATT_I");
h->att_d = param_find("MC_ATT_D");
//h->att_awu = param_find("MC_ATT_AWU");
//h->att_lim = param_find("MC_ATT_LIM");
//h->att_xoff = param_find("MC_ATT_XOFF");
//h->att_yoff = param_find("MC_ATT_YOFF");
return OK;
}
static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p)
{
param_get(h->yaw_p, &(p->yaw_p));
param_get(h->yaw_i, &(p->yaw_i));
param_get(h->yaw_d, &(p->yaw_d));
//param_get(h->yaw_awu, &(p->yaw_awu));
//param_get(h->yaw_lim, &(p->yaw_lim));
param_get(h->att_p, &(p->att_p));
param_get(h->att_i, &(p->att_i));
param_get(h->att_d, &(p->att_d));
//param_get(h->att_awu, &(p->att_awu));
//param_get(h->att_lim, &(p->att_lim));
//param_get(h->att_xoff, &(p->att_xoff));
//param_get(h->att_yoff, &(p->att_yoff));
return OK;
}
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral)
{
static uint64_t last_run = 0;
static uint64_t last_input = 0;
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
if (last_input != att_sp->timestamp) {
last_input = att_sp->timestamp;
}
static int motor_skip_counter = 0;
static PID_t pitch_controller;
static PID_t roll_controller;
static struct mc_att_control_params p;
static struct mc_att_control_param_handles h;
static bool initialized = false;
static float yaw_error;
/* initialize the pid controllers when the function is called for the first time */
if (initialized == false) {
parameters_init(&h);
parameters_update(&h, &p);
pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f);
pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f);
initialized = true;
}
/* load new parameters with lower rate */
if (motor_skip_counter % 500 == 0) {
/* update parameters from storage */
parameters_update(&h, &p);
/* apply parameters */
pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
}
/* reset integrals if needed */
if (reset_integral) {
pid_reset_integral(&pitch_controller);
pid_reset_integral(&roll_controller);
//TODO pid_reset_integral(&yaw_controller);
}
/* calculate current control outputs */
/* control pitch (forward) output */
rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body ,
att->pitch, att->pitchspeed, deltaT);
/* control roll (left/right) output */
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body ,
att->roll, att->rollspeed, deltaT);
if (control_yaw_position) {
/* control yaw rate */
// TODO use pid lib
/* positive error: rotate to right, negative error, rotate to left (NED frame) */
// yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw);
yaw_error = att_sp->yaw_body - att->yaw;
if (yaw_error > M_PI_F) {
yaw_error -= M_TWOPI_F;
} else if (yaw_error < -M_PI_F) {
yaw_error += M_TWOPI_F;
}
rates_sp->yaw = p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed);
}
rates_sp->thrust = att_sp->thrust;
//need to update the timestamp now that we've touched rates_sp
rates_sp->timestamp = hrt_absolute_time();
motor_skip_counter++;
}
src/modules/multirotor_att_control/multirotor_attitude_control.h
-65
View File
@@ -1,65 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Laurens Mackay <mackayl@student.ethz.ch>
* Tobias Naegeli <naegelit@student.ethz.ch>
* Martin Rutschmann <rutmarti@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file multirotor_attitude_control.h
*
* Definition of attitude controller for multirotors.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Laurens Mackay <mackayl@student.ethz.ch>
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#ifndef MULTIROTOR_ATTITUDE_CONTROL_H_
#define MULTIROTOR_ATTITUDE_CONTROL_H_
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_controls.h>
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral);
#endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */
src/modules/multirotor_att_control/multirotor_rate_control.c
-196
View File
@@ -1,196 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
* Julian Oes <joes@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file multirotor_rate_control.c
*
* Implementation of rate controller for multirotors.
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Julian Oes <joes@student.ethz.ch>
*/
#include "multirotor_rate_control.h"
#include <stdio.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <stdbool.h>
#include <float.h>
#include <math.h>
#include <systemlib/pid/pid.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.005f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.2f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
struct mc_rate_control_params {
float yawrate_p;
float yawrate_d;
float yawrate_i;
float attrate_p;
float attrate_d;
float attrate_i;
float rate_lim;
};
struct mc_rate_control_param_handles {
param_t yawrate_p;
param_t yawrate_i;
param_t yawrate_d;
param_t attrate_p;
param_t attrate_i;
param_t attrate_d;
};
/**
* Initialize all parameter handles and values
*
*/
static int parameters_init(struct mc_rate_control_param_handles *h);
/**
* Update all parameters
*
*/
static int parameters_update(const struct mc_rate_control_param_handles *h, struct mc_rate_control_params *p);
static int parameters_init(struct mc_rate_control_param_handles *h)
{
/* PID parameters */
h->yawrate_p = param_find("MC_YAWRATE_P");
h->yawrate_i = param_find("MC_YAWRATE_I");
h->yawrate_d = param_find("MC_YAWRATE_D");
h->attrate_p = param_find("MC_ATTRATE_P");
h->attrate_i = param_find("MC_ATTRATE_I");
h->attrate_d = param_find("MC_ATTRATE_D");
return OK;
}
static int parameters_update(const struct mc_rate_control_param_handles *h, struct mc_rate_control_params *p)
{
param_get(h->yawrate_p, &(p->yawrate_p));
param_get(h->yawrate_i, &(p->yawrate_i));
param_get(h->yawrate_d, &(p->yawrate_d));
param_get(h->attrate_p, &(p->attrate_p));
param_get(h->attrate_i, &(p->attrate_i));
param_get(h->attrate_d, &(p->attrate_d));
return OK;
}
void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[], struct actuator_controls_s *actuators, bool reset_integral)
{
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
static uint64_t last_input = 0;
if (last_input != rate_sp->timestamp) {
last_input = rate_sp->timestamp;
}
last_run = hrt_absolute_time();
static int motor_skip_counter = 0;
static PID_t pitch_rate_controller;
static PID_t roll_rate_controller;
static PID_t yaw_rate_controller;
static struct mc_rate_control_params p;
static struct mc_rate_control_param_handles h;
static bool initialized = false;
/* initialize the pid controllers when the function is called for the first time */
if (initialized == false) {
parameters_init(&h);
parameters_update(&h, &p);
initialized = true;
pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
}
/* load new parameters with lower rate */
if (motor_skip_counter % 2500 == 0) {
/* update parameters from storage */
parameters_update(&h, &p);
pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f);
}
/* reset integrals if needed */
if (reset_integral) {
pid_reset_integral(&pitch_rate_controller);
pid_reset_integral(&roll_rate_controller);
pid_reset_integral(&yaw_rate_controller);
}
/* run pitch, roll and yaw controllers */
float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
float yaw_control = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
actuators->control[0] = roll_control;
actuators->control[1] = pitch_control;
actuators->control[2] = yaw_control;
actuators->control[3] = rate_sp->thrust;
motor_skip_counter++;
}
src/modules/multirotor_pos_control/multirotor_pos_control.c
-553
View File
@@ -1,553 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file multirotor_pos_control.c
*
* Multirotor position controller
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <stdbool.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <debug.h>
#include <termios.h>
#include <time.h>
#include <sys/prctl.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <uORB/topics/mission_item_triplet.h>
#include <systemlib/systemlib.h>
#include <systemlib/pid/pid.h>
#include <mavlink/mavlink_log.h>
#include "multirotor_pos_control_params.h"
#include "thrust_pid.h"
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
__EXPORT int multirotor_pos_control_main(int argc, char *argv[]);
/**
* Mainloop of position controller.
*/
static int multirotor_pos_control_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static float scale_control(float ctl, float end, float dz);
static float norm(float x, float y);
static void usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: multirotor_pos_control {start|stop|status}\n\n");
exit(1);
}
/**
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_spawn().
*/
int multirotor_pos_control_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("already running");
/* this is not an error */
exit(0);
}
warnx("start");
thread_should_exit = false;
deamon_task = task_spawn_cmd("multirotor_pos_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 60,
4096,
multirotor_pos_control_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
warnx("stop");
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("app is running");
} else {
warnx("app not started");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
static float scale_control(float ctl, float end, float dz)
{
if (ctl > dz) {
return (ctl - dz) / (end - dz);
} else if (ctl < -dz) {
return (ctl + dz) / (end - dz);
} else {
return 0.0f;
}
}
static float norm(float x, float y)
{
return sqrtf(x * x + y * y);
}
static int multirotor_pos_control_thread_main(int argc, char *argv[])
{
/* welcome user */
warnx("started");
static int mavlink_fd;
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(mavlink_fd, "[mpc] started");
/* structures */
struct vehicle_control_mode_s control_mode;
memset(&control_mode, 0, sizeof(control_mode));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_attitude_setpoint_s att_sp;
memset(&att_sp, 0, sizeof(att_sp));
struct manual_control_setpoint_s manual;
memset(&manual, 0, sizeof(manual));
struct vehicle_local_position_s local_pos;
memset(&local_pos, 0, sizeof(local_pos));
struct mission_item_triplet_s triplet;
memset(&triplet, 0, sizeof(triplet));
struct vehicle_global_velocity_setpoint_s global_vel_sp;
memset(&global_vel_sp, 0, sizeof(global_vel_sp));
struct vehicle_local_position_setpoint_s local_pos_sp;
memset(&local_pos_sp, 0, sizeof(local_pos_sp));
/* subscribe to attitude, motor setpoints and system state */
int param_sub = orb_subscribe(ORB_ID(parameter_update));
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int mission_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet));
/* publish setpoint */
orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp);
orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp);
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
bool reset_mission_sp = false;
bool global_pos_sp_valid = false;
bool reset_man_sp_z = true;
bool reset_man_sp_xy = true;
bool reset_int_z = true;
bool reset_int_z_manual = false;
bool reset_int_xy = true;
bool was_armed = false;
bool reset_auto_sp_xy = true;
bool reset_auto_sp_z = true;
bool reset_takeoff_sp = true;
hrt_abstime t_prev = 0;
const float alt_ctl_dz = 0.2f;
const float pos_ctl_dz = 0.05f;
float ref_alt = 0.0f;
hrt_abstime ref_alt_t = 0;
uint64_t local_ref_timestamp = 0;
PID_t xy_pos_pids[2];
PID_t xy_vel_pids[2];
PID_t z_pos_pid;
thrust_pid_t z_vel_pid;
thread_running = true;
struct multirotor_position_control_params params;
struct multirotor_position_control_param_handles params_h;
parameters_init(&params_h);
parameters_update(&params_h, &params);
for (int i = 0; i < 2; i++) {
pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f);
pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
}
pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f);
thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
while (!thread_should_exit) {
bool param_updated;
orb_check(param_sub, &param_updated);
if (param_updated) {
/* clear updated flag */
struct parameter_update_s ps;
orb_copy(ORB_ID(parameter_update), param_sub, &ps);
/* update params */
parameters_update(&params_h, &params);
for (int i = 0; i < 2; i++) {
pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f);
/* use integral_limit_out = tilt_max / 2 */
float i_limit;
if (params.xy_vel_i > 0.0f) {
i_limit = params.tilt_max / params.xy_vel_i / 2.0f;
} else {
i_limit = 0.0f; // not used
}
pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max);
}
pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max);
thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min);
}
bool updated;
orb_check(control_mode_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
}
orb_check(mission_triplet_sub, &updated);
if (updated) {
orb_copy(ORB_ID(mission_item_triplet), mission_triplet_sub, &triplet);
global_pos_sp_valid = triplet.current_valid;
reset_mission_sp = true;
}
hrt_abstime t = hrt_absolute_time();
float dt;
if (t_prev != 0) {
dt = (t - t_prev) * 0.000001f;
} else {
dt = 0.0f;
}
if (control_mode.flag_armed && !was_armed) {
/* reset setpoints and integrals on arming */
reset_man_sp_z = true;
reset_man_sp_xy = true;
reset_auto_sp_z = true;
reset_auto_sp_xy = true;
reset_takeoff_sp = true;
reset_int_z = true;
reset_int_xy = true;
}
was_armed = control_mode.flag_armed;
t_prev = t;
if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) {
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f;
float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f;
float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f };
if (control_mode.flag_control_manual_enabled) {
/* manual control */
/* check for reference point updates and correct setpoint */
if (local_pos.ref_timestamp != ref_alt_t) {
if (ref_alt_t != 0) {
/* home alt changed, don't follow large ground level changes in manual flight */
local_pos_sp.z += local_pos.ref_alt - ref_alt;
}
ref_alt_t = local_pos.ref_timestamp;
ref_alt = local_pos.ref_alt;
// TODO also correct XY setpoint
}
/* reset setpoints to current position if needed */
if (control_mode.flag_control_altitude_enabled) {
if (reset_man_sp_z) {
reset_man_sp_z = false;
local_pos_sp.z = local_pos.z;
mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z);
}
/* move altitude setpoint with throttle stick */
float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
if (z_sp_ctl != 0.0f) {
sp_move_rate[2] = -z_sp_ctl * params.z_vel_max;
local_pos_sp.z += sp_move_rate[2] * dt;
if (local_pos_sp.z > local_pos.z + z_sp_offs_max) {
local_pos_sp.z = local_pos.z + z_sp_offs_max;
} else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) {
local_pos_sp.z = local_pos.z - z_sp_offs_max;
}
}
}
if (control_mode.flag_control_position_enabled) {
if (reset_man_sp_xy) {
reset_man_sp_xy = false;
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
pid_reset_integral(&xy_vel_pids[0]);
pid_reset_integral(&xy_vel_pids[1]);
mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
}
/* move position setpoint with roll/pitch stick */
float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz);
float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz);
if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) {
/* calculate direction and increment of control in NED frame */
float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl);
float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max;
sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
local_pos_sp.x += sp_move_rate[0] * dt;
local_pos_sp.y += sp_move_rate[1] * dt;
/* limit maximum setpoint from position offset and preserve direction
* fail safe, should not happen in normal operation */
float pos_vec_x = local_pos_sp.x - local_pos.x;
float pos_vec_y = local_pos_sp.y - local_pos.y;
float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max;
if (pos_vec_norm > 1.0f) {
local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm;
local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm;
}
}
}
/* copy yaw setpoint to vehicle_local_position_setpoint topic */
local_pos_sp.yaw = att_sp.yaw_body;
/* local position setpoint is valid and can be used for auto loiter after position controlled mode */
reset_auto_sp_xy = !control_mode.flag_control_position_enabled;
reset_auto_sp_z = !control_mode.flag_control_altitude_enabled;
reset_takeoff_sp = true;
/* force reprojection of global setpoint after manual mode */
reset_mission_sp = true;
}
/* AUTO not implemented */
/* publish local position setpoint */
orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
/* run position & altitude controllers, calculate velocity setpoint */
if (control_mode.flag_control_altitude_enabled) {
global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2];
} else {
reset_man_sp_z = true;
global_vel_sp.vz = 0.0f;
}
if (control_mode.flag_control_position_enabled) {
/* calculate velocity set point in NED frame */
global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0];
global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1];
/* limit horizontal speed */
float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max;
if (xy_vel_sp_norm > 1.0f) {
global_vel_sp.vx /= xy_vel_sp_norm;
global_vel_sp.vy /= xy_vel_sp_norm;
}
} else {
reset_man_sp_xy = true;
global_vel_sp.vx = 0.0f;
global_vel_sp.vy = 0.0f;
}
/* publish new velocity setpoint */
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp);
// TODO subscribe to velocity setpoint if altitude/position control disabled
if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) {
/* run velocity controllers, calculate thrust vector with attitude-thrust compensation */
float thrust_sp[3] = { 0.0f, 0.0f, 0.0f };
if (control_mode.flag_control_climb_rate_enabled) {
if (reset_int_z) {
reset_int_z = false;
float i = params.thr_min;
if (reset_int_z_manual) {
i = manual.throttle;
if (i < params.thr_min) {
i = params.thr_min;
} else if (i > params.thr_max) {
i = params.thr_max;
}
}
thrust_pid_set_integral(&z_vel_pid, -i);
mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i);
}
thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]);
att_sp.thrust = -thrust_sp[2];
} else {
/* reset thrust integral when altitude control enabled */
reset_int_z = true;
}
if (control_mode.flag_control_velocity_enabled) {
/* calculate thrust set point in NED frame */
if (reset_int_xy) {
reset_int_xy = false;
pid_reset_integral(&xy_vel_pids[0]);
pid_reset_integral(&xy_vel_pids[1]);
mavlink_log_info(mavlink_fd, "[mpc] reset pos integral");
}
thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt);
thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt);
/* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */
/* limit horizontal part of thrust */
float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]);
/* assuming that vertical component of thrust is g,
* horizontal component = g * tan(alpha) */
float tilt = atanf(norm(thrust_sp[0], thrust_sp[1]));
if (tilt > params.tilt_max) {
tilt = params.tilt_max;
}
/* convert direction to body frame */
thrust_xy_dir -= att.yaw;
/* calculate roll and pitch */
att_sp.roll_body = sinf(thrust_xy_dir) * tilt;
att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body);
} else {
reset_int_xy = true;
}
att_sp.timestamp = hrt_absolute_time();
/* publish new attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
} else {
/* position controller disabled, reset setpoints */
reset_man_sp_z = true;
reset_man_sp_xy = true;
reset_int_z = true;
reset_int_xy = true;
reset_mission_sp = true;
reset_auto_sp_xy = true;
reset_auto_sp_z = true;
}
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled;
/* run at approximately 50 Hz */
usleep(20000);
}
warnx("stopped");
mavlink_log_info(mavlink_fd, "[mpc] stopped");
thread_running = false;
fflush(stdout);
return 0;
}
src/modules/multirotor_pos_control/multirotor_pos_control_params.c
-112
View File
@@ -1,112 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file multirotor_pos_control_params.c
*
* Parameters for multirotor_pos_control
*/
#include "multirotor_pos_control_params.h"
/* controller parameters */
PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.2f);
PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.8f);
PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f);
PARAM_DEFINE_FLOAT(MPC_Z_D, 0.0f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.0f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f);
PARAM_DEFINE_FLOAT(MPC_XY_P, 0.5f);
PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.2f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.0f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f);
PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f);
int parameters_init(struct multirotor_position_control_param_handles *h)
{
h->takeoff_alt = param_find("NAV_TAKEOFF_ALT");
h->takeoff_gap = param_find("NAV_TAKEOFF_GAP");
h->thr_min = param_find("MPC_THR_MIN");
h->thr_max = param_find("MPC_THR_MAX");
h->z_p = param_find("MPC_Z_P");
h->z_d = param_find("MPC_Z_D");
h->z_vel_p = param_find("MPC_Z_VEL_P");
h->z_vel_i = param_find("MPC_Z_VEL_I");
h->z_vel_d = param_find("MPC_Z_VEL_D");
h->z_vel_max = param_find("MPC_Z_VEL_MAX");
h->xy_p = param_find("MPC_XY_P");
h->xy_d = param_find("MPC_XY_D");
h->xy_vel_p = param_find("MPC_XY_VEL_P");
h->xy_vel_i = param_find("MPC_XY_VEL_I");
h->xy_vel_d = param_find("MPC_XY_VEL_D");
h->xy_vel_max = param_find("MPC_XY_VEL_MAX");
h->tilt_max = param_find("MPC_TILT_MAX");
h->rc_scale_pitch = param_find("RC_SCALE_PITCH");
h->rc_scale_roll = param_find("RC_SCALE_ROLL");
h->rc_scale_yaw = param_find("RC_SCALE_YAW");
return OK;
}
int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p)
{
param_get(h->takeoff_alt, &(p->takeoff_alt));
param_get(h->takeoff_gap, &(p->takeoff_gap));
param_get(h->thr_min, &(p->thr_min));
param_get(h->thr_max, &(p->thr_max));
param_get(h->z_p, &(p->z_p));
param_get(h->z_d, &(p->z_d));
param_get(h->z_vel_p, &(p->z_vel_p));
param_get(h->z_vel_i, &(p->z_vel_i));
param_get(h->z_vel_d, &(p->z_vel_d));
param_get(h->z_vel_max, &(p->z_vel_max));
param_get(h->xy_p, &(p->xy_p));
param_get(h->xy_d, &(p->xy_d));
param_get(h->xy_vel_p, &(p->xy_vel_p));
param_get(h->xy_vel_i, &(p->xy_vel_i));
param_get(h->xy_vel_d, &(p->xy_vel_d));
param_get(h->xy_vel_max, &(p->xy_vel_max));
param_get(h->tilt_max, &(p->tilt_max));
param_get(h->rc_scale_pitch, &(p->rc_scale_pitch));
param_get(h->rc_scale_roll, &(p->rc_scale_roll));
param_get(h->rc_scale_yaw, &(p->rc_scale_yaw));
return OK;
}
src/modules/multirotor_pos_control/multirotor_pos_control_params.h
-101
View File
@@ -1,101 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file multirotor_pos_control_params.h
*
* Parameters for multirotor_pos_control
*/
#include <systemlib/param/param.h>
struct multirotor_position_control_params {
float takeoff_alt;
float takeoff_gap;
float thr_min;
float thr_max;
float z_p;
float z_d;
float z_vel_p;
float z_vel_i;
float z_vel_d;
float z_vel_max;
float xy_p;
float xy_d;
float xy_vel_p;
float xy_vel_i;
float xy_vel_d;
float xy_vel_max;
float tilt_max;
float rc_scale_pitch;
float rc_scale_roll;
float rc_scale_yaw;
};
struct multirotor_position_control_param_handles {
param_t takeoff_alt;
param_t takeoff_gap;
param_t thr_min;
param_t thr_max;
param_t z_p;
param_t z_d;
param_t z_vel_p;
param_t z_vel_i;
param_t z_vel_d;
param_t z_vel_max;
param_t xy_p;
param_t xy_d;
param_t xy_vel_p;
param_t xy_vel_i;
param_t xy_vel_d;
param_t xy_vel_max;
param_t tilt_max;
param_t rc_scale_pitch;
param_t rc_scale_roll;
param_t rc_scale_yaw;
};
/**
* Initialize all parameter handles and values
*
*/
int parameters_init(struct multirotor_position_control_param_handles *h);
/**
* Update all parameters
*
*/
int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p);
src/modules/multirotor_pos_control/thrust_pid.c
-189
View File
@@ -1,189 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file thrust_pid.c
*
* Implementation of thrust control PID.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include "thrust_pid.h"
#include <math.h>
__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min)
{
pid->kp = kp;
pid->ki = ki;
pid->kd = kd;
pid->limit_min = limit_min;
pid->limit_max = limit_max;
pid->mode = mode;
pid->dt_min = dt_min;
pid->last_output = 0.0f;
pid->sp = 0.0f;
pid->error_previous = 0.0f;
pid->integral = 0.0f;
}
__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max)
{
int ret = 0;
if (isfinite(kp)) {
pid->kp = kp;
} else {
ret = 1;
}
if (isfinite(ki)) {
pid->ki = ki;
} else {
ret = 1;
}
if (isfinite(kd)) {
pid->kd = kd;
} else {
ret = 1;
}
if (isfinite(limit_min)) {
pid->limit_min = limit_min;
} else {
ret = 1;
}
if (isfinite(limit_max)) {
pid->limit_max = limit_max;
} else {
ret = 1;
}
return ret;
}
__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22)
{
/* Alternative integral component calculation
*
* start:
* error = setpoint - current_value
* integral = integral + (Ki * error * dt)
* derivative = (error - previous_error) / dt
* previous_error = error
* output = (Kp * error) + integral + (Kd * derivative)
* wait(dt)
* goto start
*/
if (!isfinite(sp) || !isfinite(val) || !isfinite(dt)) {
return pid->last_output;
}
float i, d;
pid->sp = sp;
// Calculated current error value
float error = pid->sp - val;
// Calculate or measured current error derivative
if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC) {
d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
pid->error_previous = error;
} else if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC_NO_SP) {
d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
pid->error_previous = -val;
} else {
d = 0.0f;
}
if (!isfinite(d)) {
d = 0.0f;
}
/* calculate the error integral */
i = pid->integral + (pid->ki * error * dt);
/* attitude-thrust compensation
* r22 is (2, 2) componet of rotation matrix for current attitude */
float att_comp;
if (r22 > 0.8f)
att_comp = 1.0f / r22;
else if (r22 > 0.0f)
att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * r22 + 1.0f;
else
att_comp = 1.0f;
/* calculate output */
float output = ((error * pid->kp) + i + (d * pid->kd)) * att_comp;
/* check for saturation */
if (output < pid->limit_min || output > pid->limit_max) {
/* saturated, recalculate output with old integral */
output = (error * pid->kp) + pid->integral + (d * pid->kd);
} else {
if (isfinite(i)) {
pid->integral = i;
}
}
if (isfinite(output)) {
if (output > pid->limit_max) {
output = pid->limit_max;
} else if (output < pid->limit_min) {
output = pid->limit_min;
}
pid->last_output = output;
}
return pid->last_output;
}
__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i)
{
pid->integral = i;
}
src/modules/navigator/navigator_main.cpp
+588 -423
View File
File diff suppressed because it is too large Load Diff
src/modules/navigator/navigator_params.c
+5 -2
View File
@@ -1,8 +1,9 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @autho Lorenz Meier <lm@inf.ethz.ch>
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -40,6 +41,7 @@
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <nuttx/config.h>
@@ -55,6 +57,7 @@ PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f);
PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f);
PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 100.0f);
PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0);
PARAM_DEFINE_FLOAT(NAV_TAAKEOFF_ALT, 10.0f); // default TAKEOFF altitude
PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); // default TAKEOFF altitude
PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing
PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode
PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, 10.0f); // delay after descend before landing
src/modules/position_estimator_inav/position_estimator_inav_main.c
+425 -203
View File
@@ -71,15 +71,21 @@
#include "position_estimator_inav_params.h"
#include "inertial_filter.h"
#define MIN_VALID_W 0.00001f
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 const hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s
static const hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s
static const hrt_abstime gps_topic_timeout = 1000000; // GPS topic timeout = 1s
static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s
static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms
static const hrt_abstime sonar_valid_timeout = 1000000; // estimate sonar distance during this time after sonar loss
static const hrt_abstime xy_src_timeout = 2000000; // estimate position during this time after position sources loss
static const uint32_t updates_counter_len = 1000000;
static const uint32_t pub_interval = 4000; // limit publish rate to 250 Hz
static const uint32_t pub_interval = 10000; // limit publish rate to 100 Hz
static const float max_flow = 1.0f; // max flow value that can be used, rad/s
__EXPORT int position_estimator_inav_main(int argc, char *argv[]);
@@ -95,8 +101,7 @@ static void usage(const char *reason)
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr,
"usage: position_estimator_inav {start|stop|status} [-v]\n\n");
fprintf(stderr, "usage: position_estimator_inav {start|stop|status} [-v]\n\n");
exit(1);
}
@@ -115,7 +120,7 @@ int position_estimator_inav_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("position_estimator_inav already running\n");
warnx("already running");
/* this is not an error */
exit(0);
}
@@ -135,16 +140,23 @@ int position_estimator_inav_main(int argc, char *argv[])
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
if (thread_running) {
warnx("stop");
thread_should_exit = true;
} else {
warnx("app not started");
}
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tposition_estimator_inav is running\n");
warnx("app is running");
} else {
printf("\tposition_estimator_inav not started\n");
warnx("app not started");
}
exit(0);
@@ -154,32 +166,92 @@ int position_estimator_inav_main(int argc, char *argv[])
exit(1);
}
void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) {
FILE *f = fopen("/fs/microsd/inav.log", "a");
if (f) {
char *s = malloc(256);
snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2]);
fputs(f, s);
snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v);
fputs(f, s);
free(s);
}
fclose(f);
}
/****************************************************************************
* main
****************************************************************************/
int position_estimator_inav_thread_main(int argc, char *argv[])
{
warnx("started.");
warnx("started");
int mavlink_fd;
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(mavlink_fd, "[inav] started");
/* initialize values */
float x_est[3] = { 0.0f, 0.0f, 0.0f };
float y_est[3] = { 0.0f, 0.0f, 0.0f };
float z_est[3] = { 0.0f, 0.0f, 0.0f };
int baro_init_cnt = 0;
int baro_init_num = 200;
float baro_alt0 = 0.0f; /* to determine while start up */
float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted
float surface_offset = 0.0f; // ground level offset from reference altitude
float surface_offset_rate = 0.0f; // surface offset change rate
float alt_avg = 0.0f;
bool landed = true;
hrt_abstime landed_time = 0;
bool flag_armed = false;
uint32_t accel_counter = 0;
uint32_t baro_counter = 0;
bool ref_inited = false;
hrt_abstime ref_init_start = 0;
const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix
uint16_t accel_updates = 0;
uint16_t baro_updates = 0;
uint16_t gps_updates = 0;
uint16_t attitude_updates = 0;
uint16_t flow_updates = 0;
hrt_abstime updates_counter_start = hrt_absolute_time();
hrt_abstime pub_last = hrt_absolute_time();
hrt_abstime t_prev = 0;
/* acceleration in NED frame */
float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G };
/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
float corr_acc[] = { 0.0f, 0.0f, 0.0f }; // N E D
float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame
float corr_baro = 0.0f; // D
float corr_gps[3][2] = {
{ 0.0f, 0.0f }, // N (pos, vel)
{ 0.0f, 0.0f }, // E (pos, vel)
{ 0.0f, 0.0f }, // D (pos, vel)
};
float w_gps_xy = 1.0f;
float w_gps_z = 1.0f;
float corr_sonar = 0.0f;
float corr_sonar_filtered = 0.0f;
float corr_flow[] = { 0.0f, 0.0f }; // N E
float w_flow = 0.0f;
float sonar_prev = 0.0f;
hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered)
hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered)
hrt_abstime xy_src_time = 0; // time of last available position data
bool gps_valid = false; // GPS is valid
bool sonar_valid = false; // sonar is valid
bool flow_valid = false; // flow is valid
bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false)
/* declare and safely initialize all structs */
struct actuator_controls_s actuator;
memset(&actuator, 0, sizeof(actuator));
@@ -247,75 +319,29 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* mean calculation over several measurements */
if (baro_init_cnt < baro_init_num) {
baro_alt0 += sensor.baro_alt_meter;
baro_offset += sensor.baro_alt_meter;
baro_init_cnt++;
} else {
wait_baro = false;
baro_alt0 /= (float) baro_init_cnt;
warnx("init baro: alt = %.3f", baro_alt0);
mavlink_log_info(mavlink_fd, "[inav] init baro: alt = %.3f", baro_alt0);
local_pos.ref_alt = baro_alt0;
local_pos.ref_timestamp = hrt_absolute_time();
baro_offset /= (float) baro_init_cnt;
warnx("baro offs: %.2f", baro_offset);
mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset);
local_pos.z_valid = true;
local_pos.v_z_valid = true;
local_pos.z_global = true;
}
}
}
}
}
bool ref_xy_inited = false;
hrt_abstime ref_xy_init_start = 0;
const hrt_abstime ref_xy_init_delay = 5000000; // wait for 5s after 3D fix
hrt_abstime t_prev = 0;
uint16_t accel_updates = 0;
uint16_t baro_updates = 0;
uint16_t gps_updates = 0;
uint16_t attitude_updates = 0;
uint16_t flow_updates = 0;
hrt_abstime updates_counter_start = hrt_absolute_time();
hrt_abstime pub_last = hrt_absolute_time();
/* acceleration in NED frame */
float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G };
/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
float accel_corr[] = { 0.0f, 0.0f, 0.0f }; // N E D
float accel_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame
float baro_corr = 0.0f; // D
float gps_corr[2][2] = {
{ 0.0f, 0.0f }, // N (pos, vel)
{ 0.0f, 0.0f }, // E (pos, vel)
};
float sonar_corr = 0.0f;
float sonar_corr_filtered = 0.0f;
float flow_corr[] = { 0.0f, 0.0f }; // X, Y
float sonar_prev = 0.0f;
hrt_abstime sonar_time = 0;
/* main loop */
struct pollfd fds[7] = {
{ .fd = parameter_update_sub, .events = POLLIN },
{ .fd = actuator_sub, .events = POLLIN },
{ .fd = armed_sub, .events = POLLIN },
struct pollfd fds[1] = {
{ .fd = vehicle_attitude_sub, .events = POLLIN },
{ .fd = sensor_combined_sub, .events = POLLIN },
{ .fd = optical_flow_sub, .events = POLLIN },
{ .fd = vehicle_gps_position_sub, .events = POLLIN }
};
if (!thread_should_exit) {
warnx("main loop started.");
}
while (!thread_should_exit) {
int ret = poll(fds, 7, 10); // wait maximal this 10 ms = 100 Hz minimum rate
int ret = poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate
hrt_abstime t = hrt_absolute_time();
if (ret < 0) {
@@ -324,40 +350,60 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
continue;
} else if (ret > 0) {
/* act on attitude updates */
/* vehicle attitude */
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
attitude_updates++;
bool updated;
/* parameter update */
if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag */
orb_check(parameter_update_sub, &updated);
if (updated) {
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), parameter_update_sub,
&update);
/* update parameters */
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
parameters_update(&pos_inav_param_handles, &params);
}
/* actuator */
if (fds[1].revents & POLLIN) {
orb_check(actuator_sub, &updated);
if (updated) {
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator);
}
/* armed */
if (fds[2].revents & POLLIN) {
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
}
orb_check(armed_sub, &updated);
/* vehicle attitude */
if (fds[3].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
attitude_updates++;
if (updated) {
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
/* reset ground level on arm */
if (armed.armed && !flag_armed) {
flag_armed = armed.armed;
baro_offset -= z_est[0];
corr_baro = 0.0f;
local_pos.ref_alt -= z_est[0];
local_pos.ref_timestamp = t;
z_est[0] = 0.0f;
alt_avg = 0.0f;
}
}
/* sensor combined */
if (fds[4].revents & POLLIN) {
orb_check(sensor_combined_sub, &updated);
if (updated) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
if (sensor.accelerometer_counter != accel_counter) {
if (att.R_valid) {
/* correct accel bias, now only for Z */
sensor.accelerometer_m_s2[2] -= accel_bias[2];
/* correct accel bias */
sensor.accelerometer_m_s2[0] -= acc_bias[0];
sensor.accelerometer_m_s2[1] -= acc_bias[1];
sensor.accelerometer_m_s2[2] -= acc_bias[2];
/* transform acceleration vector from body frame to NED frame */
for (int i = 0; i < 3; i++) {
@@ -368,12 +414,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
}
accel_corr[0] = accel_NED[0] - x_est[2];
accel_corr[1] = accel_NED[1] - y_est[2];
accel_corr[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2];
corr_acc[0] = accel_NED[0] - x_est[2];
corr_acc[1] = accel_NED[1] - y_est[2];
corr_acc[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2];
} else {
memset(accel_corr, 0, sizeof(accel_corr));
memset(corr_acc, 0, sizeof(corr_acc));
}
accel_counter = sensor.accelerometer_counter;
@@ -381,180 +427,352 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
if (sensor.baro_counter != baro_counter) {
baro_corr = - sensor.baro_alt_meter - z_est[0];
corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0];
baro_counter = sensor.baro_counter;
baro_updates++;
}
}
/* optical flow */
if (fds[5].revents & POLLIN) {
orb_check(optical_flow_sub, &updated);
if (updated) {
orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && (flow.ground_distance_m != sonar_prev || t - sonar_time < 150000)) {
if (flow.ground_distance_m != sonar_prev) {
sonar_time = t;
sonar_prev = flow.ground_distance_m;
sonar_corr = -flow.ground_distance_m - z_est[0];
sonar_corr_filtered += (sonar_corr - sonar_corr_filtered) * params.sonar_filt;
if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && flow.ground_distance_m != sonar_prev) {
sonar_time = t;
sonar_prev = flow.ground_distance_m;
corr_sonar = flow.ground_distance_m + surface_offset + z_est[0];
corr_sonar_filtered += (corr_sonar - corr_sonar_filtered) * params.sonar_filt;
if (fabsf(sonar_corr) > params.sonar_err) {
// correction is too large: spike or new ground level?
if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) {
// spike detected, ignore
sonar_corr = 0.0f;
if (fabsf(corr_sonar) > params.sonar_err) {
/* correction is too large: spike or new ground level? */
if (fabsf(corr_sonar - corr_sonar_filtered) > params.sonar_err) {
/* spike detected, ignore */
corr_sonar = 0.0f;
sonar_valid = false;
} else {
// new ground level
baro_alt0 += sonar_corr;
mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0);
local_pos.ref_alt = baro_alt0;
local_pos.ref_timestamp = hrt_absolute_time();
z_est[0] += sonar_corr;
sonar_corr = 0.0f;
sonar_corr_filtered = 0.0f;
}
} else {
/* new ground level */
surface_offset -= corr_sonar;
surface_offset_rate = 0.0f;
corr_sonar = 0.0f;
corr_sonar_filtered = 0.0f;
sonar_valid_time = t;
sonar_valid = true;
local_pos.surface_bottom_timestamp = t;
mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", surface_offset);
}
} else {
/* correction is ok, use it */
sonar_valid_time = t;
sonar_valid = true;
}
}
float flow_q = flow.quality / 255.0f;
float dist_bottom = - z_est[0] - surface_offset;
if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7) {
/* distance to surface */
float flow_dist = dist_bottom / att.R[2][2];
/* check if flow if too large for accurate measurements */
/* calculate estimated velocity in body frame */
float body_v_est[2] = { 0.0f, 0.0f };
for (int i = 0; i < 2; i++) {
body_v_est[i] = att.R[0][i] * x_est[1] + att.R[1][i] * y_est[1] + att.R[2][i] * z_est[1];
}
/* set this flag if flow should be accurate according to current velocity and attitude rate estimate */
flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow &&
fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow;
/* convert raw flow to angular flow */
float flow_ang[2];
flow_ang[0] = flow.flow_raw_x * params.flow_k;
flow_ang[1] = flow.flow_raw_y * params.flow_k;
/* flow measurements vector */
float flow_m[3];
flow_m[0] = -flow_ang[0] * flow_dist;
flow_m[1] = -flow_ang[1] * flow_dist;
flow_m[2] = z_est[1];
/* velocity in NED */
float flow_v[2] = { 0.0f, 0.0f };
/* project measurements vector to NED basis, skip Z component */
for (int i = 0; i < 2; i++) {
for (int j = 0; j < 3; j++) {
flow_v[i] += att.R[i][j] * flow_m[j];
}
}
/* velocity correction */
corr_flow[0] = flow_v[0] - x_est[1];
corr_flow[1] = flow_v[1] - y_est[1];
/* adjust correction weight */
float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min);
w_flow = att.R[2][2] * flow_q_weight / fmaxf(1.0f, flow_dist);
/* if flow is not accurate, reduce weight for it */
// TODO make this more fuzzy
if (!flow_accurate)
w_flow *= 0.05f;
flow_valid = true;
} else {
sonar_corr = 0.0f;
w_flow = 0.0f;
flow_valid = false;
}
flow_updates++;
}
/* vehicle GPS position */
if (fds[6].revents & POLLIN) {
orb_check(vehicle_gps_position_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) {
/* initialize reference position if needed */
if (!ref_xy_inited) {
/* require EPH < 10m */
if (gps.eph_m < 10.0f) {
if (ref_xy_init_start == 0) {
ref_xy_init_start = t;
} else if (t > ref_xy_init_start + ref_xy_init_delay) {
ref_xy_inited = true;
/* reference GPS position */
double lat = gps.lat * 1e-7;
double lon = gps.lon * 1e-7;
local_pos.ref_lat = gps.lat;
local_pos.ref_lon = gps.lon;
local_pos.ref_timestamp = t;
/* initialize projection */
map_projection_init(lat, lon);
warnx("init GPS: lat = %.10f, lon = %.10f", lat, lon);
mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon);
}
} else {
ref_xy_init_start = 0;
if (gps.fix_type >= 3) {
/* hysteresis for GPS quality */
if (gps_valid) {
if (gps.eph_m > 10.0f || gps.epv_m > 10.0f) {
gps_valid = false;
mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
}
}
if (ref_xy_inited) {
/* project GPS lat lon to plane */
float gps_proj[2];
map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1]));
/* calculate correction for position */
gps_corr[0][0] = gps_proj[0] - x_est[0];
gps_corr[1][0] = gps_proj[1] - y_est[0];
/* calculate correction for velocity */
if (gps.vel_ned_valid) {
gps_corr[0][1] = gps.vel_n_m_s - x_est[1];
gps_corr[1][1] = gps.vel_e_m_s - y_est[1];
} else {
gps_corr[0][1] = 0.0f;
gps_corr[1][1] = 0.0f;
} else {
if (gps.eph_m < 5.0f && gps.epv_m < 5.0f) {
gps_valid = true;
mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
}
}
} else {
gps_valid = false;
}
if (gps_valid) {
/* initialize reference position if needed */
if (!ref_inited) {
if (ref_init_start == 0) {
ref_init_start = t;
} else if (t > ref_init_start + ref_init_delay) {
ref_inited = true;
/* reference GPS position */
double lat = gps.lat * 1e-7;
double lon = gps.lon * 1e-7;
float alt = gps.alt * 1e-3;
local_pos.ref_lat = gps.lat;
local_pos.ref_lon = gps.lon;
local_pos.ref_alt = alt + z_est[0];
local_pos.ref_timestamp = t;
/* initialize projection */
map_projection_init(lat, lon);
warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
}
}
if (ref_inited) {
/* project GPS lat lon to plane */
float gps_proj[2];
map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1]));
/* calculate correction for position */
corr_gps[0][0] = gps_proj[0] - x_est[0];
corr_gps[1][0] = gps_proj[1] - y_est[0];
corr_gps[2][0] = local_pos.ref_alt - gps.alt * 1e-3 - z_est[0];
/* calculate correction for velocity */
if (gps.vel_ned_valid) {
corr_gps[0][1] = gps.vel_n_m_s - x_est[1];
corr_gps[1][1] = gps.vel_e_m_s - y_est[1];
corr_gps[2][1] = gps.vel_d_m_s - z_est[1];
} else {
corr_gps[0][1] = 0.0f;
corr_gps[1][1] = 0.0f;
corr_gps[2][1] = 0.0f;
}
w_gps_xy = 1.0f / fmaxf(1.0f, gps.eph_m);
w_gps_z = 1.0f / fmaxf(1.0f, gps.epv_m);
}
} else {
/* no GPS lock */
memset(gps_corr, 0, sizeof(gps_corr));
ref_xy_init_start = 0;
memset(corr_gps, 0, sizeof(corr_gps));
ref_init_start = 0;
}
gps_updates++;
}
}
/* end of poll return value check */
/* check for timeout on FLOW topic */
if ((flow_valid || sonar_valid) && t > flow.timestamp + flow_topic_timeout) {
flow_valid = false;
sonar_valid = false;
warnx("FLOW timeout");
mavlink_log_info(mavlink_fd, "[inav] FLOW timeout");
}
/* check for timeout on GPS topic */
if (gps_valid && t > gps.timestamp_position + gps_topic_timeout) {
gps_valid = false;
warnx("GPS timeout");
mavlink_log_info(mavlink_fd, "[inav] GPS timeout");
}
/* check for sonar measurement timeout */
if (sonar_valid && t > sonar_time + sonar_timeout) {
corr_sonar = 0.0f;
sonar_valid = false;
}
float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
t_prev = t;
/* reset ground level on arm */
if (armed.armed && !flag_armed) {
baro_alt0 -= z_est[0];
z_est[0] = 0.0f;
local_pos.ref_alt = baro_alt0;
local_pos.ref_timestamp = hrt_absolute_time();
mavlink_log_info(mavlink_fd, "[inav] new home on arm: alt = %.3f", baro_alt0);
/* use GPS if it's valid and reference position initialized */
bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W;
bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W;
/* use flow if it's valid and (accurate or no GPS available) */
bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);
/* try to estimate position during some time after position sources lost */
if (use_gps_xy || use_flow) {
xy_src_time = t;
}
/* accel bias correction, now only for Z
* not strictly correct, but stable and works */
accel_bias[2] += (accel_NED[2] + CONSTANTS_ONE_G) * params.w_acc_bias * dt;
bool can_estimate_xy = (t < xy_src_time + xy_src_timeout);
bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout);
if (dist_bottom_valid) {
/* surface distance prediction */
surface_offset += surface_offset_rate * dt;
/* surface distance correction */
if (sonar_valid) {
surface_offset_rate -= corr_sonar * 0.5f * params.w_z_sonar * params.w_z_sonar * dt;
surface_offset -= corr_sonar * params.w_z_sonar * dt;
}
}
float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy;
float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy;
float w_z_gps_p = params.w_z_gps_p * w_gps_z;
/* reduce GPS weight if optical flow is good */
if (use_flow && flow_accurate) {
w_xy_gps_p *= params.w_gps_flow;
w_xy_gps_v *= params.w_gps_flow;
}
/* baro offset correction */
if (use_gps_z) {
float offs_corr = corr_gps[2][0] * w_z_gps_p * dt;
baro_offset += offs_corr;
baro_counter += offs_corr;
}
/* accelerometer bias correction */
float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f };
if (use_gps_xy) {
accel_bias_corr[0] -= corr_gps[0][0] * w_xy_gps_p * w_xy_gps_p;
accel_bias_corr[0] -= corr_gps[0][1] * w_xy_gps_v;
accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p;
accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v;
}
if (use_gps_z) {
accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p;
}
if (use_flow) {
accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow;
accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow;
}
accel_bias_corr[2] -= corr_baro * params.w_z_baro * params.w_z_baro;
/* transform error vector from NED frame to body frame */
for (int i = 0; i < 3; i++) {
float c = 0.0f;
for (int j = 0; j < 3; j++) {
c += att.R[j][i] * accel_bias_corr[j];
}
acc_bias[i] += c * params.w_acc_bias * dt;
}
/* inertial filter prediction for altitude */
inertial_filter_predict(dt, z_est);
/* inertial filter correction for altitude */
baro_alt0 += sonar_corr * params.w_alt_sonar * dt;
inertial_filter_correct(baro_corr + baro_alt0, dt, z_est, 0, params.w_alt_baro);
inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar);
inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc);
bool gps_valid = ref_xy_inited && gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout;
bool flow_valid = false; // TODO implement opt flow
/* try to estimate xy even if no absolute position source available,
* if using optical flow velocity will be correct in this case */
bool can_estimate_xy = gps_valid || flow_valid;
inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro);
inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc);
if (can_estimate_xy) {
/* inertial filter prediction for position */
inertial_filter_predict(dt, x_est);
inertial_filter_predict(dt, y_est);
if (!isfinite(x_est[0]) || !isfinite(y_est[0])) {
write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
thread_should_exit = true;
}
/* inertial filter correction for position */
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);
inertial_filter_correct(corr_acc[0], dt, x_est, 2, params.w_xy_acc);
inertial_filter_correct(corr_acc[1], dt, y_est, 2, params.w_xy_acc);
if (gps_valid) {
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 (use_flow) {
inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow);
inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow);
}
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);
if (use_gps_xy) {
inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p);
inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p);
if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_topic_timeout) {
inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v);
inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_gps_v);
}
}
if (!isfinite(x_est[0]) || !isfinite(y_est[0])) {
write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
thread_should_exit = true;
}
}
/* detect land */
alt_avg += (z_est[0] - alt_avg) * dt / params.land_t;
float alt_disp = z_est[0] - alt_avg;
alt_disp = alt_disp * alt_disp;
alt_avg += (- z_est[0] - alt_avg) * dt / params.land_t;
float alt_disp2 = - z_est[0] - alt_avg;
alt_disp2 = alt_disp2 * alt_disp2;
float land_disp2 = params.land_disp * params.land_disp;
/* get actual thrust output */
float thrust = armed.armed ? actuator.control[3] : 0.0f;
if (landed) {
if (alt_disp > land_disp2 && thrust > params.land_thr) {
if (alt_disp2 > land_disp2 && thrust > params.land_thr) {
landed = false;
landed_time = 0;
}
} else {
if (alt_disp < land_disp2 && thrust < params.land_thr) {
if (alt_disp2 < land_disp2 && thrust < params.land_thr) {
if (landed_time == 0) {
landed_time = t; // land detected first time
@@ -593,10 +811,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (t > pub_last + pub_interval) {
pub_last = t;
/* publish local position */
local_pos.timestamp = t;
local_pos.xy_valid = can_estimate_xy && gps_valid;
local_pos.xy_valid = can_estimate_xy && use_gps_xy;
local_pos.v_xy_valid = can_estimate_xy;
local_pos.xy_global = local_pos.xy_valid && gps_valid; // will make sense when local position sources (e.g. vicon) will be implemented
local_pos.xy_global = local_pos.xy_valid && use_gps_xy;
local_pos.z_global = local_pos.z_valid && use_gps_z;
local_pos.x = x_est[0];
local_pos.vx = x_est[1];
local_pos.y = y_est[0];
@@ -605,6 +823,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
local_pos.vz = z_est[1];
local_pos.landed = landed;
local_pos.yaw = att.yaw;
local_pos.dist_bottom_valid = dist_bottom_valid;
if (local_pos.dist_bottom_valid) {
local_pos.dist_bottom = -z_est[0] - surface_offset;
local_pos.dist_bottom_rate = -z_est[1] - surface_offset_rate;
}
local_pos.timestamp = t;
orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
@@ -614,19 +840,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (local_pos.xy_global) {
double est_lat, est_lon;
map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon);
global_pos.lat = (int32_t)(est_lat * 1e7d);
global_pos.lon = (int32_t)(est_lon * 1e7d);
global_pos.lat = est_lat;
global_pos.lon = est_lon;
global_pos.time_gps_usec = gps.time_gps_usec;
}
/* set valid values even if position is not valid */
if (local_pos.v_xy_valid) {
global_pos.vx = local_pos.vx;
global_pos.vy = local_pos.vy;
}
if (local_pos.z_valid) {
global_pos.relative_alt = -local_pos.z;
global_pos.vel_n = local_pos.vx;
global_pos.vel_e = local_pos.vy;
}
if (local_pos.z_global) {
@@ -634,19 +856,19 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
if (local_pos.v_z_valid) {
global_pos.vz = local_pos.vz;
global_pos.vel_d = local_pos.vz;
}
global_pos.yaw = local_pos.yaw;
global_pos.timestamp = t;
orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
}
flag_armed = armed.armed;
}
warnx("exiting.");
mavlink_log_info(mavlink_fd, "[inav] exiting");
warnx("stopped");
mavlink_log_info(mavlink_fd, "[inav] stopped");
thread_running = false;
return 0;
}
src/modules/position_estimator_inav/position_estimator_inav_params.c
+33 -24
View File
@@ -40,16 +40,19 @@
#include "position_estimator_inav_params.h"
PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 0.5f);
PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f);
PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f);
PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f);
PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 2.0f);
PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f);
PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 0.0f);
PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f);
PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f);
PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.02f);
PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 1.0f);
PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f);
PARAM_DEFINE_FLOAT(INAV_W_Z_ACC, 20.0f);
PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_ACC, 20.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f);
PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f);
PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f);
PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.0165f);
PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f);
PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f);
PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f);
PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f);
@@ -57,15 +60,18 @@ PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.3f);
int parameters_init(struct position_estimator_inav_param_handles *h)
{
h->w_alt_baro = param_find("INAV_W_ALT_BARO");
h->w_alt_acc = param_find("INAV_W_ALT_ACC");
h->w_alt_sonar = param_find("INAV_W_ALT_SONAR");
h->w_pos_gps_p = param_find("INAV_W_POS_GPS_P");
h->w_pos_gps_v = param_find("INAV_W_POS_GPS_V");
h->w_pos_acc = param_find("INAV_W_POS_ACC");
h->w_pos_flow = param_find("INAV_W_POS_FLOW");
h->w_z_baro = param_find("INAV_W_Z_BARO");
h->w_z_gps_p = param_find("INAV_W_Z_GPS_P");
h->w_z_acc = param_find("INAV_W_Z_ACC");
h->w_z_sonar = param_find("INAV_W_Z_SONAR");
h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P");
h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V");
h->w_xy_acc = param_find("INAV_W_XY_ACC");
h->w_xy_flow = param_find("INAV_W_XY_FLOW");
h->w_gps_flow = param_find("INAV_W_GPS_FLOW");
h->w_acc_bias = param_find("INAV_W_ACC_BIAS");
h->flow_k = param_find("INAV_FLOW_K");
h->flow_q_min = param_find("INAV_FLOW_Q_MIN");
h->sonar_filt = param_find("INAV_SONAR_FILT");
h->sonar_err = param_find("INAV_SONAR_ERR");
h->land_t = param_find("INAV_LAND_T");
@@ -77,15 +83,18 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p)
{
param_get(h->w_alt_baro, &(p->w_alt_baro));
param_get(h->w_alt_acc, &(p->w_alt_acc));
param_get(h->w_alt_sonar, &(p->w_alt_sonar));
param_get(h->w_pos_gps_p, &(p->w_pos_gps_p));
param_get(h->w_pos_gps_v, &(p->w_pos_gps_v));
param_get(h->w_pos_acc, &(p->w_pos_acc));
param_get(h->w_pos_flow, &(p->w_pos_flow));
param_get(h->w_z_baro, &(p->w_z_baro));
param_get(h->w_z_gps_p, &(p->w_z_gps_p));
param_get(h->w_z_acc, &(p->w_z_acc));
param_get(h->w_z_sonar, &(p->w_z_sonar));
param_get(h->w_xy_gps_p, &(p->w_xy_gps_p));
param_get(h->w_xy_gps_v, &(p->w_xy_gps_v));
param_get(h->w_xy_acc, &(p->w_xy_acc));
param_get(h->w_xy_flow, &(p->w_xy_flow));
param_get(h->w_gps_flow, &(p->w_gps_flow));
param_get(h->w_acc_bias, &(p->w_acc_bias));
param_get(h->flow_k, &(p->flow_k));
param_get(h->flow_q_min, &(p->flow_q_min));
param_get(h->sonar_filt, &(p->sonar_filt));
param_get(h->sonar_err, &(p->sonar_err));
param_get(h->land_t, &(p->land_t));
src/modules/position_estimator_inav/position_estimator_inav_params.h
+20 -14
View File
@@ -41,15 +41,18 @@
#include <systemlib/param/param.h>
struct position_estimator_inav_params {
float w_alt_baro;
float w_alt_acc;
float w_alt_sonar;
float w_pos_gps_p;
float w_pos_gps_v;
float w_pos_acc;
float w_pos_flow;
float w_z_baro;
float w_z_gps_p;
float w_z_acc;
float w_z_sonar;
float w_xy_gps_p;
float w_xy_gps_v;
float w_xy_acc;
float w_xy_flow;
float w_gps_flow;
float w_acc_bias;
float flow_k;
float flow_q_min;
float sonar_filt;
float sonar_err;
float land_t;
@@ -58,15 +61,18 @@ struct position_estimator_inav_params {
};
struct position_estimator_inav_param_handles {
param_t w_alt_baro;
param_t w_alt_acc;
param_t w_alt_sonar;
param_t w_pos_gps_p;
param_t w_pos_gps_v;
param_t w_pos_acc;
param_t w_pos_flow;
param_t w_z_baro;
param_t w_z_gps_p;
param_t w_z_acc;
param_t w_z_sonar;
param_t w_xy_gps_p;
param_t w_xy_gps_v;
param_t w_xy_acc;
param_t w_xy_flow;
param_t w_gps_flow;
param_t w_acc_bias;
param_t flow_k;
param_t flow_q_min;
param_t sonar_filt;
param_t sonar_err;
param_t land_t;
src/modules/px4iofirmware/adc.c
+23 -30
View File
@@ -83,6 +83,14 @@ adc_init(void)
{
adc_perf = perf_alloc(PC_ELAPSED, "adc");
/* put the ADC into power-down mode */
rCR2 &= ~ADC_CR2_ADON;
up_udelay(10);
/* bring the ADC out of power-down mode */
rCR2 |= ADC_CR2_ADON;
up_udelay(10);
/* do calibration if supported */
#ifdef ADC_CR2_CAL
rCR2 |= ADC_CR2_RSTCAL;
@@ -96,41 +104,25 @@ adc_init(void)
if (rCR2 & ADC_CR2_CAL)
return -1;
#endif
/* arbitrarily configure all channels for 55 cycle sample time */
rSMPR1 = 0b00000011011011011011011011011011;
/*
* Configure sampling time.
*
* For electrical protection reasons, we want to be able to have
* 10K in series with ADC inputs that leave the board. At 12MHz this
* means we need 28.5 cycles of sampling time (per table 43 in the
* datasheet).
*/
rSMPR1 = 0b00000000011011011011011011011011;
rSMPR2 = 0b00011011011011011011011011011011;
/* XXX for F2/4, might want to select 12-bit mode? */
rCR1 = 0;
/* enable the temperature sensor / Vrefint channel if supported*/
rCR2 =
#ifdef ADC_CR2_TSVREFE
/* enable the temperature sensor in CR2 */
ADC_CR2_TSVREFE |
#endif
0;
#ifdef ADC_CCR_TSVREFE
/* enable temperature sensor in CCR */
rCCR = ADC_CCR_TSVREFE;
#endif
rCR2 |= ADC_CR2_TSVREFE; /* enable the temperature sensor / Vrefint channel */
/* configure for a single-channel sequence */
rSQR1 = 0;
rSQR2 = 0;
rSQR3 = 0; /* will be updated with the channel each tick */
/* power-cycle the ADC and turn it on */
rCR2 &= ~ADC_CR2_ADON;
up_udelay(10);
rCR2 |= ADC_CR2_ADON;
up_udelay(10);
rCR2 |= ADC_CR2_ADON;
up_udelay(10);
rSQR3 = 0; /* will be updated with the channel at conversion time */
return 0;
}
@@ -141,11 +133,12 @@ adc_init(void)
uint16_t
adc_measure(unsigned channel)
{
perf_begin(adc_perf);
/* clear any previous EOC */
if (rSR & ADC_SR_EOC)
rSR &= ~ADC_SR_EOC;
rSR = 0;
(void)rDR;
/* run a single conversion right now - should take about 60 cycles (a few microseconds) max */
rSQR3 = channel;
@@ -158,7 +151,6 @@ adc_measure(unsigned channel)
/* never spin forever - this will give a bogus result though */
if (hrt_elapsed_time(&now) > 100) {
debug("adc timeout");
perf_end(adc_perf);
return 0xffff;
}
@@ -166,6 +158,7 @@ adc_measure(unsigned channel)
/* read the result and clear EOC */
uint16_t result = rDR;
rSR = 0;
perf_end(adc_perf);
return result;
src/modules/px4iofirmware/controls.c
+11
View File
@@ -114,9 +114,20 @@ controls_tick() {
perf_begin(c_gather_sbus);
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS);
bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS);
if (sbus_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
}
/* switch S.Bus output pin as needed */
if (sbus_status != (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)) {
#ifdef ENABLE_SBUS_OUT
ENABLE_SBUS_OUT((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS));
#endif
}
perf_end(c_gather_sbus);
/*
src/modules/px4iofirmware/dsm.c
+6
View File
@@ -203,6 +203,12 @@ dsm_guess_format(bool reset)
int
dsm_init(const char *device)
{
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
// enable power on DSM connector
POWER_SPEKTRUM(true);
#endif
if (dsm_fd < 0)
dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
src/modules/px4iofirmware/mixer.cpp
+23 -7
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -71,6 +71,7 @@ extern "C" {
static bool mixer_servos_armed = false;
static bool should_arm = false;
static bool should_always_enable_pwm = false;
static volatile bool in_mixer = false;
/* selected control values and count for mixing */
enum mixer_source {
@@ -95,6 +96,7 @@ static void mixer_set_failsafe();
void
mixer_tick(void)
{
/* check that we are receiving fresh data from the FMU */
if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
@@ -199,13 +201,17 @@ mixer_tick(void)
}
} else if (source != MIX_NONE) {
} else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
float outputs[PX4IO_SERVO_COUNT];
unsigned mixed;
/* mix */
/* poor mans mutex */
in_mixer = true;
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
in_mixer = false;
pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
@@ -297,12 +303,17 @@ mixer_callback(uintptr_t handle,
static char mixer_text[256]; /* large enough for one mixer */
static unsigned mixer_text_length = 0;
void
int
mixer_handle_text(const void *buffer, size_t length)
{
/* do not allow a mixer change while safety off */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
return;
return 1;
}
/* abort if we're in the mixer */
if (in_mixer) {
return 1;
}
px4io_mixdata *msg = (px4io_mixdata *)buffer;
@@ -310,7 +321,7 @@ mixer_handle_text(const void *buffer, size_t length)
isr_debug(2, "mix txt %u", length);
if (length < sizeof(px4io_mixdata))
return;
return 0;
unsigned text_length = length - sizeof(px4io_mixdata);
@@ -328,13 +339,16 @@ mixer_handle_text(const void *buffer, size_t length)
case F2I_MIXER_ACTION_APPEND:
isr_debug(2, "append %d", length);
/* disable mixing during the update */
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
/* check for overflow - this would be really fatal */
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
return;
return 0;
}
/* append mixer text and nul-terminate */
/* append mixer text and nul-terminate, guard against overflow */
memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
mixer_text_length += text_length;
mixer_text[mixer_text_length] = '\0';
@@ -369,6 +383,8 @@ mixer_handle_text(const void *buffer, size_t length)
break;
}
return 0;
}
static void
src/modules/px4iofirmware/px4io.c
+64 -18
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -125,6 +125,25 @@ heartbeat_blink(void)
LED_BLUE(heartbeat = !heartbeat);
}
static uint64_t reboot_time;
/**
schedule a reboot in time_delta_usec microseconds
*/
void schedule_reboot(uint32_t time_delta_usec)
{
reboot_time = hrt_absolute_time() + time_delta_usec;
}
/**
check for a scheduled reboot
*/
static void check_reboot(void)
{
if (reboot_time != 0 && hrt_absolute_time() > reboot_time) {
up_systemreset();
}
}
static void
calculate_fw_crc(void)
@@ -177,6 +196,11 @@ user_start(int argc, char *argv[])
POWER_SERVO(true);
#endif
/* turn off S.Bus out (if supported) */
#ifdef ENABLE_SBUS_OUT
ENABLE_SBUS_OUT(false);
#endif
/* start the safety switch handler */
safety_init();
@@ -186,6 +210,9 @@ user_start(int argc, char *argv[])
/* initialise the control inputs */
controls_init();
/* set up the ADC */
adc_init();
/* start the FMU interface */
interface_init();
@@ -204,24 +231,41 @@ user_start(int argc, char *argv[])
/* initialize PWM limit lib */
pwm_limit_init(&pwm_limit);
#if 0
/* not enough memory, lock down */
if (minfo.mxordblk < 500) {
/*
* P O L I C E L I G H T S
*
* Not enough memory, lock down.
*
* We might need to allocate mixers later, and this will
* ensure that a developer doing a change will notice
* that he just burned the remaining RAM with static
* allocations. We don't want him to be able to
* get past that point. This needs to be clearly
* documented in the dev guide.
*
*/
if (minfo.mxordblk < 600) {
lowsyslog("ERR: not enough MEM");
bool phase = false;
if (phase) {
LED_AMBER(true);
LED_BLUE(false);
} else {
LED_AMBER(false);
LED_BLUE(true);
}
while (true) {
phase = !phase;
usleep(300000);
if (phase) {
LED_AMBER(true);
LED_BLUE(false);
} else {
LED_AMBER(false);
LED_BLUE(true);
}
up_udelay(250000);
phase = !phase;
}
}
#endif
/* Start the failsafe led init */
failsafe_led_init();
/*
* Run everything in a tight loop.
@@ -249,11 +293,14 @@ user_start(int argc, char *argv[])
heartbeat_blink();
}
#if 0
/* check for debug activity */
check_reboot();
/* check for debug activity (default: none) */
show_debug_messages();
/* post debug state at ~1Hz */
/* post debug state at ~1Hz - this is via an auxiliary serial port
* DEFAULTS TO OFF!
*/
if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {
struct mallinfo minfo = mallinfo();
@@ -266,7 +313,6 @@ user_start(int argc, char *argv[])
(unsigned)minfo.mxordblk);
last_debug_time = hrt_absolute_time();
}
#endif
}
}
src/modules/px4iofirmware/px4io.h
+7 -1
View File
@@ -160,6 +160,7 @@ extern pwm_limit_t pwm_limit;
# define PX4IO_RELAY_CHANNELS 0
# define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s))
# define ENABLE_SBUS_OUT(_s) stm32_gpiowrite(GPIO_SBUS_OENABLE, !(_s))
# define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT))
@@ -177,12 +178,13 @@ extern pwm_limit_t pwm_limit;
* Mixer
*/
extern void mixer_tick(void);
extern void mixer_handle_text(const void *buffer, size_t length);
extern int mixer_handle_text(const void *buffer, size_t length);
/**
* Safety switch/LED.
*/
extern void safety_init(void);
extern void failsafe_led_init(void);
/**
* FMU communications
@@ -220,3 +222,7 @@ extern volatile uint8_t debug_level;
/** send a debug message to the console */
extern void isr_debug(uint8_t level, const char *fmt, ...);
/** schedule a reboot */
extern void schedule_reboot(uint32_t time_delta_usec);
src/modules/px4iofirmware/registers.c
+11 -15
View File
@@ -382,7 +382,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
/* handle text going to the mixer parser */
case PX4IO_PAGE_MIXERLOAD:
mixer_handle_text(values, num_values * sizeof(*values));
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
(r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
return mixer_handle_text(values, num_values * sizeof(*values));
}
break;
default:
@@ -509,8 +512,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
case PX4IO_P_SETUP_REBOOT_BL:
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ||
(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
(r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
// don't allow reboot while armed
break;
}
@@ -518,16 +520,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
// check the magic value
if (value != PX4IO_REBOOT_BL_MAGIC)
break;
// note that we don't set BL_WAIT_MAGIC in
// BKP_DR1 as that is not necessary given the
// timing of the forceupdate command. The
// bootloader on px4io waits for enough time
// anyway, and this method works with older
// bootloader versions (tested with both
// revision 3 and revision 4).
up_systemreset();
// we schedule a reboot rather than rebooting
// immediately to allow the IO board to ACK
// the reboot command
schedule_reboot(100000);
break;
case PX4IO_P_SETUP_DSM:
@@ -545,8 +542,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
* do not allow a RC config change while outputs armed
*/
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ||
(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
(r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
break;
}
@@ -606,7 +602,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) {
disabled = true;
} else if (REG_TO_SIGNED(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
} else if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
count++;
}
src/modules/px4iofirmware/safety.c
+6 -2
View File
@@ -83,7 +83,11 @@ safety_init(void)
{
/* arrange for the button handler to be called at 10Hz */
hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL);
}
void
failsafe_led_init(void)
{
/* arrange for the failsafe blinker to be called at 8Hz */
hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL);
}
@@ -164,8 +168,8 @@ failsafe_blink(void *arg)
/* indicate that a serious initialisation error occured */
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) {
LED_AMBER(true);
return;
}
return;
}
static bool failsafe = false;
src/modules/px4iofirmware/sbus.c
+24 -2
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -218,11 +218,33 @@ static bool
sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values)
{
/* check frame boundary markers to avoid out-of-sync cases */
if ((frame[0] != 0x0f) || (frame[24] != 0x00)) {
if ((frame[0] != 0x0f)) {
sbus_frame_drops++;
return false;
}
switch (frame[24]) {
case 0x00:
/* this is S.BUS 1 */
break;
case 0x03:
/* S.BUS 2 SLOT0: RX battery and external voltage */
break;
case 0x83:
/* S.BUS 2 SLOT1 */
break;
case 0x43:
case 0xC3:
case 0x23:
case 0xA3:
case 0x63:
case 0xE3:
break;
default:
/* we expect one of the bits above, but there are some we don't know yet */
break;
}
/* we have received something we think is a frame */
last_frame_time = frame_time;
src/modules/sdlog2/sdlog2.c
+248 -128
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
*
@@ -62,6 +62,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -72,7 +73,7 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/mission_item_triplet.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
@@ -85,13 +86,13 @@
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <version/version.h>
#include <mavlink/mavlink_log.h>
#include "logbuffer.h"
#include "sdlog2_format.h"
#include "sdlog2_messages.h"
#include "sdlog2_version.h"
#define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \
log_msgs_written++; \
@@ -108,13 +109,13 @@ static bool main_thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
static bool logwriter_should_exit = false; /**< Logwriter thread exit flag */
static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log folders */
static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log dirs */
static const int MAX_NO_LOGFILE = 999; /**< Maximum number of log files */
static const int LOG_BUFFER_SIZE_DEFAULT = 8192;
static const int MAX_WRITE_CHUNK = 512;
static const int MIN_BYTES_TO_WRITE = 512;
static const char *mountpoint = "/fs/microsd";
static const char *log_root = "/fs/microsd/log";
static int mavlink_fd = -1;
struct logbuffer_s lb;
@@ -122,14 +123,17 @@ struct logbuffer_s lb;
static pthread_mutex_t logbuffer_mutex;
static pthread_cond_t logbuffer_cond;
static char folder_path[64];
static char log_dir[32];
/* statistics counters */
static unsigned long log_bytes_written = 0;
static uint64_t start_time = 0;
static unsigned long log_bytes_written = 0;
static unsigned long log_msgs_written = 0;
static unsigned long log_msgs_skipped = 0;
/* GPS time, used for log files naming */
static uint64_t gps_time = 0;
/* current state of logging */
static bool logging_enabled = false;
/* enable logging on start (-e option) */
@@ -138,11 +142,14 @@ static bool log_on_start = false;
static bool log_when_armed = false;
/* delay = 1 / rate (rate defined by -r option) */
static useconds_t sleep_delay = 0;
/* use date/time for naming directories and files (-t option) */
static bool log_name_timestamp = false;
/* helper flag to track system state changes */
static bool flag_system_armed = false;
static pthread_t logwriter_pthread = 0;
static pthread_attr_t logwriter_attr;
/**
* Log buffer writing thread. Open and close file here.
@@ -203,14 +210,14 @@ static void handle_command(struct vehicle_command_s *cmd);
static void handle_status(struct vehicle_status_s *cmd);
/**
* Create folder for current logging session. Store folder name in 'log_folder'.
* Create dir for current logging session. Store dir name in 'log_dir'.
*/
static int create_logfolder(void);
static int create_log_dir(void);
/**
* Select first free log file name and open it.
*/
static int open_logfile(void);
static int open_log_file(void);
static void
sdlog2_usage(const char *reason)
@@ -218,11 +225,12 @@ sdlog2_usage(const char *reason)
if (reason)
fprintf(stderr, "%s\n", reason);
errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a\n"
errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t\n"
"\t-r\tLog rate in Hz, 0 means unlimited rate\n"
"\t-b\tLog buffer size in KiB, default is 8\n"
"\t-e\tEnable logging by default (if not, can be started by command)\n"
"\t-a\tLog only when armed (can be still overriden by command)\n");
"\t-a\tLog only when armed (can be still overriden by command)\n"
"\t-t\tUse date/time for naming log directories and files\n");
}
/**
@@ -280,82 +288,112 @@ int sdlog2_main(int argc, char *argv[])
exit(1);
}
int create_logfolder()
int create_log_dir()
{
/* make folder on sdcard */
uint16_t folder_number = 1; // start with folder sess001
/* create dir on sdcard if needed */
uint16_t dir_number = 1; // start with dir sess001
int mkdir_ret;
/* look for the next folder that does not exist */
while (folder_number <= MAX_NO_LOGFOLDER) {
/* set up folder path: e.g. /fs/microsd/sess001 */
sprintf(folder_path, "%s/sess%03u", mountpoint, folder_number);
mkdir_ret = mkdir(folder_path, S_IRWXU | S_IRWXG | S_IRWXO);
/* the result is -1 if the folder exists */
if (log_name_timestamp && gps_time != 0) {
/* use GPS date for log dir naming: e.g. /fs/microsd/2014-01-19 */
time_t gps_time_sec = gps_time / 1000000;
struct tm t;
gmtime_r(&gps_time_sec, &t);
int n = snprintf(log_dir, sizeof(log_dir), "%s/", log_root);
strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &t);
mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO);
if (mkdir_ret == 0) {
/* folder does not exist, success */
break;
if (mkdir_ret == OK) {
warnx("log dir created: %s", log_dir);
} else if (mkdir_ret == -1) {
/* folder exists already */
folder_number++;
} else if (errno != EEXIST) {
warn("failed creating new dir: %s", log_dir);
return -1;
}
} else {
/* look for the next dir that does not exist */
while (dir_number <= MAX_NO_LOGFOLDER) {
/* format log dir: e.g. /fs/microsd/sess001 */
sprintf(log_dir, "%s/sess%03u", log_root, dir_number);
mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO);
if (mkdir_ret == 0) {
warnx("log dir created: %s", log_dir);
break;
} else if (errno != EEXIST) {
warn("failed creating new dir: %s", log_dir);
return -1;
}
/* dir exists already */
dir_number++;
continue;
}
} else {
warn("failed creating new folder");
if (dir_number >= MAX_NO_LOGFOLDER) {
/* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */
warnx("all %d possible dirs exist already", MAX_NO_LOGFOLDER);
return -1;
}
}
if (folder_number >= MAX_NO_LOGFOLDER) {
/* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */
warnx("all %d possible folders exist already.", MAX_NO_LOGFOLDER);
return -1;
}
/* print logging path, important to find log file later */
warnx("log dir: %s", log_dir);
mavlink_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir);
return 0;
}
int open_logfile()
int open_log_file()
{
/* make folder on sdcard */
uint16_t file_number = 1; // start with file log001
/* string to hold the path to the log */
char path_buf[64] = "";
char log_file_name[16] = "";
char log_file_path[48] = "";
int fd = 0;
if (log_name_timestamp && gps_time != 0) {
/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
time_t gps_time_sec = gps_time / 1000000;
struct tm t;
gmtime_r(&gps_time_sec, &t);
strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.bin", &t);
snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name);
/* look for the next file that does not exist */
while (file_number <= MAX_NO_LOGFILE) {
/* set up file path: e.g. /fs/microsd/sess001/log001.bin */
sprintf(path_buf, "%s/log%03u.bin", folder_path, file_number);
} else {
uint16_t file_number = 1; // start with file log001
/* look for the next file that does not exist */
while (file_number <= MAX_NO_LOGFILE) {
/* format log file path: e.g. /fs/microsd/sess001/log001.bin */
snprintf(log_file_name, sizeof(log_file_name), "log%03u.bin", file_number);
snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name);
if (!file_exist(log_file_path)) {
break;
}
if (file_exist(path_buf)) {
file_number++;
continue;
}
fd = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC);
if (fd == 0) {
warn("opening %s failed", path_buf);
if (file_number > MAX_NO_LOGFILE) {
/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
warnx("all %d possible files exist already", MAX_NO_LOGFILE);
return -1;
}
warnx("logging to: %s.", path_buf);
mavlink_log_info(mavlink_fd, "[sdlog2] log: %s", path_buf);
return fd;
}
if (file_number > MAX_NO_LOGFILE) {
/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
warnx("all %d possible files exist already.", MAX_NO_LOGFILE);
return -1;
int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC);
if (fd < 0) {
warn("failed opening log: %s", log_file_name);
mavlink_log_info(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);
} else {
warnx("log file: %s", log_file_name);
mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name);
}
return 0;
return fd;
}
static void *logwriter_thread(void *arg)
@@ -363,9 +401,12 @@ static void *logwriter_thread(void *arg)
/* set name */
prctl(PR_SET_NAME, "sdlog2_writer", 0);
struct logbuffer_s *logbuf = (struct logbuffer_s *)arg;
int log_fd = open_log_file();
int log_fd = open_logfile();
if (log_fd < 0)
return;
struct logbuffer_s *logbuf = (struct logbuffer_s *)arg;
/* write log messages formats, version and parameters */
log_bytes_written += write_formats(log_fd);
@@ -443,14 +484,20 @@ static void *logwriter_thread(void *arg)
fsync(log_fd);
close(log_fd);
return OK;
return;
}
void sdlog2_start_log()
{
warnx("start logging.");
warnx("start logging");
mavlink_log_info(mavlink_fd, "[sdlog2] start logging");
/* create log dir if needed */
if (create_log_dir() != 0) {
mavlink_log_critical(mavlink_fd, "[sdlog2] error creating log dir");
errx(1, "error creating log dir");
}
/* initialize statistics counter */
log_bytes_written = 0;
start_time = hrt_absolute_time();
@@ -458,30 +505,28 @@ void sdlog2_start_log()
log_msgs_skipped = 0;
/* initialize log buffer emptying thread */
pthread_attr_t receiveloop_attr;
pthread_attr_init(&receiveloop_attr);
pthread_attr_init(&logwriter_attr);
struct sched_param param;
/* low priority, as this is expensive disk I/O */
param.sched_priority = SCHED_PRIORITY_DEFAULT - 40;
(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
(void)pthread_attr_setschedparam(&logwriter_attr, &param);
pthread_attr_setstacksize(&receiveloop_attr, 2048);
pthread_attr_setstacksize(&logwriter_attr, 2048);
logwriter_should_exit = false;
/* start log buffer emptying thread */
if (0 != pthread_create(&logwriter_pthread, &receiveloop_attr, logwriter_thread, &lb)) {
if (0 != pthread_create(&logwriter_pthread, &logwriter_attr, logwriter_thread, &lb)) {
errx(1, "error creating logwriter thread");
}
logging_enabled = true;
// XXX we have to destroy the attr at some point
}
void sdlog2_stop_log()
{
warnx("stop logging.");
warnx("stop logging");
mavlink_log_info(mavlink_fd, "[sdlog2] stop logging");
logging_enabled = false;
@@ -501,6 +546,7 @@ void sdlog2_stop_log()
}
logwriter_pthread = 0;
pthread_attr_destroy(&logwriter_attr);
sdlog2_status();
}
@@ -569,8 +615,8 @@ int write_parameters(int fd)
}
case PARAM_TYPE_FLOAT:
param_get(param, &value);
break;
param_get(param, &value);
break;
default:
break;
@@ -588,18 +634,25 @@ int sdlog2_thread_main(int argc, char *argv[])
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
if (mavlink_fd < 0) {
warnx("failed to open MAVLink log stream, start mavlink app first.");
warnx("failed to open MAVLink log stream, start mavlink app first");
}
/* log buffer size */
int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT;
logging_enabled = false;
log_on_start = false;
log_when_armed = false;
log_name_timestamp = false;
flag_system_armed = false;
/* work around some stupidity in task_create's argv handling */
argc -= 2;
argv += 2;
int ch;
while ((ch = getopt(argc, argv, "r:b:ea")) != EOF) {
while ((ch = getopt(argc, argv, "r:b:eat")) != EOF) {
switch (ch) {
case 'r': {
unsigned long r = strtoul(optarg, NULL, 10);
@@ -632,49 +685,52 @@ int sdlog2_thread_main(int argc, char *argv[])
log_when_armed = true;
break;
case 't':
log_name_timestamp = true;
break;
case '?':
if (optopt == 'c') {
warnx("Option -%c requires an argument.", optopt);
warnx("option -%c requires an argument", optopt);
} else if (isprint(optopt)) {
warnx("Unknown option `-%c'.", optopt);
warnx("unknown option `-%c'", optopt);
} else {
warnx("Unknown option character `\\x%x'.", optopt);
warnx("unknown option character `\\x%x'", optopt);
}
default:
sdlog2_usage("unrecognized flag");
errx(1, "exiting.");
errx(1, "exiting");
}
}
if (!file_exist(mountpoint)) {
errx(1, "logging mount point %s not present, exiting.", mountpoint);
}
if (create_logfolder()) {
errx(1, "unable to create logging folder, exiting.");
gps_time = 0;
/* create log root dir */
int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO);
if (mkdir_ret != 0 && errno != EEXIST) {
err("failed creating log root dir: %s", log_root);
}
/* copy conversion scripts */
const char *converter_in = "/etc/logging/conv.zip";
char *converter_out = malloc(120);
sprintf(converter_out, "%s/conv.zip", folder_path);
char *converter_out = malloc(64);
snprintf(converter_out, 64, "%s/conv.zip", log_root);
if (file_copy(converter_in, converter_out)) {
errx(1, "unable to copy conversion scripts, exiting.");
if (file_copy(converter_in, converter_out) != OK) {
warn("unable to copy conversion scripts");
}
free(converter_out);
/* only print logging path, important to find log file later */
warnx("logging to directory: %s", folder_path);
/* initialize log buffer with specified size */
warnx("log buffer size: %i bytes.", log_buffer_size);
warnx("log buffer size: %i bytes", log_buffer_size);
if (OK != logbuffer_init(&lb, log_buffer_size)) {
errx(1, "can't allocate log buffer, exiting.");
errx(1, "can't allocate log buffer, exiting");
}
struct vehicle_status_s buf_status;
@@ -684,6 +740,7 @@ int sdlog2_thread_main(int argc, char *argv[])
/* warning! using union here to save memory, elements should be used separately! */
union {
struct vehicle_command_s cmd;
struct vehicle_control_mode_s control_mode;
struct sensor_combined_s sensor;
struct vehicle_attitude_s att;
struct vehicle_attitude_setpoint_s att_sp;
@@ -693,7 +750,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vehicle_local_position_s local_pos;
struct vehicle_local_position_setpoint_s local_pos_sp;
struct vehicle_global_position_s global_pos;
struct mission_item_triplet_s triplet;
struct position_setpoint_triplet_s triplet;
struct vehicle_gps_position_s gps_pos;
struct vehicle_vicon_position_s vicon_pos;
struct optical_flow_s flow;
@@ -702,6 +759,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct airspeed_s airspeed;
struct esc_status_s esc;
struct vehicle_global_velocity_setpoint_s global_vel_sp;
struct battery_status_s battery;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -709,6 +767,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct {
int cmd_sub;
int status_sub;
int control_mode_sub;
int sensor_sub;
int att_sub;
int att_sp_sub;
@@ -726,6 +785,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int airspeed_sub;
int esc_sub;
int global_vel_sp_sub;
int battery_sub;
} subs;
/* log message buffer: header + body */
@@ -752,6 +812,8 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_GPSP_s log_GPSP;
struct log_ESC_s log_ESC;
struct log_GVSP_s log_GVSP;
struct log_BATT_s log_BATT;
struct log_DIST_s log_DIST;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@@ -760,9 +822,9 @@ int sdlog2_thread_main(int argc, char *argv[])
memset(&log_msg.body, 0, sizeof(log_msg.body));
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* number of subscriptions */
const ssize_t fdsc = 19;
/* sanity check variable and index */
/* number of messages */
const ssize_t fdsc = 25;
/* Sanity check variable and index */
ssize_t fdsc_count = 0;
/* file descriptors to wait for */
struct pollfd fds[fdsc];
@@ -785,6 +847,12 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- VEHICLE CONTROL MODE --- */
subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
fds[fdsc_count].fd = subs.control_mode_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- SENSORS COMBINED --- */
subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
fds[fdsc_count].fd = subs.sensor_sub;
@@ -840,7 +908,7 @@ int sdlog2_thread_main(int argc, char *argv[])
fdsc_count++;
/* --- GLOBAL POSITION SETPOINT--- */
subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet));
subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
fds[fdsc_count].fd = subs.triplet_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
@@ -881,12 +949,18 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- BATTERY --- */
subs.battery_sub = orb_subscribe(ORB_ID(battery_status));
fds[fdsc_count].fd = subs.battery_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* WARNING: If you get the error message below,
* then the number of registered messages (fdsc)
* differs from the number of messages in the above list.
*/
if (fdsc_count > fdsc) {
warn("WARNING: Not enough space for poll fds allocated. Check %s:%d.", __FILE__, __LINE__);
warn("WARNING: Not enough space for poll fds allocated. Check %s:%d", __FILE__, __LINE__);
fdsc_count = fdsc;
}
@@ -909,20 +983,31 @@ int sdlog2_thread_main(int argc, char *argv[])
uint16_t baro_counter = 0;
uint16_t differential_pressure_counter = 0;
/* track changes in distance status */
bool dist_bottom_present = false;
/* enable logging on start if needed */
if (log_on_start)
if (log_on_start) {
/* check GPS topic to get GPS time */
if (log_name_timestamp) {
if (OK == orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos)) {
gps_time = buf.gps_pos.time_gps_usec;
}
}
sdlog2_start_log();
}
while (!main_thread_should_exit) {
/* decide use usleep() or blocking poll() */
bool use_sleep = sleep_delay > 0 && logging_enabled;
/* poll all topics if logging enabled or only management (first 2) if not */
int poll_ret = poll(fds, logging_enabled ? fdsc_count : 2, use_sleep ? 0 : poll_timeout);
int poll_ret = poll(fds, logging_enabled ? fdsc_count : 3, use_sleep ? 0 : poll_timeout);
/* handle the poll result */
if (poll_ret < 0) {
warnx("ERROR: poll error, stop logging.");
warnx("ERROR: poll error, stop logging");
main_thread_should_exit = true;
} else if (poll_ret > 0) {
@@ -936,6 +1021,7 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- VEHICLE COMMAND - LOG MANAGEMENT --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd);
handle_command(&buf.cmd);
handled_topics++;
}
@@ -951,11 +1037,22 @@ int sdlog2_thread_main(int argc, char *argv[])
handled_topics++;
}
/* --- GPS POSITION - LOG MANAGEMENT --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
if (log_name_timestamp) {
gps_time = buf.gps_pos.time_gps_usec;
}
handled_topics++;
}
if (!logging_enabled || !check_data || handled_topics >= poll_ret) {
continue;
}
ifds = 1; // begin from fds[1] again
ifds = 1; // begin from VEHICLE STATUS again
pthread_mutex_lock(&logbuffer_mutex);
@@ -966,14 +1063,13 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- VEHICLE STATUS --- */
if (fds[ifds++].revents & POLLIN) {
// Don't orb_copy, it's already done few lines above
/* don't orb_copy, it's already done few lines above */
/* copy VEHICLE CONTROL MODE control mode here to construct STAT message */
orb_copy(ORB_ID(vehicle_control_mode), subs.control_mode_sub, &buf.control_mode);
log_msg.msg_type = LOG_STAT_MSG;
log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state;
// TODO use control_mode topic
//log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state;
log_msg.body.log_STAT.main_state = (uint8_t) buf.control_mode.main_state;
log_msg.body.log_STAT.navigation_state = (uint8_t) buf.control_mode.nav_state;
log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage;
log_msg.body.log_STAT.battery_current = buf_status.battery_current;
log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning;
log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed;
@@ -998,6 +1094,8 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(GPS);
}
ifds++; // skip CONTROL MODE, already handled
/* --- SENSOR COMBINED --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor);
@@ -1063,6 +1161,9 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_ATT.roll_rate = buf.att.rollspeed;
log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed;
log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed;
log_msg.body.log_ATT.gx = buf.att.g_comp[0];
log_msg.body.log_ATT.gy = buf.att.g_comp[1];
log_msg.body.log_ATT.gz = buf.att.g_comp[2];
LOGBUFFER_WRITE_AND_COUNT(ATT);
}
@@ -1123,6 +1224,17 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0);
log_msg.body.log_LPOS.landed = buf.local_pos.landed;
LOGBUFFER_WRITE_AND_COUNT(LPOS);
if (buf.local_pos.dist_bottom_valid) {
dist_bottom_present = true;
}
if (dist_bottom_present) {
log_msg.msg_type = LOG_DIST_MSG;
log_msg.body.log_DIST.bottom = buf.local_pos.dist_bottom;
log_msg.body.log_DIST.bottom_rate = buf.local_pos.dist_bottom_rate;
log_msg.body.log_DIST.flags = (buf.local_pos.dist_bottom_valid ? 1 : 0);
LOGBUFFER_WRITE_AND_COUNT(DIST);
}
}
/* --- LOCAL POSITION SETPOINT --- */
@@ -1140,29 +1252,26 @@ int sdlog2_thread_main(int argc, char *argv[])
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
log_msg.msg_type = LOG_GPOS_MSG;
log_msg.body.log_GPOS.lat = buf.global_pos.lat;
log_msg.body.log_GPOS.lon = buf.global_pos.lon;
log_msg.body.log_GPOS.lat = buf.global_pos.lat * 1e7;
log_msg.body.log_GPOS.lon = buf.global_pos.lon * 1e7;
log_msg.body.log_GPOS.alt = buf.global_pos.alt;
log_msg.body.log_GPOS.vel_n = buf.global_pos.vx;
log_msg.body.log_GPOS.vel_e = buf.global_pos.vy;
log_msg.body.log_GPOS.vel_d = buf.global_pos.vz;
log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n;
log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e;
log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d;
LOGBUFFER_WRITE_AND_COUNT(GPOS);
}
/* --- GLOBAL POSITION SETPOINT --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(mission_item_triplet), subs.triplet_sub, &buf.triplet);
orb_copy(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet);
log_msg.msg_type = LOG_GPSP_MSG;
log_msg.body.log_GPSP.altitude_is_relative = buf.triplet.current.altitude_is_relative;
log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d);
log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d);
log_msg.body.log_GPSP.altitude = buf.triplet.current.altitude;
log_msg.body.log_GPSP.alt = buf.triplet.current.alt;
log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw;
log_msg.body.log_GPSP.nav_cmd = buf.triplet.current.nav_cmd;
log_msg.body.log_GPSP.type = buf.triplet.current.type;
log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius;
log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction;
log_msg.body.log_GPSP.acceptance_radius = buf.triplet.current.acceptance_radius;
log_msg.body.log_GPSP.time_inside = buf.triplet.current.time_inside;
log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min;
LOGBUFFER_WRITE_AND_COUNT(GPSP);
}
@@ -1238,6 +1347,17 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(GVSP);
}
/* --- BATTERY --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(battery_status), subs.battery_sub, &buf.battery);
log_msg.msg_type = LOG_BATT_MSG;
log_msg.body.log_BATT.voltage = buf.battery.voltage_v;
log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v;
log_msg.body.log_BATT.current = buf.battery.current_a;
log_msg.body.log_BATT.discharged = buf.battery.discharged_mah;
LOGBUFFER_WRITE_AND_COUNT(BATT);
}
/* signal the other thread new data, but not yet unlock */
if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
/* only request write if several packets can be written at once */
@@ -1261,7 +1381,7 @@ int sdlog2_thread_main(int argc, char *argv[])
free(lb.data);
warnx("exiting.");
warnx("exiting");
thread_running = false;
@@ -1274,8 +1394,8 @@ void sdlog2_status()
float mebibytes = kibibytes / 1024.0f;
float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f;
warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs.", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped);
mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs.", log_msgs_written, log_msgs_skipped);
warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped);
mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped);
}
/**
@@ -1294,7 +1414,7 @@ int file_copy(const char *file_old, const char *file_new)
int ret = 0;
if (source == NULL) {
warnx("failed opening input file to copy.");
warnx("failed opening input file to copy");
return 1;
}
@@ -1302,7 +1422,7 @@ int file_copy(const char *file_old, const char *file_new)
if (target == NULL) {
fclose(source);
warnx("failed to open output file to copy.");
warnx("failed to open output file to copy");
return 1;
}
@@ -1313,7 +1433,7 @@ int file_copy(const char *file_old, const char *file_new)
ret = fwrite(buf, 1, nread, target);
if (ret <= 0) {
warnx("error writing file.");
warnx("error writing file");
ret = 1;
break;
}
src/modules/sdlog2/sdlog2_messages.h
+29 -10
View File
@@ -57,6 +57,9 @@ struct log_ATT_s {
float roll_rate;
float pitch_rate;
float yaw_rate;
float gx;
float gy;
float gz;
};
/* --- ATSP - ATTITUDE SET POINT --- */
@@ -148,8 +151,6 @@ struct log_STAT_s {
uint8_t main_state;
uint8_t navigation_state;
uint8_t arming_state;
float battery_voltage;
float battery_current;
float battery_remaining;
uint8_t battery_warning;
uint8_t landed;
@@ -209,16 +210,13 @@ struct log_GPOS_s {
/* --- GPSP - GLOBAL POSITION SETPOINT --- */
#define LOG_GPSP_MSG 17
struct log_GPSP_s {
uint8_t altitude_is_relative;
int32_t lat;
int32_t lon;
float altitude;
float alt;
float yaw;
uint8_t nav_cmd;
uint8_t type;
float loiter_radius;
int8_t loiter_direction;
float acceptance_radius;
float time_inside;
float pitch_min;
};
@@ -247,6 +245,25 @@ struct log_GVSP_s {
float vz;
};
/* --- BATT - BATTERY --- */
#define LOG_BATT_MSG 20
struct log_BATT_s {
float voltage;
float voltage_filtered;
float current;
float discharged;
};
/* --- DIST - DISTANCE TO SURFACE --- */
#define LOG_DIST_MSG 21
struct log_DIST_s {
float bottom;
float bottom_rate;
uint8_t flags;
};
/********** SYSTEM MESSAGES, ID > 0x80 **********/
/* --- TIME - TIME STAMP --- */
#define LOG_TIME_MSG 129
struct log_TIME_s {
@@ -272,7 +289,7 @@ struct log_PARM_s {
/* construct list of all message formats */
static const struct log_format_s log_formats[] = {
/* business-level messages, ID < 0x80 */
LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"),
LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"),
@@ -280,16 +297,18 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT(STAT, "BBBfffBB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn,Landed"),
LOG_FORMAT(STAT, "BBBfBB", "MainState,NavState,ArmState,BatRem,BatWarn,Landed"),
LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
LOG_FORMAT(GPSP, "BLLffBfbfff", "AltRel,Lat,Lon,Alt,Yaw,NavCmd,LoitR,LoitDir,AccR,TimeIn,PitMin"),
LOG_FORMAT(GPSP, "LLffBfbf", "Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
/* system-level messages, ID >= 0x80 */
// FMT: don't write format of format message, it's useless
LOG_FORMAT(TIME, "Q", "StartTime"),
src/modules/sensors/module.mk
+1 -1
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
# Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
src/modules/sensors/sensor_params.c
+233 -17
View File
@@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -38,6 +35,10 @@
* @file sensor_params.c
*
* Parameters defined by the sensors task.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
*/
#include <nuttx/config.h>
@@ -45,41 +46,98 @@
#include <systemlib/param/param.h>
/**
* Gyro X offset FIXME
* Gyro X offset
*
* This is an X-axis offset for the gyro.
* Adjust it according to the calibration data.
* This is an X-axis offset for the gyro. Adjust it according to the calibration data.
*
* @min -10.0
* @max 10.0
* @group Gyro Config
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);
/**
* Gyro Y offset FIXME with dot.
* Gyro Y offset
*
* @min -10.0
* @max 10.0
* @group Gyro Config
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);
/**
* Gyro Z offset FIXME
* Gyro Z offset
*
* @min -5.0
* @max 5.0
* @group Gyro Config
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);
/**
* Gyro X scaling
*
* X-axis scaling.
*
* @min -1.5
* @max 1.5
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f);
/**
* Gyro Y scaling
*
* Y-axis scaling.
*
* @min -1.5
* @max 1.5
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f);
/**
* Gyro Z scaling
*
* Z-axis scaling.
*
* @min -1.5
* @max 1.5
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f);
/**
* Magnetometer X offset
*
* This is an X-axis offset for the magnetometer.
*
* @min -500.0
* @max 500.0
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f);
/**
* Magnetometer Y offset
*
* This is an Y-axis offset for the magnetometer.
*
* @min -500.0
* @max 500.0
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f);
/**
* Magnetometer Z offset
*
* This is an Z-axis offset for the magnetometer.
*
* @min -500.0
* @max 500.0
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_MAG_XSCALE, 1.0f);
@@ -100,16 +158,114 @@ PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0);
PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0);
/**
* RC Channel 1 Minimum
*
* Minimum value for RC channel 1
*
* @min 800.0
* @max 1500.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
/**
* RC Channel 1 Trim
*
* Mid point value (same as min for throttle)
*
* @min 800.0
* @max 2200.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
/**
* RC Channel 1 Maximum
*
* Maximum value for RC channel 1
*
* @min 1500.0
* @max 2200.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f);
/**
* RC Channel 1 Reverse
*
* Set to -1 to reverse channel.
*
* @min -1.0
* @max 1.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC1_REV, 1.0f);
/**
* RC Channel 1 dead zone
*
* The +- range of this value around the trim value will be considered as zero.
*
* @min 0.0
* @max 100.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC1_DZ, 10.0f);
PARAM_DEFINE_FLOAT(RC2_MIN, 1000);
PARAM_DEFINE_FLOAT(RC2_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC2_MAX, 2000);
/**
* RC Channel 2 Minimum
*
* Minimum value for RC channel 2
*
* @min 800.0
* @max 1500.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC2_MIN, 1000.0f);
/**
* RC Channel 2 Trim
*
* Mid point value (same as min for throttle)
*
* @min 800.0
* @max 2200.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC2_TRIM, 1500.0f);
/**
* RC Channel 2 Maximum
*
* Maximum value for RC channel 2
*
* @min 1500.0
* @max 2200.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC2_MAX, 2000.0f);
/**
* RC Channel 2 Reverse
*
* Set to -1 to reverse channel.
*
* @min -1.0
* @max 1.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC2_REV, 1.0f);
/**
* RC Channel 2 dead zone
*
* The +- range of this value around the trim value will be considered as zero.
*
* @min 0.0
* @max 100.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC2_DZ, 10.0f);
PARAM_DEFINE_FLOAT(RC3_MIN, 1000);
@@ -223,15 +379,75 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
/* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */
PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f);
#endif
PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124); /* scaling for 3DR power brick */
/**
* Roll control channel mapping.
*
* The channel index (starting from 1 for channel 1) indicates
* which channel should be used for reading roll inputs from.
* A value of zero indicates the switch is not assigned.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
/**
* Pitch control channel mapping.
*
* The channel index (starting from 1 for channel 1) indicates
* which channel should be used for reading pitch inputs from.
* A value of zero indicates the switch is not assigned.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
/**
* Throttle control channel mapping.
*
* The channel index (starting from 1 for channel 1) indicates
* which channel should be used for reading throttle inputs from.
* A value of zero indicates the switch is not assigned.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
/**
* Yaw control channel mapping.
*
* The channel index (starting from 1 for channel 1) indicates
* which channel should be used for reading yaw inputs from.
* A value of zero indicates the switch is not assigned.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5);
/**
* Mode switch channel mapping.
*
* This is the main flight mode selector.
* The channel index (starting from 1 for channel 1) indicates
* which channel should be used for deciding about the main mode.
* A value of zero indicates the switch is not assigned.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 6);
PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0);
//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
src/modules/sensors/sensors.cpp
+67 -39
View File
@@ -114,6 +114,7 @@
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
#define ADC_BATTERY_VOLTAGE_CHANNEL 10
#define ADC_BATTERY_CURRENT_CHANNEL -1
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11
#endif
@@ -124,10 +125,8 @@
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 15
#endif
#define BAT_VOL_INITIAL 0.f
#define BAT_VOL_LOWPASS_1 0.99f
#define BAT_VOL_LOWPASS_2 0.01f
#define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f
#define BATT_V_LOWPASS 0.001f
#define BATT_V_IGNORE_THRESHOLD 3.5f
/**
* HACK - true temperature is much less than indicated temperature in baro,
@@ -211,10 +210,13 @@ private:
struct differential_pressure_s _diff_pres;
struct airspeed_s _airspeed;
math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
math::Matrix<3,3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
math::Matrix<3,3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
bool _mag_is_external; /**< true if the active mag is on an external board */
uint64_t _battery_discharged; /**< battery discharged current in mA*ms */
hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */
struct {
float min[_rc_max_chan_count];
float trim[_rc_max_chan_count];
@@ -265,6 +267,7 @@ private:
float rc_fs_thr;
float battery_voltage_scaling;
float battery_current_scaling;
} _parameters; /**< local copies of interesting parameters */
@@ -314,6 +317,7 @@ private:
param_t rc_fs_thr;
param_t battery_voltage_scaling;
param_t battery_current_scaling;
param_t board_rotation;
param_t external_mag_rotation;
@@ -465,9 +469,9 @@ Sensors::Sensors() :
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
_board_rotation(3, 3),
_external_mag_rotation(3, 3),
_mag_is_external(false)
_mag_is_external(false),
_battery_discharged(0),
_battery_current_timestamp(0)
{
/* basic r/c parameters */
@@ -560,6 +564,7 @@ Sensors::Sensors() :
_parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA");
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
_parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING");
/* rotations */
_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
@@ -740,6 +745,11 @@ Sensors::parameters_update()
warnx("Failed updating voltage scaling param");
}
/* scaling of ADC ticks to battery current */
if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) {
warnx("Failed updating current scaling param");
}
param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation));
@@ -785,7 +795,6 @@ Sensors::accel_init()
#endif
warnx("using system accel");
close(fd);
}
}
@@ -825,7 +834,6 @@ Sensors::gyro_init()
#endif
warnx("using system gyro");
close(fd);
}
}
@@ -920,7 +928,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z};
math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
vect = _board_rotation * vect;
raw.accelerometer_m_s2[0] = vect(0);
@@ -946,7 +954,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z};
math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
vect = _board_rotation * vect;
raw.gyro_rad_s[0] = vect(0);
@@ -972,7 +980,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z};
math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
if (_mag_is_external)
vect = _external_mag_rotation * vect;
@@ -1157,17 +1165,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
if (!_publishing)
return;
hrt_abstime t = hrt_absolute_time();
/* rate limit to 100 Hz */
if (hrt_absolute_time() - _last_adc >= 10000) {
if (t - _last_adc >= 10000) {
/* make space for a maximum of eight channels */
struct adc_msg_s buf_adc[8];
/* read all channels available */
int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc));
for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) {
if (ret >= (int)sizeof(buf_adc[0])) {
if (ret >= (int)sizeof(buf_adc[0])) {
for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) {
/* Save raw voltage values */
if (i < (sizeof(raw.adc_voltage_v)) / sizeof(raw.adc_voltage_v[0])) {
raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f);
@@ -1178,27 +1185,40 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
/* Voltage in volts */
float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling);
if (voltage > VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
if (voltage > BATT_V_IGNORE_THRESHOLD) {
_battery_status.voltage_v = voltage;
/* one-time initialization of low-pass value to avoid long init delays */
if (_battery_status.voltage_v < 3.0f) {
_battery_status.voltage_v = voltage;
if (_battery_status.voltage_filtered_v < BATT_V_IGNORE_THRESHOLD) {
_battery_status.voltage_filtered_v = voltage;
}
_battery_status.timestamp = hrt_absolute_time();
_battery_status.voltage_v = (BAT_VOL_LOWPASS_1 * (_battery_status.voltage_v + BAT_VOL_LOWPASS_2 * voltage));;
/* current and discharge are unknown */
_battery_status.current_a = -1.0f;
_battery_status.discharged_mah = -1.0f;
_battery_status.timestamp = t;
_battery_status.voltage_filtered_v += (voltage - _battery_status.voltage_filtered_v) * BATT_V_LOWPASS;
/* announce the battery voltage if needed, just publish else */
if (_battery_pub > 0) {
orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
} else {
/* mark status as invalid */
_battery_status.voltage_v = -1.0f;
_battery_status.voltage_filtered_v = -1.0f;
}
} else {
_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
} else if (ADC_BATTERY_CURRENT_CHANNEL == buf_adc[i].am_channel) {
/* handle current only if voltage is valid */
if (_battery_status.voltage_v > 0.0f) {
float current = (buf_adc[i].am_data * _parameters.battery_current_scaling);
/* check measured current value */
if (current >= 0.0f) {
_battery_status.timestamp = t;
_battery_status.current_a = current;
if (_battery_current_timestamp != 0) {
/* initialize discharged value */
if (_battery_status.discharged_mah < 0.0f)
_battery_status.discharged_mah = 0.0f;
_battery_discharged += current * (t - _battery_current_timestamp);
_battery_status.discharged_mah = ((float) _battery_discharged) / 3600000.0f;
}
}
}
_battery_current_timestamp = t;
} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
@@ -1214,7 +1234,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor
_diff_pres.timestamp = hrt_absolute_time();
_diff_pres.timestamp = t;
_diff_pres.differential_pressure_pa = diff_pres_pa;
_diff_pres.voltage = voltage;
@@ -1227,8 +1247,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
}
}
}
}
_last_adc = t;
if (_battery_status.voltage_v > 0.0f) {
/* announce the battery status if needed, just publish else */
if (_battery_pub > 0) {
orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
_last_adc = hrt_absolute_time();
} else {
_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
}
}
}
}
@@ -1475,9 +1503,6 @@ void
Sensors::task_main()
{
/* inform about start */
warnx("Initializing..");
/* start individual sensors */
accel_init();
gyro_init();
@@ -1516,7 +1541,10 @@ Sensors::task_main()
raw.adc_voltage_v[3] = 0.0f;
memset(&_battery_status, 0, sizeof(_battery_status));
_battery_status.voltage_v = BAT_VOL_INITIAL;
_battery_status.voltage_v = -1.0f;
_battery_status.voltage_filtered_v = -1.0f;
_battery_status.current_a = -1.0f;
_battery_status.discharged_mah = -1.0f;
/* get a set of initial values */
accel_poll(raw);
src/modules/multirotor_att_control/multirotor_rate_control.h → src/modules/systemlib/board_serial.c
+21 -25
View File
@@ -1,12 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Laurens Mackay <mackayl@student.ethz.ch>
* Tobias Naegeli <naegelit@student.ethz.ch>
* Martin Rutschmann <rutmarti@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -37,28 +31,30 @@
*
****************************************************************************/
/*
* @file multirotor_attitude_control.h
/**
* @file board_serial.h
* Read off the board serial
*
* Definition of rate controller for multirotors.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Laurens Mackay <mackayl@student.ethz.ch>
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author David "Buzz" Bussenschutt <davidbuzz@gmail.com>
*
*/
#ifndef MULTIROTOR_RATE_CONTROL_H_
#define MULTIROTOR_RATE_CONTROL_H_
#include "otp.h"
#include "board_config.h"
#include "board_serial.h"
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_controls.h>
int get_board_serial(char *serialid)
{
const volatile unsigned *udid_ptr = (const unsigned *)UDID_START;
union udid id;
val_read((unsigned *)&id, udid_ptr, sizeof(id));
void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[], struct actuator_controls_s *actuators, bool reset_integral);
#endif /* MULTIROTOR_RATE_CONTROL_H_ */
/* Copy the serial from the chips non-write memory and swap endianess */
serialid[0] = id.data[3]; serialid[1] = id.data[2]; serialid[2] = id.data[1]; serialid[3] = id.data[0];
serialid[4] = id.data[7]; serialid[5] = id.data[6]; serialid[6] = id.data[5]; serialid[7] = id.data[4];
serialid[8] = id.data[11]; serialid[9] = id.data[10]; serialid[10] = id.data[9]; serialid[11] = id.data[8];
return 0;
}
src/modules/sdlog2/sdlog2_version.h → src/modules/systemlib/board_serial.h
+9 -22
View File
@@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <anton.babushkin@me.com>
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -33,30 +32,18 @@
****************************************************************************/
/**
* @file sdlog2_version.h
* @file board_serial.h
* Read off the board serial
*
* Tools for system version detection.
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author David "Buzz" Bussenschutt <davidbuzz@gmail.com>
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef SDLOG2_VERSION_H_
#define SDLOG2_VERSION_H_
#pragma once
/*
GIT_VERSION is defined at build time via a Makefile call to the
git command line.
*/
#define FREEZE_STR(s) #s
#define STRINGIFY(s) FREEZE_STR(s)
#define FW_GIT STRINGIFY(GIT_VERSION)
__BEGIN_DECLS
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
#define HW_ARCH "PX4FMU_V1"
#endif
__EXPORT int get_board_serial(char *serialid);
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
#define HW_ARCH "PX4FMU_V2"
#endif
#endif /* SDLOG2_VERSION_H_ */
__END_DECLS
src/modules/systemlib/bson/tinybson.c
+3
View File
@@ -407,6 +407,9 @@ bson_encoder_fini(bson_encoder_t encoder)
memcpy(encoder->buf, &len, sizeof(len));
}
/* sync file */
fsync(encoder->fd);
return 0;
}
src/modules/systemlib/module.mk
+5 -1
View File
@@ -49,4 +49,8 @@ SRCS = err.c \
airspeed.c \
system_params.c \
mavlink_log.c \
rc_check.c
rc_check.c \
otp.c \
board_serial.c \
pwm_limit/pwm_limit.c
src/modules/systemlib/otp.c
+224
View File
@@ -0,0 +1,224 @@
/****************************************************************************
*
* Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
* Authors:
* Lorenz Meier <lm@inf.ethz.ch>
* David "Buzz" Bussenschutt <davidbuzz@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file otp.c
* otp estimation
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author David "Buzz" Bussenschutt <davidbuzz@gmail.com>
*
*/
#include <nuttx/config.h>
#include <board_config.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <string.h> // memset
#include "conversions.h"
#include "otp.h"
#include "err.h" // warnx
#include <assert.h>
int val_read(void *dest, volatile const void *src, int bytes)
{
int i;
for (i = 0; i < bytes / 4; i++) {
*(((volatile unsigned *)dest) + i) = *(((volatile unsigned *)src) + i);
}
return i * 4;
}
int write_otp(uint8_t id_type, uint32_t vid, uint32_t pid, char *signature)
{
warnx("write_otp: PX4 / %02X / %02X / %02X / ... etc \n", id_type, vid, pid);
int errors = 0;
// descriptor
if (F_write_byte(ADDR_OTP_START, 'P'))
errors++;
// write the 'P' from PX4. to first byte in OTP
if (F_write_byte(ADDR_OTP_START + 1, 'X'))
errors++; // write the 'P' from PX4. to first byte in OTP
if (F_write_byte(ADDR_OTP_START + 2, '4'))
errors++;
if (F_write_byte(ADDR_OTP_START + 3, '\0'))
errors++;
//id_type
if (F_write_byte(ADDR_OTP_START + 4, id_type))
errors++;
// vid and pid are 4 bytes each
if (F_write_word(ADDR_OTP_START + 5, vid))
errors++;
if (F_write_word(ADDR_OTP_START + 9, pid))
errors++;
// leave some 19 bytes of space, and go to the next block...
// then the auth sig starts
for (int i = 0 ; i < 128 ; i++) {
if (F_write_byte(ADDR_OTP_START + 32 + i, signature[i]))
errors++;
}
return errors;
}
int lock_otp(void)
{
//determine the required locking size - can only write full lock bytes */
// int size = sizeof(struct otp) / 32;
//
// struct otp_lock otp_lock_mem;
//
// memset(&otp_lock_mem, OTP_LOCK_UNLOCKED, sizeof(otp_lock_mem));
// for (int i = 0; i < sizeof(otp_lock_mem) / sizeof(otp_lock_mem.lock_bytes[0]); i++)
// otp_lock_mem.lock_bytes[i] = OTP_LOCK_LOCKED;
//XXX add the actual call here to write the OTP_LOCK bytes only at final stage
// val_copy(lock_ptr, &otp_lock_mem, sizeof(otp_lock_mem));
int locksize = 5;
int errors = 0;
// or just realise it's exctly 5x 32byte blocks we need to lock. 1 block for ID,type,vid,pid, and 4 blocks for certificate, which is 128 bytes.
for (int i = 0 ; i < locksize ; i++) {
if (F_write_byte(ADDR_OTP_LOCK_START + i, OTP_LOCK_LOCKED))
errors++;
}
return errors;
}
// COMPLETE, BUSY, or other flash error?
int F_GetStatus(void)
{
int fs = F_COMPLETE;
if ((FLASH->status & F_BSY) == F_BSY) { fs = F_BUSY; } else {
if ((FLASH->status & F_WRPERR) != (uint32_t)0x00) { fs = F_ERROR_WRP; } else {
if ((FLASH->status & (uint32_t)0xEF) != (uint32_t)0x00) { fs = F_ERROR_PROGRAM; } else {
if ((FLASH->status & F_OPERR) != (uint32_t)0x00) { fs = F_ERROR_OPERATION; } else {
fs = F_COMPLETE;
}
}
}
}
return fs;
}
// enable FLASH Registers
void F_unlock(void)
{
if ((FLASH->control & F_CR_LOCK) != 0) {
FLASH->key = F_KEY1;
FLASH->key = F_KEY2;
}
}
// lock the FLASH Registers
void F_lock(void)
{
FLASH->control |= F_CR_LOCK;
}
// flash write word.
int F_write_word(uint32_t Address, uint32_t Data)
{
unsigned char octet[4] = {0, 0, 0, 0};
int ret = 0;
for (int i = 0; i < 4; i++) {
octet[i] = (Data >> (i * 8)) & 0xFF;
ret = F_write_byte(Address + i, octet[i]);
}
return ret;
}
// flash write byte
int F_write_byte(uint32_t Address, uint8_t Data)
{
volatile int status = F_COMPLETE;
//warnx("F_write_byte: %08X %02d", Address , Data ) ;
//Check the parameters
assert(IS_F_ADDRESS(Address));
//Wait for FLASH operation to complete by polling on BUSY flag.
status = F_GetStatus();
while (status == F_BUSY) { status = F_GetStatus();}
if (status == F_COMPLETE) {
//if the previous operation is completed, proceed to program the new data
FLASH->control &= CR_PSIZE_MASK;
FLASH->control |= F_PSIZE_BYTE;
FLASH->control |= F_CR_PG;
*(volatile uint8_t *)Address = Data;
//Wait for FLASH operation to complete by polling on BUSY flag.
status = F_GetStatus();
while (status == F_BUSY) { status = F_GetStatus();}
//if the program operation is completed, disable the PG Bit
FLASH->control &= (~F_CR_PG);
}
//Return the Program Status
return !(status == F_COMPLETE);
}
src/modules/systemlib/otp.h
+151
View File
@@ -0,0 +1,151 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file otp.h
* One TIme Programmable ( OTP ) Flash routine/s.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author David "Buzz" Bussenschutt <davidbuzz@gmail.com>
*
*/
#ifndef OTP_H_
#define OTP_H_
__BEGIN_DECLS
#define ADDR_OTP_START 0x1FFF7800
#define ADDR_OTP_LOCK_START 0x1FFF7A00
#define OTP_LOCK_LOCKED 0x00
#define OTP_LOCK_UNLOCKED 0xFF
#include <unistd.h>
#include <stdio.h>
// possible flash statuses
#define F_BUSY 1
#define F_ERROR_WRP 2
#define F_ERROR_PROGRAM 3
#define F_ERROR_OPERATION 4
#define F_COMPLETE 5
typedef struct {
volatile uint32_t accesscontrol; // 0x00
volatile uint32_t key; // 0x04
volatile uint32_t optionkey; // 0x08
volatile uint32_t status; // 0x0C
volatile uint32_t control; // 0x10
volatile uint32_t optioncontrol; //0x14
} flash_registers;
#define PERIPH_BASE ((uint32_t)0x40000000) //Peripheral base address
#define AHB1PERIPH_BASE (PERIPH_BASE + 0x00020000)
#define F_R_BASE (AHB1PERIPH_BASE + 0x3C00)
#define FLASH ((flash_registers *) F_R_BASE)
#define F_BSY ((uint32_t)0x00010000) //FLASH Busy flag bit
#define F_OPERR ((uint32_t)0x00000002) //FLASH operation Error flag bit
#define F_WRPERR ((uint32_t)0x00000010) //FLASH Write protected error flag bit
#define CR_PSIZE_MASK ((uint32_t)0xFFFFFCFF)
#define F_PSIZE_WORD ((uint32_t)0x00000200)
#define F_PSIZE_BYTE ((uint32_t)0x00000000)
#define F_CR_PG ((uint32_t)0x00000001) // a bit in the F_CR register
#define F_CR_LOCK ((uint32_t)0x80000000) // also another bit.
#define F_KEY1 ((uint32_t)0x45670123)
#define F_KEY2 ((uint32_t)0xCDEF89AB)
#define IS_F_ADDRESS(ADDRESS) ((((ADDRESS) >= 0x08000000) && ((ADDRESS) < 0x080FFFFF)) || (((ADDRESS) >= 0x1FFF7800) && ((ADDRESS) < 0x1FFF7A0F)))
#pragma pack(push, 1)
/*
* The OTP area is divided into 16 OTP data blocks of 32 bytes and one lock OTP block of 16 bytes.
* The OTP data and lock blocks cannot be erased. The lock block contains 16 bytes LOCKBi (0 ≤ i ≤ 15)
* to lock the corresponding OTP data block (blocks 0 to 15). Each OTP data block can be programmed
* until the value 0x00 is programmed in the corresponding OTP lock byte. The lock bytes must only
* contain 0x00 and 0xFF values, otherwise the OTP bytes might not be taken into account correctly.
*/
struct otp {
// first 32 bytes = the '0' Block
char id[4]; ///4 bytes < 'P' 'X' '4' '\n'
uint8_t id_type; ///1 byte < 0 for USB VID, 1 for generic VID
uint32_t vid; ///4 bytes
uint32_t pid; ///4 bytes
char unused[19]; ///19 bytes
// Cert-of-Auth is next 4 blocks ie 1-4 ( where zero is first block )
char signature[128];
// insert extras here
uint32_t lock_bytes[4];
};
struct otp_lock {
uint8_t lock_bytes[16];
};
#pragma pack(pop)
#define ADDR_F_SIZE 0x1FFF7A22
#pragma pack(push, 1)
union udid {
uint32_t serial[3];
char data[12];
};
#pragma pack(pop)
/**
* s
*/
//__EXPORT float calc_indicated_airspeed(float differential_pressure);
__EXPORT void F_unlock(void);
__EXPORT void F_lock(void);
__EXPORT int val_read(void *dest, volatile const void *src, int bytes);
__EXPORT int val_write(volatile void *dest, const void *src, int bytes);
__EXPORT int write_otp(uint8_t id_type, uint32_t vid, uint32_t pid, char *signature);
__EXPORT int lock_otp(void);
__EXPORT int F_write_byte(uint32_t Address, uint8_t Data);
__EXPORT int F_write_word(uint32_t Address, uint32_t Data);
__END_DECLS
#endif
src/modules/systemlib/param/param.c
+29 -6
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -61,7 +61,7 @@
#include "uORB/uORB.h"
#include "uORB/topics/parameter_update.h"
#if 1
#if 0
# define debug(fmt, args...) do { warnx(fmt, ##args); } while(0)
#else
# define debug(fmt, args...) do { } while(0)
@@ -512,6 +512,28 @@ param_save_default(void)
int fd;
const char *filename = param_get_default_file();
/* write parameters to temp file */
fd = open(filename, O_WRONLY | O_CREAT);
if (fd < 0) {
warn("failed to open param file: %s", filename);
return ERROR;
}
if (res == OK) {
res = param_export(fd, false);
if (res != OK) {
warnx("failed to write parameters to file: %s", filename);
}
}
close(fd);
return res;
#if 0
const char *filename_tmp = malloc(strlen(filename) + 5);
sprintf(filename_tmp, "%s.tmp", filename);
@@ -565,6 +587,7 @@ param_save_default(void)
free(filename_tmp);
return res;
#endif
}
/**
@@ -573,9 +596,9 @@ param_save_default(void)
int
param_load_default(void)
{
int fd = open(param_get_default_file(), O_RDONLY);
int fd_load = open(param_get_default_file(), O_RDONLY);
if (fd < 0) {
if (fd_load < 0) {
/* no parameter file is OK, otherwise this is an error */
if (errno != ENOENT) {
warn("open '%s' for reading failed", param_get_default_file());
@@ -584,8 +607,8 @@ param_load_default(void)
return 1;
}
int result = param_load(fd);
close(fd);
int result = param_load(fd_load);
close(fd_load);
if (result != 0) {
warn("error reading parameters from '%s'", param_get_default_file());
src/modules/systemlib/pid/pid.c
+37 -69
View File
@@ -39,7 +39,7 @@
/**
* @file pid.c
*
* Implementation of generic PID control interface.
* Implementation of generic PID controller.
*
* @author Laurens Mackay <mackayl@student.ethz.ch>
* @author Tobias Naegeli <naegelit@student.ethz.ch>
@@ -53,24 +53,21 @@
#define SIGMA 0.000001f
__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
float limit, uint8_t mode, float dt_min)
__EXPORT void pid_init(PID_t *pid, uint8_t mode, float dt_min)
{
pid->kp = kp;
pid->ki = ki;
pid->kd = kd;
pid->intmax = intmax;
pid->limit = limit;
pid->mode = mode;
pid->dt_min = dt_min;
pid->count = 0.0f;
pid->saturated = 0.0f;
pid->last_output = 0.0f;
pid->sp = 0.0f;
pid->error_previous = 0.0f;
pid->kp = 0.0f;
pid->ki = 0.0f;
pid->kd = 0.0f;
pid->integral = 0.0f;
pid->integral_limit = 0.0f;
pid->output_limit = 0.0f;
pid->error_previous = 0.0f;
pid->last_output = 0.0f;
}
__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit)
__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit)
{
int ret = 0;
@@ -95,15 +92,15 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float
ret = 1;
}
if (isfinite(intmax)) {
pid->intmax = intmax;
if (isfinite(integral_limit)) {
pid->integral_limit = integral_limit;
} else {
ret = 1;
}
if (isfinite(limit)) {
pid->limit = limit;
if (isfinite(output_limit)) {
pid->output_limit = output_limit;
} else {
ret = 1;
@@ -112,42 +109,18 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float
return ret;
}
//void pid_set(PID_t *pid, float sp)
//{
// pid->sp = sp;
// pid->error_previous = 0;
// pid->integral = 0;
//}
/**
*
* @param pid
* @param val
* @param dt
* @return
*/
__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
{
/* error = setpoint - actual_position
integral = integral + (error*dt)
derivative = (error - previous_error)/dt
output = (Kp*error) + (Ki*integral) + (Kd*derivative)
previous_error = error
wait(dt)
goto start
*/
if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt)) {
return pid->last_output;
}
float i, d;
pid->sp = sp;
// Calculated current error value
float error = pid->sp - val;
/* current error value */
float error = sp - val;
// Calculate or measured current error derivative
/* current error derivative */
if (pid->mode == PID_MODE_DERIVATIV_CALC) {
d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
pid->error_previous = error;
@@ -167,39 +140,34 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
d = 0.0f;
}
if (pid->ki > 0.0f) {
/* calculate PD output */
float output = (error * pid->kp) + (d * pid->kd);
if (pid->ki > SIGMA) {
// Calculate the error integral and check for saturation
i = pid->integral + (error * dt);
if ((pid->limit > SIGMA && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) ||
fabsf(i) > pid->intmax) {
i = pid->integral; // If saturated then do not update integral value
pid->saturated = 1;
} else {
if (!isfinite(i)) {
i = 0.0f;
/* check for saturation */
if (isfinite(i)) {
if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) &&
fabsf(i) <= pid->integral_limit) {
/* not saturated, use new integral value */
pid->integral = i;
}
pid->integral = i;
pid->saturated = 0;
}
} else {
i = 0.0f;
pid->saturated = 0;
/* add I component to output */
output += pid->integral * pid->ki;
}
// Calculate the output. Limit output magnitude to pid->limit
float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
/* limit output */
if (isfinite(output)) {
if (pid->limit > SIGMA) {
if (output > pid->limit) {
output = pid->limit;
if (pid->output_limit > SIGMA) {
if (output > pid->output_limit) {
output = pid->output_limit;
} else if (output < -pid->limit) {
output = -pid->limit;
} else if (output < -pid->output_limit) {
output = -pid->output_limit;
}
}
@@ -212,5 +180,5 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
__EXPORT void pid_reset_integral(PID_t *pid)
{
pid->integral = 0;
pid->integral = 0.0f;
}
src/modules/systemlib/pid/pid.h
+19 -22
View File
@@ -39,7 +39,7 @@
/**
* @file pid.h
*
* Definition of generic PID control interface.
* Definition of generic PID controller.
*
* @author Laurens Mackay <mackayl@student.ethz.ch>
* @author Tobias Naegeli <naegelit@student.ethz.ch>
@@ -55,38 +55,35 @@
__BEGIN_DECLS
/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error
* val_dot in pid_calculate() will be ignored */
#define PID_MODE_DERIVATIV_CALC 0
/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored
* val_dot in pid_calculate() will be ignored */
#define PID_MODE_DERIVATIV_CALC_NO_SP 1
/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
#define PID_MODE_DERIVATIV_SET 2
// Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID)
#define PID_MODE_DERIVATIV_NONE 9
typedef enum PID_MODE {
/* Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) */
PID_MODE_DERIVATIV_NONE = 0,
/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error,
* val_dot in pid_calculate() will be ignored */
PID_MODE_DERIVATIV_CALC,
/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value,
* setpoint derivative will be ignored, val_dot in pid_calculate() will be ignored */
PID_MODE_DERIVATIV_CALC_NO_SP,
/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
PID_MODE_DERIVATIV_SET
} pid_mode_t;
typedef struct {
pid_mode_t mode;
float dt_min;
float kp;
float ki;
float kd;
float intmax;
float sp;
float integral;
float integral_limit;
float output_limit;
float error_previous;
float last_output;
float limit;
float dt_min;
uint8_t mode;
uint8_t count;
uint8_t saturated;
} PID_t;
__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode, float dt_min);
__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit);
//void pid_set(PID_t *pid, float sp);
__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min);
__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit);
__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
__EXPORT void pid_reset_integral(PID_t *pid);
__END_DECLS
src/modules/systemlib/pwm_limit/pwm_limit.c
+64 -46
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Author: Julian Oes <joes@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
@@ -44,38 +44,53 @@
#include <math.h>
#include <stdbool.h>
#include <drivers/drv_hrt.h>
#include <stdio.h>
void pwm_limit_init(pwm_limit_t *limit)
{
limit->state = LIMIT_STATE_OFF;
limit->state = PWM_LIMIT_STATE_INIT;
limit->time_armed = 0;
return;
}
void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit)
void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit)
{
/* first evaluate state changes */
switch (limit->state) {
case LIMIT_STATE_OFF:
if (armed)
limit->state = LIMIT_STATE_RAMP;
limit->time_armed = hrt_absolute_time();
case PWM_LIMIT_STATE_INIT:
if (armed) {
/* set arming time for the first call */
if (limit->time_armed == 0) {
limit->time_armed = hrt_absolute_time();
}
if (hrt_elapsed_time(&limit->time_armed) >= INIT_TIME_US) {
limit->state = PWM_LIMIT_STATE_OFF;
}
}
break;
case LIMIT_STATE_INIT:
if (!armed)
limit->state = LIMIT_STATE_OFF;
else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US)
limit->state = LIMIT_STATE_RAMP;
case PWM_LIMIT_STATE_OFF:
if (armed) {
limit->state = PWM_LIMIT_STATE_RAMP;
/* reset arming time, used for ramp timing */
limit->time_armed = hrt_absolute_time();
}
break;
case LIMIT_STATE_RAMP:
if (!armed)
limit->state = LIMIT_STATE_OFF;
else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US + RAMP_TIME_US)
limit->state = LIMIT_STATE_ON;
case PWM_LIMIT_STATE_RAMP:
if (!armed) {
limit->state = PWM_LIMIT_STATE_OFF;
} else if (hrt_elapsed_time(&limit->time_armed) >= RAMP_TIME_US) {
limit->state = PWM_LIMIT_STATE_ON;
}
break;
case LIMIT_STATE_ON:
if (!armed)
limit->state = LIMIT_STATE_OFF;
case PWM_LIMIT_STATE_ON:
if (!armed) {
limit->state = PWM_LIMIT_STATE_OFF;
}
break;
default:
break;
@@ -86,44 +101,47 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
/* then set effective_pwm based on state */
switch (limit->state) {
case LIMIT_STATE_OFF:
case LIMIT_STATE_INIT:
case PWM_LIMIT_STATE_OFF:
case PWM_LIMIT_STATE_INIT:
for (unsigned i=0; i<num_channels; i++) {
effective_pwm[i] = disarmed_pwm[i];
output[i] = 0.0f;
}
break;
case LIMIT_STATE_RAMP:
case PWM_LIMIT_STATE_RAMP:
{
hrt_abstime diff = hrt_elapsed_time(&limit->time_armed);
progress = (hrt_absolute_time() - INIT_TIME_US - limit->time_armed)*10000 / RAMP_TIME_US;
for (unsigned i=0; i<num_channels; i++) {
uint16_t ramp_min_pwm;
/* if a disarmed pwm value was set, blend between disarmed and min */
if (disarmed_pwm[i] > 0) {
progress = diff * 10000 / RAMP_TIME_US;
/* safeguard against overflows */
uint16_t disarmed = disarmed_pwm[i];
if (disarmed > min_pwm[i])
disarmed = min_pwm[i];
for (unsigned i=0; i<num_channels; i++) {
uint16_t ramp_min_pwm;
/* if a disarmed pwm value was set, blend between disarmed and min */
if (disarmed_pwm[i] > 0) {
uint16_t disarmed_min_diff = min_pwm[i] - disarmed;
ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000;
} else {
/* no disarmed pwm value set, choose min pwm */
ramp_min_pwm = min_pwm[i];
/* safeguard against overflows */
unsigned disarmed = disarmed_pwm[i];
if (disarmed > min_pwm[i]) {
disarmed = min_pwm[i];
}
unsigned disarmed_min_diff = min_pwm[i] - disarmed;
ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000;
} else {
/* no disarmed pwm value set, choose min pwm */
ramp_min_pwm = min_pwm[i];
}
effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2;
}
effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2;
output[i] = (float)progress/10000.0f * output[i];
}
break;
case LIMIT_STATE_ON:
case PWM_LIMIT_STATE_ON:
for (unsigned i=0; i<num_channels; i++) {
effective_pwm[i] = output[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2;
/* effective_output stays the same */
}
break;
default:
src/modules/systemlib/pwm_limit/pwm_limit.h
+11 -9
View File
@@ -46,6 +46,8 @@
#include <stdint.h>
#include <stdbool.h>
__BEGIN_DECLS
/*
* time for the ESCs to initialize
* (this is not actually needed if PWM is sent right after boot)
@@ -56,21 +58,21 @@
*/
#define RAMP_TIME_US 2500000
enum pwm_limit_state {
PWM_LIMIT_STATE_OFF = 0,
PWM_LIMIT_STATE_INIT,
PWM_LIMIT_STATE_RAMP,
PWM_LIMIT_STATE_ON
};
typedef struct {
enum {
LIMIT_STATE_OFF = 0,
LIMIT_STATE_INIT,
LIMIT_STATE_RAMP,
LIMIT_STATE_ON
} state;
enum pwm_limit_state state;
uint64_t time_armed;
} pwm_limit_t;
__BEGIN_DECLS
__EXPORT void pwm_limit_init(pwm_limit_t *limit);
__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit);
__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit);
__END_DECLS
src/modules/uORB/objects_common.cpp
+2 -2
View File
@@ -117,8 +117,8 @@ ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoi
#include "topics/vehicle_bodyframe_speed_setpoint.h"
ORB_DEFINE(vehicle_bodyframe_speed_setpoint, struct vehicle_bodyframe_speed_setpoint_s);
#include "topics/mission_item_triplet.h"
ORB_DEFINE(mission_item_triplet, struct mission_item_triplet_s);
#include "topics/position_setpoint_triplet.h"
ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s);
#include "topics/vehicle_global_velocity_setpoint.h"
ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s);
src/modules/uORB/topics/battery_status.h
+5 -4
View File
@@ -53,9 +53,10 @@
*/
struct battery_status_s {
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
float voltage_v; /**< Battery voltage in volts, filtered */
float current_a; /**< Battery current in amperes, filtered, -1 if unknown */
float discharged_mah; /**< Discharged amount in mAh, filtered, -1 if unknown */
float voltage_v; /**< Battery voltage in volts, 0 if unknown */
float voltage_filtered_v; /**< Battery voltage in volts, filtered, 0 if unknown */
float current_a; /**< Battery current in amperes, -1 if unknown */
float discharged_mah; /**< Discharged amount in mAh, -1 if unknown */
};
/**
@@ -65,4 +66,4 @@ struct battery_status_s {
/* register this as object request broker structure */
ORB_DECLARE(battery_status);
#endif
#endif
src/modules/uORB/topics/home_position.h
+1 -1
View File
@@ -62,7 +62,7 @@ struct home_position_s
//bool altitude_is_relative; // TODO what means home relative altitude? we need clear definition of reference altitude then
double lat; /**< Latitude in degrees */
double lon; /**< Longitude in degrees */
float altitude; /**< Altitude in meters */
float alt; /**< Altitude in meters */
};
/**
src/modules/uORB/topics/mission_item_triplet.h → src/modules/uORB/topics/position_setpoint_triplet.h
+26 -15
View File
@@ -46,31 +46,42 @@
#include <stdbool.h>
#include "../uORB.h"
#include "mission.h"
/**
* @addtogroup topics
* @{
*/
enum SETPOINT_TYPE
{
SETPOINT_TYPE_NORMAL = 0,
SETPOINT_TYPE_LOITER,
SETPOINT_TYPE_TAKEOFF,
SETPOINT_TYPE_LAND,
};
struct position_setpoint_s
{
bool valid; /**< true if setpoint is valid */
enum SETPOINT_TYPE type; /**< setpoint type to adjust behavior of position controller */
double lat; /**< latitude, in deg */
double lon; /**< longitude, in deg */
float alt; /**< altitude AMSL, in m */
float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */
float loiter_radius; /**< loiter radius (only for fixed wing), in m */
int8_t loiter_direction; /**< loiter direction: 1 = CW, -1 = CCW */
float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */
};
/**
* Global position setpoint triplet in WGS84 coordinates.
*
* This are the three next waypoints (or just the next two or one).
*/
struct mission_item_triplet_s
struct position_setpoint_triplet_s
{
bool previous_valid;
bool current_valid; /**< flag indicating previous mission item is valid */
bool next_valid; /**< flag indicating next mission item is valid */
struct mission_item_s previous;
struct mission_item_s current;
struct mission_item_s next;
int previous_index;
int current_index;
int next_index;
struct position_setpoint_s previous;
struct position_setpoint_s current;
struct position_setpoint_s next;
};
/**
@@ -78,6 +89,6 @@ struct mission_item_triplet_s
*/
/* register this as object request broker structure */
ORB_DECLARE(mission_item_triplet);
ORB_DECLARE(position_setpoint_triplet);
#endif
src/modules/uORB/topics/vehicle_attitude.h
+1
View File
@@ -76,6 +76,7 @@ struct vehicle_attitude_s {
float rate_offsets[3]; /**< Offsets of the body angular rates from zero */
float R[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */
float q[4]; /**< Quaternion (NED) */
float g_comp[3]; /**< Compensated gravity vector */
bool R_valid; /**< Rotation matrix valid */
bool q_valid; /**< Quaternion valid */
src/modules/uORB/topics/vehicle_attitude_setpoint.h
+1 -1
View File
@@ -61,7 +61,7 @@ struct vehicle_attitude_setpoint_s
float yaw_body; /**< body angle in NED frame */
//float body_valid; /**< Set to true if body angles are valid */
float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */
float R_body[3][3]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */
bool R_valid; /**< Set to true if rotation matrix is valid */
//! For quaternion-based attitude control
src/modules/uORB/topics/vehicle_control_mode.h
+2 -1
View File
@@ -67,6 +67,7 @@ typedef enum {
NAV_STATE_LOITER,
NAV_STATE_MISSION,
NAV_STATE_RTL,
NAV_STATE_LAND,
NAV_STATE_MAX
} nav_state_t;
@@ -92,7 +93,7 @@ struct vehicle_control_mode_s
bool flag_control_position_enabled; /**< true if position is controlled */
bool flag_control_altitude_enabled; /**< true if altitude is controlled */
bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */
bool flag_control_flighttermination_enabled; /**< true if flighttermination is enabled */
bool flag_control_termination_enabled; /**< true if flighttermination is enabled */
};
/**
src/modules/uORB/topics/vehicle_global_position.h
+8 -9
View File
@@ -54,7 +54,7 @@
/**
* Fused global position in WGS84.
*
* This struct contains the system's believ about its position. It is not the raw GPS
* This struct contains global position estimation. It is not the raw GPS
* measurement (@see vehicle_gps_position). This topic is usually published by the position
* estimator, which will take more sources of information into account than just GPS,
* e.g. control inputs of the vehicle in a Kalman-filter implementation.
@@ -65,14 +65,13 @@ struct vehicle_global_position_s
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 */
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 yaw; /**< Compass heading in radians -PI..+PI. */
double lat; /**< Latitude in degrees */
double lon; /**< Longitude in degrees */
float alt; /**< Altitude in meters */
float vel_n; /**< Ground north velocity, m/s */
float vel_e; /**< Ground east velocity, m/s */
float vel_d; /**< Ground downside velocity, m/s */
float yaw; /**< Yaw in radians -PI..+PI. */
};
/**
src/modules/uORB/topics/vehicle_local_position.h
+5
View File
@@ -77,6 +77,11 @@ struct vehicle_local_position_s
int32_t ref_lon; /**< Reference point longitude in 1E7 degrees */
float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */
bool landed; /**< true if vehicle is landed */
/* Distance to surface */
float dist_bottom; /**< Distance to bottom surface (ground) */
float dist_bottom_rate; /**< Distance to bottom surface (ground) change rate */
uint64_t surface_bottom_timestamp; /**< Time when new bottom surface found */
bool dist_bottom_valid; /**< true if distance to bottom surface is valid */
};
/**
src/modules/uORB/topics/vehicle_status.h
+10 -6
View File
@@ -64,6 +64,7 @@ typedef enum {
MAIN_STATE_SEATBELT,
MAIN_STATE_EASY,
MAIN_STATE_AUTO,
MAIN_STATE_MAX
} main_state_t;
typedef enum {
@@ -73,7 +74,8 @@ typedef enum {
ARMING_STATE_ARMED_ERROR,
ARMING_STATE_STANDBY_ERROR,
ARMING_STATE_REBOOT,
ARMING_STATE_IN_AIR_RESTORE
ARMING_STATE_IN_AIR_RESTORE,
ARMING_STATE_MAX
} arming_state_t;
typedef enum {
@@ -82,9 +84,12 @@ typedef enum {
} hil_state_t;
typedef enum {
FLIGHTTERMINATION_STATE_OFF = 0,
FLIGHTTERMINATION_STATE_ON
} flighttermination_state_t;
FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */
FAILSAFE_STATE_RTL, /**< Return To Launch */
FAILSAFE_STATE_LAND, /**< Land without position control */
FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */
FAILSAFE_STATE_MAX
} failsafe_state_t;
typedef enum {
MODE_SWITCH_MANUAL = 0,
@@ -173,6 +178,7 @@ struct vehicle_status_s
uint64_t set_nav_state_timestamp; /**< timestamp of latest change of set_nav_state */
arming_state_t arming_state; /**< current arming state */
hil_state_t hil_state; /**< current hil state */
failsafe_state_t failsafe_state; /**< current failsafe state */
int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */
int32_t system_id; /**< system id, inspired by MAVLink's system ID field */
@@ -223,8 +229,6 @@ struct vehicle_status_s
uint16_t errors_count2;
uint16_t errors_count3;
uint16_t errors_count4;
flighttermination_state_t flighttermination_state;
};
/**