竖起从80度改成了76度

增加了MPM258这个深度传感器。

ArduSub/control_althold.cpp

@@ -79,7 +79,7 @@ void Sub::handle_attitude()
 		   target_yaw = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
 	   }else if(prepare_state == Vertical)
 	   	{
-		   pitch_input = 8000;
+		   pitch_input = 7600;
 		   target_roll = -((float)channel_yaw->get_control_in()*0.5);
 		   
 		   target_yaw = 0;

libraries/AP_Baro/AP_Baro_MPM258.cpp

@@ -0,0 +1,210 @@
+/*
+   This program 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,
+   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.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include "AP_Baro_MPM258.h"
+
+#include <utility>
+#include <stdio.h>
+
+#include <AP_Math/AP_Math.h>
+#include <AP_Math/crc.h>
+#include "../../ArduSub/Sub.h"
+
+extern const AP_HAL::HAL &hal;
+
+static const uint8_t MPM258_ADDR =0x6D;
+
+static const uint8_t MPM258_ADDR_CMD_I2C_WRITE =0xDA;
+static const uint8_t MPM258_ADDR_CMD_I2C_READ=  0xDB;
+
+
+/* write to one of these addresses to start pressure conversion */
+
+static const uint8_t ADDR_CMD_PRESS1=  0x06;
+static const uint8_t ADDR_CMD_PRESS2= 0x07;
+static const uint8_t ADDR_CMD_PRESS3=  0x08;
+
+
+
+/* write to one of these addresses to start temperature conversion */
+static const uint8_t ADDR_CMD_TEMPRATURE1= 0x09;
+static const uint8_t ADDR_CMD_TEMPRATURE2= 0x0A;
+static const uint8_t ADDR_CMD_TEMPRATURE3= 0x0B;
+
+
+
+
+/*
+  constructor
+ */
+AP_Baro_MPM258::AP_Baro_MPM258(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev)
+    : AP_Baro_Backend(baro)
+    , _dev(std::move(dev))
+{
+}
+
+AP_Baro_Backend *AP_Baro_MPM258::probe(AP_Baro &baro,
+                                       AP_HAL::OwnPtr<AP_HAL::Device> dev)
+{
+    if (!dev) {
+        return nullptr;
+    }
+    AP_Baro_MPM258 *sensor = new AP_Baro_MPM258(baro, std::move(dev));
+    if (!sensor || !sensor->_init()) {
+        delete sensor;
+        return nullptr;
+    }
+    return sensor;
+}
+
+bool AP_Baro_MPM258::_init()
+{
+	//uint8_t press_data[3];
+	//uint8_t temp_data[3];
+
+    if (!_dev) {
+        return false;
+    }
+
+    if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+        AP_HAL::panic("PANIC: AP_Baro_MPM258: failed to take serial semaphore for init");
+    }
+
+    // high retries for init
+    _dev->set_retries(10);    
+	//如果不能发送成功说明这个I2C口没有传感器
+	if(!_dev->transfer(&MPM258_ADDR_CMD_I2C_WRITE, 1, nullptr, 0)){
+        _dev->get_semaphore()->give();
+        return false;
+
+	}
+
+    _state = 0;
+    memset(&_accum, 0, sizeof(_accum));
+    _instance = _frontend.register_sensor();
+     _frontend.set_type(_instance, AP_Baro::BARO_TYPE_WATER);
+    // lower retries for run
+    _dev->set_retries(3);   
+    _dev->get_semaphore()->give();
+	hal.scheduler->delay(150);
+
+    /* Request  50HZpdate */
+    _dev->register_periodic_callback(20 * AP_USEC_PER_MSEC,
+                                     FUNCTOR_BIND_MEMBER(&AP_Baro_MPM258::_timer, void));
+    return true;
+}
+
+
+
+/*
+ * Read the sensor with a state machine
+ * We read one time temperature (state=0) and then 4 times pressure (states 1-4)
+ *
+ * Temperature is used to calculate the compensated pressure and doesn't vary
+ * as fast as pressure. Hence we reuse the same temperature for 4 samples of
+ * pressure.
+*/
+void AP_Baro_MPM258::_timer(void)
+{
+	_read();
+
+
+}
+
+void AP_Baro_MPM258::_update_and_wrap_accumulator(int32_t pressure, int32_t temperature, uint8_t max_count)
+{
+    _accum.sum_pressure += pressure;
+    _accum.sum_temperature += temperature;
+    _accum.num_samples += 1;
+
+    if (_accum.num_samples == max_count) {
+        _accum.sum_pressure /= 2;
+        _accum.sum_temperature /= 2;
+        _accum.num_samples /= 2;
+    }
+}
+bool AP_Baro_MPM258::_read(){
+	  uint8_t press_data[3]={0,0,0};
+	  uint8_t temp_data[3] = {0,0,0};
+	  uint32_t pressure_raw =0;
+	  uint32_t temperature_raw =0;
+	  
+	  
+
+		_dev->transfer(&MPM258_ADDR_CMD_I2C_WRITE, 1, nullptr, 0);
+		_dev->transfer(&ADDR_CMD_PRESS1, 1, nullptr, 0);
+		if(!_dev->transfer(&MPM258_ADDR_CMD_I2C_READ, 0,press_data, 3))
+		{
+			return FALSE;
+		}
+
+		_dev->transfer(&MPM258_ADDR_CMD_I2C_WRITE, 1, nullptr, 0);
+		_dev->transfer(&ADDR_CMD_TEMPRATURE1, 1, nullptr, 0);			
+		if(!_dev->transfer(&MPM258_ADDR_CMD_I2C_READ, 0,temp_data, 3))
+		{
+			return FALSE;
+		}
+
+		WITH_SEMAPHORE(_sem);
+	   pressure_raw = ((int32_t)press_data[0] << 16) |((int32_t)press_data[1] << 8) | press_data[2];
+       temperature_raw = ((int32_t)temp_data[0] << 16) |((int32_t)temp_data[1] << 8) | temp_data[2];
+	  _update_and_wrap_accumulator(pressure_raw, temperature_raw, 128);
+
+	  return TRUE;
+
+}
+
+void AP_Baro_MPM258::update()
+{
+    int32_t sum_pressure, sum_temperature;
+    int32_t num_samples;
+
+    {
+        WITH_SEMAPHORE(_sem);
+
+        if (_accum.num_samples == 0) {
+            return;
+        }
+
+        sum_pressure = _accum.sum_pressure;
+        sum_temperature = _accum.sum_temperature;
+        num_samples = _accum.num_samples;
+        memset(&_accum, 0, sizeof(_accum));
+    }
+
+    int32_t raw_pressure_avg = sum_pressure / num_samples;
+    int32_t raw_temperature_avg = sum_temperature / num_samples;
+	if(raw_pressure_avg>8388608){
+		raw_pressure_avg  = raw_pressure_avg- 16777216;
+		}
+		if(raw_temperature_avg>8388608){
+		raw_temperature_avg  = raw_temperature_avg- 16777216;
+		}
+
+	float pressure = 1000000*((float)raw_pressure_avg/8388608-0.1)/(0.9-0.1);//量程是1MPA
+	float temperature = (float)raw_temperature_avg/65536+25;
+	
+	  static int16_t cnt = 0;
+	cnt++;
+	if(cnt>20){
+		cnt = 0;
+		gcs().send_text(MAV_SEVERITY_INFO, "pressure %d %d %f %f.",raw_pressure_avg,raw_temperature_avg,pressure,temperature);
+		}
+
+	_copy_to_frontend(_instance, pressure, temperature);
+
+
+}
+
+

