@ -19,9 +19,9 @@
# include <errno.h>
# include <errno.h>
# include <fcntl.h>
# include <fcntl.h>
# include <inttypes.h>
# include <stdio.h>
# include <stdio.h>
# include <stdlib.h>
# include <stdlib.h>
# include <inttypes.h>
# include <unistd.h>
# include <unistd.h>
# include <AP_HAL/AP_HAL.h>
# include <AP_HAL/AP_HAL.h>
@ -37,7 +37,7 @@ Led_Sysfs::Led_Sysfs(const char *led_name)
Led_Sysfs : : ~ Led_Sysfs ( )
Led_Sysfs : : ~ Led_Sysfs ( )
{
{
if ( _brightness_fd ) {
if ( _brightness_fd > = 0 ) {
close ( _brightness_fd ) ;
close ( _brightness_fd ) ;
}
}
}
}
@ -48,7 +48,7 @@ bool Led_Sysfs::init()
char * max_br_path ;
char * max_br_path ;
if ( asprintf ( & br_path , " /sys/class/leds/%s/brightness " , _led_name ) = = - 1 ) {
if ( asprintf ( & br_path , " /sys/class/leds/%s/brightness " , _led_name ) = = - 1 ) {
AP_HAL : : panic ( " LinuxLed_Sysfs : Couldn't allocate brightness path \n " ) ;
AP_HAL : : panic ( " LinuxLed_Sysfs : Couldn't allocate brightness path " ) ;
}
}
_brightness_fd = open ( br_path , O_WRONLY | O_CLOEXEC ) ;
_brightness_fd = open ( br_path , O_WRONLY | O_CLOEXEC ) ;
@ -59,11 +59,11 @@ bool Led_Sysfs::init()
}
}
if ( asprintf ( & max_br_path , " /sys/class/leds/%s/max_brightness " , _led_name ) = = - 1 ) {
if ( asprintf ( & max_br_path , " /sys/class/leds/%s/max_brightness " , _led_name ) = = - 1 ) {
AP_HAL : : panic ( " LinuxLed_Sysfs : Couldn't allocate max_brightness path \n " ) ;
AP_HAL : : panic ( " LinuxLed_Sysfs : Couldn't allocate max_brightness path " ) ;
}
}
if ( Util : : from ( hal . util ) - > read_file ( max_br_path , " %u " , & _max_brightness ) < 0 ) {
if ( Util : : from ( hal . util ) - > read_file ( max_br_path , " %u " , & _max_brightness ) < 0 ) {
AP_HAL : : panic ( " LinuxLed_Sysfs : Unable to read max_brightness in %s \n " ,
AP_HAL : : panic ( " LinuxLed_Sysfs : Unable to read max_brightness in %s " ,
max_br_path ) ;
max_br_path ) ;
}
}
@ -79,7 +79,7 @@ bool Led_Sysfs::set_brightness(uint8_t brightness)
return false ;
return false ;
}
}
int br = brightness * _max_brightness / UINT8_MAX ;
unsigned int br = brightness * _max_brightness / UINT8_MAX ;
/* Don't log fails since this could spam the console */
/* Don't log fails since this could spam the console */
if ( dprintf ( _brightness_fd , " %u " , br ) < 0 ) {
if ( dprintf ( _brightness_fd , " %u " , br ) < 0 ) {