add another set of uorb headers

This commit is contained in:
Thomas Gubler 2015-01-25 15:32:40 +01:00
parent 7634147c0f
commit cd35ab2661
3 changed files with 24 additions and 15 deletions

View File

@ -54,6 +54,7 @@
#include <mavlink/mavlink_log.h> #include <mavlink/mavlink_log.h>
#include <uORB/Publication.hpp> #include <uORB/Publication.hpp>
#include <uORB/topics/actuator_controls_0.h>
#include <uORB/topics/debug_key_value.h> #include <uORB/topics/debug_key_value.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
@ -114,7 +115,7 @@ int RoboClaw::readEncoder(e_motor motor)
uint8_t rbuf[50]; uint8_t rbuf[50];
usleep(5000); usleep(5000);
int nread = read(_uart, rbuf, 50); int nread = read(_uart, rbuf, 50);
if (nread < 6) { if (nread < 6) {
printf("failed to read\n"); printf("failed to read\n");
return -1; return -1;
} }
@ -131,7 +132,7 @@ int RoboClaw::readEncoder(e_motor motor)
countBytes[0] = rbuf[3]; countBytes[0] = rbuf[3];
uint8_t status = rbuf[4]; uint8_t status = rbuf[4];
uint8_t checksum = rbuf[5]; uint8_t checksum = rbuf[5];
uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) & uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) &
checksum_mask; checksum_mask;
// check if checksum is valid // check if checksum is valid
if (checksum != checksum_computed) { if (checksum != checksum_computed) {
@ -156,11 +157,11 @@ int RoboClaw::readEncoder(e_motor motor)
static int64_t overflowAmount = 0x100000000LL; static int64_t overflowAmount = 0x100000000LL;
if (motor == MOTOR_1) { if (motor == MOTOR_1) {
_motor1Overflow += overFlow; _motor1Overflow += overFlow;
_motor1Position = float(int64_t(count) + _motor1Position = float(int64_t(count) +
_motor1Overflow*overflowAmount)/_pulsesPerRev; _motor1Overflow*overflowAmount)/_pulsesPerRev;
} else if (motor == MOTOR_2) { } else if (motor == MOTOR_2) {
_motor2Overflow += overFlow; _motor2Overflow += overFlow;
_motor2Position = float(int64_t(count) + _motor2Position = float(int64_t(count) +
_motor2Overflow*overflowAmount)/_pulsesPerRev; _motor2Overflow*overflowAmount)/_pulsesPerRev;
} }
return 0; return 0;
@ -242,7 +243,7 @@ int RoboClaw::setMotorDutyCycle(e_motor motor, float value)
return -1; return -1;
} }
int RoboClaw::resetEncoders() int RoboClaw::resetEncoders()
{ {
uint16_t sum = 0; uint16_t sum = 0;
return _sendCommand(CMD_RESET_ENCODERS, return _sendCommand(CMD_RESET_ENCODERS,
@ -278,7 +279,7 @@ uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n)
return sum; return sum;
} }
int RoboClaw::_sendCommand(e_command cmd, uint8_t * data, int RoboClaw::_sendCommand(e_command cmd, uint8_t * data,
size_t n_data, uint16_t & prev_sum) size_t n_data, uint16_t & prev_sum)
{ {
tcflush(_uart, TCIOFLUSH); // flush buffers tcflush(_uart, TCIOFLUSH); // flush buffers
@ -299,7 +300,7 @@ int RoboClaw::_sendCommand(e_command cmd, uint8_t * data,
return write(_uart, buf, n_data + 3); return write(_uart, buf, n_data + 3);
} }
int roboclawTest(const char *deviceName, uint8_t address, int roboclawTest(const char *deviceName, uint8_t address,
uint16_t pulsesPerRev) uint16_t pulsesPerRev)
{ {
printf("roboclaw test: starting\n"); printf("roboclaw test: starting\n");

View File

@ -61,6 +61,10 @@
#include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_0.h>
#include <uORB/topics/actuator_controls_1.h>
#include <uORB/topics/actuator_controls_2.h>
#include <uORB/topics/actuator_controls_3.h>
#include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/parameter_update.h> #include <uORB/topics/parameter_update.h>
@ -108,7 +112,7 @@ static void usage(const char *reason);
* @param att The current attitude. The controller should make the attitude match the setpoint * @param att The current attitude. The controller should make the attitude match the setpoint
* @param rates_sp The angular rate setpoint. This is the output of the controller. * @param rates_sp The angular rate setpoint. This is the output of the controller.
*/ */
void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp,
struct actuator_controls_s *actuators); struct actuator_controls_s *actuators);
/** /**
@ -133,18 +137,18 @@ static int deamon_task; /**< Handle of deamon task / thread */
static struct params p; static struct params p;
static struct param_handles ph; static struct param_handles ph;
void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp,
struct actuator_controls_s *actuators) struct actuator_controls_s *actuators)
{ {
/* /*
* The PX4 architecture provides a mixer outside of the controller. * The PX4 architecture provides a mixer outside of the controller.
* The mixer is fed with a default vector of actuator controls, representing * The mixer is fed with a default vector of actuator controls, representing
* moments applied to the vehicle frame. This vector * moments applied to the vehicle frame. This vector
* is structured as: * is structured as:
* *
* Control Group 0 (attitude): * Control Group 0 (attitude):
* *
* 0 - roll (-1..+1) * 0 - roll (-1..+1)
* 1 - pitch (-1..+1) * 1 - pitch (-1..+1)
* 2 - yaw (-1..+1) * 2 - yaw (-1..+1)
@ -226,7 +230,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
* *
* Wikipedia description: * Wikipedia description:
* http://en.wikipedia.org/wiki/Publishsubscribe_pattern * http://en.wikipedia.org/wiki/Publishsubscribe_pattern
* *
*/ */
@ -234,7 +238,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
/* /*
* Declare and safely initialize all structs to zero. * Declare and safely initialize all structs to zero.
* *
* These structs contain the system state and things * These structs contain the system state and things
* like attitude, position, the current waypoint, etc. * like attitude, position, the current waypoint, etc.
*/ */
@ -300,7 +304,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
if (ret < 0) { if (ret < 0) {
/* /*
* Poll error, this will not really happen in practice, * Poll error, this will not really happen in practice,
* but its good design practice to make output an error message. * but its good design practice to make output an error message.
*/ */
warnx("poll error"); warnx("poll error");
@ -340,7 +344,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
} }
if (manual_sp_updated) if (manual_sp_updated)
/* get the RC (or otherwise user based) input */ /* get the RC (or otherwise user based) input */
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
/* check if the throttle was ever more than 50% - go later only to failsafe if yes */ /* check if the throttle was ever more than 50% - go later only to failsafe if yes */

View File

@ -47,6 +47,10 @@
#include <systemlib/err.h> #include <systemlib/err.h>
#include <uORB/topics/actuator_armed.h> #include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_0.h>
#include <uORB/topics/actuator_controls_1.h>
#include <uORB/topics/actuator_controls_2.h>
#include <uORB/topics/actuator_controls_3.h>
#include <uORB/uORB.h> #include <uORB/uORB.h>
__EXPORT int ex_hwtest_main(int argc, char *argv[]); __EXPORT int ex_hwtest_main(int argc, char *argv[]);