|
@@ -1030,21 +1030,21 @@ void GCS_MAVLINK::update_send()
|
|
|
|
|
|
mav_motor_speed_back.Ltrack = 0;
|
|
|
mav_motor_speed_back.Rtrack =0;
|
|
|
- mav_motor_speed_back.motor1 =(float)sub._telemetry[0].rpm;//uint32 rpm
|
|
|
- mav_motor_speed_back.motor2 =(float)sub._telemetry[1].rpm;
|
|
|
- mav_motor_speed_back.motor3 =(float)sub._telemetry[2].rpm;
|
|
|
- mav_motor_speed_back.motor4 =(float)sub._telemetry[3].rpm;
|
|
|
- mav_motor_speed_back.motor5 =(float)sub._telemetry[4].rpm;
|
|
|
- mav_motor_speed_back.motor6 =(float)sub._telemetry[5].rpm;
|
|
|
+ mav_motor_speed_back.motor1 =(float)sub._telemetry[0].rpm/7;//uint32 rpm
|
|
|
+ mav_motor_speed_back.motor2 =(float)sub._telemetry[1].rpm/7;
|
|
|
+ mav_motor_speed_back.motor3 =(float)sub._telemetry[2].rpm/7;
|
|
|
+ mav_motor_speed_back.motor4 =(float)sub._telemetry[3].rpm/7;
|
|
|
+ mav_motor_speed_back.motor5 =(float)sub._telemetry[4].rpm/7;
|
|
|
+ mav_motor_speed_back.motor6 =(float)sub._telemetry[5].rpm/7;
|
|
|
mav_motor_speed_back.motor7 =0;
|
|
|
mav_motor_speed_back.motor8 =0;
|
|
|
|
|
|
rov_message.floodlight = sub.lights;
|
|
|
//pressure_level 在各个模式中已经赋值
|
|
|
- rov_message.low_voltage = (float)sub._telemetry[0].voltage;;
|
|
|
- rov_message.high_voltage = (float)sub._telemetry[1].voltage;//sub._telemetry[0].voltage;//电压
|
|
|
+ rov_message.low_voltage = (float)sub._telemetry[0].voltage/10;
|
|
|
+ rov_message.high_voltage = (float)sub._telemetry[1].voltage/10;//sub._telemetry[0].voltage;//电压
|
|
|
rov_message.deep =fabsf(sub.barometer.get_altitude());//深度
|
|
|
- rov_message.temp = 0;
|
|
|
+ //rov_message.temp = 0;
|
|
|
rov_message.motor_block_flag = 0;
|
|
|
rov_message.motor_twine_flag[0] = 0;//(uint16_t)uavcan.thruster[0].error_count;//故障码?
|
|
|
rov_message.motor_twine_flag[1] = 0;//(uint16_t)uavcan.thruster[1].error_count;
|