mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-03 14:08:52 +08:00
Merge branch 'master' of github.com:PX4/Firmware into fmuv2_bringup
This commit is contained in:
Lorenz Meier
+49
-52
@@ -176,17 +176,17 @@ UBX::configure(unsigned &baudrate)
|
||||
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
|
||||
// continue;
|
||||
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC,
|
||||
1);
|
||||
UBX_CFG_MSG_PAYLOAD_RATE1_1HZ);
|
||||
// /* insist of receiving the ACK for this packet */
|
||||
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
|
||||
// continue;
|
||||
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL,
|
||||
1);
|
||||
UBX_CFG_MSG_PAYLOAD_RATE1_5HZ);
|
||||
// /* insist of receiving the ACK for this packet */
|
||||
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
|
||||
// continue;
|
||||
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED,
|
||||
1);
|
||||
UBX_CFG_MSG_PAYLOAD_RATE1_5HZ);
|
||||
// /* insist of receiving the ACK for this packet */
|
||||
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
|
||||
// continue;
|
||||
@@ -196,7 +196,7 @@ UBX::configure(unsigned &baudrate)
|
||||
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
|
||||
// continue;
|
||||
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO,
|
||||
0);
|
||||
UBX_CFG_MSG_PAYLOAD_RATE1_05HZ);
|
||||
// /* insist of receiving the ACK for this packet */
|
||||
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
|
||||
// continue;
|
||||
@@ -224,35 +224,18 @@ UBX::receive(unsigned timeout)
|
||||
fds[0].fd = _fd;
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
uint8_t buf[32];
|
||||
uint8_t buf[128];
|
||||
|
||||
/* timeout additional to poll */
|
||||
uint64_t time_started = hrt_absolute_time();
|
||||
|
||||
int j = 0;
|
||||
ssize_t count = 0;
|
||||
|
||||
bool position_updated = false;
|
||||
|
||||
while (true) {
|
||||
|
||||
/* pass received bytes to the packet decoder */
|
||||
while (j < count) {
|
||||
if (parse_char(buf[j]) > 0) {
|
||||
/* return to configure during configuration or to the gps driver during normal work
|
||||
* if a packet has arrived */
|
||||
if (handle_message() > 0)
|
||||
return 1;
|
||||
}
|
||||
/* in case we keep trying but only get crap from GPS */
|
||||
if (time_started + timeout*1000 < hrt_absolute_time() ) {
|
||||
return -1;
|
||||
}
|
||||
j++;
|
||||
}
|
||||
|
||||
/* everything is read */
|
||||
j = count = 0;
|
||||
|
||||
/* then poll for new data */
|
||||
/* poll for new data */
|
||||
int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout);
|
||||
|
||||
if (ret < 0) {
|
||||
@@ -272,8 +255,26 @@ UBX::receive(unsigned timeout)
|
||||
* available, we'll go back to poll() again...
|
||||
*/
|
||||
count = ::read(_fd, buf, sizeof(buf));
|
||||
/* pass received bytes to the packet decoder */
|
||||
for (int i = 0; i < count; i++) {
|
||||
if (parse_char(buf[i])) {
|
||||
/* return to configure during configuration or to the gps driver during normal work
|
||||
* if a packet has arrived */
|
||||
if (handle_message())
|
||||
position_updated = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* return success after receiving a packet */
|
||||
if (position_updated)
|
||||
return 1;
|
||||
|
||||
/* abort after timeout if no packet parsed successfully */
|
||||
if (time_started + timeout*1000 < hrt_absolute_time() ) {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -327,6 +328,7 @@ UBX::parse_char(uint8_t b)
|
||||
}
|
||||
break;
|
||||
case UBX_DECODE_GOT_CLASS:
|
||||
{
|
||||
add_byte_to_checksum(b);
|
||||
switch (_message_class) {
|
||||
case NAV:
|
||||
@@ -413,6 +415,14 @@ UBX::parse_char(uint8_t b)
|
||||
// config_needed = true;
|
||||
break;
|
||||
}
|
||||
// Evaluate state machine - if the state changed,
|
||||
// the state machine was reset via decode_init()
|
||||
// and we want to tell the module to stop sending this message
|
||||
|
||||
// disable unknown message
|
||||
//warnx("disabled class %d, msg %d", (int)_message_class, (int)b);
|
||||
//configure_message_rate(_message_class, b, 0);
|
||||
}
|
||||
break;
|
||||
case UBX_DECODE_GOT_MESSAGEID:
|
||||
add_byte_to_checksum(b);
|
||||
@@ -539,7 +549,7 @@ UBX::handle_message()
|
||||
}
|
||||
|
||||
case NAV_SVINFO: {
|
||||
// printf("GOT NAV_SVINFO MESSAGE\n");
|
||||
// printf("GOT NAV_SVINFO MESSAGE\n");
|
||||
|
||||
if (!_waiting_for_ack) {
|
||||
//this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message
|
||||
@@ -560,40 +570,27 @@ UBX::handle_message()
|
||||
|
||||
uint8_t satellites_used = 0;
|
||||
int i;
|
||||
|
||||
// printf("Number of Channels: %d\n", packet_part1->numCh);
|
||||
for (i = 0; i < packet_part1->numCh; i++) { //for each channel
|
||||
|
||||
/* Get satellite information from the buffer */
|
||||
memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2);
|
||||
packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2;
|
||||
|
||||
/* Write satellite information to global storage */
|
||||
uint8_t sv_used = packet_part2->flags & 0x01;
|
||||
|
||||
/* Write satellite information in the global storage */
|
||||
_gps_position->satellite_prn[i] = packet_part2->svid;
|
||||
|
||||
//if satellite information is healthy store the data
|
||||
uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield
|
||||
|
||||
if (!unhealthy) {
|
||||
if ((packet_part2->flags) & 1) { //flags is a bitfield
|
||||
_gps_position->satellite_used[i] = 1;
|
||||
satellites_used++;
|
||||
|
||||
} else {
|
||||
_gps_position->satellite_used[i] = 0;
|
||||
}
|
||||
|
||||
_gps_position->satellite_snr[i] = packet_part2->cno;
|
||||
_gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev);
|
||||
_gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f);
|
||||
|
||||
} else {
|
||||
_gps_position->satellite_used[i] = 0;
|
||||
_gps_position->satellite_snr[i] = 0;
|
||||
_gps_position->satellite_elevation[i] = 0;
|
||||
_gps_position->satellite_azimuth[i] = 0;
|
||||
if ( sv_used ) {
|
||||
// Count SVs used for NAV.
|
||||
satellites_used++;
|
||||
}
|
||||
|
||||
|
||||
// Record info for all channels, whether or not the SV is used for NAV.
|
||||
_gps_position->satellite_used[i] = sv_used;
|
||||
_gps_position->satellite_snr[i] = packet_part2->cno;
|
||||
_gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev);
|
||||
_gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f);
|
||||
_gps_position->satellite_prn[i] = packet_part2->svid;
|
||||
}
|
||||
|
||||
for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused
|
||||
|
||||
@@ -74,6 +74,7 @@
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_controls_effective.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
|
||||
#include <systemlib/err.h>
|
||||
|
||||
@@ -87,7 +88,7 @@
|
||||
#define MOTOR_STATE_PRESENT_MASK 0x80
|
||||
#define MOTOR_STATE_ERROR_MASK 0x7F
|
||||
#define MOTOR_SPINUP_COUNTER 2500
|
||||
|
||||
#define ESC_UORB_PUBLISH_DELAY 200
|
||||
|
||||
class MK : public device::I2C
|
||||
{
|
||||
@@ -119,6 +120,7 @@ public:
|
||||
int set_pwm_rate(unsigned rate);
|
||||
int set_motor_count(unsigned count);
|
||||
int set_motor_test(bool motortest);
|
||||
int set_overrideSecurityChecks(bool overrideSecurityChecks);
|
||||
int set_px4mode(int px4mode);
|
||||
int set_frametype(int frametype);
|
||||
unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput);
|
||||
@@ -136,11 +138,15 @@ private:
|
||||
unsigned int _motor;
|
||||
int _px4mode;
|
||||
int _frametype;
|
||||
|
||||
orb_advert_t _t_outputs;
|
||||
orb_advert_t _t_actuators_effective;
|
||||
orb_advert_t _t_esc_status;
|
||||
|
||||
unsigned int _num_outputs;
|
||||
bool _primary_pwm_device;
|
||||
bool _motortest;
|
||||
bool _overrideSecurityChecks;
|
||||
|
||||
volatile bool _task_should_exit;
|
||||
bool _armed;
|
||||
@@ -214,6 +220,7 @@ struct MotorData_t {
|
||||
unsigned int Version; // the version of the BL (0 = old)
|
||||
unsigned int SetPoint; // written by attitude controller
|
||||
unsigned int SetPointLowerBits; // for higher Resolution of new BLs
|
||||
float SetPoint_PX4; // Values from PX4
|
||||
unsigned int State; // 7 bit for I2C error counter, highest bit indicates if motor is present
|
||||
unsigned int ReadMode; // select data to read
|
||||
unsigned short RawPwmValue; // length of PWM pulse
|
||||
@@ -243,8 +250,10 @@ MK::MK(int bus) :
|
||||
_t_armed(-1),
|
||||
_t_outputs(0),
|
||||
_t_actuators_effective(0),
|
||||
_t_esc_status(0),
|
||||
_num_outputs(0),
|
||||
_motortest(false),
|
||||
_overrideSecurityChecks(false),
|
||||
_motor(-1),
|
||||
_px4mode(MAPPING_MK),
|
||||
_frametype(FRAME_PLUS),
|
||||
@@ -464,6 +473,13 @@ MK::set_motor_test(bool motortest)
|
||||
return OK;
|
||||
}
|
||||
|
||||
int
|
||||
MK::set_overrideSecurityChecks(bool overrideSecurityChecks)
|
||||
{
|
||||
_overrideSecurityChecks = overrideSecurityChecks;
|
||||
return OK;
|
||||
}
|
||||
|
||||
short
|
||||
MK::scaling(float val, float inMin, float inMax, float outMin, float outMax)
|
||||
{
|
||||
@@ -506,8 +522,6 @@ MK::task_main()
|
||||
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
|
||||
&outputs);
|
||||
|
||||
|
||||
|
||||
/* advertise the effective control inputs */
|
||||
actuator_controls_effective_s controls_effective;
|
||||
memset(&controls_effective, 0, sizeof(controls_effective));
|
||||
@@ -515,6 +529,12 @@ MK::task_main()
|
||||
_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
|
||||
&controls_effective);
|
||||
|
||||
/* advertise the blctrl status */
|
||||
esc_status_s esc;
|
||||
memset(&esc, 0, sizeof(esc));
|
||||
_t_esc_status = orb_advertise(ORB_ID(esc_status), &esc);
|
||||
|
||||
|
||||
|
||||
pollfd fds[2];
|
||||
fds[0].fd = _t_actuators;
|
||||
@@ -602,9 +622,11 @@ MK::task_main()
|
||||
}
|
||||
}
|
||||
|
||||
/* don't go under BLCTRL_MIN_VALUE */
|
||||
if (outputs.output[i] < BLCTRL_MIN_VALUE) {
|
||||
outputs.output[i] = BLCTRL_MIN_VALUE;
|
||||
if(!_overrideSecurityChecks) {
|
||||
/* don't go under BLCTRL_MIN_VALUE */
|
||||
if (outputs.output[i] < BLCTRL_MIN_VALUE) {
|
||||
outputs.output[i] = BLCTRL_MIN_VALUE;
|
||||
}
|
||||
}
|
||||
|
||||
/* output to BLCtrl's */
|
||||
@@ -612,7 +634,10 @@ MK::task_main()
|
||||
mk_servo_test(i);
|
||||
|
||||
} else {
|
||||
mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024)); // scale the output to 0 - 1024 and sent to output routine
|
||||
//mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024)); // scale the output to 0 - 1024 and sent to output routine
|
||||
// 11 Bit
|
||||
Motor[i].SetPoint_PX4 = outputs.output[i];
|
||||
mk_servo_set(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 2047)); // scale the output to 0 - 2047 and sent to output routine
|
||||
}
|
||||
|
||||
}
|
||||
@@ -635,8 +660,43 @@ MK::task_main()
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* Only update esc topic every half second.
|
||||
*/
|
||||
|
||||
if (hrt_absolute_time() - esc.timestamp > 500000) {
|
||||
esc.counter++;
|
||||
esc.timestamp = hrt_absolute_time();
|
||||
esc.esc_count = (uint8_t) _num_outputs;
|
||||
esc.esc_connectiontype = ESC_CONNECTION_TYPE_I2C;
|
||||
|
||||
for (unsigned int i = 0; i < _num_outputs; i++) {
|
||||
esc.esc[i].esc_address = (uint8_t) BLCTRL_BASE_ADDR + i;
|
||||
esc.esc[i].esc_vendor = ESC_VENDOR_MIKROKOPTER;
|
||||
esc.esc[i].esc_version = (uint16_t) Motor[i].Version;
|
||||
esc.esc[i].esc_voltage = (uint16_t) 0;
|
||||
esc.esc[i].esc_current = (uint16_t) Motor[i].Current;
|
||||
esc.esc[i].esc_rpm = (uint16_t) 0;
|
||||
esc.esc[i].esc_setpoint = (float) Motor[i].SetPoint_PX4;
|
||||
if (Motor[i].Version == 1) {
|
||||
// BLCtrl 2.0 (11Bit)
|
||||
esc.esc[i].esc_setpoint_raw = (uint16_t) (Motor[i].SetPoint<<3) | Motor[i].SetPointLowerBits;
|
||||
} else {
|
||||
// BLCtrl < 2.0 (8Bit)
|
||||
esc.esc[i].esc_setpoint_raw = (uint16_t) Motor[i].SetPoint;
|
||||
}
|
||||
esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature;
|
||||
esc.esc[i].esc_state = (uint16_t) Motor[i].State;
|
||||
esc.esc[i].esc_errorcount = (uint16_t) 0;
|
||||
}
|
||||
|
||||
orb_publish(ORB_ID(esc_status), _t_esc_status, &esc);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
//::close(_t_esc_status);
|
||||
::close(_t_actuators);
|
||||
::close(_t_actuators_effective);
|
||||
::close(_t_armed);
|
||||
@@ -715,17 +775,17 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput)
|
||||
fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i, Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature);
|
||||
}
|
||||
|
||||
if (foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) {
|
||||
_task_should_exit = true;
|
||||
|
||||
if(!_overrideSecurityChecks) {
|
||||
if (foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) {
|
||||
_task_should_exit = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return foundMotorCount;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
int
|
||||
MK::mk_servo_set(unsigned int chan, short val)
|
||||
{
|
||||
@@ -738,17 +798,15 @@ MK::mk_servo_set(unsigned int chan, short val)
|
||||
|
||||
tmpVal = val;
|
||||
|
||||
if (tmpVal > 1024) {
|
||||
tmpVal = 1024;
|
||||
if (tmpVal > 2047) {
|
||||
tmpVal = 2047;
|
||||
|
||||
} else if (tmpVal < 0) {
|
||||
tmpVal = 0;
|
||||
}
|
||||
|
||||
Motor[chan].SetPoint = (uint8_t)(tmpVal / 4);
|
||||
//Motor[chan].SetPointLowerBits = (uint8_t) tmpVal % 4;
|
||||
|
||||
Motor[chan].SetPointLowerBits = 0;
|
||||
Motor[chan].SetPoint = (uint8_t)(tmpVal>>3)&0xff;
|
||||
Motor[chan].SetPointLowerBits = ((uint8_t)tmpVal%8)&0x07;
|
||||
|
||||
if (_armed == false) {
|
||||
Motor[chan].SetPoint = 0;
|
||||
@@ -1014,8 +1072,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
||||
case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
|
||||
if (arg < 2150) {
|
||||
Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue = (unsigned short)arg;
|
||||
mk_servo_set_value(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 1024));
|
||||
|
||||
mk_servo_set(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 2047));
|
||||
} else {
|
||||
ret = -EINVAL;
|
||||
}
|
||||
@@ -1112,25 +1169,19 @@ ssize_t
|
||||
MK::write(file *filp, const char *buffer, size_t len)
|
||||
{
|
||||
unsigned count = len / 2;
|
||||
// loeschen uint16_t values[4];
|
||||
uint16_t values[8];
|
||||
|
||||
// loeschen if (count > 4) {
|
||||
// loeschen // we only have 4 PWM outputs on the FMU
|
||||
// loeschen count = 4;
|
||||
// loeschen }
|
||||
if (count > _num_outputs) {
|
||||
// we only have 8 I2C outputs in the driver
|
||||
count = _num_outputs;
|
||||
}
|
||||
|
||||
|
||||
// allow for misaligned values
|
||||
memcpy(values, buffer, count * 2);
|
||||
|
||||
for (uint8_t i = 0; i < count; i++) {
|
||||
Motor[i].RawPwmValue = (unsigned short)values[i];
|
||||
mk_servo_set_value(i, scaling(values[i], 1010, 2100, 0, 1024));
|
||||
mk_servo_set(i, scaling(values[i], 1010, 2100, 0, 2047));
|
||||
}
|
||||
|
||||
return count * 2;
|
||||
@@ -1282,7 +1333,7 @@ enum FrameType {
|
||||
PortMode g_port_mode;
|
||||
|
||||
int
|
||||
mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype)
|
||||
mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks)
|
||||
{
|
||||
uint32_t gpio_bits;
|
||||
int shouldStop = 0;
|
||||
@@ -1341,6 +1392,9 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
|
||||
/* motortest if enabled */
|
||||
g_mk->set_motor_test(motortest);
|
||||
|
||||
/* ovveride security checks if enabled */
|
||||
g_mk->set_overrideSecurityChecks(overrideSecurityChecks);
|
||||
|
||||
|
||||
/* count used motors */
|
||||
do {
|
||||
@@ -1406,6 +1460,7 @@ mkblctrl_main(int argc, char *argv[])
|
||||
int px4mode = MAPPING_PX4;
|
||||
int frametype = FRAME_PLUS; // + plus is default
|
||||
bool motortest = false;
|
||||
bool overrideSecurityChecks = false;
|
||||
bool showHelp = false;
|
||||
bool newMode = false;
|
||||
|
||||
@@ -1461,11 +1516,21 @@ mkblctrl_main(int argc, char *argv[])
|
||||
showHelp = true;
|
||||
}
|
||||
|
||||
/* look for the optional --override-security-checks parameter */
|
||||
if (strcmp(argv[i], "--override-security-checks") == 0) {
|
||||
overrideSecurityChecks = true;
|
||||
newMode = true;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (showHelp) {
|
||||
fprintf(stderr, "mkblctrl: help:\n");
|
||||
fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [-h / --help]\n");
|
||||
fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [--override-security-checks] [-h / --help]\n\n");
|
||||
fprintf(stderr, "\t -mkmode frame {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n");
|
||||
fprintf(stderr, "\t -b i2c_bus_number \t\t Set the i2c bus where the ESCs are connected to (default 1).\n");
|
||||
fprintf(stderr, "\t -t motortest \t\t\t Spin up once every motor in order of motoraddress. (DANGER !!!)\n");
|
||||
fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
@@ -1483,7 +1548,7 @@ mkblctrl_main(int argc, char *argv[])
|
||||
/* parameter set ? */
|
||||
if (newMode) {
|
||||
/* switch parameter */
|
||||
return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype);
|
||||
return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks);
|
||||
}
|
||||
|
||||
/* test, etc. here g*/
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 2012 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
|
||||
@@ -272,6 +272,11 @@ private:
|
||||
*/
|
||||
void _set_dlpf_filter(uint16_t frequency_hz);
|
||||
|
||||
/*
|
||||
set sample rate (approximate) - 1kHz to 5Hz
|
||||
*/
|
||||
void _set_sample_rate(uint16_t desired_sample_rate_hz);
|
||||
|
||||
};
|
||||
|
||||
/**
|
||||
@@ -378,7 +383,8 @@ MPU6000::init()
|
||||
up_udelay(1000);
|
||||
|
||||
// SAMPLE RATE
|
||||
write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz
|
||||
//write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz
|
||||
_set_sample_rate(200); // default sample rate = 200Hz
|
||||
usleep(1000);
|
||||
|
||||
// FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter)
|
||||
@@ -493,6 +499,18 @@ MPU6000::probe()
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
/*
|
||||
set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro
|
||||
*/
|
||||
void
|
||||
MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz)
|
||||
{
|
||||
uint8_t div = 1000 / desired_sample_rate_hz;
|
||||
if(div>200) div=200;
|
||||
if(div<1) div=1;
|
||||
write_reg(MPUREG_SMPLRT_DIV, div-1);
|
||||
}
|
||||
|
||||
/*
|
||||
set the DLPF filter frequency. This affects both accel and gyro.
|
||||
*/
|
||||
@@ -644,8 +662,8 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
|
||||
case ACCELIOCSSAMPLERATE:
|
||||
case ACCELIOCGSAMPLERATE:
|
||||
/* XXX not implemented */
|
||||
return -EINVAL;
|
||||
_set_sample_rate(arg);
|
||||
return OK;
|
||||
|
||||
case ACCELIOCSLOWPASS:
|
||||
case ACCELIOCGLOWPASS:
|
||||
@@ -702,8 +720,8 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
|
||||
case GYROIOCSSAMPLERATE:
|
||||
case GYROIOCGSAMPLERATE:
|
||||
/* XXX not implemented */
|
||||
return -EINVAL;
|
||||
_set_sample_rate(arg);
|
||||
return OK;
|
||||
|
||||
case GYROIOCSLOWPASS:
|
||||
case GYROIOCGLOWPASS:
|
||||
|
||||
Reference in New Issue
Block a user