libraries/AP_Baro/AP_Baro_MPM258.h

@@ -0,0 +1,55 @@
+#pragma once
+
+#include "AP_Baro_Backend.h"
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/Semaphores.h>
+#include <AP_HAL/Device.h>
+
+#ifndef HAL_BARO_MPM258_I2C_ADDR
+#define HAL_BARO_MPM258_I2C_ADDR 0x6D
+#endif
+
+
+class AP_Baro_MPM258 : public AP_Baro_Backend
+{
+public:
+    void update() override;
+
+    static AP_Baro_Backend *probe(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev);
+
+private:
+    /*
+     * Update @accum and @count with the new sample in @val, taking into
+     * account a maximum number of samples given by @max_count; in case
+     * maximum number is reached, @accum and @count are updated appropriately
+     */
+	void _update_and_wrap_accumulator(int32_t pressure, int32_t temperature, uint8_t max_count);
+
+
+    AP_Baro_MPM258(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev);
+
+    bool _init();
+
+
+
+    bool _read();
+
+    void _timer();
+
+    AP_HAL::OwnPtr<AP_HAL::Device> _dev;
+
+    /* Shared values between thread sampling the HW and main thread */
+    struct {
+        int32_t sum_pressure;
+        int32_t sum_temperature;
+        uint8_t num_samples;
+    } _accum;
+
+
+    uint8_t _state;
+    uint8_t _instance;
+
+
+    
+};

libraries/AP_UAVCAN/AP_UAVCAN.cpp

