diff --git a/.travis.yml b/.travis.yml index 3684988c3..f64d7ac58 100755 --- a/.travis.yml +++ b/.travis.yml @@ -11,11 +11,9 @@ notifications: on_success: change on_failure: always recipients: - - kkw@robotis.com - - chc@robotis.com - - pyo@robotis.com - - akh@robotis.com + - willson@robotis.com branches: only: - master + - develop diff --git a/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_burger/turtlebot3_core/turtlebot3_core_config.h b/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_burger/turtlebot3_core/turtlebot3_core_config.h index 808509060..43ee980a0 100644 --- a/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_burger/turtlebot3_core/turtlebot3_core_config.h +++ b/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_burger/turtlebot3_core/turtlebot3_core_config.h @@ -18,18 +18,15 @@ #ifndef TURTLEBOT3_CORE_CONFIG_H_ #define TURTLEBOT3_CORE_CONFIG_H_ +// #define NOETIC_SUPPORT //uncomment this if writing code for ROS1 Noetic #include #include #include #include #include -#include #include -#include -#include #include -#include #include #include #include @@ -43,7 +40,7 @@ #include -#define FIRMWARE_VER "1.2.3" +#define FIRMWARE_VER "1.2.6" #define CONTROL_MOTOR_SPEED_FREQUENCY 30 //hz #define CONTROL_MOTOR_TIMEOUT 500 //ms @@ -168,7 +165,11 @@ sensor_msgs::JointState joint_states; ros::Publisher joint_states_pub("joint_states", &joint_states); // Battey state of Turtlebot3 +#if defined NOETIC_SUPPORT +sensor_msgs::BatteryStateNoetic battery_state_msg; +#else sensor_msgs::BatteryState battery_state_msg; +#endif ros::Publisher battery_state_pub("battery_state", &battery_state_msg); // Magnetic field diff --git a/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_waffle/turtlebot3_core/turtlebot3_core_config.h b/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_waffle/turtlebot3_core/turtlebot3_core_config.h index 74bcde5d7..09a4e487b 100644 --- a/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_waffle/turtlebot3_core/turtlebot3_core_config.h +++ b/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_waffle/turtlebot3_core/turtlebot3_core_config.h @@ -18,18 +18,15 @@ #ifndef TURTLEBOT3_CORE_CONFIG_H_ #define TURTLEBOT3_CORE_CONFIG_H_ +// #define NOETIC_SUPPORT //uncomment this if writing code for ROS1 Noetic #include #include #include #include #include -#include #include -#include -#include #include -#include #include #include #include @@ -43,7 +40,7 @@ #include -#define FIRMWARE_VER "1.2.3" +#define FIRMWARE_VER "1.2.6" #define CONTROL_MOTOR_SPEED_FREQUENCY 30 //hz #define CONTROL_MOTOR_TIMEOUT 500 //ms diff --git a/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_with_open_manipulator/turtlebot3_with_open_manipulator_core/turtlebot3_with_open_manipulator_core_config.h b/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_with_open_manipulator/turtlebot3_with_open_manipulator_core/turtlebot3_with_open_manipulator_core_config.h index 96fba5680..22690fda7 100644 --- a/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_with_open_manipulator/turtlebot3_with_open_manipulator_core/turtlebot3_with_open_manipulator_core_config.h +++ b/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_with_open_manipulator/turtlebot3_with_open_manipulator_core/turtlebot3_with_open_manipulator_core_config.h @@ -18,6 +18,7 @@ #ifndef TURTLEBOT3_WITH_OPEN_MANIPULATOR_CORE_CONFIG_H_ #define TURTLEBOT3_WITH_OPEN_MANIPULATOR_CORE_CONFIG_H_ +// #define NOETIC_SUPPORT //uncomment this if writing code for ROS1 Noetic #include #include @@ -26,10 +27,7 @@ #include #include #include -#include #include -#include -#include #include #include #include @@ -45,7 +43,7 @@ #include -#define FIRMWARE_VER "2.0.1" +#define FIRMWARE_VER "2.0.2" #define CONTROL_MOTOR_SPEED_FREQUENCY 30 //hz #define IMU_PUBLISH_FREQUENCY 200 //hz diff --git a/arduino/opencr_arduino/opencr/libraries/turtlebot3/include/turtlebot3/turtlebot3_sensor.h b/arduino/opencr_arduino/opencr/libraries/turtlebot3/include/turtlebot3/turtlebot3_sensor.h index 760880ea6..1bbe93900 100644 --- a/arduino/opencr_arduino/opencr/libraries/turtlebot3/include/turtlebot3/turtlebot3_sensor.h +++ b/arduino/opencr_arduino/opencr/libraries/turtlebot3/include/turtlebot3/turtlebot3_sensor.h @@ -22,9 +22,14 @@ #include #include -#include #include +#if defined NOETIC_SUPPORT + #include +#else + #include +#endif + #include "OLLO.h" #define ACCEL_FACTOR 0.000598550415 // (ADC_Value / Scale) * 9.80665 => Range : +- 2[g] @@ -98,7 +103,11 @@ class Turtlebot3Sensor void setLedPattern(double linear_vel, double angular_vel); private: sensor_msgs::Imu imu_msg_; - sensor_msgs::BatteryState battery_state_msg_; + #if defined NOETIC_SUPPORT + sensor_msgs::BatteryStateNoetic battery_state_msg_; + #else + sensor_msgs::BatteryState battery_state_msg_; + #endif sensor_msgs::MagneticField mag_msg_; cIMU imu_; diff --git a/arduino/opencr_arduino/opencr/libraries/turtlebot3/src/turtlebot3/turtlebot3_sensor.cpp b/arduino/opencr_arduino/opencr/libraries/turtlebot3/src/turtlebot3/turtlebot3_sensor.cpp index 8db594684..1adf40cd8 100644 --- a/arduino/opencr_arduino/opencr/libraries/turtlebot3/src/turtlebot3/turtlebot3_sensor.cpp +++ b/arduino/opencr_arduino/opencr/libraries/turtlebot3/src/turtlebot3/turtlebot3_sensor.cpp @@ -38,6 +38,10 @@ bool Turtlebot3Sensor::init(void) uint8_t get_error_code = 0x00; + #if defined NOETIC_SUPPORT + battery_state_msg_.temperature = NAN; + #endif + battery_state_msg_.current = NAN; battery_state_msg_.charge = NAN; battery_state_msg_.capacity = NAN; diff --git a/arduino/opencr_arduino/opencr/libraries/turtlebot3_ros_lib/sensor_msgs/BatteryState.h b/arduino/opencr_arduino/opencr/libraries/turtlebot3_ros_lib/sensor_msgs/BatteryState.h index 4e9cba443..3aa77c5f2 100644 --- a/arduino/opencr_arduino/opencr/libraries/turtlebot3_ros_lib/sensor_msgs/BatteryState.h +++ b/arduino/opencr_arduino/opencr/libraries/turtlebot3_ros_lib/sensor_msgs/BatteryState.h @@ -323,4 +323,4 @@ namespace sensor_msgs }; } -#endif \ No newline at end of file +#endif diff --git a/arduino/opencr_arduino/opencr/libraries/turtlebot3_ros_lib/sensor_msgs/BatteryStateNoetic.h b/arduino/opencr_arduino/opencr/libraries/turtlebot3_ros_lib/sensor_msgs/BatteryStateNoetic.h new file mode 100644 index 000000000..237e3bfcc --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/turtlebot3_ros_lib/sensor_msgs/BatteryStateNoetic.h @@ -0,0 +1,394 @@ +#ifndef _ROS_sensor_msgs_BatteryStateNoetic_h +#define _ROS_sensor_msgs_BatteryStateNoetic_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" + +namespace sensor_msgs +{ + + class BatteryStateNoetic : public ros::Msg + { + public: + typedef std_msgs::Header _header_type; + _header_type header; + typedef float _voltage_type; + _voltage_type voltage; + typedef float _temperature_type; + _temperature_type temperature; + typedef float _current_type; + _current_type current; + typedef float _charge_type; + _charge_type charge; + typedef float _capacity_type; + _capacity_type capacity; + typedef float _design_capacity_type; + _design_capacity_type design_capacity; + typedef float _percentage_type; + _percentage_type percentage; + typedef uint8_t _power_supply_status_type; + _power_supply_status_type power_supply_status; + typedef uint8_t _power_supply_health_type; + _power_supply_health_type power_supply_health; + typedef uint8_t _power_supply_technology_type; + _power_supply_technology_type power_supply_technology; + typedef bool _present_type; + _present_type present; + uint32_t cell_voltage_length; + typedef float _cell_voltage_type; + _cell_voltage_type st_cell_voltage; + _cell_voltage_type * cell_voltage; + uint32_t cell_temperature_length; + typedef float _cell_temperature_type; + _cell_temperature_type st_cell_temperature; + _cell_temperature_type * cell_temperature; + typedef const char* _location_type; + _location_type location; + typedef const char* _serial_number_type; + _serial_number_type serial_number; + enum { POWER_SUPPLY_STATUS_UNKNOWN = 0 }; + enum { POWER_SUPPLY_STATUS_CHARGING = 1 }; + enum { POWER_SUPPLY_STATUS_DISCHARGING = 2 }; + enum { POWER_SUPPLY_STATUS_NOT_CHARGING = 3 }; + enum { POWER_SUPPLY_STATUS_FULL = 4 }; + enum { POWER_SUPPLY_HEALTH_UNKNOWN = 0 }; + enum { POWER_SUPPLY_HEALTH_GOOD = 1 }; + enum { POWER_SUPPLY_HEALTH_OVERHEAT = 2 }; + enum { POWER_SUPPLY_HEALTH_DEAD = 3 }; + enum { POWER_SUPPLY_HEALTH_OVERVOLTAGE = 4 }; + enum { POWER_SUPPLY_HEALTH_UNSPEC_FAILURE = 5 }; + enum { POWER_SUPPLY_HEALTH_COLD = 6 }; + enum { POWER_SUPPLY_HEALTH_WATCHDOG_TIMER_EXPIRE = 7 }; + enum { POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE = 8 }; + enum { POWER_SUPPLY_TECHNOLOGY_UNKNOWN = 0 }; + enum { POWER_SUPPLY_TECHNOLOGY_NIMH = 1 }; + enum { POWER_SUPPLY_TECHNOLOGY_LION = 2 }; + enum { POWER_SUPPLY_TECHNOLOGY_LIPO = 3 }; + enum { POWER_SUPPLY_TECHNOLOGY_LIFE = 4 }; + enum { POWER_SUPPLY_TECHNOLOGY_NICD = 5 }; + enum { POWER_SUPPLY_TECHNOLOGY_LIMN = 6 }; + + BatteryStateNoetic(): + header(), + voltage(0), + temperature(0), + current(0), + charge(0), + capacity(0), + design_capacity(0), + percentage(0), + power_supply_status(0), + power_supply_health(0), + power_supply_technology(0), + present(0), + cell_voltage_length(0), cell_voltage(NULL), + cell_temperature_length(0), cell_temperature(NULL), + location(""), + serial_number("") + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + union { + float real; + uint32_t base; + } u_voltage; + u_voltage.real = this->voltage; + *(outbuffer + offset + 0) = (u_voltage.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_voltage.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_voltage.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_voltage.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->voltage); + union { + float real; + uint32_t base; + } u_temperature; + u_temperature.real = this->temperature; + *(outbuffer + offset + 0) = (u_temperature.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_temperature.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_temperature.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_temperature.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->temperature); + union { + float real; + uint32_t base; + } u_current; + u_current.real = this->current; + *(outbuffer + offset + 0) = (u_current.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_current.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_current.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_current.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->current); + union { + float real; + uint32_t base; + } u_charge; + u_charge.real = this->charge; + *(outbuffer + offset + 0) = (u_charge.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_charge.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_charge.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_charge.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->charge); + union { + float real; + uint32_t base; + } u_capacity; + u_capacity.real = this->capacity; + *(outbuffer + offset + 0) = (u_capacity.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_capacity.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_capacity.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_capacity.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->capacity); + union { + float real; + uint32_t base; + } u_design_capacity; + u_design_capacity.real = this->design_capacity; + *(outbuffer + offset + 0) = (u_design_capacity.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_design_capacity.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_design_capacity.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_design_capacity.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->design_capacity); + union { + float real; + uint32_t base; + } u_percentage; + u_percentage.real = this->percentage; + *(outbuffer + offset + 0) = (u_percentage.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_percentage.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_percentage.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_percentage.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->percentage); + *(outbuffer + offset + 0) = (this->power_supply_status >> (8 * 0)) & 0xFF; + offset += sizeof(this->power_supply_status); + *(outbuffer + offset + 0) = (this->power_supply_health >> (8 * 0)) & 0xFF; + offset += sizeof(this->power_supply_health); + *(outbuffer + offset + 0) = (this->power_supply_technology >> (8 * 0)) & 0xFF; + offset += sizeof(this->power_supply_technology); + union { + bool real; + uint8_t base; + } u_present; + u_present.real = this->present; + *(outbuffer + offset + 0) = (u_present.base >> (8 * 0)) & 0xFF; + offset += sizeof(this->present); + *(outbuffer + offset + 0) = (this->cell_voltage_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->cell_voltage_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->cell_voltage_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->cell_voltage_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->cell_voltage_length); + for( uint32_t i = 0; i < cell_voltage_length; i++){ + union { + float real; + uint32_t base; + } u_cell_voltagei; + u_cell_voltagei.real = this->cell_voltage[i]; + *(outbuffer + offset + 0) = (u_cell_voltagei.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_cell_voltagei.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_cell_voltagei.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_cell_voltagei.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->cell_voltage[i]); + } + *(outbuffer + offset + 0) = (this->cell_temperature_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->cell_temperature_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->cell_temperature_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->cell_temperature_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->cell_temperature_length); + for( uint32_t i = 0; i < cell_temperature_length; i++){ + union { + float real; + uint32_t base; + } u_cell_temperaturei; + u_cell_temperaturei.real = this->cell_temperature[i]; + *(outbuffer + offset + 0) = (u_cell_temperaturei.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_cell_temperaturei.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_cell_temperaturei.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_cell_temperaturei.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->cell_temperature[i]); + } + uint32_t length_location = strlen(this->location); + varToArr(outbuffer + offset, length_location); + offset += 4; + memcpy(outbuffer + offset, this->location, length_location); + offset += length_location; + uint32_t length_serial_number = strlen(this->serial_number); + varToArr(outbuffer + offset, length_serial_number); + offset += 4; + memcpy(outbuffer + offset, this->serial_number, length_serial_number); + offset += length_serial_number; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + union { + float real; + uint32_t base; + } u_voltage; + u_voltage.base = 0; + u_voltage.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_voltage.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_voltage.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_voltage.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->voltage = u_voltage.real; + offset += sizeof(this->voltage); + union { + float real; + uint32_t base; + } u_temperature; + u_temperature.base = 0; + u_temperature.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_temperature.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_temperature.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_temperature.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->temperature = u_temperature.real; + offset += sizeof(this->temperature); + union { + float real; + uint32_t base; + } u_current; + u_current.base = 0; + u_current.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_current.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_current.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_current.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->current = u_current.real; + offset += sizeof(this->current); + union { + float real; + uint32_t base; + } u_charge; + u_charge.base = 0; + u_charge.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_charge.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_charge.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_charge.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->charge = u_charge.real; + offset += sizeof(this->charge); + union { + float real; + uint32_t base; + } u_capacity; + u_capacity.base = 0; + u_capacity.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_capacity.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_capacity.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_capacity.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->capacity = u_capacity.real; + offset += sizeof(this->capacity); + union { + float real; + uint32_t base; + } u_design_capacity; + u_design_capacity.base = 0; + u_design_capacity.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_design_capacity.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_design_capacity.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_design_capacity.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->design_capacity = u_design_capacity.real; + offset += sizeof(this->design_capacity); + union { + float real; + uint32_t base; + } u_percentage; + u_percentage.base = 0; + u_percentage.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_percentage.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_percentage.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_percentage.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->percentage = u_percentage.real; + offset += sizeof(this->percentage); + this->power_supply_status = ((uint8_t) (*(inbuffer + offset))); + offset += sizeof(this->power_supply_status); + this->power_supply_health = ((uint8_t) (*(inbuffer + offset))); + offset += sizeof(this->power_supply_health); + this->power_supply_technology = ((uint8_t) (*(inbuffer + offset))); + offset += sizeof(this->power_supply_technology); + union { + bool real; + uint8_t base; + } u_present; + u_present.base = 0; + u_present.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); + this->present = u_present.real; + offset += sizeof(this->present); + uint32_t cell_voltage_lengthT = ((uint32_t) (*(inbuffer + offset))); + cell_voltage_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + cell_voltage_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + cell_voltage_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->cell_voltage_length); + if(cell_voltage_lengthT > cell_voltage_length) + this->cell_voltage = (float*)realloc(this->cell_voltage, cell_voltage_lengthT * sizeof(float)); + cell_voltage_length = cell_voltage_lengthT; + for( uint32_t i = 0; i < cell_voltage_length; i++){ + union { + float real; + uint32_t base; + } u_st_cell_voltage; + u_st_cell_voltage.base = 0; + u_st_cell_voltage.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_cell_voltage.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_st_cell_voltage.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_st_cell_voltage.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->st_cell_voltage = u_st_cell_voltage.real; + offset += sizeof(this->st_cell_voltage); + memcpy( &(this->cell_voltage[i]), &(this->st_cell_voltage), sizeof(float)); + } + uint32_t cell_temperature_lengthT = ((uint32_t) (*(inbuffer + offset))); + cell_temperature_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + cell_temperature_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + cell_temperature_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->cell_temperature_length); + if(cell_temperature_lengthT > cell_temperature_length) + this->cell_temperature = (float*)realloc(this->cell_temperature, cell_temperature_lengthT * sizeof(float)); + cell_temperature_length = cell_temperature_lengthT; + for( uint32_t i = 0; i < cell_temperature_length; i++){ + union { + float real; + uint32_t base; + } u_st_cell_temperature; + u_st_cell_temperature.base = 0; + u_st_cell_temperature.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_cell_temperature.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_st_cell_temperature.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_st_cell_temperature.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->st_cell_temperature = u_st_cell_temperature.real; + offset += sizeof(this->st_cell_temperature); + memcpy( &(this->cell_temperature[i]), &(this->st_cell_temperature), sizeof(float)); + } + uint32_t length_location; + arrToVar(length_location, (inbuffer + offset)); + offset += 4; + for(unsigned int k= offset; k< offset+length_location; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_location-1]=0; + this->location = (char *)(inbuffer + offset-1); + offset += length_location; + uint32_t length_serial_number; + arrToVar(length_serial_number, (inbuffer + offset)); + offset += 4; + for(unsigned int k= offset; k< offset+length_serial_number; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_serial_number-1]=0; + this->serial_number = (char *)(inbuffer + offset-1); + offset += length_serial_number; + return offset; + } + + const char * getType(){ return "sensor_msgs/BatteryState"; }; + const char * getMD5(){ return "4ddae7f048e32fda22cac764685e3974"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/arduino/opencr_arduino/opencr/platform.txt b/arduino/opencr_arduino/opencr/platform.txt index 9093f60f0..ab7030362 100755 --- a/arduino/opencr_arduino/opencr/platform.txt +++ b/arduino/opencr_arduino/opencr/platform.txt @@ -37,6 +37,7 @@ compiler.elf2hex.cmd=arm-none-eabi-objcopy compiler.ldflags= compiler.size.cmd=arm-none-eabi-size compiler.define=-DARDUINO= +compiler.libraries.ldflags= build.cpu_flags= @@ -80,7 +81,7 @@ recipe.s.o.pattern="{compiler.path}{compiler.c.cmd}" {compiler.S.flags} -mcpu={b ## Create archives recipe.ar.pattern="{compiler.path}{compiler.ar.cmd}" {compiler.ar.flags} {compiler.ar.extra_flags} "{archive_file_path}" "{object_file}" -recipe.c.combine.pattern="{compiler.path}{compiler.c.elf.cmd}" {compiler.c.elf.flags} -mcpu={build.mcu} "-T{build.variant.path}/{build.ldscript}" "-Wl,-Map,{build.path}/{build.project_name}.map" {compiler.c.elf.extra_flags} -o "{build.path}/{build.project_name}.elf" "-L{build.path}" -lm -lgcc -mthumb -Wl,--cref -Wl,--check-sections -Wl,--gc-sections -Wl,--unresolved-symbols=report-all -Wl,--warn-common -Wl,--warn-unresolved-symbols -Wl,--start-group {object_files} -Wl,--whole-archive "{build.path}/{archive_file}" -Wl,--no-whole-archive -Wl,--end-group +recipe.c.combine.pattern="{compiler.path}{compiler.c.elf.cmd}" {compiler.c.elf.flags} -mcpu={build.mcu} "-T{build.variant.path}/{build.ldscript}" "-Wl,-Map,{build.path}/{build.project_name}.map" {compiler.c.elf.extra_flags} -o "{build.path}/{build.project_name}.elf" "-L{build.path}" -lm -lgcc -mthumb -Wl,--cref -Wl,--check-sections -Wl,--gc-sections -Wl,--unresolved-symbols=report-all -Wl,--warn-common -Wl,--warn-unresolved-symbols -Wl,--start-group {object_files} -Wl,--whole-archive {compiler.libraries.ldflags} "{build.path}/{archive_file}" -Wl,--no-whole-archive -Wl,--end-group ## Create eeprom recipe.objcopy.eep.pattern= diff --git a/arduino/opencr_develop/opencr_ld_shell/opencr_update/update.sh b/arduino/opencr_develop/opencr_ld_shell/opencr_update/update.sh index 06bb0f573..6a16ad8dc 100755 --- a/arduino/opencr_develop/opencr_ld_shell/opencr_update/update.sh +++ b/arduino/opencr_develop/opencr_ld_shell/opencr_update/update.sh @@ -3,11 +3,12 @@ architecture="" case $(uname -m) in - i386) architecture="386" ;; - i686) architecture="386" ;; - x86_64) architecture="amd64" ;; - armv7l) architecture="arm" ;; - arm) dpkg --print-architecture | grep -q "arm64" && architecture="arm64" || architecture="arm" ;; + i386) architecture="386" ;; + i686) architecture="386" ;; + x86_64) architecture="amd64" ;; + armv7l) architecture="arm" ;; + aarch64) architecture="arm" ;; + arm) dpkg --print-architecture | grep -q "arm64" && architecture="arm64" || architecture="arm" ;; esac echo $(uname -m) diff --git a/arduino/opencr_release/package_opencr_index.json b/arduino/opencr_release/package_opencr_index.json index d14711090..130539ee9 100644 --- a/arduino/opencr_release/package_opencr_index.json +++ b/arduino/opencr_release/package_opencr_index.json @@ -1311,6 +1311,37 @@ "version": "1.0.0" } ] + }, + { + "name": "OpenCR", + "architecture": "OpenCR", + "version": "1.4.16", + "category": "Arduino", + "help": { + "online": "https://github.com/ROBOTIS-GIT/OpenCR" + }, + "url": "https://github.com/ROBOTIS-GIT/OpenCR/releases/download/1.4.16/opencr.tar.bz2", + "archiveFileName": "opencr.tar.bz2", + "checksum": "SHA-256:0FF2503C7CBFB857B745273100E66C0F6E0A86417C761A223E9F90B33BF7EF33", + "size": "2592671", + "help": { + "online": "http://emanual.robotis.com/docs/en/parts/controller/opencr10/" + }, + "boards": [ + {"name": "OpenCR"} + ], + "toolsDependencies": [ + { + "packager": "OpenCR", + "name": "opencr_gcc", + "version": "5.4.0-2016q2" + }, + { + "packager": "OpenCR", + "name": "opencr_tools", + "version": "1.0.0" + } + ] }