Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Shutdown ESCs when IDLE #97

Open
wants to merge 4 commits into
base: main
Choose a base branch
from
Open

Shutdown ESCs when IDLE #97

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
19eadd1
Shutdown ESCs when IDLE
olliewalsh May 16, 2024
d432b54
Update xesc configs
olliewalsh May 18, 2024
3802351
Merge branch 'main' into esc_shutdown
ClemensElflein Jul 23, 2024
26678ef
Merge branch 'main' into esc_shutdown
ClemensElflein Jul 23, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions Firmware/LowLevel/platformio.ini
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,8 @@ debug_build_flags = -O0 -g -ggdb

build_src_filter = +<*> -<.git/> -<.svn/> -<imu/*> -<soundsystem.cpp>

build_flags = -DSHUTDOWN_ESC_WHEN_IDLE

[env:0_13_X]
lib_ignore = JY901_SERIAL,JY901_I2C
lib_deps = ${env.lib_deps}
Expand Down
2 changes: 1 addition & 1 deletion Firmware/LowLevel/src/datatypes.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@ struct ll_status {
// Bit 0: Initialized (i.e. setup() was a success). If this is 0, all other bits are meaningless.
// Bit 1: Raspberry Power
// Bit 2: Charging enabled
// Bit 3: don't care
// Bit 3: ESC power
// Bit 4: Rain detected
// Bit 5: Sound available
// Bit 6: Sound busy
Expand Down
12 changes: 8 additions & 4 deletions Firmware/LowLevel/src/imu/LSM6DSO/imu.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,12 +49,16 @@ bool imu_read(float *acceleration_mss, float *gyro_rads, float *mag_uT) {
success &= IMU.Get_X_Axes(accelerometer) == 0;
success &= IMU.Get_G_Axes(gyroscope) == 0;

acceleration_mss[0] = accelerometer[0] * 9.81 / 1000.0;
acceleration_mss[1] = accelerometer[1] * 9.81 / 1000.0;
// Left down: Y = -g
acceleration_mss[1] = accelerometer[0] * 9.81 / 1000.0;
// Nose down: X = -g
acceleration_mss[0] = -accelerometer[1] * 9.81 / 1000.0;
// Flat: Z = +g
acceleration_mss[2] = accelerometer[2] * 9.81 / 1000.0;

gyro_rads[0] = gyroscope[0] * (PI / 180.0) / 1000.0;
gyro_rads[1] = gyroscope[1] * (PI / 180.0) / 1000.0;
// Datasheet shows gyro and acceleromter axes are aligned
gyro_rads[1] = -gyroscope[0] * (PI / 180.0) / 1000.0;
gyro_rads[0] = gyroscope[1] * (PI / 180.0) / 1000.0;
gyro_rads[2] = gyroscope[2] * (PI / 180.0) / 1000.0;

mag_uT[0] = 0;
Expand Down
14 changes: 10 additions & 4 deletions Firmware/LowLevel/src/imu/MPU9250/imu.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,14 +20,20 @@ bool imu_read(float *acceleration_mss, float *gyro_rads, float *mag_uT)
{
IMU.readSensor();

acceleration_mss[0] = IMU.getAccelX_mss();
acceleration_mss[1] = IMU.getAccelY_mss();
// TODO: test this
// Left down: Y = -g
acceleration_mss[1] = -IMU.getAccelX_mss();
// Nose down: X = -g
acceleration_mss[0] = IMU.getAccelY_mss();
// Flat: Z = +g
acceleration_mss[2] = IMU.getAccelZ_mss();

gyro_rads[0] = IMU.getGyroX_rads();
gyro_rads[1] = IMU.getGyroY_rads();
// MPU-9250 Datasheet shows gyro and acceleromter axes are aligned
gyro_rads[1] = -IMU.getGyroX_rads();
gyro_rads[0] = IMU.getGyroY_rads();
gyro_rads[2] = -IMU.getGyroZ_rads();

// Mag is NED coordinates
mag_uT[0] = IMU.getMagX_uT();
mag_uT[1] = IMU.getMagY_uT();
mag_uT[2] = IMU.getMagZ_uT();
Expand Down
1 change: 1 addition & 0 deletions Firmware/LowLevel/src/imu/WT901_I2C/imu.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@ bool imu_read(float *acceleration_mss, float *gyro_rads, float *mag_uT)
IMU.GetMag();
IMU.GetGyro();

// WT901 axes seem to be corrected?
acceleration_mss[0] = (float)IMU.stcAcc.a[0] / 32768.0f * 16.0f * 9.81f;
acceleration_mss[1] = (float)IMU.stcAcc.a[1] / 32768.0f * 16.0f * 9.81f;
acceleration_mss[2] = (float)IMU.stcAcc.a[2] / 32768.0f * 16.0f * 9.81f;
Expand Down
1 change: 1 addition & 0 deletions Firmware/LowLevel/src/imu/WT901_SERIAL/imu.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@ bool init_imu()

bool imu_read(float *acceleration_mss, float *gyro_rads, float *mag_uT)
{
// WT901 axes seem to be corrected?
acceleration_mss[0] = (float)IMU.stcAcc.a[0] / 32768.0f * 16.0f * 9.81f;
acceleration_mss[1] = (float)IMU.stcAcc.a[1] / 32768.0f * 16.0f * 9.81f;
acceleration_mss[2] = (float)IMU.stcAcc.a[2] / 32768.0f * 16.0f * 9.81f;
Expand Down
26 changes: 26 additions & 0 deletions Firmware/LowLevel/src/main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@
#define LIFT_EMERGENCY_MILLIS 100 // Time for both wheels to be lifted in order to count as emergency (0 disable). This is to filter uneven ground.
#define BUTTON_EMERGENCY_MILLIS 20 // Time for button emergency to activate. This is to debounce the button.

#define SHUTDOWN_ESC_MAX_PITCH 15.0 // Do not shutdown ESCs if absolute pitch angle is greater than this
// Define to stream debugging messages via USB
// #define USB_DEBUG

Expand Down Expand Up @@ -118,6 +119,7 @@ bool ROS_running = false;
unsigned long charging_disabled_time = 0;

float imu_temp[9];
float pitch_angle = 0, roll_angle = 0, tilt_angle = 0;

uint16_t ui_version = 0; // Last received UI firmware version
uint8_t ui_topic_bitmask = Topic_set_leds; // UI subscription, default to Set_LEDs
Expand Down Expand Up @@ -388,6 +390,9 @@ void setup() {
pinMode(PIN_ENABLE_CHARGE, OUTPUT);
digitalWrite(PIN_ENABLE_CHARGE, HIGH);

pinMode(PIN_ESC_SHUTDOWN, OUTPUT);
digitalWrite(PIN_ESC_SHUTDOWN, LOW);

gpio_init(PIN_RASPI_POWER);
gpio_put(PIN_RASPI_POWER, true);
gpio_set_dir(PIN_RASPI_POWER, true);
Expand Down Expand Up @@ -671,6 +676,12 @@ void loop() {
sendMessage(&imu_message, sizeof(struct ll_imu));

last_imu_millis = now;

// Update pitch, roll, tilt
pitch_angle = atan2f(imu_temp[0], imu_temp[2]) * 180.0f / M_PI;
roll_angle = atan2f(imu_temp[1], imu_temp[2]) * 180.0f / M_PI;
float accXY = sqrtf((imu_temp[0]*imu_temp[0]) + (imu_temp[1]*imu_temp[1]));
tilt_angle = atan2f(accXY, imu_temp[2]) * 180.0f / M_PI;
}

if (now - last_status_update_millis > STATUS_CYCLETIME) {
Expand All @@ -686,6 +697,21 @@ void loop() {
#else
status_message.charging_current = -1.0f;
#endif

#ifdef SHUTDOWN_ESC_WHEN_IDLE
// ESC power saving when mower is IDLE
if(!ROS_running || last_high_level_state.current_mode != HighLevelMode::MODE_IDLE || fabs(pitch_angle) > SHUTDOWN_ESC_MAX_PITCH) {
// Enable escs if not idle, or if ROS is not running, or on a slope
digitalWrite(PIN_ESC_SHUTDOWN, LOW);
status_message.status_bitmask |= 0b1000;
} else {
digitalWrite(PIN_ESC_SHUTDOWN, HIGH);
status_message.status_bitmask &= 0b11110111;
}
#else
status_message.status_bitmask |= 0b1000;
#endif

status_message.status_bitmask = (status_message.status_bitmask & 0b11111011) | ((charging_allowed & 0b1) << 2);
status_message.status_bitmask = (status_message.status_bitmask & 0b11011111) | ((sound_available & 0b1) << 5);

Expand Down
2 changes: 1 addition & 1 deletion configs/xESC/YardForce_Classic_Drive_App.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@
<uavcan_raw_mode>0</uavcan_raw_mode>
<uavcan_raw_rpm_max>50000</uavcan_raw_rpm_max>
<servo_out_enable>0</servo_out_enable>
<kill_sw_mode>0</kill_sw_mode>
<kill_sw_mode>4</kill_sw_mode>
<app_to_use>3</app_to_use>
<app_ppm_conf.ctrl_type>0</app_ppm_conf.ctrl_type>
<app_ppm_conf.pid_max_erpm>15000</app_ppm_conf.pid_max_erpm>
Expand Down
9 changes: 4 additions & 5 deletions configs/xESC/YardForce_Classic_Drive_Motor.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,14 +1,13 @@
<?xml version="1.0" encoding="UTF-8"?>
<MCConfiguration>
<ConfigVersion>2</ConfigVersion>
<pwm_mode>1</pwm_mode>
<comm_mode>0</comm_mode>
<motor_type>2</motor_type>
<sensor_mode>0</sensor_mode>
<l_current_max>2</l_current_max>
<l_current_min>-2</l_current_min>
<l_in_current_max>5</l_in_current_max>
<l_in_current_min>-5</l_in_current_min>
<l_in_current_max>1</l_in_current_max>
<l_in_current_min>-1</l_in_current_min>
<l_abs_current_max>15</l_abs_current_max>
<l_min_erpm>-100000</l_min_erpm>
<l_max_erpm>100000</l_max_erpm>
Expand All @@ -24,7 +23,7 @@
<l_temp_fet_end>90</l_temp_fet_end>
<l_temp_motor_start>85</l_temp_motor_start>
<l_temp_motor_end>100</l_temp_motor_end>
<l_temp_accel_dec>0.15</l_temp_accel_dec>
<l_temp_accel_dec>0</l_temp_accel_dec>
<l_min_duty>0.005</l_min_duty>
<l_max_duty>0.95</l_max_duty>
<l_watt_max>1.5e+06</l_watt_max>
Expand Down Expand Up @@ -187,7 +186,7 @@ p, li { white-space: pre-wrap; }
p, li { white-space: pre-wrap; }
&lt;/style&gt;&lt;/head&gt;&lt;body style=&quot; font-family:'Roboto'; font-size:12pt; font-weight:400; font-style:normal;&quot;&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;Some comments about the motor quality. Images can be added as well.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</motor_quality_description>
<bms.type>1</bms.type>
<bms.type>0</bms.type>
<bms.t_limit_start>45</bms.t_limit_start>
<bms.t_limit_end>65</bms.t_limit_end>
<bms.soc_limit_start>0.05</bms.soc_limit_start>
Expand Down
2 changes: 1 addition & 1 deletion configs/xESC/YardForce_Classic_Mower_App.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@
<uavcan_raw_mode>0</uavcan_raw_mode>
<uavcan_raw_rpm_max>50000</uavcan_raw_rpm_max>
<servo_out_enable>0</servo_out_enable>
<kill_sw_mode>0</kill_sw_mode>
<kill_sw_mode>4</kill_sw_mode>
<app_to_use>3</app_to_use>
<app_ppm_conf.ctrl_type>0</app_ppm_conf.ctrl_type>
<app_ppm_conf.pid_max_erpm>15000</app_ppm_conf.pid_max_erpm>
Expand Down
39 changes: 19 additions & 20 deletions configs/xESC/YardForce_Classic_Mower_Motor.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,5 @@
<?xml version="1.0" encoding="UTF-8"?>
<MCConfiguration>
<ConfigVersion>2</ConfigVersion>
<pwm_mode>1</pwm_mode>
<comm_mode>0</comm_mode>
<motor_type>2</motor_type>
Expand All @@ -10,9 +9,9 @@
<l_in_current_max>2</l_in_current_max>
<l_in_current_min>-1</l_in_current_min>
<l_abs_current_max>15</l_abs_current_max>
<l_min_erpm>-100000</l_min_erpm>
<l_max_erpm>100000</l_max_erpm>
<l_erpm_start>0.8</l_erpm_start>
<l_min_erpm>-14100</l_min_erpm>
<l_max_erpm>14100</l_max_erpm>
<l_erpm_start>0.95</l_erpm_start>
<l_max_erpm_fbrake>300</l_max_erpm_fbrake>
<l_max_erpm_fbrake_cc>1500</l_max_erpm_fbrake_cc>
<l_min_vin>8</l_min_vin>
Expand All @@ -24,7 +23,7 @@
<l_temp_fet_end>90</l_temp_fet_end>
<l_temp_motor_start>85</l_temp_motor_start>
<l_temp_motor_end>100</l_temp_motor_end>
<l_temp_accel_dec>0.15</l_temp_accel_dec>
<l_temp_accel_dec>0</l_temp_accel_dec>
<l_min_duty>0.005</l_min_duty>
<l_max_duty>0.95</l_max_duty>
<l_watt_max>1.5e+06</l_watt_max>
Expand All @@ -48,8 +47,8 @@
<hall_table__6>4</hall_table__6>
<hall_table__7>-1</hall_table__7>
<hall_sl_erpm>2000</hall_sl_erpm>
<foc_current_kp>0.597682</foc_current_kp>
<foc_current_ki>574.348</foc_current_ki>
<foc_current_kp>0.5977</foc_current_kp>
<foc_current_ki>574.35</foc_current_ki>
<foc_f_zv>25000</foc_f_zv>
<foc_dt_us>0.12</foc_dt_us>
<foc_encoder_inverted>0</foc_encoder_inverted>
Expand All @@ -63,11 +62,11 @@
<foc_sensor_mode>2</foc_sensor_mode>
<foc_pll_kp>2000</foc_pll_kp>
<foc_pll_ki>30000</foc_pll_ki>
<foc_motor_l>0.000597682</foc_motor_l>
<foc_motor_ld_lq_diff>2.3181e-05</foc_motor_ld_lq_diff>
<foc_motor_r>0.574348</foc_motor_r>
<foc_motor_flux_linkage>0.00712734</foc_motor_flux_linkage>
<foc_observer_gain>1.96854e+07</foc_observer_gain>
<foc_motor_l>0.00059768</foc_motor_l>
<foc_motor_ld_lq_diff>2.318e-05</foc_motor_ld_lq_diff>
<foc_motor_r>0.5743</foc_motor_r>
<foc_motor_flux_linkage>0.007127</foc_motor_flux_linkage>
<foc_observer_gain>1.969e+07</foc_observer_gain>
<foc_observer_gain_slow>0.05</foc_observer_gain_slow>
<foc_observer_offset>-1</foc_observer_offset>
<foc_duty_dowmramp_kp>10</foc_duty_dowmramp_kp>
Expand All @@ -94,7 +93,7 @@
<foc_sample_high_current>0</foc_sample_high_current>
<foc_sat_comp>0</foc_sat_comp>
<foc_temp_comp>0</foc_temp_comp>
<foc_temp_comp_base_temp>22.51</foc_temp_comp_base_temp>
<foc_temp_comp_base_temp>22.5</foc_temp_comp_base_temp>
<foc_current_filter_const>0.1</foc_current_filter_const>
<foc_cc_decoupling>0</foc_cc_decoupling>
<foc_observer_type>0</foc_observer_type>
Expand All @@ -106,12 +105,12 @@
<foc_hfi_obs_ovr_sec>0.001</foc_hfi_obs_ovr_sec>
<foc_hfi_samples>1</foc_hfi_samples>
<foc_offsets_cal_on_boot>1</foc_offsets_cal_on_boot>
<foc_offsets_current__0>2086.73</foc_offsets_current__0>
<foc_offsets_current__1>2047.37</foc_offsets_current__1>
<foc_offsets_current__2>2114.23</foc_offsets_current__2>
<foc_offsets_voltage__0>0.1375</foc_offsets_voltage__0>
<foc_offsets_voltage__1>0.1366</foc_offsets_voltage__1>
<foc_offsets_voltage__2>-0.2742</foc_offsets_voltage__2>
<foc_offsets_current__0>1996.01</foc_offsets_current__0>
<foc_offsets_current__1>2023.24</foc_offsets_current__1>
<foc_offsets_current__2>2074.1</foc_offsets_current__2>
<foc_offsets_voltage__0>0.0011</foc_offsets_voltage__0>
<foc_offsets_voltage__1>-0.0005</foc_offsets_voltage__1>
<foc_offsets_voltage__2>-0.0005</foc_offsets_voltage__2>
<foc_offsets_voltage_undriven__0>0</foc_offsets_voltage_undriven__0>
<foc_offsets_voltage_undriven__1>0</foc_offsets_voltage_undriven__1>
<foc_offsets_voltage_undriven__2>0</foc_offsets_voltage_undriven__2>
Expand Down Expand Up @@ -187,7 +186,7 @@ p, li { white-space: pre-wrap; }
p, li { white-space: pre-wrap; }
&lt;/style&gt;&lt;/head&gt;&lt;body style=&quot; font-family:'Roboto'; font-size:12pt; font-weight:400; font-style:normal;&quot;&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;Some comments about the motor quality. Images can be added as well.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</motor_quality_description>
<bms.type>1</bms.type>
<bms.type>0</bms.type>
<bms.t_limit_start>45</bms.t_limit_start>
<bms.t_limit_end>65</bms.t_limit_end>
<bms.soc_limit_start>0.05</bms.soc_limit_start>
Expand Down
Loading
Loading