@@ -267,71 +267,46 @@ static void motor_res_st_cb(const uavcan::ReceivedDataStructure<uavcan::equipmen
 
 	if (msg.motor_index == 0){
 		//推进器0，1，2
-		motors_receive[0] = msg.rpm0 & 0x0FFF;//推进器0
+		motors_receive[0] = msg.rpm0 & 0x0FFF;//推进器0 速度
 		propeller_block.bit.motor0 = (msg.rpm0 & 0xF000)>>12;
-		motors_receive[1] = msg.rpm1 & 0x0FFF;//推进器1
+		motors_receive[1] = msg.rpm1 & 0x0FFF;//推进器1 速度
 		propeller_block.bit.motor1 = (msg.rpm1 & 0xF000)>>12;
 		
 	}
 	else if(msg.motor_index == 3){
 		//推进器0.1.履带左的给定
-		int16_t rmp1  = msg.rpm0;
-		int16_t rmp2 =  msg.rpm1;
-		int16_t rmp3 =  msg.rpm2;
-
-		static int16_t c2  = 0;
-		c2++;
-		if(c2>50){
-			c2 =0 ;
-			gcs().send_text(MAV_SEVERITY_WARNING, "motor_index == 3 %d %d %d.",(int)rmp1,(int)rmp2,(int)rmp3);
-				
-		}
+		//int16_t rmp1  = msg.rpm0;//左推进器pwm
+		//int16_t rmp2 =  msg.rpm1;//右推进器PWM
+		//int16_t rmp3 =  msg.rpm2;//做履带PWM
+
+
 
 	}
 	else if(msg.motor_index == 6){
 		
 		
-		int16_t rpm = msg.rpm0;//履带右的给定
+		//int16_t rpm = msg.rpm0;//履带右的给定
 		
 
-		motors_receive[8] = msg.rpm2 & 0x0FFF;//履带1
+		motors_receive[8] = msg.rpm2 & 0x0FFF;//左履带速度
 		track_block.bit.motor0 = (msg.rpm2 & 0xF000)>>12;
 
-		static int16_t c3  = 0;
-		c3++;
-		if(c3>50){
-			c3 =0 ;
-			gcs().send_text(MAV_SEVERITY_WARNING, "motor_index == 6 %d %d ",(int)rpm,motors_receive[8]);
-			
-		}
 	}
 	else if(msg.motor_index == 9){
 	////履带2
-		motors_receive[9] = msg.rpm0 & 0x0FFF;//履带2速度
+		motors_receive[9] = msg.rpm0 & 0x0FFF;//右履带2速度
 		
 		track_block.bit.motor1 = (msg.rpm0 & 0xF000)>>12;
 		
-		static int16_t c4  = 0;
-		c4++;
-		if(c4>100){
-			c4 =0 ;
-			gcs().send_text(MAV_SEVERITY_WARNING, "motor_index == 9 %d.",(int)motors_receive[9]);
-			
-		}
+
 	}
 	else if(msg.motor_index == 12){
 	////
 		voltage48V = msg.rpm0;
 		temperature_power = msg.rpm1;
-		motor_stall_state.all = msg.rpm2;//所有电机的堵转// 履带没有堵转了
+		motor_stall_state.all = msg.rpm2;//所有电机的堵转// 履带没有堵转了 这个故障不需要了
+
 
-		static int16_t c5  = 0;
-		c5++;
-		if(c5>100){
-			c5 =0 ;
-			//gcs().send_text(MAV_SEVERITY_WARNING, "motor_index == 12 %d %d %x.",(int)voltage48V,(int)temperature_power,motor_stall_state.all);
-			
-		}
 	}
 	else if(msg.motor_index == 15){
 	////履带2,毛刷1,毛刷2
@@ -340,6 +315,7 @@ static void motor_res_st_cb(const uavcan::ReceivedDataStructure<uavcan::equipmen
 		light_leak = msg.rpm2;//灯 漏水
 
 
+
 	}
 }
 
@@ -1081,7 +1057,7 @@ void AP_UAVCAN::loop(void)
 			
 			temperature_48Vpower = temperature_power;//can获取18B20温度
 			board_voltage = voltage48V;//can获取48V电源
-			driverleakstate = light_leak;//灯 漏水
+			driverleakstate = light_leak;//灯 + 漏水
 			sublightstate = lightstate;
 
 			HVmotor1 = Thruster1;

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -1049,7 +1049,7 @@ void GCS_MAVLINK::update_send()
 			rov_message.high_voltage = (float)(uavcan.HVmotor2.voltage);//电调1的电压
 			rov_message.deep = fabsf(sub.barometer.get_altitude());//深度
 			rov_message.temp = (float)(uavcan.temperature_48Vpower)/10;//传过来的是*100的？
-			rov_message.motor_block_flag =  (uint16_t)(uavcan.motor_stall_flag);
+			rov_message.motor_block_flag =  (uint16_t)(uavcan.motor_stall_flag);//这个故障不需要了 2024.2.20
 
 
 			rov_message.track_fault_flag[0] = 0;//lvtruster_fault(0,uavcan.motor_stall_flag,uavcan.trackblock_flag);

modules/ChibiOS/os/common/startup/ARMCMx/compilers/GCC/mk/arm-none-eabi.mk

@@ -1,23 +0,0 @@
-##############################################################################
-# Compiler settings
-#
-
-TRGT = arm-none-eabi-
-CC   = $(TRGT)gcc
-CPPC = $(TRGT)g++
-# Enable loading with g++ only if you need C++ runtime support.
-# NOTE: You can use C++ even without C++ support if you are careful. C++
-#       runtime support makes code size explode.
-LD   = $(TRGT)gcc
-#LD   = $(TRGT)g++
-CP   = $(TRGT)objcopy
-AS   = $(TRGT)gcc -x assembler-with-cpp
-AR   = $(TRGT)ar
-OD   = $(TRGT)objdump
-SZ   = $(TRGT)size
-HEX  = $(CP) -O ihex
-BIN  = $(CP) -O binary
-
-#
-# Compiler settings
-##############################################################################