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

View File

@ -61,6 +61,10 @@
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.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_global_position.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 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);
/**
@ -133,18 +137,18 @@ static int deamon_task; /**< Handle of deamon task / thread */
static struct params p;
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)
{
/*
/*
* The PX4 architecture provides a mixer outside of the controller.
* The mixer is fed with a default vector of actuator controls, representing
* moments applied to the vehicle frame. This vector
* is structured as:
*
* Control Group 0 (attitude):
*
*
* 0 - roll (-1..+1)
* 1 - pitch (-1..+1)
* 2 - yaw (-1..+1)
@ -226,7 +230,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
*
* Wikipedia description:
* 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.
*
*
* These structs contain the system state and things
* like attitude, position, the current waypoint, etc.
*/
@ -300,7 +304,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
if (ret < 0) {
/*
* 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");
@ -340,7 +344,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
}
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);
/* 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 <uORB/topics/actuator_armed.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>
__EXPORT int ex_hwtest_main(int argc, char *argv[]);