Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-08 02:17:07 +08:00
Code Issues Actions 33 Packages Projects Releases Wiki Activity

Merge branch 'master' into px4io-i2c

Browse Source
This commit is contained in:
px4dev
2013-01-22 19:56:14 -08:00
parent d7632b1794 cc74874d7e
commit b66b234acd
15 changed files with 1502 additions and 615 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/commander/commander.c
+72 -67
View File
@@ -72,8 +72,10 @@
#include <uORB/topics/battery_status.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/offboard_control_setpoint.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/actuator_controls.h>
@@ -1251,6 +1253,7 @@ int commander_thread_main(int argc, char *argv[])
{
/* not yet initialized */
commander_initialized = false;
bool home_position_set = false;
/* set parameters */
failsafe_lowlevel_timeout_ms = 0;
@@ -1302,6 +1305,11 @@ int commander_thread_main(int argc, char *argv[])
/* publish current state machine */
state_machine_publish(stat_pub, &current_status, mavlink_fd);
/* home position */
orb_advert_t home_pub = -1;
struct home_position_s home;
memset(&home, 0, sizeof(home));
if (stat_pub < 0) {
warnx("ERROR: orb_advertise for topic vehicle_status failed.\n");
exit(ERROR);
@@ -1332,10 +1340,6 @@ int commander_thread_main(int argc, char *argv[])
uint16_t stick_off_counter = 0;
uint16_t stick_on_counter = 0;
float hdop = 65535.0f;
int gps_quality_good_counter = 0;
/* Subscribe to manual control data */
int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
struct manual_control_setpoint_s sp_man;
@@ -1356,6 +1360,15 @@ int commander_thread_main(int argc, char *argv[])
memset(&local_position, 0, sizeof(local_position));
uint64_t last_local_position_time = 0;
/*
* The home position is set based on GPS only, to prevent a dependency between
* position estimator and commander. RAW GPS is more than good enough for a
* non-flying vehicle.
*/
int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
struct vehicle_gps_position_s gps_position;
memset(&gps_position, 0, sizeof(gps_position));
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
struct sensor_combined_s sensors;
memset(&sensors, 0, sizeof(sensors));
@@ -1660,65 +1673,57 @@ int commander_thread_main(int argc, char *argv[])
state_changed = true;
}
if (orb_check(ORB_ID(vehicle_gps_position), &new_data)) {
/* Check if last transition deserved an audio event */
// #warning This code depends on state that is no longer? maintained
// #if 0
// trigger_audio_alarm(vehicle_mode_previous, vehicle_state_previous, current_status.mode, current_status.state_machine);
// #endif
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
/* only check gps fix if we are outdoor */
// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
//
// hdop = (float)(gps.eph) / 100.0f;
//
// /* check if gps fix is ok */
// if (gps.fix_type == GPS_FIX_TYPE_3D) { //TODO: is 2d-fix ok? //see http://en.wikipedia.org/wiki/Dilution_of_precision_%28GPS%29
//
// if (gotfix_counter >= GPS_GOTFIX_COUNTER_REQUIRED) { //TODO: add also a required time?
// update_state_machine_got_position_fix(stat_pub, &current_status);
// gotfix_counter = 0;
// } else {
// gotfix_counter++;
// }
// nofix_counter = 0;
//
// if (hdop < 5.0f) { //TODO: this should be a parameter
// if (gps_quality_good_counter > GPS_QUALITY_GOOD_COUNTER_LIMIT) {
// current_status.gps_valid = true;//--> position estimator can use the gps measurements
// }
//
// gps_quality_good_counter++;
//
//
//// if(counter%10 == 0)//for testing only
//// warnx("gps_quality_good_counter = %u\n", gps_quality_good_counter);//for testing only
//
// } else {
// gps_quality_good_counter = 0;
// current_status.gps_valid = false;//--> position estimator can not use the gps measurements
// }
//
// } else {
// gps_quality_good_counter = 0;
// current_status.gps_valid = false;//--> position estimator can not use the gps measurements
//
// if (nofix_counter > GPS_NOFIX_COUNTER_LIMIT) { //TODO: add also a timer limit?
// update_state_machine_no_position_fix(stat_pub, &current_status);
// nofix_counter = 0;
// } else {
// nofix_counter++;
// }
// gotfix_counter = 0;
// }
//
// }
//
//
// if (flight_env == PX4_FLIGHT_ENVIRONMENT_TESTING) //simulate position fix for quick indoor tests
//update_state_machine_got_position_fix(stat_pub, &current_status, mavlink_fd);
/* end: check gps */
/* check for first, long-term and valid GPS lock -> set home position */
float hdop_m = gps_position.eph / 100.0f;
float vdop_m = gps_position.epv / 100.0f;
/* check if gps fix is ok */
// XXX magic number
float dop_threshold_m = 2.0f;
/*
* If horizontal dilution of precision (hdop / eph)
* and vertical diluation of precision (vdop / epv)
* are below a certain threshold (e.g. 4 m), AND
* home position is not yet set AND the last GPS
* GPS measurement is not older than two seconds AND
* the system is currently not armed, set home
* position to the current position.
*/
if (gps_position.fix_type == GPS_FIX_TYPE_3D && (hdop_m < dop_threshold_m)
&& (vdop_m < dop_threshold_m)
&& !home_position_set
&& (hrt_absolute_time() - gps_position.timestamp < 2000000)
&& !current_status.flag_system_armed) {
warnx("setting home position");
/* copy position data to uORB home message, store it locally as well */
home.lat = gps_position.lat;
home.lon = gps_position.lon;
home.alt = gps_position.alt;
home.eph = gps_position.eph;
home.epv = gps_position.epv;
home.s_variance = gps_position.s_variance;
home.p_variance = gps_position.p_variance;
/* announce new home position */
if (home_pub > 0) {
orb_publish(ORB_ID(home_position), home_pub, &home);
} else {
home_pub = orb_advertise(ORB_ID(home_position), &home);
}
/* mark home position as set */
home_position_set = true;
tune_confirm();
}
}
/* ignore RC signals if in offboard control mode */
if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) {
@@ -1845,15 +1850,16 @@ int commander_thread_main(int argc, char *argv[])
update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
} else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) {
/* check auto mode switch for correct mode */
if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) {
/* enable guided mode */
update_state_machine_mode_guided(stat_pub, &current_status, mavlink_fd);
// /* check auto mode switch for correct mode */
// if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) {
// /* enable guided mode */
// update_state_machine_mode_guided(stat_pub, &current_status, mavlink_fd);
} else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) {
// } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) {
// XXX hardcode to auto for now
update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
}
// }
} else {
/* center stick position, set SAS for all vehicle types */
@@ -2041,4 +2047,3 @@ int commander_thread_main(int argc, char *argv[])
return 0;
}
apps/drivers/blinkm/blinkm.cpp
+422 -27
View File
File diff suppressed because it is too large Load Diff
apps/examples/control_demo/params.c
+22 -22
View File
@@ -14,50 +14,50 @@ PARAM_DEFINE_FLOAT(FWB_R_LP, 300.0f); // yaw rate low pass cut freq
PARAM_DEFINE_FLOAT(FWB_R_HP, 1.0f); // yaw rate high pass
// stabilization mode
PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.5f); // roll rate 2 aileron
PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.5f); // pitch rate 2 elevator
PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.2f); // yaw rate 2 rudder
PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.3f); // roll rate 2 aileron
PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.1f); // pitch rate 2 elevator
PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.1f); // yaw rate 2 rudder
// psi -> phi -> p
PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 0.5f); // heading 2 roll
PARAM_DEFINE_FLOAT(FWB_PHI2P, 1.0f); // roll to roll rate
PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.5f); // roll limit, 28 deg
PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.3f); // roll limit, 28 deg
// velocity -> theta
PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.2f);
PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f);
PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f);
PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f);
PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f);
PARAM_DEFINE_FLOAT(FWB_THE_MIN, -0.5f);
PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f);
PARAM_DEFINE_FLOAT(FWB_V2THE_P, 1.0f); // velocity to pitch angle PID, prop gain
PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f); // integral gain
PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f); // derivative gain
PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f); // derivative low-pass
PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f); // integrator wind up guard
PARAM_DEFINE_FLOAT(FWB_THE_MIN, -0.5f); // the max commanded pitch angle
PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f); // the min commanded pitch angle
// theta -> q
PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f);
PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f); // pitch angle to pitch-rate PID
PARAM_DEFINE_FLOAT(FWB_THE2Q_I, 0.0f);
PARAM_DEFINE_FLOAT(FWB_THE2Q_D, 0.0f);
PARAM_DEFINE_FLOAT(FWB_THE2Q_D_LP, 0.0f);
PARAM_DEFINE_FLOAT(FWB_THE2Q_I_MAX, 0.0f);
// h -> thr
PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.01f);
PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.01f); // altitude to throttle PID
PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.0f);
PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.0f);
PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 0.0f);
PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 0.0f);
// crosstrack
PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f); // 90 deg
PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.002f);
PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f); // cross-track to yaw angle limit 90 deg
PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.005f); // cross-track to yaw angle gain
// speed command
PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f);
PARAM_DEFINE_FLOAT(FWB_V_CMD, 22.0f);
PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f);
PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f); // minimum commanded velocity
PARAM_DEFINE_FLOAT(FWB_V_CMD, 22.0f); // commanded velocity
PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f); // maximum commanded velocity
// trim
PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f);
PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.005f);
PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f);
PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.81f);
PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f); // trim aileron, normalized (-1,1)
PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.005f); // trim elevator (-1,1)
PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f); // trim rudder (-1,1)
PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f); // trim throttle (0,1)
apps/examples/kalman_demo/KalmanNav.cpp
+228 -173
View File
File diff suppressed because it is too large Load Diff
apps/examples/kalman_demo/KalmanNav.hpp
+95 -39
View File
@@ -68,52 +68,108 @@
class KalmanNav : public control::SuperBlock
{
public:
/**
* Constructor
*/
KalmanNav(SuperBlock *parent, const char *name);
/**
* Deconstuctor
*/
virtual ~KalmanNav() {};
/**
* The main callback function for the class
*/
void update();
/**
* Publication update
*/
virtual void updatePublications();
void predictFast(float dt);
void predictSlow(float dt);
void correctAtt();
void correctPos();
/**
* State prediction
* Continuous, non-linear
*/
int predictState(float dt);
/**
* State covariance prediction
* Continuous, linear
*/
int predictStateCovariance(float dt);
/**
* Attitude correction
*/
int correctAtt();
/**
* Position correction
*/
int correctPos();
/**
* Overloaded update parameters
*/
virtual void updateParams();
protected:
math::Matrix F;
math::Matrix G;
math::Matrix P;
math::Matrix V;
math::Matrix HAtt;
math::Matrix RAtt;
math::Matrix HPos;
math::Matrix RPos;
math::Dcm C_nb;
math::Quaternion q;
control::UOrbSubscription<sensor_combined_s> _sensors;
control::UOrbSubscription<vehicle_gps_position_s> _gps;
control::UOrbSubscription<parameter_update_s> _param_update;
control::UOrbPublication<vehicle_global_position_s> _pos;
control::UOrbPublication<vehicle_attitude_s> _att;
uint64_t _pubTimeStamp;
uint64_t _fastTimeStamp;
uint64_t _slowTimeStamp;
uint64_t _attTimeStamp;
uint64_t _outTimeStamp;
uint16_t _navFrames;
uint16_t _missFast;
uint16_t _missSlow;
float fN, fE, fD;
// 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 */
// attitude
math::Dcm 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. */
control::UOrbSubscription<vehicle_gps_position_s> _gps; /**< gps sub. */
control::UOrbSubscription<parameter_update_s> _param_update; /**< parameter update sub. */
// publications
control::UOrbPublication<vehicle_global_position_s> _pos; /**< position pub. */
control::UOrbPublication<vehicle_attitude_s> _att; /**< attitude pub. */
// time stamps
uint64_t _pubTimeStamp; /**< output data publication time stamp */
uint64_t _predictTimeStamp; /**< prediction time stamp */
uint64_t _attTimeStamp; /**< attitude correction time stamp */
uint64_t _outTimeStamp; /**< output time stamp */
// frame count
uint16_t _navFrames; /**< navigation frames completed in output cycle */
// miss counts
uint16_t _miss; /**< number of times fast prediction loop missed */
// accelerations
float fN, fE, fD; /**< navigation frame acceleration */
// states
enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT};
float phi, theta, psi;
float vN, vE, vD;
double lat, lon, alt;
control::BlockParam<float> _vGyro;
control::BlockParam<float> _vAccel;
control::BlockParam<float> _rMag;
control::BlockParam<float> _rGpsVel;
control::BlockParam<float> _rGpsPos;
control::BlockParam<float> _rGpsAlt;
control::BlockParam<float> _rAccel;
enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT}; /**< state enumeration */
float phi, theta, psi; /**< 3-2-1 euler angles */
float vN, vE, vD; /**< navigation velocity, m/s */
double lat, lon, alt; /**< lat, lon, alt, radians */
// parameters
control::BlockParam<float> _vGyro; /**< gyro process noise */
control::BlockParam<float> _vAccel; /**< accelerometer process noise */
control::BlockParam<float> _rMag; /**< magnetometer measurement noise */
control::BlockParam<float> _rGpsVel; /**< gps velocity measurement noise */
control::BlockParam<float> _rGpsPos; /**< gps position measurement noise */
control::BlockParam<float> _rGpsAlt; /**< gps altitude measurement noise */
control::BlockParam<float> _rAccel; /**< accelerometer measurement noise */
control::BlockParam<float> _magDip; /**< magnetic inclination with level */
control::BlockParam<float> _magDec; /**< magnetic declination, clockwise rotation */
control::BlockParam<float> _g; /**< gravitational constant */
control::BlockParam<float> _faultPos; /**< fault detection threshold for position */
control::BlockParam<float> _faultAtt; /**< fault detection threshold for attitude */
// status
bool _attitudeInitialized;
bool _positionInitialized;
uint16_t _attitudeInitCounter;
// accessors
int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); }
void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; }
int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); }
apps/examples/kalman_demo/params.c
+10 -4
View File
@@ -1,10 +1,16 @@
#include <systemlib/param/param.h>
/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f);
PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f);
PARAM_DEFINE_FLOAT(KF_V_GYRO, 1.0f);
PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f);
PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 1.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 1.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 5.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 5.0f);
PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f);
PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f);
PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f);
PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f);
PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f);
PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f);
apps/gps/gps.c
+2 -3
View File
@@ -107,8 +107,8 @@ enum GPS_MODES {
#define AUTO_DETECTION_COUNT 8
const int autodetection_baudrates[] = {B9600, B38400, B38400, B9600, B9600, B38400, B9600, B38400};
const enum GPS_MODES autodetection_gpsmodes[] = {GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_NMEA, GPS_MODE_NMEA}; //nmea is the fall-back if nothing else works, therefore we try the standard modes again before finally trying nmea
const int autodetection_baudrates[] = {B38400, B9600, B38400, B9600, B38400, B9600, B38400, B9600};
const enum GPS_MODES autodetection_gpsmodes[] = {GPS_MODE_UBX, GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_MTK, GPS_MODE_UBX, GPS_MODE_UBX, GPS_MODE_NMEA, GPS_MODE_NMEA}; //nmea is the fall-back if nothing else works, therefore we try the standard modes again before finally trying nmea
/****************************************************************************
* Private functions
@@ -368,7 +368,6 @@ int gps_thread_main(int argc, char *argv[]) {
args.thread_should_exit_ptr = &thread_should_exit;
pthread_create(&ubx_thread, &ubx_loop_attr, ubx_loop, (void *)&args);
sleep(2); // XXX TODO Check if this is too short, try to lower sleeps in UBX driver
pthread_attr_t ubx_wd_attr;
pthread_attr_init(&ubx_wd_attr);
apps/gps/ubx.c
+231 -85
View File
@@ -52,7 +52,7 @@
#define UBX_HEALTH_FAIL_COUNTER_LIMIT 3
#define UBX_HEALTH_PROBE_COUNTER_LIMIT 4
#define UBX_BUFFER_SIZE 1000
#define UBX_BUFFER_SIZE 500
extern bool gps_mode_try_all;
extern bool gps_mode_success;
@@ -63,18 +63,10 @@ extern int current_gps_speed;
pthread_mutex_t *ubx_mutex;
gps_bin_ubx_state_t *ubx_state;
enum UBX_CONFIG_STATE ubx_config_state;
static struct vehicle_gps_position_s *ubx_gps;
//Definitions for ubx, last two bytes are checksum which is calculated below
uint8_t UBX_CONFIG_MESSAGE_PRT[] = {0xB5 , 0x62 , 0x06 , 0x00 , 0x14 , 0x00 , 0x01 , 0x00 , 0x00 , 0x00 , 0xD0 , 0x08 , 0x00 , 0x00 , 0x80 , 0x25 , 0x00 , 0x00 , 0x07 , 0x00 , 0x01 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00};
uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_POSLLH[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x02, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00};
uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_TIMEUTC[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x21, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00};
uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_DOP[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x04, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00};
uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_SVINFO[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x30, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00};
uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_SOL[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x06, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00};
uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_VELNED[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x12, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00};
uint8_t UBX_CONFIG_MESSAGE_MSG_RXM_SVSI[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x02, 0x20, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00};
void ubx_decode_init(void)
{
@@ -100,15 +92,15 @@ void ubx_checksum(uint8_t b, uint8_t *ck_a, uint8_t *ck_b)
int ubx_parse(uint8_t b, char *gps_rx_buffer)
{
// printf("b=%x\n",b);
//printf("b=%x\n",b);
if (ubx_state->decode_state == UBX_DECODE_UNINIT) {
if (b == 0xb5) {
if (b == UBX_SYNC_1) {
ubx_state->decode_state = UBX_DECODE_GOT_SYNC1;
}
} else if (ubx_state->decode_state == UBX_DECODE_GOT_SYNC1) {
if (b == 0x62) {
if (b == UBX_SYNC_2) {
ubx_state->decode_state = UBX_DECODE_GOT_SYNC2;
} else {
@@ -122,16 +114,25 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer)
//check for known class
switch (b) {
case UBX_CLASS_NAV: //NAV
case UBX_CLASS_ACK:
ubx_state->decode_state = UBX_DECODE_GOT_CLASS;
ubx_state->message_class = ACK;
break;
case UBX_CLASS_NAV:
ubx_state->decode_state = UBX_DECODE_GOT_CLASS;
ubx_state->message_class = NAV;
break;
case UBX_CLASS_RXM: //RXM
case UBX_CLASS_RXM:
ubx_state->decode_state = UBX_DECODE_GOT_CLASS;
ubx_state->message_class = RXM;
break;
case UBX_CLASS_CFG:
ubx_state->decode_state = UBX_DECODE_GOT_CLASS;
ubx_state->message_class = CFG;
break;
default: //unknown class: reset state machine
ubx_decode_init();
break;
@@ -193,9 +194,36 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer)
ubx_decode_init();
break;
}
break;
case CFG:
switch (b) {
case UBX_MESSAGE_CFG_NAV5:
ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID;
ubx_state->message_id = CFG_NAV5;
break;
default: //unknown class: reset state machine, should not happen
ubx_decode_init();
break;
}
break;
case ACK:
switch (b) {
case UBX_MESSAGE_ACK_ACK:
ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID;
ubx_state->message_id = ACK_ACK;
break;
case UBX_MESSAGE_ACK_NAK:
ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID;
ubx_state->message_id = ACK_NAK;
break;
default: //unknown class: reset state machine, should not happen
ubx_decode_init();
break;
}
break;
default: //should not happen
ubx_decode_init();
break;
@@ -542,6 +570,75 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer)
break;
}
case ACK_ACK: {
// printf("GOT ACK_ACK\n");
gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) gps_rx_buffer;
//Check if checksum is valid
if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) {
switch (ubx_config_state) {
case UBX_CONFIG_STATE_PRT:
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_PRT)
ubx_config_state++;
break;
case UBX_CONFIG_STATE_NAV5:
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_NAV5)
ubx_config_state++;
break;
case UBX_CONFIG_STATE_MSG_NAV_POSLLH:
case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC:
case UBX_CONFIG_STATE_MSG_NAV_DOP:
case UBX_CONFIG_STATE_MSG_NAV_SVINFO:
case UBX_CONFIG_STATE_MSG_NAV_SOL:
case UBX_CONFIG_STATE_MSG_NAV_VELNED:
case UBX_CONFIG_STATE_MSG_RXM_SVSI:
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
ubx_config_state++;
break;
default:
break;
}
ret = 1;
} else {
if (gps_verbose) printf("[gps] ACK_ACK: checksum invalid\n");
ret = 0;
}
// Reset state machine to decode next packet
ubx_decode_init();
return ret;
break;
}
case ACK_NAK: {
// printf("GOT ACK_NAK\n");
gps_bin_ack_nak_packet_t *packet = (gps_bin_ack_nak_packet_t *) gps_rx_buffer;
//Check if checksum is valid
if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) {
if (gps_verbose) printf("[gps] the ubx gps returned: not acknowledged\n");
ret = 1;
} else {
if (gps_verbose) printf("[gps] ACK_NAK: checksum invalid\n");
ret = 0;
}
// Reset state machine to decode next packet
ubx_decode_init();
return ret;
break;
}
default: //something went wrong
ubx_decode_init();
@@ -574,53 +671,105 @@ void calculate_ubx_checksum(uint8_t *message, uint8_t length)
message[length - 2] = ck_a;
message[length - 1] = ck_b;
// printf("[%x,%x]", ck_a, ck_b);
// printf("[%x,%x]\n", message[length-2], message[length-1]);
}
int configure_gps_ubx(int *fd)
{
//fflush(((FILE *)fd));
// only needed once like this
const type_gps_bin_cfg_prt_packet_t cfg_prt_packet = {
.clsID = UBX_CLASS_CFG,
.msgID = UBX_MESSAGE_CFG_PRT,
.length = UBX_CFG_PRT_LENGTH,
.portID = UBX_CFG_PRT_PAYLOAD_PORTID,
.mode = UBX_CFG_PRT_PAYLOAD_MODE,
.baudRate = current_gps_speed,
.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK,
.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK,
.ck_a = 0,
.ck_b = 0
};
//TODO: write this in a loop once it is tested
//UBX_CFG_PRT_PART:
write_config_message_ubx(UBX_CONFIG_MESSAGE_PRT, sizeof(UBX_CONFIG_MESSAGE_PRT) / sizeof(uint8_t) , *fd);
// only needed once like this
const type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet = {
.clsID = UBX_CLASS_CFG,
.msgID = UBX_MESSAGE_CFG_NAV5,
.length = UBX_CFG_NAV5_LENGTH,
.mask = UBX_CFG_NAV5_PAYLOAD_MASK,
.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL,
.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE,
.ck_a = 0,
.ck_b = 0
};
usleep(100000);
// this message is reusable for different configuration commands, so not const
type_gps_bin_cfg_msg_packet cfg_msg_packet = {
.clsID = UBX_CLASS_CFG,
.msgID = UBX_MESSAGE_CFG_MSG,
.length = UBX_CFG_MSG_LENGTH,
.rate = UBX_CFG_MSG_PAYLOAD_RATE
};
//NAV_POSLLH:
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_POSLLH, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_POSLLH) / sizeof(uint8_t) , *fd);
usleep(100000);
uint64_t time_before_config = hrt_absolute_time();
//NAV_TIMEUTC:
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_TIMEUTC, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_TIMEUTC) / sizeof(uint8_t) , *fd);
usleep(100000);
while(hrt_absolute_time() < time_before_config + UBX_CONFIG_TIMEOUT) {
//NAV_DOP:
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_DOP, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_DOP) / sizeof(uint8_t) , *fd);
usleep(100000);
// if (gps_verbose) printf("[gps] ubx config state: %d\n", ubx_config_state);
//NAV_SOL:
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_SOL, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_SOL) / sizeof(uint8_t) , *fd);
usleep(100000);
switch (ubx_config_state) {
case UBX_CONFIG_STATE_PRT:
// if (gps_verbose) printf("[gps] Configuring ubx with baudrate: %d\n", cfg_prt_packet.baudRate);
//NAV_SVINFO:
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_SVINFO, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_SVINFO) / sizeof(uint8_t) , *fd);
usleep(100000);
//NAV_VELNED:
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_VELNED, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_VELNED) / sizeof(uint8_t) , *fd);
usleep(100000);
//RXM_SVSI:
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_RXM_SVSI, sizeof(UBX_CONFIG_MESSAGE_MSG_RXM_SVSI) / sizeof(uint8_t) , *fd);
usleep(100000);
return 0;
write_config_message_ubx((uint8_t*)(&cfg_prt_packet), sizeof(cfg_prt_packet), fd);
break;
case UBX_CONFIG_STATE_NAV5:
write_config_message_ubx((uint8_t*)(&cfg_nav5_packet), sizeof(cfg_nav5_packet), fd);
break;
case UBX_CONFIG_STATE_MSG_NAV_POSLLH:
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH;
write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd);
break;
case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC:
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC;
write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd);
break;
case UBX_CONFIG_STATE_MSG_NAV_DOP:
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP;
write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd);
break;
case UBX_CONFIG_STATE_MSG_NAV_SVINFO:
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO;
write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd);
break;
case UBX_CONFIG_STATE_MSG_NAV_SOL:
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL;
write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd);
break;
case UBX_CONFIG_STATE_MSG_NAV_VELNED:
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED;
write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd);
break;
case UBX_CONFIG_STATE_MSG_RXM_SVSI:
cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI;
write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd);
break;
case UBX_CONFIG_STATE_CONFIGURED:
if (gps_verbose) printf("[gps] ubx configuration finished\n");
return OK;
break;
default:
break;
}
usleep(10000);
}
if (gps_verbose) printf("[gps] ubx configuration timeout\n");
return ERROR;
}
@@ -637,22 +786,17 @@ int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size)
fds.events = POLLIN;
// UBX GPS mode
// This blocks the task until there is something on the buffer
while (1) {
//check if the thread should terminate
if (terminate_gps_thread == true) {
// printf("terminate_gps_thread=%u ", terminate_gps_thread);
// printf("exiting mtk thread\n");
// fflush(stdout);
ret = 1;
break;
}
if (poll(&fds, 1, 1000) > 0) {
if (read(*fd, &c, 1) > 0) {
// printf("Read %x\n",c);
// printf("Read %x\n",c);
if (rx_count >= buffer_size) {
// The buffer is already full and we haven't found a valid ubx sentence.
// Flush the buffer and note the overflow event.
@@ -671,7 +815,7 @@ int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size)
int msg_read = ubx_parse(c, gps_rx_buffer);
if (msg_read > 0) {
// printf("Found sequence\n");
//printf("Found sequence\n");
break;
}
@@ -688,28 +832,41 @@ int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size)
return ret;
}
int write_config_message_ubx(uint8_t *message, size_t length, int fd)
int write_config_message_ubx(const uint8_t *message, const size_t length, const int *fd)
{
//calculate and write checksum to the end
uint8_t ck_a = 0;
uint8_t ck_b = 0;
unsigned int i;
for (i = 2; i < length; i++) {
uint8_t buffer[2];
ssize_t result_write = 0;
//calculate and write checksum to the end
for (i = 0; i < length-2; i++) {
ck_a = ck_a + message[i];
ck_b = ck_b + ck_a;
}
// printf("[%x,%x]\n", ck_a, ck_b);
// write sync bytes first
buffer[0] = UBX_SYNC_1;
buffer[1] = UBX_SYNC_2;
unsigned int result_write = write(fd, message, length);
result_write += write(fd, &ck_a, 1);
result_write += write(fd, &ck_b, 1);
fsync(fd);
// write config message without the checksum
result_write = write(*fd, buffer, sizeof(buffer));
result_write += write(*fd, message, length-2);
return (result_write != length + 2); //return 0 as success
buffer[0] = ck_a;
buffer[1] = ck_b;
// write the checksum
result_write += write(*fd, buffer, sizeof(buffer));
fsync(*fd);
if ((unsigned int)result_write != length + 2)
return ERROR;
return OK;
}
void *ubx_watchdog_loop(void *args)
@@ -723,6 +880,10 @@ void *ubx_watchdog_loop(void *args)
int *fd = arguments->fd_ptr;
bool *thread_should_exit = arguments->thread_should_exit_ptr;
ubx_config_state = UBX_CONFIG_STATE_PRT;
/* first try to configure the GPS anyway */
configure_gps_ubx(fd);
/* GPS watchdog error message skip counter */
bool ubx_healthy = false;
@@ -780,7 +941,9 @@ void *ubx_watchdog_loop(void *args)
ubx_healthy = false;
ubx_success_count = 0;
}
/* trying to reconfigure the gps configuration */
ubx_config_state = UBX_CONFIG_STATE_PRT;
configure_gps_ubx(fd);
fflush(stdout);
sleep(1);
@@ -833,23 +996,6 @@ void *ubx_loop(void *args)
/* set parameters for ubx */
if (configure_gps_ubx(fd) != 0) {
printf("[gps] Configuration of gps module to ubx failed\n");
/* Write shared variable sys_status */
// TODO enable this again
//global_data_send_subsystem_info(&ubx_present);
} else {
if (gps_verbose) printf("[gps] Attempting to configure GPS to UBX binary protocol\n");
// XXX Shouldn't the system status only change if the module is known to work ok?
/* Write shared variable sys_status */
// TODO enable this again
//global_data_send_subsystem_info(&ubx_present_enabled);
}
struct vehicle_gps_position_s ubx_gps_d = {.counter = 0};
ubx_gps = &ubx_gps_d;
apps/gps/ubx.h
+108 -9
View File
@@ -49,15 +49,17 @@
//internal definitions (not depending on the ubx protocol
#define CONFIGURE_UBX_FINISHED 0
#define CONFIGURE_UBX_MESSAGE_ACKNOWLEDGED 1
#define CONFIGURE_UBX_MESSAGE_NOT_ACKNOWLEDGED 2
#define UBX_NO_OF_MESSAGES 7 /**< Read 7 UBX GPS messages */
#define UBX_WATCHDOG_CRITICAL_TIME_MICROSECONDS 3000000 /**< Allow 3 seconds maximum inter-message time */
#define UBX_WATCHDOG_WAIT_TIME_MICROSECONDS 500000 /**< Check for current state every 0.4 seconds */
#define UBX_WATCHDOG_WAIT_TIME_MICROSECONDS 2000000 /**< Check for current state every two seconds */
#define UBX_CONFIG_TIMEOUT 1000000
#define APPNAME "gps: ubx"
#define UBX_SYNC_1 0xB5
#define UBX_SYNC_2 0x62
//UBX Protocoll definitions (this is the subset of the messages that are parsed)
#define UBX_CLASS_NAV 0x01
#define UBX_CLASS_RXM 0x02
@@ -72,6 +74,24 @@
#define UBX_MESSAGE_RXM_SVSI 0x20
#define UBX_MESSAGE_ACK_ACK 0x01
#define UBX_MESSAGE_ACK_NAK 0x00
#define UBX_MESSAGE_CFG_PRT 0x00
#define UBX_MESSAGE_CFG_NAV5 0x24
#define UBX_MESSAGE_CFG_MSG 0x01
#define UBX_CFG_PRT_LENGTH 20
#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< port 1 */
#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */
#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */
#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< ubx in */
#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< ubx out */
#define UBX_CFG_NAV5_LENGTH 36
#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0001 /**< only update dynamic model and fix mode */
#define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7 /**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */
#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */
#define UBX_CFG_MSG_LENGTH 8
#define UBX_CFG_MSG_PAYLOAD_RATE {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} /**< UART1 chosen */
// ************
@@ -216,7 +236,7 @@ typedef type_gps_bin_rxm_svsi_packet gps_bin_rxm_svsi_packet_t;
typedef struct {
uint8_t clsID;
uint8_t msgId;
uint8_t msgID;
uint8_t ck_a;
uint8_t ck_b;
@@ -226,7 +246,7 @@ typedef type_gps_bin_ack_ack_packet gps_bin_ack_ack_packet_t;
typedef struct {
uint8_t clsID;
uint8_t msgId;
uint8_t msgID;
uint8_t ck_a;
uint8_t ck_b;
@@ -234,15 +254,91 @@ typedef struct {
typedef type_gps_bin_ack_nak_packet gps_bin_ack_nak_packet_t;
typedef struct {
uint8_t clsID;
uint8_t msgID;
uint16_t length;
uint8_t portID;
uint8_t res0;
uint16_t res1;
uint32_t mode;
uint32_t baudRate;
uint16_t inProtoMask;
uint16_t outProtoMask;
uint16_t flags;
uint16_t pad;
uint8_t ck_a;
uint8_t ck_b;
} type_gps_bin_cfg_prt_packet;
typedef type_gps_bin_cfg_prt_packet type_gps_bin_cfg_prt_packet_t;
typedef struct {
uint8_t clsID;
uint8_t msgID;
uint16_t length;
uint16_t mask;
uint8_t dynModel;
uint8_t fixMode;
int32_t fixedAlt;
uint32_t fixedAltVar;
int8_t minElev;
uint8_t drLimit;
uint16_t pDop;
uint16_t tDop;
uint16_t pAcc;
uint16_t tAcc;
uint8_t staticHoldThresh;
uint8_t dgpsTimeOut;
uint32_t reserved2;
uint32_t reserved3;
uint32_t reserved4;
uint8_t ck_a;
uint8_t ck_b;
} type_gps_bin_cfg_nav5_packet;
typedef type_gps_bin_cfg_nav5_packet type_gps_bin_cfg_nav5_packet_t;
typedef struct {
uint8_t clsID;
uint8_t msgID;
uint16_t length;
uint8_t msgClass_payload;
uint8_t msgID_payload;
uint8_t rate[6];
uint8_t ck_a;
uint8_t ck_b;
} type_gps_bin_cfg_msg_packet;
typedef type_gps_bin_cfg_msg_packet type_gps_bin_cfg_msg_packet_t;
// END the structures of the binary packets
// ************
enum UBX_CONFIG_STATE {
UBX_CONFIG_STATE_NONE = 0,
UBX_CONFIG_STATE_PRT = 1,
UBX_CONFIG_STATE_NAV5 = 2,
UBX_CONFIG_STATE_MSG_NAV_POSLLH = 3,
UBX_CONFIG_STATE_MSG_NAV_TIMEUTC = 4,
UBX_CONFIG_STATE_MSG_NAV_DOP = 5,
UBX_CONFIG_STATE_MSG_NAV_SVINFO = 6,
UBX_CONFIG_STATE_MSG_NAV_SOL = 7,
UBX_CONFIG_STATE_MSG_NAV_VELNED = 8,
UBX_CONFIG_STATE_MSG_RXM_SVSI = 9,
UBX_CONFIG_STATE_CONFIGURED = 10
};
enum UBX_MESSAGE_CLASSES {
CLASS_UNKNOWN = 0,
NAV = 1,
RXM = 2,
ACK = 3
ACK = 3,
CFG = 4
};
enum UBX_MESSAGE_IDS {
@@ -254,7 +350,10 @@ enum UBX_MESSAGE_IDS {
NAV_DOP = 4,
NAV_SVINFO = 5,
NAV_VELNED = 6,
RXM_SVSI = 7
RXM_SVSI = 7,
CFG_NAV5 = 8,
ACK_ACK = 9,
ACK_NAK = 10
};
enum UBX_DECODE_STATES {
@@ -304,7 +403,7 @@ int configure_gps_ubx(int *fd);
int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size);
int write_config_message_ubx(uint8_t *message, size_t length, int fd);
int write_config_message_ubx(const uint8_t *message, const size_t length, const int *fd);
void calculate_ubx_checksum(uint8_t *message, uint8_t length);
apps/mavlink/mavlink_receiver.c
+12 -7
View File
@@ -318,9 +318,11 @@ handle_message(mavlink_message_t *msg)
static uint16_t hil_frames = 0;
static uint64_t old_timestamp = 0;
/* sensors general */
hil_sensors.timestamp = imu.time_usec;
/* hil gyro */
static const float mrad2rad = 1.0e-3f;
hil_sensors.timestamp = timestamp;
hil_sensors.gyro_counter = hil_counter;
hil_sensors.gyro_raw[0] = imu.xgyro;
hil_sensors.gyro_raw[1] = imu.ygyro;
@@ -367,8 +369,8 @@ handle_message(mavlink_message_t *msg)
hil_frames += 1 ;
// output
if ((timestamp - old_timestamp) > 1000000) {
printf("receiving hil imu at %d hz\n", hil_frames);
if ((timestamp - old_timestamp) > 10000000) {
printf("receiving hil imu at %d hz\n", hil_frames/10);
old_timestamp = timestamp;
hil_frames = 0;
}
@@ -412,8 +414,8 @@ handle_message(mavlink_message_t *msg)
hil_frames += 1 ;
// output
if ((timestamp - old_timestamp) > 1000000) {
printf("receiving hil gps at %d hz\n", hil_frames);
if ((timestamp - old_timestamp) > 10000000) {
printf("receiving hil gps at %d hz\n", hil_frames/10);
old_timestamp = timestamp;
hil_frames = 0;
}
@@ -429,6 +431,9 @@ handle_message(mavlink_message_t *msg)
static uint16_t hil_frames = 0;
static uint64_t old_timestamp = 0;
/* sensors general */
hil_sensors.timestamp = press.time_usec;
/* baro */
/* TODO, set ground_press/ temp during calib */
static const float ground_press = 1013.25f; // mbar
@@ -454,8 +459,8 @@ handle_message(mavlink_message_t *msg)
hil_frames += 1 ;
// output
if ((timestamp - old_timestamp) > 1000000) {
printf("receiving hil pressure at %d hz\n", hil_frames);
if ((timestamp - old_timestamp) > 10000000) {
printf("receiving hil pressure at %d hz\n", hil_frames/10);
old_timestamp = timestamp;
hil_frames = 0;
}
apps/mavlink/orb_listener.c
+17 -1
View File
@@ -114,6 +114,7 @@ static void l_vehicle_attitude_controls(struct listener *l);
static void l_debug_key_value(struct listener *l);
static void l_optical_flow(struct listener *l);
static void l_vehicle_rates_setpoint(struct listener *l);
static void l_home(struct listener *l);
struct listener listeners[] = {
{l_sensor_combined, &mavlink_subs.sensor_sub, 0},
@@ -137,6 +138,7 @@ struct listener listeners[] = {
{l_debug_key_value, &mavlink_subs.debug_key_value, 0},
{l_optical_flow, &mavlink_subs.optical_flow, 0},
{l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0},
{l_home, &mavlink_subs.home_sub, 0},
};
static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
@@ -621,6 +623,16 @@ l_optical_flow(struct listener *l)
flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m);
}
void
l_home(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_COMM_0, home.lat, home.lon, home.alt);
}
static void *
uorb_receive_thread(void *arg)
{
@@ -688,6 +700,10 @@ uorb_receive_start(void)
mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
orb_set_interval(mavlink_subs.gps_sub, 1000); /* 1Hz updates */
/* --- HOME POSITION --- */
mavlink_subs.home_sub = orb_subscribe(ORB_ID(home_position));
orb_set_interval(mavlink_subs.home_sub, 1000); /* 1Hz updates */
/* --- SYSTEM STATE --- */
status_sub = orb_subscribe(ORB_ID(vehicle_status));
orb_set_interval(status_sub, 300); /* max 3.33 Hz updates */
@@ -702,7 +718,7 @@ uorb_receive_start(void)
/* --- GLOBAL POS VALUE --- */
mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
orb_set_interval(mavlink_subs.global_pos_sub, 1000); /* 1Hz active updates */
orb_set_interval(mavlink_subs.global_pos_sub, 100); /* 10 Hz active updates */
/* --- LOCAL POS VALUE --- */
mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
apps/mavlink/orb_topics.h
+2
View File
@@ -45,6 +45,7 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/offboard_control_setpoint.h>
#include <uORB/topics/vehicle_command.h>
@@ -81,6 +82,7 @@ struct mavlink_subscriptions {
int input_rc_sub;
int optical_flow;
int rates_setpoint_sub;
int home_sub;
};
extern struct mavlink_subscriptions mavlink_subs;
apps/sdlog/sdlog.c
+201 -178
View File
File diff suppressed because it is too large Load Diff
apps/uORB/objects_common.cpp
+3
View File
@@ -68,6 +68,9 @@ ORB_DEFINE(sensor_combined, struct sensor_combined_s);
#include "topics/vehicle_gps_position.h"
ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s);
#include "topics/home_position.h"
ORB_DEFINE(home_position, struct home_position_s);
#include "topics/vehicle_status.h"
ORB_DEFINE(vehicle_status, struct vehicle_status_s);
apps/uORB/topics/home_position.h
+77
View File
@@ -0,0 +1,77 @@
/****************************************************************************
*
* Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
* Author: 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 home_position.h
* Definition of the GPS home position uORB topic.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#ifndef TOPIC_HOME_POSITION_H_
#define TOPIC_HOME_POSITION_H_
#include <stdint.h>
#include "../uORB.h"
/**
* @addtogroup topics
* @{
*/
/**
* GPS home position in WGS84 coordinates.
*/
struct home_position_s
{
uint64_t timestamp; /**< Timestamp (microseconds since system boot) */
uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp from the gps module */
int32_t lat; /**< Latitude in 1E7 degrees */
int32_t lon; /**< Longitude in 1E7 degrees */
int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */
uint16_t eph; /**< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */
uint16_t epv; /**< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */
float s_variance; /**< speed accuracy estimate cm/s */
float p_variance; /**< position accuracy estimate cm */
};
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(home_position);
#endif