forked from Archive/PX4-Autopilot
add another set of uorb headers
This commit is contained in:
parent
7634147c0f
commit
cd35ab2661
|
@ -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");
|
||||||
|
|
|
@ -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/Publish–subscribe_pattern
|
* http://en.wikipedia.org/wiki/Publish–subscribe_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 */
|
||||||
|
|
|
@ -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[]);
|
||||||
|
|
Loading…
Reference in New Issue