diff --git a/Firmware/LowLevel/platformio.ini b/Firmware/LowLevel/platformio.ini index 67b86c6c..6dfb2fa5 100644 --- a/Firmware/LowLevel/platformio.ini +++ b/Firmware/LowLevel/platformio.ini @@ -42,6 +42,10 @@ debug_build_flags = -O0 -g -ggdb build_src_filter = +<*> -<.git/> -<.svn/> - - +; shut down ESC when not in use or pitch > 15 degree for all configurations +build_flags = -DSHUTDOWN_ESC_WHEN_IDLE + + [env:0_13_X] lib_ignore = JY901_SERIAL,JY901_I2C lib_deps = ${env.lib_deps} diff --git a/Firmware/LowLevel/src/datatypes.h b/Firmware/LowLevel/src/datatypes.h index 1b9c9a21..dcee3df3 100644 --- a/Firmware/LowLevel/src/datatypes.h +++ b/Firmware/LowLevel/src/datatypes.h @@ -30,7 +30,7 @@ enum HighLevelMode { MODE_IDLE = 1, // ROS connected, idle mode - MODE_AUTONOMOUS = 2, // ROS connected, Autonomous mode, either mowing or docking or undocking + MODE_AUTONOMOUS = 2, // ROS connected, Autonomous mode, either mowing or docking or undocking or paused MODE_RECORDING = 3 // ROS connected, Manual mode during recording etc }; diff --git a/Firmware/LowLevel/src/imu/LSM6DSO/imu.cpp b/Firmware/LowLevel/src/imu/LSM6DSO/imu.cpp index a22af4bf..4b8c3fa2 100644 --- a/Firmware/LowLevel/src/imu/LSM6DSO/imu.cpp +++ b/Firmware/LowLevel/src/imu/LSM6DSO/imu.cpp @@ -49,12 +49,23 @@ 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; + // acceleration_mss[0] = accelerometer[0] * 9.81 / 1000.0; + // acceleration_mss[1] = accelerometer[1] * 9.81 / 1000.0; + + + // Nose down: X = -g + acceleration_mss[0] = -accelerometer[1] * 9.81 / 1000.0; + // Left down: Y = -g + acceleration_mss[1] = accelerometer[0] * 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; + //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[0] = gyroscope[1] * (PI / 180.0) / 1000.0; + gyro_rads[1] = -gyroscope[0] * (PI / 180.0) / 1000.0; gyro_rads[2] = gyroscope[2] * (PI / 180.0) / 1000.0; mag_uT[0] = 0; diff --git a/Firmware/LowLevel/src/imu/MPU9250/imu.cpp b/Firmware/LowLevel/src/imu/MPU9250/imu.cpp index 1a901b9a..1e8056bb 100644 --- a/Firmware/LowLevel/src/imu/MPU9250/imu.cpp +++ b/Firmware/LowLevel/src/imu/MPU9250/imu.cpp @@ -20,12 +20,13 @@ 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(); - acceleration_mss[2] = IMU.getAccelZ_mss(); - - gyro_rads[0] = IMU.getGyroX_rads(); - gyro_rads[1] = IMU.getGyroY_rads(); + + acceleration_mss[0] = IMU.getAccelY_mss(); + acceleration_mss[1] = -IMU.getAccelX_mss(); + acceleration_mss[2] = -IMU.getAccelZ_mss(); + + gyro_rads[0] = IMU.getGyroY_rads(); + gyro_rads[1] = -IMU.getGyroX_rads(); gyro_rads[2] = -IMU.getGyroZ_rads(); mag_uT[0] = IMU.getMagX_uT(); diff --git a/Firmware/LowLevel/src/imu/WT901_I2C/imu.cpp b/Firmware/LowLevel/src/imu/WT901_I2C/imu.cpp index 28c2647f..af7a115e 100644 --- a/Firmware/LowLevel/src/imu/WT901_I2C/imu.cpp +++ b/Firmware/LowLevel/src/imu/WT901_I2C/imu.cpp @@ -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; diff --git a/Firmware/LowLevel/src/imu/WT901_SERIAL/imu.cpp b/Firmware/LowLevel/src/imu/WT901_SERIAL/imu.cpp index e5dea623..771f07a9 100644 --- a/Firmware/LowLevel/src/imu/WT901_SERIAL/imu.cpp +++ b/Firmware/LowLevel/src/imu/WT901_SERIAL/imu.cpp @@ -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; diff --git a/Firmware/LowLevel/src/main.cpp b/Firmware/LowLevel/src/main.cpp index 1c1c852c..2791bbbf 100644 --- a/Firmware/LowLevel/src/main.cpp +++ b/Firmware/LowLevel/src/main.cpp @@ -14,10 +14,14 @@ // SOFTWARE. // // -// Changes 18.04.2024 Elmar - +// Changes 18.04.2024 ene9ba +// Merge optimized Soundsystem by Apehaenger pull request #80 +// // floating average integrated for UBat, UCharge and ICharge Hardware added 3,9 kohm between F2 and GND to prevent voltage injection without loading voltage // added offset for analog voltage +// +// 27.07.2024 Merge Shutdown ESCs whe Idle (pull request#97) from olliewalsh +// Merge Bugfix/wt901 via SerialPIO (#96) from Apehaenger #include #include @@ -35,15 +39,17 @@ #include #endif -#define IMU_CYCLETIME 20 // cycletime for refresh IMU data -#define STATUS_CYCLETIME 100 // cycletime for refresh analog and digital Statusvalues -#define UI_GET_VERSION_CYCLETIME 5000 // cycletime for UI Get_Version request (UI available check) -#define UI_GET_VERSION_TIMEOUT 100 // timeout for UI Get_Version response (UI available check) +#define IMU_CYCLETIME 20 // cycletime for refresh IMU data +#define STATUS_CYCLETIME 100 // cycletime for refresh analog and digital Statusvalues +#define UI_GET_VERSION_CYCLETIME 5000 // cycletime for UI Get_Version request (UI available check) +#define UI_GET_VERSION_TIMEOUT 100 // timeout for UI Get_Version response (UI available check) + +#define TILT_EMERGENCY_MILLIS 2500 // Time for a single wheel to be lifted in order to count as emergency (0 disable). This is to filter uneven ground. +#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 ANALOG_MEAN_COUNT 20 // size of array for calculation meanvalues -#define TILT_EMERGENCY_MILLIS 2500 // Time for a single wheel to be lifted in order to count as emergency (0 disable). This is to filter uneven ground. -#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 ANALOG_MEAN_COUNT 20 // size of array for calculation meanvalues +#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 @@ -70,16 +76,16 @@ SerialPIO uiSerial(PIN_UI_TX, PIN_UI_RX, 250); #define ANALOG_VOLTAGE_OFFSET 21 // not investigated, but a/d shows an Offset #define CURRENT_SENSE_GAIN 100.0f -#define BATT_ABS_MAX 28.7f -#define BATT_ABS_Min 21.7f +#define BATT_ABS_MAX 28.0f +#define BATT_ABS_Min 24.0f #define BATT_FULL BATT_ABS_MAX - 0.3f #define BATT_EMPTY BATT_ABS_Min + 0.3f //Values for chargecontrol set limits for overurrent and overvoltage #define CHARGE_MAX_CURRENT 1.5f // max allowed loading current -#define CHARGE_MAX_BATTERY_VOLTAGE 29.0f // max allowed battery voltage -#define CHARGE_MAX_CHARGE_VOLTAGE 30.0f // max allowed charge voltage +#define CHARGE_MAX_BATTERY_VOLTAGE 28.0f // max allowed battery voltage +#define CHARGE_MAX_CHARGE_VOLTAGE 29.0f // max allowed charge voltage // Emergency will be engaged, if no heartbeat was received in this time frame. #define HEARTBEAT_MILLIS 500 @@ -130,7 +136,9 @@ bool charging_allowed = false; 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 @@ -412,6 +420,12 @@ void setup() { pinMode(PIN_ENABLE_CHARGE, OUTPUT); digitalWrite(PIN_ENABLE_CHARGE, HIGH); + // set output ESC shutdown + 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); @@ -766,17 +780,23 @@ void loop() { imu_message.acceleration_mss[0] = imu_temp[0]; imu_message.acceleration_mss[1] = imu_temp[1]; imu_message.acceleration_mss[2] = imu_temp[2]; - imu_message.gyro_rads[0] = imu_temp[3]; - imu_message.gyro_rads[1] = imu_temp[4]; - imu_message.gyro_rads[2] = imu_temp[5]; - imu_message.mag_uT[0] = imu_temp[6]; - imu_message.mag_uT[1] = imu_temp[7]; - imu_message.mag_uT[2] = imu_temp[8]; + imu_message.gyro_rads[0] = imu_temp[3]; + imu_message.gyro_rads[1] = imu_temp[4]; + imu_message.gyro_rads[2] = imu_temp[5]; + imu_message.mag_uT[0] = imu_temp[6]; + imu_message.mag_uT[1] = imu_temp[7]; + imu_message.mag_uT[2] = imu_temp[8]; imu_message.dt_millis = now - last_imu_millis; 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; + + last_imu_millis = now; } if (now - last_status_update_millis > STATUS_CYCLETIME) { @@ -796,9 +816,34 @@ void loop() { #else ad_value = -1.0f; #endif + + + + +#ifdef SHUTDOWN_ESC_WHEN_IDLE + // ESC power saving when mower is IDLE + + + if((ROS_running) && (fabs(pitch_angle) <= SHUTDOWN_ESC_MAX_PITCH) && (last_high_level_state.current_mode != HighLevelMode::MODE_IDLE)) { + // Enable escs if not idle, or if ROS is running, or on TILT and Mode is not idle + digitalWrite(PIN_ESC_SHUTDOWN, LOW); + //ToDo set this message state to a new statusbyte + //status_message.status_bitmask |= LL_STATUS_BIT_CHARGE_ERROR; + } else { + digitalWrite(PIN_ESC_SHUTDOWN, HIGH); + // Disable ESCs + //status_message.status_bitmask &= ~LL_STATUS_BIT_CHARGE_ERROR; + } +#else + //status_message.status_bitmask |= 0b1000; // ToDo Collision with charging error bit +#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); + + // calculate percent value accu filling float delta = BATT_FULL - BATT_EMPTY; float vo = status_message.v_battery - BATT_EMPTY; diff --git a/configs/xESC/YardForce_Classic_Drive_App.xml b/configs/xESC/YardForce_Classic_Drive_App.xml index 576b43a9..271c2919 100644 --- a/configs/xESC/YardForce_Classic_Drive_App.xml +++ b/configs/xESC/YardForce_Classic_Drive_App.xml @@ -15,7 +15,7 @@ 0 50000 0 - 0 + 4 3 0 15000 diff --git a/configs/xESC/YardForce_Classic_Drive_Motor.xml b/configs/xESC/YardForce_Classic_Drive_Motor.xml index e0d0093c..2957a3e9 100644 --- a/configs/xESC/YardForce_Classic_Drive_Motor.xml +++ b/configs/xESC/YardForce_Classic_Drive_Motor.xml @@ -1,14 +1,13 @@ - 2 1 0 2 0 2 -2 - 5 - -5 + 1 + -1 15 -100000 100000 @@ -24,7 +23,7 @@ 90 85 100 - 0.15 + 0 0.005 0.95 1.5e+06 @@ -187,7 +186,7 @@ p, li { white-space: pre-wrap; } p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'Roboto'; font-size:12pt; font-weight:400; font-style:normal;"> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Some comments about the motor quality. Images can be added as well.</p></body></html> - 1 + 0 45 65 0.05 diff --git a/configs/xESC/YardForce_Classic_Mower_App.xml b/configs/xESC/YardForce_Classic_Mower_App.xml index 576b43a9..271c2919 100644 --- a/configs/xESC/YardForce_Classic_Mower_App.xml +++ b/configs/xESC/YardForce_Classic_Mower_App.xml @@ -15,7 +15,7 @@ 0 50000 0 - 0 + 4 3 0 15000