From 944bf9073b2c99a7767bf88360b5e34c06692341 Mon Sep 17 00:00:00 2001 From: eastWillow Date: Sun, 24 Feb 2019 18:10:34 +0800 Subject: [PATCH 1/8] Update datatypes --- vesc_driver/include/vesc_driver/datatypes.h | 979 ++++++++++++++------ 1 file changed, 697 insertions(+), 282 deletions(-) diff --git a/vesc_driver/include/vesc_driver/datatypes.h b/vesc_driver/include/vesc_driver/datatypes.h index d71df93..c6f6f5e 100644 --- a/vesc_driver/include/vesc_driver/datatypes.h +++ b/vesc_driver/include/vesc_driver/datatypes.h @@ -1,12 +1,14 @@ /* - Copyright 2012-2014 Benjamin Vedder benjamin@vedder.se + Copyright 2016 - 2019 Benjamin Vedder benjamin@vedder.se - This program is free software: you can redistribute it and/or modify + This file is part of the VESC firmware. + + The VESC firmware is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. - This program is distributed in the hope that it will be useful, + The VESC firmware is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. @@ -20,6 +22,8 @@ * * Created on: 14 sep 2014 * Author: benjamin + * Update on: 29 Feb 2019 + * Author: benjamin */ #ifndef DATATYPES_H_ @@ -31,341 +35,752 @@ typedef uint32_t systime_t; // defined in ch.h // Data types -typedef enum { - MC_STATE_OFF = 0, - MC_STATE_DETECTING, - MC_STATE_RUNNING, - MC_STATE_FULL_BRAKE, +typedef enum +{ + MC_STATE_OFF = 0, + MC_STATE_DETECTING, + MC_STATE_RUNNING, + MC_STATE_FULL_BRAKE, } mc_state; -typedef enum { - PWM_MODE_NONSYNCHRONOUS_HISW = 0, // This mode is not recommended - PWM_MODE_SYNCHRONOUS, // The recommended and most tested mode - PWM_MODE_BIPOLAR // Some glitches occasionally, can kill MOSFETs +typedef enum +{ + PWM_MODE_NONSYNCHRONOUS_HISW = 0, // This mode is not recommended + PWM_MODE_SYNCHRONOUS, // The recommended and most tested mode + PWM_MODE_BIPOLAR // Some glitches occasionally, can kill MOSFETs } mc_pwm_mode; -typedef enum { - COMM_MODE_INTEGRATE = 0, - COMM_MODE_DELAY +typedef enum +{ + COMM_MODE_INTEGRATE = 0, + COMM_MODE_DELAY } mc_comm_mode; -typedef enum { - SENSOR_MODE_SENSORLESS = 0, - SENSOR_MODE_SENSORED, - SENSOR_MODE_HYBRID +typedef enum +{ + SENSOR_MODE_SENSORLESS = 0, + SENSOR_MODE_SENSORED, + SENSOR_MODE_HYBRID } mc_sensor_mode; -typedef enum { - MOTOR_TYPE_BLDC = 0, - MOTOR_TYPE_DC, +typedef enum +{ + FOC_SENSOR_MODE_SENSORLESS = 0, + FOC_SENSOR_MODE_ENCODER, + FOC_SENSOR_MODE_HALL +} mc_foc_sensor_mode; + +// Auxiliary output mode +typedef enum +{ + OUT_AUX_MODE_OFF = 0, + OUT_AUX_MODE_ON_AFTER_2S, + OUT_AUX_MODE_ON_AFTER_5S, + OUT_AUX_MODE_ON_AFTER_10S +} out_aux_mode; + +// General purpose drive output mode +typedef enum +{ + GPD_OUTPUT_MODE_NONE = 0, + GPD_OUTPUT_MODE_MODULATION, + GPD_OUTPUT_MODE_VOLTAGE, + GPD_OUTPUT_MODE_CURRENT +} gpd_output_mode; + +typedef enum +{ + MOTOR_TYPE_BLDC = 0, + MOTOR_TYPE_DC, + MOTOR_TYPE_FOC, + MOTOR_TYPE_GPD } mc_motor_type; -typedef enum { - FAULT_CODE_NONE = 0, - FAULT_CODE_OVER_VOLTAGE, - FAULT_CODE_UNDER_VOLTAGE, - FAULT_CODE_DRV8302, - FAULT_CODE_ABS_OVER_CURRENT, - FAULT_CODE_OVER_TEMP_FET, - FAULT_CODE_OVER_TEMP_MOTOR +typedef enum +{ + FAULT_CODE_NONE = 0, + FAULT_CODE_OVER_VOLTAGE, + FAULT_CODE_UNDER_VOLTAGE, + FAULT_CODE_DRV, + FAULT_CODE_ABS_OVER_CURRENT, + FAULT_CODE_OVER_TEMP_FET, + FAULT_CODE_OVER_TEMP_MOTOR, + FAULT_CODE_GATE_DRIVER_OVER_VOLTAGE, + FAULT_CODE_GATE_DRIVER_UNDER_VOLTAGE, + FAULT_CODE_MCU_UNDER_VOLTAGE, + FAULT_CODE_BOOTING_FROM_WATCHDOG_RESET } mc_fault_code; -typedef enum { - CONTROL_MODE_DUTY = 0, - CONTROL_MODE_SPEED, - CONTROL_MODE_CURRENT, - CONTROL_MODE_CURRENT_BRAKE, - CONTROL_MODE_POS, - CONTROL_MODE_NONE +typedef enum +{ + CONTROL_MODE_DUTY = 0, + CONTROL_MODE_SPEED, + CONTROL_MODE_CURRENT, + CONTROL_MODE_CURRENT_BRAKE, + CONTROL_MODE_POS, + CONTROL_MODE_HANDBRAKE, + CONTROL_MODE_OPENLOOP, + CONTROL_MODE_OPENLOOP_PHASE, + CONTROL_MODE_NONE } mc_control_mode; -typedef struct { - float cycle_int_limit; - float cycle_int_limit_running; - float cycle_int_limit_max; - float comm_time_sum; - float comm_time_sum_min_rpm; - int32_t comms; - uint32_t time_at_comm; +typedef enum +{ + DISP_POS_MODE_NONE = 0, + DISP_POS_MODE_INDUCTANCE, + DISP_POS_MODE_OBSERVER, + DISP_POS_MODE_ENCODER, + DISP_POS_MODE_PID_POS, + DISP_POS_MODE_PID_POS_ERROR, + DISP_POS_MODE_ENCODER_OBSERVER_ERROR +} disp_pos_mode; + +typedef enum +{ + SENSOR_PORT_MODE_HALL = 0, + SENSOR_PORT_MODE_ABI, + SENSOR_PORT_MODE_AS5047_SPI, + SENSOR_PORT_MODE_AD2S1205 +} sensor_port_mode; + +typedef struct +{ + float cycle_int_limit; + float cycle_int_limit_running; + float cycle_int_limit_max; + float comm_time_sum; + float comm_time_sum_min_rpm; + int32_t comms; + uint32_t time_at_comm; } mc_rpm_dep_struct; -typedef struct { - // Switching and drive - mc_pwm_mode pwm_mode; - mc_comm_mode comm_mode; - mc_motor_type motor_type; - mc_sensor_mode sensor_mode; - // Limits - float l_current_max; - float l_current_min; - float l_in_current_max; - float l_in_current_min; - float l_abs_current_max; - float l_min_erpm; - float l_max_erpm; - float l_max_erpm_fbrake; - float l_max_erpm_fbrake_cc; - float l_min_vin; - float l_max_vin; - bool l_slow_abs_current; - bool l_rpm_lim_neg_torque; - float l_temp_fet_start; - float l_temp_fet_end; - float l_temp_motor_start; - float l_temp_motor_end; - float l_min_duty; - float l_max_duty; - // Overridden limits (Computed during runtime) - float lo_current_max; - float lo_current_min; - float lo_in_current_max; - float lo_in_current_min; - // Sensorless - float sl_min_erpm; - float sl_min_erpm_cycle_int_limit; - float sl_max_fullbreak_current_dir_change; - float sl_cycle_int_limit; - float sl_phase_advance_at_br; - float sl_cycle_int_rpm_br; - float sl_bemf_coupling_k; - // Hall sensor - int8_t hall_table[8]; - float hall_sl_erpm; - // Speed PID - float s_pid_kp; - float s_pid_ki; - float s_pid_kd; - float s_pid_min_rpm; - // Pos PID - float p_pid_kp; - float p_pid_ki; - float p_pid_kd; - // Current controller - float cc_startup_boost_duty; - float cc_min_current; - float cc_gain; - float cc_ramp_step_max; - // Misc - int32_t m_fault_stop_time_ms; +typedef enum +{ + DRV8301_OC_LIMIT = 0, + DRV8301_OC_LATCH_SHUTDOWN, + DRV8301_OC_REPORT_ONLY, + DRV8301_OC_DISABLED +} drv8301_oc_mode; + +typedef enum +{ + DEBUG_SAMPLING_OFF = 0, + DEBUG_SAMPLING_NOW, + DEBUG_SAMPLING_START, + DEBUG_SAMPLING_TRIGGER_START, + DEBUG_SAMPLING_TRIGGER_FAULT, + DEBUG_SAMPLING_TRIGGER_START_NOSEND, + DEBUG_SAMPLING_TRIGGER_FAULT_NOSEND, + DEBUG_SAMPLING_SEND_LAST_SAMPLES +} debug_sampling_mode; + +typedef enum +{ + CAN_BAUD_125K = 0, + CAN_BAUD_250K, + CAN_BAUD_500K, + CAN_BAUD_1M +} CAN_BAUD; + +typedef enum +{ + BATTERY_TYPE_LIION_3_0__4_2, + BATTERY_TYPE_LIIRON_2_6__3_6, + BATTERY_TYPE_LEAD_ACID +} BATTERY_TYPE; + +typedef struct +{ + // Switching and drive + mc_pwm_mode pwm_mode; + mc_comm_mode comm_mode; + mc_motor_type motor_type; + mc_sensor_mode sensor_mode; + // Limits + float l_current_max; + float l_current_min; + float l_in_current_max; + float l_in_current_min; + float l_abs_current_max; + float l_min_erpm; + float l_max_erpm; + float l_erpm_start; + float l_max_erpm_fbrake; + float l_max_erpm_fbrake_cc; + float l_min_vin; + float l_max_vin; + float l_battery_cut_start; + float l_battery_cut_end; + bool l_slow_abs_current; + float l_temp_fet_start; + float l_temp_fet_end; + float l_temp_motor_start; + float l_temp_motor_end; + float l_temp_accel_dec; + float l_min_duty; + float l_max_duty; + float l_watt_max; + float l_watt_min; + float l_current_max_scale; + float l_current_min_scale; + // Overridden limits (Computed during runtime) + float lo_current_max; + float lo_current_min; + float lo_in_current_max; + float lo_in_current_min; + float lo_current_motor_max_now; + float lo_current_motor_min_now; + // Sensorless (bldc) + float sl_min_erpm; + float sl_min_erpm_cycle_int_limit; + float sl_max_fullbreak_current_dir_change; + float sl_cycle_int_limit; + float sl_phase_advance_at_br; + float sl_cycle_int_rpm_br; + float sl_bemf_coupling_k; + // Hall sensor + int8_t hall_table[8]; + float hall_sl_erpm; + // FOC + float foc_current_kp; + float foc_current_ki; + float foc_f_sw; + float foc_dt_us; + float foc_encoder_offset; + bool foc_encoder_inverted; + float foc_encoder_ratio; + float foc_motor_l; + float foc_motor_r; + float foc_motor_flux_linkage; + float foc_observer_gain; + float foc_observer_gain_slow; + float foc_pll_kp; + float foc_pll_ki; + float foc_duty_dowmramp_kp; + float foc_duty_dowmramp_ki; + float foc_openloop_rpm; + float foc_sl_openloop_hyst; + float foc_sl_openloop_time; + float foc_sl_d_current_duty; + float foc_sl_d_current_factor; + mc_foc_sensor_mode foc_sensor_mode; + uint8_t foc_hall_table[8]; + float foc_sl_erpm; + bool foc_sample_v0_v7; + bool foc_sample_high_current; + float foc_sat_comp; + bool foc_temp_comp; + float foc_temp_comp_base_temp; + float foc_current_filter_const; + // GPDrive + int gpd_buffer_notify_left; + int gpd_buffer_interpol; + float gpd_current_filter_const; + float gpd_current_kp; + float gpd_current_ki; + // Speed PID + float s_pid_kp; + float s_pid_ki; + float s_pid_kd; + float s_pid_kd_filter; + float s_pid_min_erpm; + bool s_pid_allow_braking; + // Pos PID + float p_pid_kp; + float p_pid_ki; + float p_pid_kd; + float p_pid_kd_filter; + float p_pid_ang_div; + // Current controller + float cc_startup_boost_duty; + float cc_min_current; + float cc_gain; + float cc_ramp_step_max; + // Misc + int32_t m_fault_stop_time_ms; + float m_duty_ramp_step; + float m_current_backoff_gain; + uint32_t m_encoder_counts; + sensor_port_mode m_sensor_port_mode; + bool m_invert_direction; + drv8301_oc_mode m_drv8301_oc_mode; + int m_drv8301_oc_adj; + float m_bldc_f_sw_min; + float m_bldc_f_sw_max; + float m_dc_f_sw; + float m_ntc_motor_beta; + out_aux_mode m_out_aux_mode; + // Setup info + uint8_t si_motor_poles; + float si_gear_ratio; + float si_wheel_diameter; + BATTERY_TYPE si_battery_type; + int si_battery_cells; + float si_battery_ah; } mc_configuration; // Applications to use -typedef enum { - APP_NONE = 0, - APP_PPM, - APP_ADC, - APP_UART, - APP_PPM_UART, - APP_ADC_UART, - APP_NUNCHUK, - APP_NRF, - APP_CUSTOM +typedef enum +{ + APP_NONE = 0, + APP_PPM, + APP_ADC, + APP_UART, + APP_PPM_UART, + APP_ADC_UART, + APP_NUNCHUK, + APP_NRF, + APP_CUSTOM } app_use; +// Throttle curve mode +typedef enum +{ + THR_EXP_EXPO = 0, + THR_EXP_NATURAL, + THR_EXP_POLY +} thr_exp_mode; + // PPM control types -typedef enum { - PPM_CTRL_TYPE_NONE = 0, - PPM_CTRL_TYPE_CURRENT, - PPM_CTRL_TYPE_CURRENT_NOREV, - PPM_CTRL_TYPE_CURRENT_NOREV_BRAKE, - PPM_CTRL_TYPE_DUTY, - PPM_CTRL_TYPE_DUTY_NOREV, - PPM_CTRL_TYPE_PID, - PPM_CTRL_TYPE_PID_NOREV +typedef enum +{ + PPM_CTRL_TYPE_NONE = 0, + PPM_CTRL_TYPE_CURRENT, + PPM_CTRL_TYPE_CURRENT_NOREV, + PPM_CTRL_TYPE_CURRENT_NOREV_BRAKE, + PPM_CTRL_TYPE_DUTY, + PPM_CTRL_TYPE_DUTY_NOREV, + PPM_CTRL_TYPE_PID, + PPM_CTRL_TYPE_PID_NOREV } ppm_control_type; -typedef struct { - ppm_control_type ctrl_type; - float pid_max_erpm; - float hyst; - float pulse_start; - float pulse_end; - bool median_filter; - bool safe_start; - float rpm_lim_start; - float rpm_lim_end; - bool multi_esc; - bool tc; - float tc_max_diff; +typedef struct +{ + ppm_control_type ctrl_type; + float pid_max_erpm; + float hyst; + float pulse_start; + float pulse_end; + float pulse_center; + bool median_filter; + bool safe_start; + float throttle_exp; + float throttle_exp_brake; + thr_exp_mode throttle_exp_mode; + float ramp_time_pos; + float ramp_time_neg; + bool multi_esc; + bool tc; + float tc_max_diff; } ppm_config; // ADC control types -typedef enum { - ADC_CTRL_TYPE_NONE = 0, - ADC_CTRL_TYPE_CURRENT, - ADC_CTRL_TYPE_CURRENT_REV_CENTER, - ADC_CTRL_TYPE_CURRENT_REV_BUTTON, - ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_CENTER, - ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON, - ADC_CTRL_TYPE_DUTY, - ADC_CTRL_TYPE_DUTY_REV_CENTER, - ADC_CTRL_TYPE_DUTY_REV_BUTTON +typedef enum +{ + ADC_CTRL_TYPE_NONE = 0, + ADC_CTRL_TYPE_CURRENT, + ADC_CTRL_TYPE_CURRENT_REV_CENTER, + ADC_CTRL_TYPE_CURRENT_REV_BUTTON, + ADC_CTRL_TYPE_CURRENT_REV_BUTTON_BRAKE_ADC, + ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_CENTER, + ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON, + ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_ADC, + ADC_CTRL_TYPE_DUTY, + ADC_CTRL_TYPE_DUTY_REV_CENTER, + ADC_CTRL_TYPE_DUTY_REV_BUTTON, + ADC_CTRL_TYPE_PID, + ADC_CTRL_TYPE_PID_REV_CENTER, + ADC_CTRL_TYPE_PID_REV_BUTTON } adc_control_type; -typedef struct { - adc_control_type ctrl_type; - float hyst; - float voltage_start; - float voltage_end; - bool use_filter; - bool safe_start; - bool button_inverted; - bool voltage_inverted; - float rpm_lim_start; - float rpm_lim_end; - bool multi_esc; - bool tc; - float tc_max_diff; - uint32_t update_rate_hz; +typedef struct +{ + adc_control_type ctrl_type; + float hyst; + float voltage_start; + float voltage_end; + float voltage_center; + float voltage2_start; + float voltage2_end; + bool use_filter; + bool safe_start; + bool cc_button_inverted; + bool rev_button_inverted; + bool voltage_inverted; + bool voltage2_inverted; + float throttle_exp; + float throttle_exp_brake; + thr_exp_mode throttle_exp_mode; + float ramp_time_pos; + float ramp_time_neg; + bool multi_esc; + bool tc; + float tc_max_diff; + uint32_t update_rate_hz; } adc_config; // Nunchuk control types -typedef enum { - CHUK_CTRL_TYPE_NONE = 0, - CHUK_CTRL_TYPE_CURRENT, - CHUK_CTRL_TYPE_CURRENT_NOREV +typedef enum +{ + CHUK_CTRL_TYPE_NONE = 0, + CHUK_CTRL_TYPE_CURRENT, + CHUK_CTRL_TYPE_CURRENT_NOREV } chuk_control_type; -typedef struct { - chuk_control_type ctrl_type; - float hyst; - float rpm_lim_start; - float rpm_lim_end; - float ramp_time_pos; - float ramp_time_neg; - bool multi_esc; - bool tc; - float tc_max_diff; +typedef struct +{ + chuk_control_type ctrl_type; + float hyst; + float rpm_lim_start; + float rpm_lim_end; + float ramp_time_pos; + float ramp_time_neg; + bool multi_esc; + bool tc; + float tc_max_diff; } chuk_config; -typedef struct { - // Settings - uint8_t controller_id; - uint32_t timeout_msec; - float timeout_brake_current; - bool send_can_status; - uint32_t send_can_status_rate_hz; - - // Application to use - app_use app_to_use; - - // PPM application settings - ppm_config app_ppm_conf; - - // ADC application settings - adc_config app_adc_conf; - - // UART application settings - uint32_t app_uart_baudrate; - - // Nunchuk application settings - chuk_config app_chuk_conf; +// NRF Datatypes +typedef enum +{ + NRF_SPEED_250K = 0, + NRF_SPEED_1M, + NRF_SPEED_2M +} NRF_SPEED; + +typedef enum +{ + NRF_POWER_M18DBM = 0, + NRF_POWER_M12DBM, + NRF_POWER_M6DBM, + NRF_POWER_0DBM, + NRF_POWER_OFF +} NRF_POWER; + +typedef enum +{ + NRF_AW_3 = 0, + NRF_AW_4, + NRF_AW_5 +} NRF_AW; + +typedef enum +{ + NRF_CRC_DISABLED = 0, + NRF_CRC_1B, + NRF_CRC_2B +} NRF_CRC; + +typedef enum +{ + NRF_RETR_DELAY_250US = 0, + NRF_RETR_DELAY_500US, + NRF_RETR_DELAY_750US, + NRF_RETR_DELAY_1000US, + NRF_RETR_DELAY_1250US, + NRF_RETR_DELAY_1500US, + NRF_RETR_DELAY_1750US, + NRF_RETR_DELAY_2000US, + NRF_RETR_DELAY_2250US, + NRF_RETR_DELAY_2500US, + NRF_RETR_DELAY_2750US, + NRF_RETR_DELAY_3000US, + NRF_RETR_DELAY_3250US, + NRF_RETR_DELAY_3500US, + NRF_RETR_DELAY_3750US, + NRF_RETR_DELAY_4000US +} NRF_RETR_DELAY; + +typedef struct +{ + NRF_SPEED speed; + NRF_POWER power; + NRF_CRC crc_type; + NRF_RETR_DELAY retry_delay; + unsigned char retries; + unsigned char channel; + unsigned char address[3]; + bool send_crc_ack; +} nrf_config; + +// CAN status modes +typedef enum +{ + CAN_STATUS_DISABLED = 0, + CAN_STATUS_1, + CAN_STATUS_1_2, + CAN_STATUS_1_2_3, + CAN_STATUS_1_2_3_4 +} CAN_STATUS_MODE; + +typedef struct +{ + // Settings + uint8_t controller_id; + uint32_t timeout_msec; + float timeout_brake_current; + CAN_STATUS_MODE send_can_status; + uint32_t send_can_status_rate_hz; + CAN_BAUD can_baud_rate; + bool pairing_done; + + // UAVCAN + bool uavcan_enable; + uint8_t uavcan_esc_index; + + // Application to use + app_use app_to_use; + + // PPM application settings + ppm_config app_ppm_conf; + + // ADC application settings + adc_config app_adc_conf; + + // UART application settings + uint32_t app_uart_baudrate; + + // Nunchuk application settings + chuk_config app_chuk_conf; + + // NRF application settings + nrf_config app_nrf_conf; } app_configuration; // Communication commands -typedef enum { - COMM_FW_VERSION = 0, - COMM_JUMP_TO_BOOTLOADER, - COMM_ERASE_NEW_APP, - COMM_WRITE_NEW_APP_DATA, - COMM_GET_VALUES, - COMM_SET_DUTY, - COMM_SET_CURRENT, - COMM_SET_CURRENT_BRAKE, - COMM_SET_RPM, - COMM_SET_POS, - COMM_SET_DETECT, - COMM_SET_SERVO_POS, - COMM_SET_MCCONF, - COMM_GET_MCCONF, - COMM_SET_APPCONF, - COMM_GET_APPCONF, - COMM_SAMPLE_PRINT, - COMM_TERMINAL_CMD, - COMM_PRINT, - COMM_ROTOR_POSITION, - COMM_EXPERIMENT_SAMPLE, - COMM_DETECT_MOTOR_PARAM, - COMM_REBOOT, - COMM_ALIVE, - COMM_GET_DECODED_PPM, - COMM_GET_DECODED_ADC, - COMM_GET_DECODED_CHUK, - COMM_FORWARD_CAN +typedef enum +{ + COMM_FW_VERSION = 0, + COMM_JUMP_TO_BOOTLOADER, + COMM_ERASE_NEW_APP, + COMM_WRITE_NEW_APP_DATA, + COMM_GET_VALUES, + COMM_SET_DUTY, + COMM_SET_CURRENT, + COMM_SET_CURRENT_BRAKE, + COMM_SET_RPM, + COMM_SET_POS, + COMM_SET_HANDBRAKE, + COMM_SET_DETECT, + COMM_SET_SERVO_POS, + COMM_SET_MCCONF, + COMM_GET_MCCONF, + COMM_GET_MCCONF_DEFAULT, + COMM_SET_APPCONF, + COMM_GET_APPCONF, + COMM_GET_APPCONF_DEFAULT, + COMM_SAMPLE_PRINT, + COMM_TERMINAL_CMD, + COMM_PRINT, + COMM_ROTOR_POSITION, + COMM_EXPERIMENT_SAMPLE, + COMM_DETECT_MOTOR_PARAM, + COMM_DETECT_MOTOR_R_L, + COMM_DETECT_MOTOR_FLUX_LINKAGE, + COMM_DETECT_ENCODER, + COMM_DETECT_HALL_FOC, + COMM_REBOOT, + COMM_ALIVE, + COMM_GET_DECODED_PPM, + COMM_GET_DECODED_ADC, + COMM_GET_DECODED_CHUK, + COMM_FORWARD_CAN, + COMM_SET_CHUCK_DATA, + COMM_CUSTOM_APP_DATA, + COMM_NRF_START_PAIRING, + COMM_GPD_SET_FSW, + COMM_GPD_BUFFER_NOTIFY, + COMM_GPD_BUFFER_SIZE_LEFT, + COMM_GPD_FILL_BUFFER, + COMM_GPD_OUTPUT_SAMPLE, + COMM_GPD_SET_MODE, + COMM_GPD_FILL_BUFFER_INT8, + COMM_GPD_FILL_BUFFER_INT16, + COMM_GPD_SET_BUFFER_INT_SCALE, + COMM_GET_VALUES_SETUP, + COMM_SET_MCCONF_TEMP, + COMM_SET_MCCONF_TEMP_SETUP, + COMM_GET_VALUES_SELECTIVE, + COMM_GET_VALUES_SETUP_SELECTIVE, + COMM_EXT_NRF_PRESENT, + COMM_EXT_NRF_ESB_SET_CH_ADDR, + COMM_EXT_NRF_ESB_SEND_DATA, + COMM_EXT_NRF_ESB_RX_DATA, + COMM_EXT_NRF_SET_ENABLED, + COMM_DETECT_MOTOR_FLUX_LINKAGE_OPENLOOP, + COMM_DETECT_APPLY_ALL_FOC, + COMM_JUMP_TO_BOOTLOADER_ALL_CAN, + COMM_ERASE_NEW_APP_ALL_CAN, + COMM_WRITE_NEW_APP_DATA_ALL_CAN, + COMM_PING_CAN, + COMM_APP_DISABLE_OUTPUT } COMM_PACKET_ID; // CAN commands -typedef enum { - CAN_PACKET_SET_DUTY = 0, - CAN_PACKET_SET_CURRENT, - CAN_PACKET_SET_CURRENT_BRAKE, - CAN_PACKET_SET_RPM, - CAN_PACKET_SET_POS, - CAN_PACKET_FILL_RX_BUFFER, - CAN_PACKET_FILL_RX_BUFFER_LONG, - CAN_PACKET_PROCESS_RX_BUFFER, - CAN_PACKET_PROCESS_SHORT_BUFFER, - CAN_PACKET_STATUS +typedef enum +{ + CAN_PACKET_SET_DUTY = 0, + CAN_PACKET_SET_CURRENT, + CAN_PACKET_SET_CURRENT_BRAKE, + CAN_PACKET_SET_RPM, + CAN_PACKET_SET_POS, + CAN_PACKET_FILL_RX_BUFFER, + CAN_PACKET_FILL_RX_BUFFER_LONG, + CAN_PACKET_PROCESS_RX_BUFFER, + CAN_PACKET_PROCESS_SHORT_BUFFER, + CAN_PACKET_STATUS, + CAN_PACKET_SET_CURRENT_REL, + CAN_PACKET_SET_CURRENT_BRAKE_REL, + CAN_PACKET_SET_CURRENT_HANDBRAKE, + CAN_PACKET_SET_CURRENT_HANDBRAKE_REL, + CAN_PACKET_STATUS_2, + CAN_PACKET_STATUS_3, + CAN_PACKET_STATUS_4, + CAN_PACKET_PING, + CAN_PACKET_PONG, + CAN_PACKET_DETECT_APPLY_ALL_FOC, + CAN_PACKET_DETECT_APPLY_ALL_FOC_RES, + CAN_PACKET_CONF_CURRENT_LIMITS, + CAN_PACKET_CONF_STORE_CURRENT_LIMITS, + CAN_PACKET_CONF_CURRENT_LIMITS_IN, + CAN_PACKET_CONF_STORE_CURRENT_LIMITS_IN, + CAN_PACKET_CONF_FOC_ERPMS, + CAN_PACKET_CONF_STORE_FOC_ERPMS } CAN_PACKET_ID; // Logged fault data -typedef struct { - mc_fault_code fault; - float current; - float current_filtered; - float voltage; - float duty; - float rpm; - int tacho; - int tim_pwm_cnt; - int tim_samp_cnt; - int comm_step; - float temperature; +typedef struct +{ + mc_fault_code fault; + float current; + float current_filtered; + float voltage; + float gate_driver_voltage; + float duty; + float rpm; + int tacho; + int cycles_running; + int tim_val_samp; + int tim_current_samp; + int tim_top; + int comm_step; + float temperature; + int drv8301_faults; } fault_data; // External LED state -typedef enum { - LED_EXT_OFF = 0, - LED_EXT_NORMAL, - LED_EXT_BRAKE, - LED_EXT_TURN_LEFT, - LED_EXT_TURN_RIGHT, - LED_EXT_BRAKE_TURN_LEFT, - LED_EXT_BRAKE_TURN_RIGHT, - LED_EXT_BATT +typedef enum +{ + LED_EXT_OFF = 0, + LED_EXT_NORMAL, + LED_EXT_BRAKE, + LED_EXT_TURN_LEFT, + LED_EXT_TURN_RIGHT, + LED_EXT_BRAKE_TURN_LEFT, + LED_EXT_BRAKE_TURN_RIGHT, + LED_EXT_BATT } LED_EXT_STATE; -typedef struct { - int js_x; - int js_y; - int acc_x; - int acc_y; - int acc_z; - bool bt_c; - bool bt_z; +typedef struct +{ + int js_x; + int js_y; + int acc_x; + int acc_y; + int acc_z; + bool bt_c; + bool bt_z; + bool rev_has_state; + bool is_rev; } chuck_data; -typedef struct { - int id; - systime_t rx_time; - float rpm; - float current; - float duty; +typedef struct +{ + int id; + systime_t rx_time; + float rpm; + float current; + float duty; } can_status_msg; -typedef struct { - uint8_t js_x; - uint8_t js_y; - bool bt_c; - bool bt_z; - bool bt_push; - float vbat; +typedef struct +{ + int id; + systime_t rx_time; + float amp_hours; + float amp_hours_charged; +} can_status_msg_2; + +typedef struct +{ + int id; + systime_t rx_time; + float watt_hours; + float watt_hours_charged; +} can_status_msg_3; + +typedef struct +{ + int id; + systime_t rx_time; + float temp_fet; + float temp_motor; + float current_in; + float pid_pos_now; +} can_status_msg_4; + +typedef struct +{ + uint8_t js_x; + uint8_t js_y; + bool bt_c; + bool bt_z; + bool bt_push; + bool rev_has_state; + bool is_rev; + float vbat; } mote_state; -typedef enum { - MOTE_PACKET_BATT_LEVEL = 0, - MOTE_PACKET_BUTTONS, - MOTE_PACKET_ALIVE +typedef enum +{ + MOTE_PACKET_BATT_LEVEL = 0, + MOTE_PACKET_BUTTONS, + MOTE_PACKET_ALIVE, + MOTE_PACKET_FILL_RX_BUFFER, + MOTE_PACKET_FILL_RX_BUFFER_LONG, + MOTE_PACKET_PROCESS_RX_BUFFER, + MOTE_PACKET_PROCESS_SHORT_BUFFER, + MOTE_PACKET_PAIRING_INFO } MOTE_PACKET; +typedef struct +{ + float v_in; + float temp_mos1; + float temp_mos2; + float temp_mos3; + float temp_mos4; + float temp_mos5; + float temp_mos6; + float temp_pcb; + float current_motor; + float current_in; + float rpm; + float duty_now; + float amp_hours; + float amp_hours_charged; + float watt_hours; + float watt_hours_charged; + int tachometer; + int tachometer_abs; + mc_fault_code fault_code; +} mc_values; + +typedef enum +{ + NRF_PAIR_STARTED = 0, + NRF_PAIR_OK, + NRF_PAIR_FAIL +} NRF_PAIR_RES; + #endif /* DATATYPES_H_ */ From f82a174e3ae5ddc5f4c580e6d28fef88868937ca Mon Sep 17 00:00:00 2001 From: eastWillow Date: Sun, 24 Feb 2019 18:52:44 +0800 Subject: [PATCH 2/8] add Handbrake packet --- .../include/vesc_driver/vesc_interface.h | 1 + vesc_driver/include/vesc_driver/vesc_packet.h | 8 ++++++ vesc_driver/src/vesc_interface.cpp | 6 ++++ vesc_driver/src/vesc_packet.cpp | 28 ++++++++++++++++++- 4 files changed, 42 insertions(+), 1 deletion(-) diff --git a/vesc_driver/include/vesc_driver/vesc_interface.h b/vesc_driver/include/vesc_driver/vesc_interface.h index 3bc50d0..5506fea 100644 --- a/vesc_driver/include/vesc_driver/vesc_interface.h +++ b/vesc_driver/include/vesc_driver/vesc_interface.h @@ -91,6 +91,7 @@ class VescInterface : private boost::noncopyable void setSpeed(double speed); void setPosition(double position); void setServo(double servo); + void setHandbrake(double current); private: // Pimpl - hide serial port members from class users diff --git a/vesc_driver/include/vesc_driver/vesc_packet.h b/vesc_driver/include/vesc_driver/vesc_packet.h index 630634e..30aabdc 100644 --- a/vesc_driver/include/vesc_driver/vesc_packet.h +++ b/vesc_driver/include/vesc_driver/vesc_packet.h @@ -189,6 +189,14 @@ class VescPacketSetServoPos : public VescPacket // double servo_pos() const; }; +class VescPacketSetHandbrake : public VescPacket +{ +public: + VescPacketSetHandbrake(double current); + + // double handbrake() const; +}; + } // namespace vesc_driver #endif // VESC_DRIVER_VESC_PACKET_H_ diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index 84a2cf2..0273755 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -241,4 +241,10 @@ void VescInterface::setServo(double servo) send(VescPacketSetServoPos(servo)); } +void VescInterface::setHandbrake(double current) +{ + send(VescPacketSetHandbrake(current)); +} + + } // namespace vesc_driver diff --git a/vesc_driver/src/vesc_packet.cpp b/vesc_driver/src/vesc_packet.cpp index 80328a7..2e4d767 100644 --- a/vesc_driver/src/vesc_packet.cpp +++ b/vesc_driver/src/vesc_packet.cpp @@ -26,7 +26,7 @@ VescFrame::VescFrame(int payload_size) *(frame_->begin() + 1) = payload_size; payload_.first = frame_->begin() + 2; } - else { + else if(payload_size <= 65535){ // two byte payload size frame_.reset(new Buffer(VESC_MIN_FRAME_SIZE + 1 + payload_size)); *frame_->begin() = 3; @@ -34,6 +34,15 @@ VescFrame::VescFrame(int payload_size) *(frame_->begin() + 2) = payload_size & 0xFF; payload_.first = frame_->begin() + 3; } + else { + // many byte payload size + frame_.reset(new Buffer(VESC_MIN_FRAME_SIZE + 2 + payload_size)); + *frame_->begin() = 4; + *(frame_->begin() + 1) = ((payload_size >> 16) & 0xFF); + *(frame_->begin() + 2) = ((payload_size >> 8) & 0xFF); + *(frame_->begin() + 3) = payload_size & 0xFF; + payload_.first = frame_->begin() + 4; + } payload_.second = payload_.first + payload_size; *(frame_->end() - 1) = 3; @@ -365,4 +374,21 @@ VescPacketSetServoPos::VescPacketSetServoPos(double servo_pos) : *(frame_->end() - 2) = static_cast(crc & 0xFF); } +VescPacketSetHandbrake::VescPacketSetHandbrake(double current) : + VescPacket("SetHandbrake", 5, COMM_SET_HANDBRAKE) +{ + int32_t v = static_cast(current * 1000.0); + + *(payload_.first + 1) = static_cast((static_cast(v) >> 24) & 0xFF); + *(payload_.first + 2) = static_cast((static_cast(v) >> 16) & 0xFF); + *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); + *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); + + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); + uint16_t crc = crc_calc.checksum(); + *(frame_->end() - 3) = static_cast(crc >> 8); + *(frame_->end() - 2) = static_cast(crc & 0xFF); +} + } // namespace vesc_driver From c75e222ec2105df162b5124f341521b2a5049b89 Mon Sep 17 00:00:00 2001 From: eastWillow Date: Sun, 24 Feb 2019 19:06:22 +0800 Subject: [PATCH 3/8] add handbrake to vesc driver Done --- vesc_driver/include/vesc_driver/vesc_driver.h | 3 +++ .../include/vesc_driver/vesc_interface.h | 2 +- vesc_driver/include/vesc_driver/vesc_packet.h | 2 +- vesc_driver/src/vesc_driver.cpp | 17 ++++++++++++++++- vesc_driver/src/vesc_interface.cpp | 4 ++-- vesc_driver/src/vesc_packet.cpp | 4 ++-- 6 files changed, 25 insertions(+), 7 deletions(-) diff --git a/vesc_driver/include/vesc_driver/vesc_driver.h b/vesc_driver/include/vesc_driver/vesc_driver.h index 005c0be..8874fae 100644 --- a/vesc_driver/include/vesc_driver/vesc_driver.h +++ b/vesc_driver/include/vesc_driver/vesc_driver.h @@ -45,6 +45,7 @@ class VescDriver CommandLimit speed_limit_; CommandLimit position_limit_; CommandLimit servo_limit_; + CommandLimit handbrake_limit_; // ROS services ros::Publisher state_pub_; @@ -55,6 +56,7 @@ class VescDriver ros::Subscriber speed_sub_; ros::Subscriber position_sub_; ros::Subscriber servo_sub_; + ros::Subscriber handbrake_sub_; ros::Timer timer_; // driver modes (possible states) @@ -76,6 +78,7 @@ class VescDriver void speedCallback(const std_msgs::Float64::ConstPtr& speed); void positionCallback(const std_msgs::Float64::ConstPtr& position); void servoCallback(const std_msgs::Float64::ConstPtr& servo); + void handbrakeCallback(const std_msgs::Float64::ConstPtr& handbrake); }; } // namespace vesc_driver diff --git a/vesc_driver/include/vesc_driver/vesc_interface.h b/vesc_driver/include/vesc_driver/vesc_interface.h index 5506fea..d14aff6 100644 --- a/vesc_driver/include/vesc_driver/vesc_interface.h +++ b/vesc_driver/include/vesc_driver/vesc_interface.h @@ -91,7 +91,7 @@ class VescInterface : private boost::noncopyable void setSpeed(double speed); void setPosition(double position); void setServo(double servo); - void setHandbrake(double current); + void setHandbrake(double handbrake); private: // Pimpl - hide serial port members from class users diff --git a/vesc_driver/include/vesc_driver/vesc_packet.h b/vesc_driver/include/vesc_driver/vesc_packet.h index 30aabdc..c42b058 100644 --- a/vesc_driver/include/vesc_driver/vesc_packet.h +++ b/vesc_driver/include/vesc_driver/vesc_packet.h @@ -192,7 +192,7 @@ class VescPacketSetServoPos : public VescPacket class VescPacketSetHandbrake : public VescPacket { public: - VescPacketSetHandbrake(double current); + VescPacketSetHandbrake(double handbrake); // double handbrake() const; }; diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index 3c4788a..c89cf79 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -20,7 +20,8 @@ VescDriver::VescDriver(ros::NodeHandle nh, duty_cycle_limit_(private_nh, "duty_cycle", -1.0, 1.0), current_limit_(private_nh, "current"), brake_limit_(private_nh, "brake"), speed_limit_(private_nh, "speed"), position_limit_(private_nh, "position"), servo_limit_(private_nh, "servo", 0.0, 1.0), - driver_mode_(MODE_INITIALIZING), fw_version_major_(-1), fw_version_minor_(-1) + driver_mode_(MODE_INITIALIZING), fw_version_major_(-1), fw_version_minor_(-1), + handbrake_limit_(private_nh, "handbrake") { // get vesc serial port address std::string port; @@ -55,6 +56,8 @@ VescDriver::VescDriver(ros::NodeHandle nh, speed_sub_ = nh.subscribe("commands/motor/speed", 10, &VescDriver::speedCallback, this); position_sub_ = nh.subscribe("commands/motor/position", 10, &VescDriver::positionCallback, this); servo_sub_ = nh.subscribe("commands/servo/position", 10, &VescDriver::servoCallback, this); + handbrake_sub_ = nh.subscribe("commands/motor/handbrake", 10, + &VescDriver::handbrakeCallback, this); // create a 50Hz timer, used for state machine & polling VESC telemetry timer_ = nh.createTimer(ros::Duration(1.0/50.0), &VescDriver::timerCallback, this); @@ -222,6 +225,18 @@ void VescDriver::servoCallback(const std_msgs::Float64::ConstPtr& servo) } } +/** + * @param handbrake Commanded VESC braking current in Amps. Any value is accepted by this driver. + * However, it is more current than brake + */ + +void VescDriver::handbrakeCallback(const std_msgs::Float64::ConstPtr& handbrake) +{ + if (driver_mode_ = MODE_OPERATING) { + vesc_.setHandbrake(handbrake_limit_.clip(handbrake->data)); + } +} + VescDriver::CommandLimit::CommandLimit(const ros::NodeHandle& nh, const std::string& str, const boost::optional& min_lower, const boost::optional& max_upper) : diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index 0273755..bda9b6c 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -241,9 +241,9 @@ void VescInterface::setServo(double servo) send(VescPacketSetServoPos(servo)); } -void VescInterface::setHandbrake(double current) +void VescInterface::setHandbrake(double handbrake) { - send(VescPacketSetHandbrake(current)); + send(VescPacketSetHandbrake(handbrake)); } diff --git a/vesc_driver/src/vesc_packet.cpp b/vesc_driver/src/vesc_packet.cpp index 2e4d767..c37c5e0 100644 --- a/vesc_driver/src/vesc_packet.cpp +++ b/vesc_driver/src/vesc_packet.cpp @@ -374,10 +374,10 @@ VescPacketSetServoPos::VescPacketSetServoPos(double servo_pos) : *(frame_->end() - 2) = static_cast(crc & 0xFF); } -VescPacketSetHandbrake::VescPacketSetHandbrake(double current) : +VescPacketSetHandbrake::VescPacketSetHandbrake(double handbrake) : VescPacket("SetHandbrake", 5, COMM_SET_HANDBRAKE) { - int32_t v = static_cast(current * 1000.0); + int32_t v = static_cast(handbrake * 1000.0); *(payload_.first + 1) = static_cast((static_cast(v) >> 24) & 0xFF); *(payload_.first + 2) = static_cast((static_cast(v) >> 16) & 0xFF); From 7f141ba6ba6ff347fd003d07bd03ba2ada0ac997 Mon Sep 17 00:00:00 2001 From: eastWillow Date: Sun, 24 Feb 2019 19:19:59 +0800 Subject: [PATCH 4/8] Create README.md --- README.md | 3 +++ 1 file changed, 3 insertions(+) create mode 100644 README.md diff --git a/README.md b/README.md new file mode 100644 index 0000000..ef9d5c7 --- /dev/null +++ b/README.md @@ -0,0 +1,3 @@ +# vesc + +## Add the Fw3.40 function Handbrake From 6eec4120f61d378ae8821e781ac8c4383972bdf9 Mon Sep 17 00:00:00 2001 From: eastWillow Date: Wed, 27 Feb 2019 14:51:48 +0800 Subject: [PATCH 5/8] ready the find the tachometer in data package --- vesc_driver/src/vesc_driver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index c89cf79..f61fb3a 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -112,7 +112,7 @@ void VescDriver::timerCallback(const ros::TimerEvent& event) void VescDriver::vescPacketCallback(const boost::shared_ptr& packet) { - if (packet->name() == "Values") { + if (packet->name() == "Values") { // this is some error boost::shared_ptr values = boost::dynamic_pointer_cast(packet); From 7836c6ceacc4d59e0690790a79fa434249eeaba5 Mon Sep 17 00:00:00 2001 From: eastWillow Date: Tue, 5 Mar 2019 00:12:54 +0800 Subject: [PATCH 6/8] vesc driver sensor core tachometer add --- vesc_driver/include/vesc_driver/vesc_packet.h | 10 +++ vesc_driver/src/vesc_driver.cpp | 24 +++--- vesc_driver/src/vesc_interface.cpp | 8 +- vesc_driver/src/vesc_packet.cpp | 74 ++++++++++++++++++- vesc_msgs/msg/VescState.msg | 35 +++++---- 5 files changed, 119 insertions(+), 32 deletions(-) diff --git a/vesc_driver/include/vesc_driver/vesc_packet.h b/vesc_driver/include/vesc_driver/vesc_packet.h index c42b058..4122cc4 100644 --- a/vesc_driver/include/vesc_driver/vesc_packet.h +++ b/vesc_driver/include/vesc_driver/vesc_packet.h @@ -101,6 +101,7 @@ class VescPacketValues : public VescPacket public: VescPacketValues(boost::shared_ptr raw); + /* double v_in() const; double temp_mos1() const; double temp_mos2() const; @@ -119,6 +120,15 @@ class VescPacketValues : public VescPacket double watt_hours_charged() const; double tachometer() const; double tachometer_abs() const; + */ + double rpm() const; + double avg_motor_current() const; + double GET_INPUT_VOLTAGE() const; + double avg_input_current() const; + double duty_cycle_now() const; + double temp_fet() const; + double temp_motor() const; + double tachometer() const; int fault_code() const; }; diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index f61fb3a..2068798 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -109,27 +109,27 @@ void VescDriver::timerCallback(const ros::TimerEvent& event) assert(false && "unknown driver mode"); } } +/* +notes: roslaunch vesc_driver vesc_driver_node.launch port:=/dev/ttyACM0 +*/ void VescDriver::vescPacketCallback(const boost::shared_ptr& packet) { if (packet->name() == "Values") { // this is some error boost::shared_ptr values = - boost::dynamic_pointer_cast(packet); + boost::dynamic_pointer_cast(packet); vesc_msgs::VescStateStamped::Ptr state_msg(new vesc_msgs::VescStateStamped); state_msg->header.stamp = ros::Time::now(); - state_msg->state.voltage_input = values->v_in(); - state_msg->state.temperature_pcb = values->temp_pcb(); - state_msg->state.current_motor = values->current_motor(); - state_msg->state.current_input = values->current_in(); + state_msg->state.temp_fet = values->temp_fet(); + state_msg->state.temp_motor = values->temp_motor(); + state_msg->state.avg_motor_current = values->avg_motor_current(); + state_msg->state.avg_motor_current = values->avg_input_current(); + state_msg->state.duty_cycle_now = values->duty_cycle_now(); state_msg->state.speed = values->rpm(); - state_msg->state.duty_cycle = values->duty_now(); - state_msg->state.charge_drawn = values->amp_hours(); - state_msg->state.charge_regen = values->amp_hours_charged(); - state_msg->state.energy_drawn = values->watt_hours(); - state_msg->state.energy_regen = values->watt_hours_charged(); - state_msg->state.displacement = values->tachometer(); - state_msg->state.distance_traveled = values->tachometer_abs(); + state_msg->state.tachometer = values->tachometer(); + state_msg->state.tachometer_abs = 0; + state_msg->state.INPUT_VOLTAGE = values->GET_INPUT_VOLTAGE(); state_msg->state.fault_code = values->fault_code(); state_pub_.publish(state_msg); diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index bda9b6c..59a69b4 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -29,7 +29,7 @@ class VescInterface::Impl static void* rxThreadHelper(void *context) { - return ((VescInterface::Impl*)context)->rxThread(); + return ((VescInterface::Impl*)context)->rxThread(); // 所以又指回到自己這個object 為了節省記憶體加上為了自己使用 thread 去等待 } pthread_t rx_thread_; @@ -40,7 +40,7 @@ class VescInterface::Impl VescFrame::CRC send_crc_; }; -void* VescInterface::Impl::rxThread(void) +void* VescInterface::Impl::rxThread(void) // packet recive { Buffer buffer; buffer.reserve(4096); @@ -120,7 +120,7 @@ void* VescInterface::Impl::rxThread(void) VescInterface::VescInterface(const std::string& port, const PacketHandlerFunction& packet_handler, const ErrorHandlerFunction& error_handler) : - impl_(new Impl()) + impl_(new Impl()) //boost::scoped_ptr impl_ { setPacketHandler(packet_handler); setErrorHandler(error_handler); @@ -168,7 +168,7 @@ void VescInterface::connect(const std::string& port) // start up a monitoring thread impl_->rx_thread_run_ = true; int result = - pthread_create(&impl_->rx_thread_, NULL, &VescInterface::Impl::rxThreadHelper, impl_.get()); + pthread_create(&impl_->rx_thread_, NULL, &VescInterface::Impl::rxThreadHelper, impl_.get()); //impl_.get -> 返回所存儲的指針 assert(0 == result); } diff --git a/vesc_driver/src/vesc_packet.cpp b/vesc_driver/src/vesc_packet.cpp index c37c5e0..71e3b8a 100644 --- a/vesc_driver/src/vesc_packet.cpp +++ b/vesc_driver/src/vesc_packet.cpp @@ -1,5 +1,5 @@ // -*- mode:c++; fill-column: 100; -*- - +#include #include "vesc_driver/vesc_packet.h" #include @@ -106,13 +106,79 @@ VescPacketRequestFWVersion::VescPacketRequestFWVersion() : } /*------------------------------------------------------------------------------------------------*/ +/* +please following the vesc firmware:bldc +commands.c case in COMM_GET_VALUES section program +*/ VescPacketValues::VescPacketValues(boost::shared_ptr raw) : VescPacket("Values", raw) { } -double VescPacketValues::temp_mos1() const +double VescPacketValues::temp_fet() const // mc_interface_temp_fet_filtered +{ + int16_t v = static_cast((static_cast(*(payload_.first + 1)) << 8) + + static_cast(*(payload_.first + 2))); + return static_cast(v) / 1e1; +} +double VescPacketValues::temp_motor() const // mc_interface_temp_fet_filtered +{ + int16_t v = static_cast((static_cast(*(payload_.first + 3)) << 8) + + static_cast(*(payload_.first + 4))); + return static_cast(v) / 1e1; +} +double VescPacketValues::avg_motor_current() const +{ + int32_t v = static_cast((static_cast(*(payload_.first + 5)) << 24) + + (static_cast(*(payload_.first + 6)) << 16) + + (static_cast(*(payload_.first + 7)) << 8) + + static_cast(*(payload_.first + 8))); + return static_cast(v) / 1e2; +} +double VescPacketValues::avg_input_current() const +{ + int32_t v = static_cast((static_cast(*(payload_.first + 9)) << 24) + + (static_cast(*(payload_.first + 10)) << 16) + + (static_cast(*(payload_.first + 11)) << 8) + + static_cast(*(payload_.first + 12))); + return static_cast(v) / 1e2; +} +double VescPacketValues::duty_cycle_now() const +{ + int16_t v = static_cast((static_cast(*(payload_.first + 21)) << 8) + + static_cast(*(payload_.first + 22))); + return static_cast(v) / 1e3; +} +double VescPacketValues::rpm() const +{ + int64_t v = static_cast((static_cast(*(payload_.first + 23)) << 24) + + (static_cast(*(payload_.first + 24)) << 16) + + (static_cast(*(payload_.first + 25)) << 8) + + static_cast(*(payload_.first + 26))); + return static_cast(v) / 1e0; +} +double VescPacketValues::GET_INPUT_VOLTAGE() const +{ + int16_t v = static_cast((static_cast(*(payload_.first + 27)) << 8) + + static_cast(*(payload_.first + 28))); + return static_cast(v) / 1e1; +} +double VescPacketValues::tachometer() const +{ + int64_t v = static_cast((static_cast(*(payload_.first + 45)) << 24) + + (static_cast(*(payload_.first + 46)) << 16) + + (static_cast(*(payload_.first + 47)) << 8) + + static_cast(*(payload_.first + 48))); + return static_cast(v) / 1e0; +} +int VescPacketValues::fault_code() const +{ + return static_cast(*(payload_.first + 53)); +} + +/* +double VescPacketValues::temp_mos1() const // mc_interface_temp_fet_filtered { int16_t v = static_cast((static_cast(*(payload_.first + 1)) << 8) + static_cast(*(payload_.first + 2))); @@ -242,10 +308,12 @@ int VescPacketValues::fault_code() const { return static_cast(*(payload_.first + 55)); } +*/ + REGISTER_PACKET_TYPE(COMM_GET_VALUES, VescPacketValues) -VescPacketRequestValues::VescPacketRequestValues() : +VescPacketRequestValues::VescPacketRequestValues() : // i can't understand VescPacket("RequestFWVersion", 1, COMM_GET_VALUES) { VescFrame::CRC crc_calc; diff --git a/vesc_msgs/msg/VescState.msg b/vesc_msgs/msg/VescState.msg index 276d62a..f03a4bd 100644 --- a/vesc_msgs/msg/VescState.msg +++ b/vesc_msgs/msg/VescState.msg @@ -4,21 +4,30 @@ int32 FAULT_CODE_NONE=0 int32 FAULT_CODE_OVER_VOLTAGE=1 int32 FAULT_CODE_UNDER_VOLTAGE=2 -int32 FAULT_CODE_DRV8302=3 +int32 FAULT_CODE_DRV=3 int32 FAULT_CODE_ABS_OVER_CURRENT=4 int32 FAULT_CODE_OVER_TEMP_FET=5 int32 FAULT_CODE_OVER_TEMP_MOTOR=6 -float64 voltage_input # input voltage (volt) -float64 temperature_pcb # temperature of printed circuit board (degrees Celsius) -float64 current_motor # motor current (ampere) -float64 current_input # input current (ampere) -float64 speed # motor electrical speed (revolutions per minute) -float64 duty_cycle # duty cycle (0 to 1) -float64 charge_drawn # electric charge drawn from input (ampere-hour) -float64 charge_regen # electric charge regenerated to input (ampere-hour) -float64 energy_drawn # energy drawn from input (watt-hour) -float64 energy_regen # energy regenerated to input (watt-hour) -float64 displacement # net tachometer (counts) -float64 distance_traveled # total tachnometer (counts) +# plase follow the bledc firwmare: commands.c +float32 temp_fet +float32 temp_motor +float32 avg_motor_current +float32 avg_input_current +float32 avg_id +float32 avg_iq +float32 duty_cycle_now +float32 speed #rpm +float32 INPUT_VOLTAGE +float32 amp_hours +float32 amp_hours_charged +float32 watt_hours +float32 watt_hours_charged +float32 tachometer +float32 tachometer_abs int32 fault_code +float32 pid_pos_now +int8 controller_id +float32 NTC_TEMP_MOS1 +float32 NTC_TEMP_MOS2 +float32 NTC_TEMP_MOS3 \ No newline at end of file From f618bf12900f61d79e9086c85bface693d8898e3 Mon Sep 17 00:00:00 2001 From: eastWillow Date: Tue, 5 Mar 2019 13:01:17 +0800 Subject: [PATCH 7/8] all values package --- vesc_driver/include/vesc_driver/vesc_packet.h | 38 ++-- vesc_driver/src/vesc_driver.cpp | 15 +- vesc_driver/src/vesc_packet.cpp | 196 +++++++----------- vesc_msgs/msg/VescState.msg | 6 +- 4 files changed, 105 insertions(+), 150 deletions(-) diff --git a/vesc_driver/include/vesc_driver/vesc_packet.h b/vesc_driver/include/vesc_driver/vesc_packet.h index 4122cc4..1758990 100644 --- a/vesc_driver/include/vesc_driver/vesc_packet.h +++ b/vesc_driver/include/vesc_driver/vesc_packet.h @@ -101,35 +101,27 @@ class VescPacketValues : public VescPacket public: VescPacketValues(boost::shared_ptr raw); - /* - double v_in() const; - double temp_mos1() const; - double temp_mos2() const; - double temp_mos3() const; - double temp_mos4() const; - double temp_mos5() const; - double temp_mos6() const; - double temp_pcb() const; - double current_motor() const; - double current_in() const; + double temp_fet() const; + double temp_motor() const; + double avg_motor_current() const; + double avg_input_current() const; + double avg_id() const; + double avg_iq() const; + double duty_cycle_now() const; double rpm() const; - double duty_now() const; + double GET_INPUT_VOLTAGE() const; double amp_hours() const; double amp_hours_charged() const; double watt_hours() const; double watt_hours_charged() const; - double tachometer() const; - double tachometer_abs() const; - */ - double rpm() const; - double avg_motor_current() const; - double GET_INPUT_VOLTAGE() const; - double avg_input_current() const; - double duty_cycle_now() const; - double temp_fet() const; - double temp_motor() const; - double tachometer() const; + int32_t tachometer() const; + int32_t tachometer_abs() const; int fault_code() const; + double pid_pos_now() const; + int controller_id() const; + double NTC_TEMP_MOS1() const; + double NTC_TEMP_MOS2() const; + double NTC_TEMP_MOS3() const; }; diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index 2068798..e5897a1 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -125,12 +125,23 @@ void VescDriver::vescPacketCallback(const boost::shared_ptr& p state_msg->state.temp_motor = values->temp_motor(); state_msg->state.avg_motor_current = values->avg_motor_current(); state_msg->state.avg_motor_current = values->avg_input_current(); + state_msg->state.avg_id = values->avg_id(); + state_msg->state.avg_iq = values->avg_iq(); state_msg->state.duty_cycle_now = values->duty_cycle_now(); state_msg->state.speed = values->rpm(); - state_msg->state.tachometer = values->tachometer(); - state_msg->state.tachometer_abs = 0; state_msg->state.INPUT_VOLTAGE = values->GET_INPUT_VOLTAGE(); + state_msg->state.amp_hours = values->amp_hours(); + state_msg->state.amp_hours_charged = values->amp_hours_charged(); + state_msg->state.watt_hours = values->watt_hours(); + state_msg->state.watt_hours_charged = values->watt_hours_charged(); + state_msg->state.tachometer = values->tachometer(); + state_msg->state.tachometer_abs = values->tachometer_abs(); state_msg->state.fault_code = values->fault_code(); + state_msg->state.pid_pos_now = values->pid_pos_now(); + state_msg->state.controller_id = values->controller_id(); + state_msg->state.NTC_TEMP_MOS1 = values->NTC_TEMP_MOS1(); + state_msg->state.NTC_TEMP_MOS2 = values->NTC_TEMP_MOS2(); + state_msg->state.NTC_TEMP_MOS3 = values->NTC_TEMP_MOS3(); state_pub_.publish(state_msg); } diff --git a/vesc_driver/src/vesc_packet.cpp b/vesc_driver/src/vesc_packet.cpp index 71e3b8a..a4a8e7f 100644 --- a/vesc_driver/src/vesc_packet.cpp +++ b/vesc_driver/src/vesc_packet.cpp @@ -144,6 +144,22 @@ double VescPacketValues::avg_input_current() const static_cast(*(payload_.first + 12))); return static_cast(v) / 1e2; } +double VescPacketValues::avg_id() const +{ + int32_t v = static_cast((static_cast(*(payload_.first + 13)) << 24) + + (static_cast(*(payload_.first + 14)) << 16) + + (static_cast(*(payload_.first + 15)) << 8) + + static_cast(*(payload_.first + 16))); + return static_cast(v) / 1e2; +} +double VescPacketValues::avg_iq() const +{ + int32_t v = static_cast((static_cast(*(payload_.first + 17)) << 24) + + (static_cast(*(payload_.first + 18)) << 16) + + (static_cast(*(payload_.first + 19)) << 8) + + static_cast(*(payload_.first + 20))); + return static_cast(v) / 1e2; +} double VescPacketValues::duty_cycle_now() const { int16_t v = static_cast((static_cast(*(payload_.first + 21)) << 8) + @@ -164,152 +180,88 @@ double VescPacketValues::GET_INPUT_VOLTAGE() const static_cast(*(payload_.first + 28))); return static_cast(v) / 1e1; } -double VescPacketValues::tachometer() const -{ - int64_t v = static_cast((static_cast(*(payload_.first + 45)) << 24) + - (static_cast(*(payload_.first + 46)) << 16) + - (static_cast(*(payload_.first + 47)) << 8) + - static_cast(*(payload_.first + 48))); - return static_cast(v) / 1e0; -} -int VescPacketValues::fault_code() const -{ - return static_cast(*(payload_.first + 53)); -} - -/* -double VescPacketValues::temp_mos1() const // mc_interface_temp_fet_filtered -{ - int16_t v = static_cast((static_cast(*(payload_.first + 1)) << 8) + - static_cast(*(payload_.first + 2))); - return static_cast(v) / 10.0; -} -double VescPacketValues::temp_mos2() const -{ - int16_t v = static_cast((static_cast(*(payload_.first + 3)) << 8) + - static_cast(*(payload_.first + 4))); - return static_cast(v) / 10.0; -} -double VescPacketValues::temp_mos3() const -{ - int16_t v = static_cast((static_cast(*(payload_.first + 5)) << 8) + - static_cast(*(payload_.first + 6))); - return static_cast(v) / 10.0; -} -double VescPacketValues::temp_mos4() const -{ - int16_t v = static_cast((static_cast(*(payload_.first + 7)) << 8) + - static_cast(*(payload_.first + 8))); - return static_cast(v) / 10.0; -} -double VescPacketValues::temp_mos5() const -{ - int16_t v = static_cast((static_cast(*(payload_.first + 9)) << 8) + - static_cast(*(payload_.first + 10))); - return static_cast(v) / 10.0; -} -double VescPacketValues::temp_mos6() const -{ - int16_t v = static_cast((static_cast(*(payload_.first + 11)) << 8) + - static_cast(*(payload_.first + 12))); - return static_cast(v) / 10.0; -} -double VescPacketValues::temp_pcb() const -{ - int16_t v = static_cast((static_cast(*(payload_.first + 13)) << 8) + - static_cast(*(payload_.first + 14))); - return static_cast(v) / 10.0; -} -double VescPacketValues::current_motor() const +double VescPacketValues::amp_hours() const { - int32_t v = static_cast((static_cast(*(payload_.first + 15)) << 24) + - (static_cast(*(payload_.first + 16)) << 16) + - (static_cast(*(payload_.first + 17)) << 8) + - static_cast(*(payload_.first + 18))); - return static_cast(v) / 100.0; + int64_t v = static_cast((static_cast(*(payload_.first + 29)) << 24) + + (static_cast(*(payload_.first + 30)) << 16) + + (static_cast(*(payload_.first + 31)) << 8) + + static_cast(*(payload_.first + 32))); + return static_cast(v) / 1e4; } -double VescPacketValues::current_in() const +double VescPacketValues::amp_hours_charged() const { - int32_t v = static_cast((static_cast(*(payload_.first + 19)) << 24) + - (static_cast(*(payload_.first + 20)) << 16) + - (static_cast(*(payload_.first + 21)) << 8) + - static_cast(*(payload_.first + 22))); - return static_cast(v) / 100.0; + int64_t v = static_cast((static_cast(*(payload_.first + 33)) << 24) + + (static_cast(*(payload_.first + 34)) << 16) + + (static_cast(*(payload_.first + 35)) << 8) + + static_cast(*(payload_.first + 36))); + return static_cast(v) / 1e4; } -double VescPacketValues::duty_now() const +double VescPacketValues::watt_hours() const { - int16_t v = static_cast((static_cast(*(payload_.first + 23)) << 8) + - static_cast(*(payload_.first + 24))); - return static_cast(v) / 1000.0; + int64_t v = static_cast((static_cast(*(payload_.first + 37)) << 24) + + (static_cast(*(payload_.first + 38)) << 16) + + (static_cast(*(payload_.first + 39)) << 8) + + static_cast(*(payload_.first + 40))); + return static_cast(v) / 1e4; } -double VescPacketValues::rpm() const +double VescPacketValues::watt_hours_charged() const { - int32_t v = static_cast((static_cast(*(payload_.first + 25)) << 24) + - (static_cast(*(payload_.first + 26)) << 16) + - (static_cast(*(payload_.first + 27)) << 8) + - static_cast(*(payload_.first + 28))); - return static_cast(v); + int64_t v = static_cast((static_cast(*(payload_.first + 41)) << 24) + + (static_cast(*(payload_.first + 42)) << 16) + + (static_cast(*(payload_.first + 43)) << 8) + + static_cast(*(payload_.first + 44))); + return static_cast(v) / 1e4; } -double VescPacketValues::v_in() const +int32_t VescPacketValues::tachometer() const { - int16_t v = static_cast((static_cast(*(payload_.first + 29)) << 8) + - static_cast(*(payload_.first + 30))); - return static_cast(v) / 10.0; + int32_t v = static_cast((static_cast(*(payload_.first + 45)) << 24) + + (static_cast(*(payload_.first + 46)) << 16) + + (static_cast(*(payload_.first + 47)) << 8) + + static_cast(*(payload_.first + 48))); + return v; } -double VescPacketValues::amp_hours() const +int32_t VescPacketValues::tachometer_abs() const { - int32_t v = static_cast((static_cast(*(payload_.first + 31)) << 24) + - (static_cast(*(payload_.first + 32)) << 16) + - (static_cast(*(payload_.first + 33)) << 8) + - static_cast(*(payload_.first + 34))); - return static_cast(v); + int32_t v = static_cast((static_cast(*(payload_.first + 49)) << 24) + + (static_cast(*(payload_.first + 50)) << 16) + + (static_cast(*(payload_.first + 51)) << 8) + + static_cast(*(payload_.first + 52))); + return v; } -double VescPacketValues::amp_hours_charged() const +int VescPacketValues::fault_code() const { - int32_t v = static_cast((static_cast(*(payload_.first + 35)) << 24) + - (static_cast(*(payload_.first + 36)) << 16) + - (static_cast(*(payload_.first + 37)) << 8) + - static_cast(*(payload_.first + 38))); - return static_cast(v); + return static_cast(*(payload_.first + 53)); } -double VescPacketValues::watt_hours() const +double VescPacketValues::pid_pos_now() const { - int32_t v = static_cast((static_cast(*(payload_.first + 39)) << 24) + - (static_cast(*(payload_.first + 40)) << 16) + - (static_cast(*(payload_.first + 41)) << 8) + - static_cast(*(payload_.first + 42))); - return static_cast(v); + int64_t v = static_cast((static_cast(*(payload_.first + 54)) << 24) + + (static_cast(*(payload_.first + 55)) << 16) + + (static_cast(*(payload_.first + 56)) << 8) + + static_cast(*(payload_.first + 57))); + return static_cast(v) / 1e6; } -double VescPacketValues::watt_hours_charged() const +int VescPacketValues::controller_id() const { - int32_t v = static_cast((static_cast(*(payload_.first + 43)) << 24) + - (static_cast(*(payload_.first + 44)) << 16) + - (static_cast(*(payload_.first + 45)) << 8) + - static_cast(*(payload_.first + 46))); - return static_cast(v); + return static_cast(*(payload_.first + 58)); } -double VescPacketValues::tachometer() const +double VescPacketValues::NTC_TEMP_MOS1() const { - int32_t v = static_cast((static_cast(*(payload_.first + 47)) << 24) + - (static_cast(*(payload_.first + 48)) << 16) + - (static_cast(*(payload_.first + 49)) << 8) + - static_cast(*(payload_.first + 50))); - return static_cast(v); + int16_t v = static_cast((static_cast(*(payload_.first + 59)) << 8) + + static_cast(*(payload_.first + 60))); + return static_cast(v) / 1e1; } -double VescPacketValues::tachometer_abs() const +double VescPacketValues::NTC_TEMP_MOS2() const { - int32_t v = static_cast((static_cast(*(payload_.first + 51)) << 24) + - (static_cast(*(payload_.first + 52)) << 16) + - (static_cast(*(payload_.first + 53)) << 8) + - static_cast(*(payload_.first + 54))); - return static_cast(v); + int16_t v = static_cast((static_cast(*(payload_.first + 61)) << 8) + + static_cast(*(payload_.first + 62))); + return static_cast(v) / 1e1; } -int VescPacketValues::fault_code() const +double VescPacketValues::NTC_TEMP_MOS3() const { - return static_cast(*(payload_.first + 55)); + int16_t v = static_cast((static_cast(*(payload_.first + 63)) << 8) + + static_cast(*(payload_.first + 64))); + return static_cast(v) / 1e1; } -*/ - REGISTER_PACKET_TYPE(COMM_GET_VALUES, VescPacketValues) diff --git a/vesc_msgs/msg/VescState.msg b/vesc_msgs/msg/VescState.msg index f03a4bd..b03e2a6 100644 --- a/vesc_msgs/msg/VescState.msg +++ b/vesc_msgs/msg/VescState.msg @@ -20,11 +20,11 @@ float32 duty_cycle_now float32 speed #rpm float32 INPUT_VOLTAGE float32 amp_hours -float32 amp_hours_charged +float32 amp_hours_charged float32 watt_hours float32 watt_hours_charged -float32 tachometer -float32 tachometer_abs +int32 tachometer +int32 tachometer_abs int32 fault_code float32 pid_pos_now int8 controller_id From 0ec8963ac721a1ae77a1f5974b5b3c0536b54e31 Mon Sep 17 00:00:00 2001 From: eastWillow Date: Fri, 8 Mar 2019 00:31:21 +0800 Subject: [PATCH 8/8] fix rpm minus --- vesc_driver/src/vesc_packet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/vesc_driver/src/vesc_packet.cpp b/vesc_driver/src/vesc_packet.cpp index a4a8e7f..e6f4f7e 100644 --- a/vesc_driver/src/vesc_packet.cpp +++ b/vesc_driver/src/vesc_packet.cpp @@ -168,7 +168,7 @@ double VescPacketValues::duty_cycle_now() const } double VescPacketValues::rpm() const { - int64_t v = static_cast((static_cast(*(payload_.first + 23)) << 24) + + int32_t v = static_cast((static_cast(*(payload_.first + 23)) << 24) + (static_cast(*(payload_.first + 24)) << 16) + (static_cast(*(payload_.first + 25)) << 8) + static_cast(*(payload_.first + 26)));